testKF.m function function skipgen skipgen 0 end if~skipgen end if ~skipgen A A [1 -0.5 B B [1 C C [1 0] D D 0.1 R R 0.01 Q Q [0.2 0 sQ sQ chol(Q)' sR sR chol(R)' N N 3000 mu0 mu0 [0 P0 P0 200*eye(2) u u zeros(1,N) x x zeros(2,N) y y zeros(1,N) Et Et sQ*randn(2,N) Wt Wt sR*randn(1,N) for for i i 2:N end end d d [y P P P0 EP EP [0 Oxt Oxt mu0 OPt OPt P0 ll ll 0 oxt oxt mu0 oPt oPt chol(P0)' oll oll 0 tic tic for for t t 2:N mu = A*mu + B*u(t) Data update Data update Ry Ry C*P*C' + R iRy iRy inv(Ry) K K P*C'*iRy mu mu mu + K*(y(:,t)-C*mu-D*u(t)) end end exec_matlab exec_matlab toc %keyboard !cd ../ function function testKF (skipgen) if nargin<1 testKF skipgen x (:, 1) x : 1 x (:, i) x : i y (:, i) y : i itsave ('testKF.it', d, A, B, C, D, Q, R, P0, mu0) save testKF else load testKF end%init mu itsave 'testKF.it' d A B C D Q R P0 mu0 Mu (1:2, t) Mu 1:2 t MuK (1:2, t) MuK 1:2 t MuS (1:2, t) MuS 1:2 t tests testKF tests testKF itload ('testKF_res.it') itload 'testKF_res.it' hold off hold off plot (x') plot x' hold on hold on plot ([xth]','--') plot [xth] ' '--' shift the predldmatictions shift the predldmatictions plot (xth2','+') plot xth2' '+' plot (xthE','o') plot xthE' 'o' plot ([zeros(size(xth, 1), 1) MuK]','d') plot [zeros(size(xth, 1), 1) MuK] ' 'd' functiontestKF(skipgen) ifnargin<1,skipgen=0;end if~skipgen A=[1-0.5;10]; B=[1;0.1]; C=[10];%;01]; D=0.1;%[0.1;0]; R=0.01;%[10;00.1]; Q=[0.20;00.2]; sQ=chol(Q)'; sR=chol(R)'; N=3000; mu0=[0;0]; P0=200*eye(2); u=zeros(1,N); x=zeros(2,N); y=zeros(1,N); x(:,1)=[10;10]; Et=sQ*randn(2,N); Wt=sR*randn(1,N); fori=2:N; x(:,i)=A*x(:,i-1)+B*u(i)+Et(:,i); y(:,i)=C*x(:,i)+D*u(i)+Wt(:,1); end d=[y;u]; itsave('testKF.it',d,A,B,C,D,Q,R,P0,mu0) savetestKF else loadtestKF end %init mu=mu0; P=P0; EP=[0;0]; Oxt=mu0; OPt=P0; ll=0; oxt=mu0; oPt=chol(P0)'; oll=0; tic; fort=2:N mu=A*mu+B*u(t); P=A*P*A'+Q; %Dataupdate Ry=C*P*C'+R; iRy=inv(Ry); K=P*C'*iRy; P=P-K*C*P;%P=P-KCP; mu=mu+K*(y(:,t)-C*mu-D*u(t)); Mu(1:2,t)=mu; [Oxt,OPt,ll(t)]=Kalman(Oxt,y(:,t),A,C,Q,R,OPt); %[oxt,oPt,oll(t)]=KalmanSq(oxt,y(:,t),A,C,sQ,sR,oPt); MuK(1:2,t)=Oxt; %MuS(1:2,t)=oxt; end exec_matlab=toc %keyboard !cd../;./tests/testKF itload('testKF_res.it'); holdoff plot(x'); holdon plot([xth]','--');%shiftthepredldmatictions plot(xth2','+'); plot(xthE','o'); plot([zeros(size(xth,1),1)MuK]','d');%shiftthepredictions exec_times exec_matlab./exec_times keyboard end