Changeset 8 for matlab/testKF.m

Timestamp:

01/23/08 11:32:31 (16 years ago)

Author:

smidl

Message:

Kalmany funkci, PF nefunkci

Files:

• matlab/testKF.m (modified) (4 diffs)

matlab/testKF.m

@@ -11 +11 @@
 
 N =1000;
+mu0 = [0;0];
+P0 = 200*eye(2);
 
 u = ones(1,N);
@@ -24 +26 @@
 
 d=[y;u];
-itsave('testKF.it',d)
+itsave('testKF.it',d,A,B,C,D,Q,R,P0,mu0)
+save testKF
 else
 %!cd ../testKF
 
+load testKF
 % init 
-mu = [0;0];
-P = 200*eye(2);
+mu = mu0;
+P = P0;
+EP = [0;0];
 
 for t=2:N
@@ -41 +46 @@
         K = P*C'*iRy; 
         P = P- K*C*P; % P = P -KCP;
+        EP(:,t) = eig(P); 
+        if any(EP<0), keyboard; end
         mu = mu + K*(y(t)-C*mu-D*u(t));
         Mu(1:2,t)=mu;
@@ -53 +60 @@
 hold on
 plot(xth','--');
+plot(xth2','+');
 plot(Mu',':');
 end