function testKF(skipgen) if nargin<1, skipgen=0; end if ~skipgen A=[1 -0.5; 1 0]; B=[1;0.1]; C=[1 0];%; 0 1]; D=0.1;%[0.1; 0]; R=0.01;%[1 0; 0 0.1]; Q=[0.2 0 ; 0 0.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); for i=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('kalman_stress.it',d,A,B,C,D,Q,R,P0,mu0) save testKF else load testKF end % init mu = mu0; P = P0; EP = [0;0]; Oxt = mu0; OPt = P0; ll =0; oxt = mu0; oPt = chol(P0)'; oll=0; tic; for t=2:N mu = A*mu + B*u(t); P = A*P*A' + Q; %Data update 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'); hold off plot(x'); hold on plot([xth]','--'); % shift the predldmatictions plot(xth2','+'); plot(xthE','o'); plot([zeros(size(xth,1),1) MuK]','d'); % shift the predictions exec_times exec_matlab./exec_times keyboard end