[717] | 1 | function testKF(skipgen)
|
---|
| 2 | if nargin<1, skipgen=0; end
|
---|
| 3 |
|
---|
| 4 | if ~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
|
---|
| 34 | else
|
---|
| 35 | load testKF
|
---|
| 36 | end
|
---|
| 37 | % init
|
---|
| 38 | mu = mu0;
|
---|
| 39 | P = P0;
|
---|
| 40 | EP = [0;0];
|
---|
| 41 |
|
---|
| 42 | Oxt = mu0;
|
---|
| 43 | OPt = P0;
|
---|
| 44 | ll =0;
|
---|
| 45 | oxt = mu0;
|
---|
| 46 | oPt = chol(P0)';
|
---|
| 47 | oll=0;
|
---|
| 48 |
|
---|
| 49 | tic;
|
---|
| 50 | for 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;
|
---|
| 66 | end
|
---|
| 67 | exec_matlab = toc
|
---|
| 68 | %keyboard
|
---|
| 69 |
|
---|
| 70 | !cd ../;./tests/testKF
|
---|
| 71 | itload('testKF_res.it');
|
---|
| 72 |
|
---|
| 73 | hold off
|
---|
| 74 | plot(x');
|
---|
| 75 | hold on
|
---|
| 76 | plot([xth]','--'); % shift the predldmatictions
|
---|
| 77 | plot(xth2','+');
|
---|
| 78 | plot(xthE','o');
|
---|
| 79 | plot([zeros(size(xth,1),1) MuK]','d'); % shift the predictions
|
---|
| 80 |
|
---|
| 81 | exec_times
|
---|
| 82 | exec_matlab./exec_times
|
---|
| 83 | keyboard
|
---|
| 84 | end
|
---|