Changeset 91 for matlab

Timestamp:

04/30/08 15:20:07 (16 years ago)

Author:

smidl

Message:

drobnosti a dokumentace

Location:

matlab

Files:

• testKF.it (modified) (previous)
• testKF.m (modified) (2 diffs)
• testKF_big.m (added)

matlab/testKF.m

@@ -49 +49 @@
 tic;
 for t=2:N
-%       mu = A*mu + B*u(t);
-%       P  = A*P*A' + Q;
-% 
-%       %Data update
-%       Ry = C*P*C' + R;
-%       iRy = inv(Ry);
-%       K = P*C'*iRy; 
-%       P = P- K*C*P; % P = P -KCP;
-%       mu = mu + K*(y(:,t)-C*mu-D*u(t));
-%       Mu(1:2,t)=mu;
+        mu = A*mu + B*u(t);
+        P  = A*P*A' + Q;
+
+        %Data update
+        Ry = C*P*C' + R;
+        iRy = inv(Ry);
+        K = P*C'*iRy; 
+        P = P- K*C*P; % P = P -KCP;
+        mu = mu + K*(y(:,t)-C*mu-D*u(t));
+        Mu(1:2,t)=mu;
 
         [Oxt,OPt,ll(t)] = Kalman(Oxt,y(:,t),A,C,Q,R,OPt);
@@ -74 +74 @@
 plot(x');
 hold on
-plot([xth]','--'); % shift the predictions
+plot([xth]','--'); % shift the predldmatictions
 plot(xth2','+');
 plot(xthE','o');