- Timestamp:
- 04/30/08 15:20:07 (17 years ago)
- Location:
- matlab
- Files:
-
- 1 added
- 2 modified
Legend:
- Unmodified
- Added
- Removed
-
matlab/testKF.m
r62 r91 49 49 tic; 50 50 for t=2:N 51 %mu = A*mu + B*u(t);52 %P = A*P*A' + Q;53 % 54 %%Data update55 %Ry = C*P*C' + R;56 %iRy = inv(Ry);57 %K = P*C'*iRy;58 %P = P- K*C*P; % P = P -KCP;59 %mu = mu + K*(y(:,t)-C*mu-D*u(t));60 %Mu(1:2,t)=mu;51 mu = A*mu + B*u(t); 52 P = A*P*A' + Q; 53 54 %Data update 55 Ry = C*P*C' + R; 56 iRy = inv(Ry); 57 K = P*C'*iRy; 58 P = P- K*C*P; % P = P -KCP; 59 mu = mu + K*(y(:,t)-C*mu-D*u(t)); 60 Mu(1:2,t)=mu; 61 61 62 62 [Oxt,OPt,ll(t)] = Kalman(Oxt,y(:,t),A,C,Q,R,OPt); … … 74 74 plot(x'); 75 75 hold on 76 plot([xth]','--'); % shift the pred ictions76 plot([xth]','--'); % shift the predldmatictions 77 77 plot(xth2','+'); 78 78 plot(xthE','o');