function testKF(skipgen) if nargin<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); 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; Oxt2 = mu0; OPt2 = P0; ll2=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: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 %!./test_kalman itload('testKF_res.it'); hold off plot(x'); hold on 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