root/library/tests/stresssuite/kalman_stress_big.m @ 1353

Revision 721, 1.4 kB (checked in by mido, 15 years ago)

stresssuite - halfway point

Line 
1function testKF(skipgen)
2if nargin<1, skipgen=0; end
3
4if ~skipgen
5        dimx = 30;
6        Apom=rand(dimx);
7        Apom2 = Apom*Apom';
8        A = Apom2/max(1.1*eig(Apom2));
9        B=ones(dimx,1);
10        C=eye(dimx);
11        D=ones(dimx,1);
12        R=0.1*eye(dimx);
13        Q=0.2*eye(dimx);
14
15        sQ = chol(Q)';
16        sR = chol(R)';
17
18        N =1000;
19        mu0 = zeros(dimx,1);
20        P0 = 200*eye(dimx);
21
22        u = rand(1,N);
23        x = zeros(dimx,N);
24        y = zeros(dimx,N);
25
26        x(:,1) = 10*ones(dimx,1);
27        Et = sQ*randn(dimx,N);
28        Wt = sR*randn(dimx,N);
29        for i=2:N;
30                x(:,i) = A*x(:,i-1) + B*u(i) + Et(:,i);
31                y(:,i) = C*x(:,i) + D*u(i) + Wt(:,1);
32        end
33
34        d=[y;u];
35        itsave('kalman_stress.it',d,A,B,C,D,Q,R,P0,mu0)
36        save testKF
37else
38        load testKF
39end
40% init
41mu = mu0;
42P = P0;
43EP = [0;0];
44
45Oxt = mu0;
46OPt = P0;
47ll =0;
48Oxt2 = mu0;
49OPt2 = P0;
50ll2=0;
51
52tic;
53for t=2:N
54        mu = A*mu + B*u(t);
55        P  = A*P*A' + Q;
56
57        %Data update
58        Ry = C*P*C' + R;
59        iRy = inv(Ry);
60        K = P*C'*iRy;
61        P = P- K*C*P; % P = P -KCP;
62        mu = mu + K*(y(:,t)-C*mu-D*u(t));
63        Mu(1:dimx,t)=mu;
64
65%       [Oxt,OPt,ll(t)] = Kalman(Oxt,y(:,t),A,C,Q,R,OPt);
66%       [Oxt2,OPt2,ll2(t)] = Kalman(Oxt2,y(:,t),A,C,Q,R/10000,OPt2);
67end
68exec_matlab = toc
69
70%!./test_kalman
71itload('testKF_res.it');
72
73hold off
74plot(x');
75hold on
76plot(xth','--');
77plot(xth2','+');
78plot(xthE','o');
79plot(Mu','d');
80
81max(max(x(:,2:end) - xth(:,1:end-1)))
82max(max(x - xth2))
83max(max(x - xthE))
84max(max(x - Mu))
85
86exec_times
87exec_matlab./exec_times
88%keyboard
89end
Note: See TracBrowser for help on using the browser.