kalman_filter.m

Kalman滤波器经典入门matlab程序

clear
%引入一个离散控制过程的系统(线性随机微分方程)
%X(k)=A X(k-1)+B U(k)+W(k)
N=10000;Value=25;W=randn(1,N);

%带高斯噪声（过程噪声）的信号，图中为蓝线
S=Value+W;

%卡尔曼初始估计值（任意取值，P(1)不为零，为零时最优化）
X(1)=20;P(1)=8;

%均值为H*Value的高斯噪声（测量噪声）的观测变量
V=randn(1,N);H=0.2;Z=H*Value+V;
%W(k)和V(k)分别表示过程和测量的噪声,假设成高斯白噪声,协方差分别是Q，R
R=std(V).^2;Q=1e-6;     %Q假设常数,若增加过程噪声，则Q=std(W).^2;

%实际测量值，偏差是均值为H*Value的高斯噪声，图中为黄线
Y=Value+Z;              

%卡尔曼估计值，图中为红线
A=1;%假设Value是跟前一时刻的Value相同，所以A=1
B=1;U(1)=0;precision=0.0001;%状态控制量,微调精度
for k=1:N-1
%时间更新（预测）
    X(k+1)=A*X(k)+B*U(k);                           %向前推算状态变量
    %X(k+1)=A*X(k)+B*U(k)+W(k);                     %增加过程噪声
%***(1)X(k|k-1)=A X(k-1|k-1)+B U(k)
%***   A和B是系统参数
%***   X(k|k-1)是利用上一状态预测的结果
%***   X(k-1|k-1)是上一状态最优的结果
%***   U(k)为现在状态的控制量,如果没有控制量，它可以为0
%***   W(k)和V(k)分别表示过程和测量的噪声
    P(k+1)=A*P(k)*A'+Q;                             %向前推算误差协方差
%***(2)P(k|k-1)=A P(k-1|k-1)A’+Q
%***   P(k|k-1)是X(k|k-1)对应的协方差
%***   P(k-1|k-1)是X(k-1|k-1)对应的协方差
%***   A’表示 A的转置矩阵
%***   Q是系统过程的协方差

%测量更新（校正）
    Kg(k)=P(k+1)*H'*inv(H*P(k+1)*H'+R);             %计算卡尔曼增益
%***(4)Kg(k)=P(k|k-1)H’/(H P(k|k-1)H’+R)
%***   结合预测值和测量值，可以得到现在状态(k)的最优化估算值X(k|k) 
    X(k+1)=X(k+1)+Kg(k)*(Z(k)-H*X(k+1));        %由观测变量Z(k)更新估计
%***(3)X(k|k)= X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1))
%***   现在状态(k)的最优化估算值X(k|k)
%***   其中Kg为卡尔曼增益(Kalman Gain)
    P(k+1)=P(k+1)-Kg(k)*H*P(k+1);                   %更新误差协方差
%***(5)P(k|k)=（I-Kg(k) H）P(k|k-1)
%***   I为1的矩阵，对于单模型单测量，I=1
%***   当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)
%***   算法进行自回归运算

%状态控制微调
    if X(k+1)>X(k) U(k+1)=-precision;end
    if X(k+1)==X(k) U(k+1)=0;end
    if X(k+1)<X(k) U(k+1)=precision;end
end

%图形表示
t=1:N;plot(t,S,'b',t,Y,'y',t,Value,'g',t,X,'r');
%带高斯噪声（过程噪声）的信号，图中为蓝线
%实际测量值，偏差是均值为H*Value的高斯噪声，图中为黄线
%标准值，图中为绿线
%卡尔曼估计值，图中为红线