home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 10.ddi / CONTROL.DI$ / KALMDEMO.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  7.3 KB  |  238 lines
  1. echo on
  2.  
  3. % This file demonstrates MATLAB's ability to do Kalman filtering.  
  4. % Both a steady state filter and a time varying filter are designed
  5. % and simulated below.
  6.  
  7. echo off
  8. %    Copyright (c) 1986-93 by the MathWorks, Inc.
  9. echo on
  10.  
  11. pause % Press any key to continue ...
  12.  
  13. % Given the following discrete plant
  14. A=[
  15.     1.1269   -0.4940    0.1129
  16.     1.0000         0         0
  17.          0    1.0000         0];
  18. B = [
  19.    -0.3832
  20.     0.5919
  21.     0.5191];
  22. C=[ 1 0 0];
  23. D=0;
  24. printsys(A,B,C,D)
  25.  
  26. % Design a Kalman filter to filter out Gaussian noise added to the 
  27. % input (i.e sensor noise).
  28.  
  29. pause % Press any key to continue ...
  30.  
  31. % Let's do a steady state filter design first using the function 
  32. % DLQE.  The function DLQE determines the optimal steady state 
  33. % filter gain based on the process noise covariance Q, and the 
  34. % sensor noise covariance, R.
  35.  
  36. % For your information the dcgain of this plant is
  37. ddcgain(A,B,C,D)
  38.  
  39. % and the covariance of the output due to process noise with 
  40. % covariance of 1 is:
  41. dcovar(A,B,C,D,1)
  42.  
  43. % Enter the process noise covariance:
  44. Q = input('Enter a number greater than zero (e.g. 1): ')
  45.  
  46. % Enter the sensor noise covariance:
  47. R = input('Enter a number greater than zero (e.g. 1): ')
  48.  
  49. % Now design the steady state filter gain
  50. Lss = dlqe(A,B,C,Q,R)
  51.  
  52. pause % Press any key to continue ...
  53.  
  54. % The steady state Kalman filter we have just designed has the 
  55. % following update equations:
  56. %                      -         *
  57. %  time update:        x[n+1] = Ax[n] + Bu[n]
  58. %
  59. %                      *      -                -
  60. %  observation update: x[n] = x[n] + L(y[n] - Cx[n] - Du[n])
  61.  
  62. % Since this is a steady state filter we can combine these equations
  63. % into one state model (the Kalman filter)
  64. %  -                -
  65. %  x[n+1] = (A-ALC) x[n] + AL y[n]
  66. %
  67. %  *                -           
  68. %  y[n]   = (C-CLC) x[n] + CL y[n]
  69. %  
  70. % The function DESTIM produces this model (but also includes the 
  71. % estimated states as outputs):
  72. [af,bf,cf,df] = destim(A,B,C,D,Lss,[1],[1]);
  73.  
  74. % Remove est. state outputs
  75. [af,bf,cf,df] = ssdelete(af,bf,cf,df,[],[2:4]);
  76. printsys(af,bf,cf,df,'u y','y*','x1 x2 x3')
  77.  
  78. pause % Press any key to continue ...
  79.  
  80. % Let's see how it works by generating some data and comparing the 
  81. % filtered response with the true response
  82. %     noise         noise
  83. %       |             |
  84. %       V             V
  85. % u -+->O-->[Plant]-->O--> y --O->[filter]-->y*
  86. %    |                               |
  87. %    +-------------------------------+
  88. %
  89.  
  90. % To simulate the system above, we could generate the response of 
  91. % each part separately or we could generate both together.  To 
  92. % simulate each seperately we would use DLSIM with the plant first
  93. % and then the filter.  Below we illustrate how to simulate both
  94. % together.
  95.  
  96. % First, build one plant that contains the original plant and the 
  97. % filter connected as shown in the diagram above.  Use PARALLEL and
  98. % CLOOP.   Before connecting, add a process noise input to the plant
  99. % that is the same as u and a sensor noise input.
  100. a = A; b=[B,B,zeros(3,1)]; c = C; d=[D,D,1];
  101.  
  102. % Now connect the original plant input (1) with the known input (1)
  103. % of the filter using PARALLEL.
  104. [at,bt,ct,dt] = parallel(a,b,c,d,af,bf,cf,df,[1],[1],[],[]);
  105.  
  106. % Put the plant output (1) into the filter sensor input (4)
  107. [at,bt,ct,dt] = cloop(at,bt,ct,dt,[1],[4]);
  108.  
  109. pause % Press any key to continue ...
  110.  
  111. % The complete system model now has 4 inputs 
  112. %    (plant input,process noise,sensor noise,sensor input),
  113. % 2 outputs
  114. %    (plant output with noise,filtered output),
  115. % and 6 states.
  116.  
  117. % Generate a sinusoidal input vector (known)
  118. t = [0:100]';
  119. u = sin(t/5);
  120.  
  121. % Generate process noise and sensor noise vectors
  122. pnoise = sqrt(Q)*randn(length(t),1);
  123. snoise = sqrt(R)*randn(length(t),1);
  124.  
  125. pause % Press any key to continue ...
  126.  
  127. % Now simulate using DLSIM
  128. [y,x] = dlsim(at,bt,ct,dt,[u,pnoise,snoise,zeros(length(t),1)]);
  129.  
  130. % Generate "true" response
  131. ytrue = y(:,1)-snoise;
  132.  
  133. % Plot comparison
  134. clg
  135. %subplot(211), plot(t,ytrue,t,y), xlabel('No. of samples'), ylabel('Output')
  136. %subplot(212), plot(t,ytrue-y(:,2)), xlabel('No. of samples'), ylabel('Error')
  137. %pause % Press any key to continue ...
  138. echo off
  139. subplot(211), plot(t,ytrue,t,y), xlabel('No. of samples'), ylabel('Output')
  140. subplot(212), plot(t,ytrue-y(:,2)), xlabel('No. of samples'), ylabel('Error')
  141. pause % Press any key after the plot ...
  142. echo on
  143.  
  144. % Compute covariance of error
  145. err1 = ytrue-y(:,1);
  146. err1cov = sum(err1.*err1)/length(err1);
  147. err2 = ytrue-y(:,2);
  148. err2cov = sum(err2.*err2)/length(err2);
  149.  
  150. % Covariance of error before filtering
  151. err1cov
  152.  
  153. % Covariance of error after filtering
  154. err2cov
  155.  
  156. pause % Press any key to continue ...
  157.  
  158. % Now let's form a time varying Kalman filter to perform the same
  159. % task.  A time varying Kalman filter is able to perform well even
  160. % when the noise covariance is not stationary.  However for this 
  161. % demonstration, we will use stationary covariance.
  162.  
  163. % The time varying Kalman filter has the following update equations
  164. %                      -         *
  165. %  time update:        x[n+1] = Ax[n] + Bu[n]
  166. %                      -         * 
  167. %                      P[n+1] = AP[n]A' + B*Q*B'
  168. %
  169. %                             -       -       -1
  170. %  observation update: L[n] = P[n]C'(CP[n]C'+R)
  171. %                      *      -                   -
  172. %                      x[n] = x[n] + L[n](y[n] - Cx[n] - Du[n])
  173. %                      *               -
  174. %                      P[n] = (I-L[n]C)P[n]
  175. %                      *       *
  176. %                      y[n] = Cx[n] + Du[n]
  177. %
  178.  
  179. pause % Press any key to continue ...
  180.  
  181. % Generate the noisy plant response
  182. y = dlsim(A,B,C,D,u+pnoise) + snoise;
  183.  
  184. % Now filter. We can't use DLSIM here since the system is nonlinear,
  185. % so just implement the above equations in a loop.  Use the same inputs
  186. % as before.
  187.  
  188. P=B*Q*B';         % Guess for initial state covariance
  189. x=zeros(3,1);     % Initial condition on the state
  190. yest = zeros(length(t),1);
  191. ycov = zeros(length(t),1); 
  192. for i=1:length(t)
  193.   yest(i) = C*x + D*u(i);
  194.   ycov(i) = C*P*C';
  195.  
  196.   % Time update
  197.   x = A*x + B*u(i);
  198.   P = A*P*A' + B*Q*B';
  199.  
  200.   % Observation update
  201.   L = P*C'/(C*P*C'+R);
  202.   x = x + L*(y(i) - C*x - D*u(i));
  203.   P = (eye(3)-L*C)*P;
  204. end
  205.  
  206. % Compare true response with filtered response
  207. clg
  208. %subplot(211), plot(t,y-snoise,t,yest), ylabel('Output')
  209. %subplot(212), plot(t,yest-y+snoise), ylabel('Error'), pause % Press any key
  210. echo off
  211. subplot(211), plot(t,y-snoise,t,yest), ylabel('Output')
  212. subplot(212), plot(t,yest-y+snoise), ylabel('Error'), pause % Press any key
  213. echo on
  214.  
  215. % The time varying filter also estimates the output covariance
  216. % during the estimation.  Let's plot it to see if our filter reached
  217. % steady state (as we would expect with stationary input noise).
  218. subplot(111), plot(t,ycov), ylabel('Covar'), pause % Press any key after plot
  219.  
  220. % From the covariance plot we see that the output covariance did 
  221. % indeed reach a steady state in about 5 samples.  From then on,
  222. % our time varying filter has the same performance as the steady 
  223. % state version.
  224.  
  225. % Compute covariance of error
  226. err = y-snoise-yest;
  227. errcov = sum(err.*err)/length(err)
  228.  
  229. pause % Press any key to continue ...
  230.  
  231. %Let's compare the final Kalman gain matrices
  232. L,Lss
  233.  
  234. % So we see that they both obtain the same gain matrix in steady
  235. % state.
  236.  
  237. echo off
  238.