kalman.m

MATLAB实现Kalman滤波

% kalman filtering
% 用一个线性随机微分方程来描述离散控制过程系统：X(k)=A X(k-1)+B U(k)+W(k) 
% 系统的测量值：Z(k)=H X(k)+V(k) 
% X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。
% Z(k)是k时刻的测量值，H 是测量系统的参数，对于多测量系统，H为矩阵。
% W(k)和V(k)分别表示过程和测量的噪声。 他们被假设成高斯白噪声，他们的协方差矩阵分别是Q，R（假设不随系统状态变化而变化）。
% 算法过程
% 1.更新预测值X(k|k-1):X(k|k-1)=A*X(k-1|k-1)+B*U(k)
% 2.更新X(k|k-1)状态的协方差矩阵:P(k|k-1)=A*P(k-1|k-1)*A+Q
% 3.结合预测值和测量值，得到现在状态X(k)的最佳估计值X(k|k): X(k|k)= X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1)) 
% 其中卡尔曼增益 Kg(k)= P(k|k-1) H'*inv(H P(k|k-1) H’ + R)
% 4.更新X(k|k)状态的斜方差矩阵:P(k|k)=（I-Kg(k) H）P(k|k-1) 
 
load initial_track  y s; % y:initial data,s:data with noise
clc;
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);
Y0=[0;1];            %零时刻初始值 X(0|0)
Y(:,1)=Y0;
A=[1 T
    0 1];          
B=[1/2*(T)^2 T]';     %激励转移矩阵
H=[1 0];

C0=[10 0
    0 1];             %初始误差协方差矩阵赋值 P(0|0)=P(0)
C=[C0 zeros(2,2*199)];
Q=(0.1)^2;            %过程噪声方差
R=(0.1)^2;            %测量噪声方差


% kalman algorithm ieration
for n=1:199
    i=(n-1)*2+1;
    Y(:,n+1)=A*Y(:,n);                            %更新预测值X(k|k-1):X(k|k-1)=A*X(k-1|k-1)+B*U(k)
    
    C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B' ;                 %更新X(k|k-1)状态的协方差矩阵:P(k|k-1)=A*P(k-1|k-1)*A+Q
    
    K=C(:,i+2:i+3)*H'*inv(H*C(:,i+2:i+3)*H'+R);   %卡尔曼增益Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
    
    Y(:,n+1)=Y(:,n+1)+K*(s(:,n+1)-H*Y(:,n+1));    %结合预测值和测量值，得到现在状态X(k)的最佳估计值X(k|k)
                                                  %X(k|k)= X(k|k-1)+Kg(k)*(Z(k)-H X(k|k-1)) 
                                                  
    C(:,i+2:i+3)=(eye(2,2)-K*H)*C(:,i+2:i+3);     %更新X(k|k)状态的斜方差矩阵:P(k|k)=（I-Kg(k) H）P(k|k-1) 
           
end

% the diagram of position after filtering
subplot(221)
t=0:0.1:20-0.1;
yp=Y(1,:);
plot(t,yp,'+');
axis([0 20 0 20]);
xlabel('time');
ylabel('yp position');
title('the track after kalman filtering');

% the diagram of velocity after filtering
subplot(222)
yv=Y(2,:);
plot(t,yv,'+');
xlabel('time');
ylabel('yv velocity');
title('the velocity caused by random acceleration');

subplot(223);
plot(t,yp-y);
title('位置跟踪误差');

subplot(224);
plot(t,yv-0.5);
title('速度跟踪误差');