线性卡尔曼滤波算法及示例

鲁道夫·卡尔曼（Rudolf Emil Kalman），匈牙利裔美国数学家，1930年出生于匈牙利首都布达佩斯。1957年于哥伦比亚大学获得博士学位。1964年至1971年任职斯坦福大学。1971年至1992年任佛罗里达大学数学系统理论中心主任。1972起任瑞士苏黎世联邦理工学院数学系统理论中心主任。2009年获美国国家科学奖章。

一、给出离散时间线性动态系统和三个独立的Gauss随机过程：

$\begin{cases} \ x_{k}= F_{k-1}x_{k-1}+\Gamma_{k-1}w_{k-1}\\ \ z_{k}=H_{k}x_{k}+v_{k} \\ \end{cases}$

其中：
$\ x_{k}是k时刻的系统状态向量，F_{k}是系统状态转移矩阵，而w_{k}是过程演化噪声，\Gamma_{k}是噪声矩阵，z_{k}是k时刻对系统的量测向量，H_{k}是量测矩阵，而v_{k}是量测噪声。$
$\{w_{k}\}是独立过程：w_{k}\sim N(0,Q^w_{k})$
$\{v_{k}\}是独立过程：v_{k}\sim N(0,Q^v_{k})$
$系统初始状态：x_{0}\sim N(\vec x_{0}),P_{0})$
卡尔曼滤波算法公式推到可见：最佳线性无偏估计BLUE

• 状态计算分成——–时间更新和滤波更新
  时间更新：$\hat{x}_{k|k-1}=F_{k-1}\hat{x}_{k-1}$
  滤波更新：$\hat{x}_{k}=\hat{x}_{k|k-1}+G_{k|k-1}\times \tilde{z}_{k|k-1}$
  $\quad\quad\quad\quad G_{k|k-1}=P_{k|k-1}H^{T}_{k}[H_{k}P_{k|k-1}H^{T}_{k}+Q^{v}_{k}]^{-1}$

• 方差计算分成——-时间更新和滤波更新
  时间更新：$P_{k|k-1}=F_{k-1}P_{k-1}F^{T}_{k-1}+\Gamma _{k-1}Q^{w}_{k-1}\Gamma ^{T}_{k-1}$
  滤波更新：$P_{k}=(I-G_{k|k-1}H_{k})P_{k|k-1}$

1、 Kalman滤波理论的状态转移方程，就是离散时间系统的动态描述，又称为目标的动态模型，描述的是目标运动行为。
2、目标的运动行为描述主要用于对目标的跟踪滤波，因此又称为目标跟踪模型。
3、目标跟踪的数学模型
目标跟踪的主要目的就是估计移动目标的状态轨迹。虽然目标在空间上几乎从来不是一个真正的点，但通常还是把目标看作空间没有形状的一个点，特别对于目标建模更是如此。目标动态模型描述了目标状态又随时间的演化过程。

二、Kalman滤波器的应用

举例1

举例2

从题目中可得目标点在二维平面内移动，x轴方向状态变量为:$(sx,vx,ax)$,y轴方向状态变量为:$(sy,vy)$所以整个目标状态可以用:$x=(sx,vx,ax,sy,vy)$五维向量表示
初始状态为：

$$x=\left[\begin{matrix} 1000\\0\\0\\800\\-12\end{matrix}\right]$$
状态转移矩阵:
$$F_{k}=\left[ \begin{matrix} 1 & T & T^{2}/2 & 0 & 0 \\ 0 & 1 & T & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1& T \\ 0 & 0 & 0 & 0 & 1 \end{matrix} \right] \tag{3}$$
测量转移矩阵
$$H_{k} = \left[ \begin{matrix} 1 & 0 &0&0&0 \\ 0&0&0&1&0 \end{matrix} \right]$$
不考虑飞行器内部扰动
所以 $\Gamma _{k}=[0\quad0\quad0\quad0\quad0]$
先验状态初始协方差：
$$P_{0|0}=diag(\sigma^2_{x},\sigma^2_{x}/T,\sigma^2_{x}/T^2,\sigma^2_{y},\sigma^2_{y}/T)$$
以上部分为自己设计，影响的是收敛速度。
测量噪声协方差：
$$Q^v_{k}=diag(\sigma^2_{x},\sigma^2_{y})$$
matlab代码如下：

clear;
sigma = 10000;                               %测量误差
T = 2;                                       %间隔时间步长
t1 = 400;                                    %沿y轴匀速运动
t2 = 600;                                    %向x轴方向转弯 a = 0.075
t3 = 610;                                    %匀速运动
t4 = 660;                                    %90°快转弯    a = -0.3
totaltime = 700;                             %总的观测时间为700秒 350步
V_y = -12;
ax = 0;
V_x = 0;
X_add = 0;
%初始值
f_k1 = [1 T T^2/2;0 1 T;0 0 1];
f_k2 = [1 T;0 1];
F_k = blkdiag(f_k1,f_k2);                                         %预测状态转移矩阵
X_k1 = [1000 0 ax 800 -12]';                                       %状态初始值

Z_k = [1000;800]; 
H_k = [1 0 0 0 0;0 0 0 1 0];                                      %观测矩阵为2x5维
P_k1 = diag([sigma sigma/T sigma/T^2 sigma sigma/T]);             %初始协方差矩阵
Qvk = sigma*eye(2);                                               %观测噪声协方差

for i=1:450 
   if i>200 && i<=300
        ax = 0.075;
        X_k1(3,1) = ax;
    end
    if i>300 && i<=305
        ax = 0;
        X_k1(3,1) = ax;
    end
    if i>305 && i<=330
        ax = -0.3;
        X_k1(3,1) = ax;
    end
    if i>330
         ax = 0;
         X_k1(3,1) = ax;
    end    
    X_add = X_add + V_x *T + 0.5*(ax*T^2);                       %x轴方向距离改变量
    V_x = V_x + ax*T;  
    Z_k = [1000+X_add+wgn(1,1,40);800+V_y*T*(i - 1)+wgn(1,1,40)];     
    X_k2 = F_k*X_k1;                                  %先验状态值
    P_k2 = F_k*P_k1*F_k';                             %先验协方差矩阵
    Gk = P_k2*H_k'*inv(H_k*P_k2*H_k' + Qvk);          %计算卡尔曼增益
    P_k3 = (eye(5)-Gk*H_k)*P_k2;                      %后验协方差矩阵  
    X_k3 = X_k2 + Gk*(Z_k - H_k*X_k2);       
    P_k1 = P_k3;
    X_k1 = X_k3;

    X_plot(:,i) =  X_k3; 
    Z_plot(:,i) = Z_k;
 end
%画图

%plot(Z_plot(1,:),Z_plot(2,:));
%subplot(2,1,1);

plot(Z_plot(1,:),Z_plot(2,:),X_plot(1,:),X_plot(4,:),'black');
axis([0 6000 -10000 1000])
title('目标轨迹');
xlabel('X(米)');
ylabel('Y(米)');
legend('目标测量轨迹','目标真实轨迹');