testKF_big.m function function skipgen skipgen 0 end if~skipgen end if ~skipgen dimx dimx 30 Apom Apom rand(dimx) Apom2 Apom2 Apom*Apom' A A Apom2/max(1.1*eig(Apom2)) B B ones(dimx,1) C C eye(dimx) D D ones(dimx,1) R R 0.1*eye(dimx) Q Q 0.2*eye(dimx) sQ sQ chol(Q)' sR sR chol(R)' N N 1000 mu0 mu0 zeros(dimx,1) P0 P0 200*eye(dimx) u u rand(1,N) x x zeros(dimx,N) y y zeros(dimx,N) Et Et sQ*randn(dimx,N) Wt Wt sR*randn(dimx,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 Oxt2 Oxt2 mu0 OPt2 OPt2 P0 ll2 ll2 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 Kalman(Oxt,y(:,t),A,C,Q,R,OPt) Kalman(Oxt2,y(:,t),A,C,Q,R/10000,OPt2) toc !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:dimx, t) Mu 1:dimx 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' '--' plot (xth2','+') plot xth2' '+' plot (xthE','o') plot xthE' 'o' plot (Mu','d') plot Mu' 'd' functiontestKF(skipgen) ifnargin<1,skipgen=0;end if~skipgen dimx=30; Apom=rand(dimx); Apom2=Apom*Apom'; A=Apom2/max(1.1*eig(Apom2)); B=ones(dimx,1); C=eye(dimx); D=ones(dimx,1); R=0.1*eye(dimx); Q=0.2*eye(dimx); sQ=chol(Q)'; sR=chol(R)'; N=1000; mu0=zeros(dimx,1); P0=200*eye(dimx); u=rand(1,N); x=zeros(dimx,N); y=zeros(dimx,N); x(:,1)=10*ones(dimx,1); Et=sQ*randn(dimx,N); Wt=sR*randn(dimx,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; Oxt2=mu0; OPt2=P0; ll2=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:dimx,t)=mu; %[Oxt,OPt,ll(t)]=Kalman(Oxt,y(:,t),A,C,Q,R,OPt); %[Oxt2,OPt2,ll2(t)]=Kalman(Oxt2,y(:,t),A,C,Q,R/10000,OPt2); end exec_matlab=toc !cd../;./tests/testKF itload('testKF_res.it'); holdoff plot(x'); holdon plot(xth','--'); plot(xth2','+'); plot(xthE','o'); plot(Mu','d'); max(max(x(:,2:end)-xth(:,1:end-1))) max(max(x-xth2)) max(max(x-xthE)) max(max(x-Mu)) exec_times exec_matlab./exec_times keyboard end