% simulate a Kalman filter with a singer acceleration model
% written by Geoffrey Goldman
% 9/04/2012
% acknowledgements: I would like to thank Jemin George for his help with the code.
% continous time model
% xdot = Ax + Bw
% y = Cx + D*v
close all
clear all
% target model = singer acceleration model
meas_model=1; % 0= meas position, 1=meas average position
T=2048/1e3; % sample time, works for t=1
w_std=0.01*pi/180; % std of process noise
R1=(0.1*pi/180)^2; % variance of measurement
KK=100; % number of iterations
N=300; % number of samples per run
PP0= [1 0 0; % initial estimate of covariance measurement error
0 ((5*pi/180)/2)^2 0;
0 0 ((0.5*pi/180))^2];
T_constant=1;
rho1=1/T_constant;
rho2=0;
rho3=0;
A= [-rho3 1 0; % continuous time model
0 -rho2 1;
0 0 -rho1];
B=[0 0 1]'*w_std;
PHI= expm(A*T);
% [0 Y ...
% 0 1 ...
% 0 0 e(-alpha*T)] % singer model
VL=[-A B*B'; zeros(3,3) A']*T; % Van Loan identity
%VL=[-A B*w_std^2*B'; zeros(3,3) A']*T; % Van Loan identity
eVL=expm(VL);
Q=PHI*eVL(1:3,4:6);
Q_test=[T^5/20 T^4/8 T^3/6; % for large T
T^4/8 T^3/3 T^2/2;
T^3/6 T^2/2 T]*w_std^2;
if (meas_model==0)
C=[1 0 0]; % measurement matrix
else
C=[1 T/2 T^2/4]; % measurement matrix
end
D=0;
% sys=ss(A,B,C,D); % define state-space model
T_array=(0:(N-1))*T; % time array
x0 =[0, 0*pi/180 , 0.0*pi/180];
Q_sqrt=chol(Q);
x1=zeros(N,3);
y1=zeros(N,1);
y=zeros(N,1);
% allocate storage for results
x1_sq_error=zeros(1,N); % squared error array for x1
x2_sq_error=zeros(1,N); % squared error array for x2
x3_sq_error=zeros(1,N); % squared error array for x3
gain_array=zeros(3,N); % kalman filter gain
innov_array=zeros(1,N);
ave_innov_array=zeros(1,N); % average innovation value
x1_hat_array=zeros(3,N); % estimate state variables
PPU_array=zeros(3,3,N); % estimate covarance matrix
Q_array=zeros(3,3,N); % estimate covarance matrix
Q_array_ave=zeros(3,3,N); % estimate covarance matrix
for k=1:KK % iterate over trials
randn('seed',k);
x_init=x0' + [randn(1,1) ; randn(1,1) ;randn(1,1)].*[PP0(1,1)^0.5 ; PP0(2,2)^0.5; PP0(3,3)^0.5];
for n=1:N % iterate over time samples
if (n==1)
PP = PP0; % predicted in|in-1
x1_pred=x0';
else
PP = PHI*PPU*PHI' + Q; % P predicted in|in-1
x1_pred=PHI*x1_hat; % predict x1
end
if (n==1)
x1(n,1:3)=x_init;
else
x1(n,1:3)=PHI*x1(n-1,1:3)' + Q_sqrt'*randn(3,1);
end
y1(n)=C*x1(n,1:3)';
y(n)=y1(n) + R1^0.5*randn(1,1);
if (n>1)
e1=(PHI*x1(n-1,:)'-x1(n,:)');
Q_array(:,:,n)=e1*e1';
end
y_pred=C*x1_pred; % predicted measurement
innov = y(n)-y_pred; % innovation
S=C*PP*C' + R1;
K = PP*C'*pinv(S); % kalman gain
x1_hat = x1_pred + K*innov; % estimate of x1
PPU = (eye(3) - K*C)*PP; % P updated in|in
x1_hat_array(:,n)=x1_hat;
PPU_array(:,:,n)=PPU;
gain_array(:,n)=K;
innov_array(n)=innov;
end % N
Q_array_ave=Q_array_ave + Q_array/KK;
x1_sq_error=x1_sq_error + ((x1_hat_array(1,:)-x1(:,1)').^2)/KK;
x2_sq_error=x2_sq_error + ((x1_hat_array(2,:)-x1(:,2)').^2)/KK;
x3_sq_error=x3_sq_error + ((x1_hat_array(3,:)-x1(:,3)').^2)/KK;
ave_innov_array=ave_innov_array + innov_array/KK;
end
figure
plot(T_array',squeeze(Q_array_ave(1,1,:)),'.-')
hold on
plot(T_array',squeeze(Q_array_ave(2,2,:)),'g.-')
hold on
plot(T_array,Q(1,1)*ones(1,N),'b')
hold on
plot(T_array,Q(2,2)*ones(1,N),'g')
hold on
legend('Q(1,1) sim','Q(2,2) sim','Q(1,1) pred','Q(2,2) pred')
% results for last trial
figure
plot(T_array',y);
hold on
plot(T_array',y1,'r.')
xlabel('time (sec)')
legend('meas','exact')
ylabel('H*y')
title('measurements')
figure
plot(T_array',x1_hat_array(1,:),'x')
hold on
plot(T_array',x1(:,1)','r')
xlabel('time (sec)')
ylabel('position')
legend('est','exact')
title('kalman filter results')
figure
plot(T_array',x1_hat_array(2,:),'x')
hold on
plot(T_array',x1(:,2)','r')
xlabel('time (sec)')
ylabel('velocity')
title('kalman filter results')
legend('est','exact')
figure
plot(T_array',ave_innov_array)
hold on
plot(T_array',innov_array,'r')
xlabel('time (sec)')
ylabel('innov')
legend('ave','last')
title('kalman filter results')
figure
plot(T_array',gain_array)
xlabel('time (sec)')
ylabel('gain')
legend('1','2','3')
title(['kalman filter gain, Q scalar=',num2str(w_std)])
figure
plot(T_array',x1_hat_array(1,:)-x1(:,1)','x')
hold on
plot(T_array',squeeze(PPU_array(1,1,:)).^0.5,'g.-')
hold on
plot(T_array',-squeeze(PPU_array(1,1,:)).^0.5,'g.-')
xlabel('time (sec)')
ylabel('position error')
legend('est pos','PPU est')
title('kalman filter results for last trial')
figure
plot(T_array',x1_hat_array(2,:)-x1(:,2)','x')
hold on
plot(T_array',squeeze(PPU_array(2,2,:)).^0.5,'g.-')
hold on
plot(T_array',-squeeze(PPU_array(2,2,:)).^0.5,'g.-')
xlabel('time (sec)')
ylabel('velocity error')
legend('est vel','PPU est')
title('kalman filter results for last trial')
figure
plot(T_array',x1_hat_array(3,:)-x1(:,3)','x')
hold on
plot(T_array',squeeze(PPU_array(3,3,:)).^0.5,'g.-')
hold on
plot(T_array',-squeeze(PPU_array(3,3,:)).^0.5,'g.-')
xlabel('time (sec)')
ylabel('acceleration error')
legend('est acc','PPU est')
title('kalman filter results for last trial')
figure
plot(T_array',x1_sq_error.^0.5,'x')
hold on
plot(T_array',squeeze(PPU_array(1,1,:)).^0.5,'g.-')
xlabel('time (sec)')
ylabel('position')
legend('meas','pred')
title('average position rms error')
plot(0,0)
figure
plot(T_array',x2_sq_error.^0.5,'x')
hold on
plot(T_array',squeeze(PPU_array(2,2,:)).^0.5,'g.-')
xlabel('time (sec)')
ylabel('velocity')
legend('meas','pred')
title('average velocity rms error ')
plot(0,0)
figure
plot(T_array',x3_sq_error.^0.5,'x')
hold on
plot(T_array',squeeze(PPU_array(3,3,:)).^0.5,'g.-')
ylabel('accel')
legend('meas','pred')
xlabel('time (sec)')
title('average acceleration rms error')
plot(0,0)
