root/library/tests/testKF.m @ 631

Revision 386, 1.3 kB (checked in by mido, 15 years ago)

possibly broken? 4th part

  • Property svn:eol-style set to native
  • Property svn:keywords set to Rev Author Date
Line 
1function testKF(skipgen)
2if nargin<1, skipgen=0; end
3
4if ~skipgen
5        A=[1 -0.5; 1 0];
6        B=[1;0.1];
7        C=[1 0];%; 0 1];
8        D=0.1;%[0.1; 0];
9        R=0.01;%[1 0; 0 0.1];
10        Q=[0.2 0 ; 0 0.2];
11
12        sQ = chol(Q)';
13        sR = chol(R)';
14
15        N =3000;
16        mu0 = [0;0];
17        P0 = 200*eye(2);
18
19        u = zeros(1,N);
20        x = zeros(2,N);
21        y = zeros(1,N);
22
23        x(:,1) = [10;10];
24        Et = sQ*randn(2,N);
25        Wt = sR*randn(1,N);
26        for i=2:N;
27                x(:,i) = A*x(:,i-1) + B*u(i) + Et(:,i);
28                y(:,i) = C*x(:,i) + D*u(i) + Wt(:,1);
29        end
30
31        d=[y;u];
32        itsave('testKF.it',d,A,B,C,D,Q,R,P0,mu0)
33        save testKF
34else
35        load testKF
36end
37% init
38mu = mu0;
39P = P0;
40EP = [0;0];
41
42Oxt = mu0;
43OPt = P0;
44ll =0;
45oxt = mu0;
46oPt = chol(P0)';
47oll=0;
48
49tic;
50for t=2:N
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
62        [Oxt,OPt,ll(t)] = Kalman(Oxt,y(:,t),A,C,Q,R,OPt);
63%       [oxt,oPt,oll(t)] = KalmanSq(oxt,y(:,t),A,C,sQ,sR,oPt);
64        MuK(1:2,t) = Oxt;
65%       MuS(1:2,t) = oxt;
66end
67exec_matlab = toc
68%keyboard
69
70!cd ../;./tests/testKF
71itload('testKF_res.it');
72
73hold off
74plot(x');
75hold on
76plot([xth]','--'); % shift the predldmatictions
77plot(xth2','+');
78plot(xthE','o');
79plot([zeros(size(xth,1),1) MuK]','d'); % shift the predictions
80
81exec_times
82exec_matlab./exec_times
83keyboard
84end
Note: See TracBrowser for help on using the browser.