% kalman filtering
load initial_track s y; % y:initial data,s:data with noise
T=0.1;
% yp denotes the sample value of position
% yv denotes the sample value of velocity
% Y=[yp(n);yv(n)];
% error deviation caused by the random acceleration
% known data
Y=zeros(2,200); %产生2*200的全零矩阵
Y0=[0;1];
Y(:,1)=Y0; %Y的第一列等于Y0
A=[1 T
0 1];
B=[1/2*(T)^2 T]';
H=[1 0];
P0=[0 0
0 1];
P=[P0 zeros(2,2*199)];
Q=(0.25)^2;
R=(0.25)^2;
X=zeros(1,200);
% kalman algorithm ieration
for n=1:200
i=(n-1)*2+1;
K=P(:,i:i+1)*H'*inv(H*P(:,i:i+1)*H'+R);%滤波增益矩阵
Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n)); %状态估计
Y(:,n+1)=A*Y(:,n); %状态一步预测
P(:,i:i+1)=(eye(2,2)-K*H)*P(:,i:i+1);%误差方程阵
P(:,i+2:i+3)=A*P(:,i:i+1)*A'+B*Q*B';%随机线性离散系统kalman滤波基本方程
X(n)=y(n)-Y(1,n);
end
average=zeros(1,1);
variance=zeros(1,1);
for i=1:200
average=average+X(i);
end
average=average/200;
for i=1:200
variance=variance+(X(i)-average)^2;
end
variance=variance/200;
% the diagram of position after filtering
figure(3)
t=0:0.1:20;
yp=Y(1,:);
plot(t,yp,'-');average
axis([0 20 0 20]);
xlabel('time');
ylabel('yp position');
title('the track after kalman filtering');
% the diagram of velocity after filtering
figure(4)
yv=Y(2,:);
plot(t,yv,'-');variance
xlabel('time');
ylabel('yv velocity');
title('the velocity caused by random acceleration');
