Changeset 9 for matlab

Timestamp:

01/23/08 13:27:35 (16 years ago)

Author:

smidl

Message:

 

Location:

matlab

Files:

• testKF (added)
• testKF.m (modified) (2 diffs)

matlab/testKF.m

@@ -1 @@
-if 0
-A=[1 -0.5; 1 0];
-B=[1;0];
-C=[1 0];
-D=[0];
-R=[0.1];
-Q=[0.2 0 ; 0 0.02]; 
+function testKF(skipgen)
+if nargin<1, skipgen=0;
 
-sQ = chol(Q)';
-sR = chol(R)';
+if ~skipgen
+        A=[1 -0.5; 1 0];
+        B=[1;0];
+        C=[1 0];
+        D=[0];
+        R=[0.1];
+        Q=[0.2 0 ; 0 0.02]; 
 
-N =1000;
-mu0 = [0;0];
-P0 = 200*eye(2);
+        sQ = chol(Q)';
+        sR = chol(R)';
 
-u = ones(1,N);
-x = zeros(2,N);
-y = zeros(1,N);
+        N =1000;
+        mu0 = [0;0];
+        P0 = 200*eye(2);
 
-x(:,1) = [10;10];
-for i=2:N;
-        et(1:2,i) = sQ*randn(2,1);
-        x(:,i) = A*x(:,i-1) + B*u(i) + et(:,i);
-        y(:,i) = C*x(:,i) + D*u(i) + sR*randn(1,1);
+        u = ones(1,N);
+        x = zeros(2,N);
+        y = zeros(1,N);
+
+        x(:,1) = [10;10];
+        for i=2:N;
+                et(1:2,i) = sQ*randn(2,1);
+                x(:,i) = A*x(:,i-1) + B*u(i) + et(:,i);
+                y(:,i) = C*x(:,i) + D*u(i) + sR*randn(1,1);
+        end
+
+        d=[y;u];
+        itsave('testKF.it',d,A,B,C,D,Q,R,P0,mu0)
+        save testKF
+else
+        load testKF
 end
-
-d=[y;u];
-itsave('testKF.it',d,A,B,C,D,Q,R,P0,mu0)
-save testKF
-else
-%!cd ../testKF
-
-load testKF
 % init 
 mu = mu0;
@@ -46 +48 @@
         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;
 end
 
-
-
+!./testKF
 itload('testKF_res.it');