导航系统设计专题（三）—卡尔曼滤波算法入门_人工智能

前言

扩展卡尔曼滤波（EKF）是多旋翼飞行器导航系统设计中，最常用的最优估计算法之一。其原理是在卡尔曼滤波的基础上，对非线性模型进行线性化，再进行最优估计，因此，其核心算法原理与卡尔曼滤波算法相似。本篇将围绕卡尔曼滤波展开，阐述其核心五大公式，同时，以一个简单的线性问题的MATLAB仿真实例，展示如何搭建一个完整的卡尔曼滤波器。

卡尔曼滤波算法实现

卡尔曼滤波算法的公式推导有很多，本篇不作赘述，如果希望了解其算法原理，推荐阅读秦永元的《卡尔曼滤波与组合导原理》。下面直接放出卡尔曼滤波算法的五大核心公式：

状态更新方程

导航系统设计专题（三）—卡尔曼滤波算法入门

量测更新方程

导航系统设计专题（三）—卡尔曼滤波算法入门
$F_{k}$ 称为状态转移矩阵，其描述了系统的状态方程模型。
$u_{k}$ 为模型的修正向量，用于对建立模型的修正，该项在卡尔曼滤波算法中不是必备的。
$Q_{k}$ 为过程噪声，描述了建立系统的模型准确度。
$P_{k}$ 为协方差矩阵，描述了各状态量之间的相关性。
$P_{k}^{'}$ 为经过修正的协方差矩阵。
$\hat x_{k}^{'}$ 为经过量测方程修正的状态量估计值。
$K$ 为卡尔曼增益，描述的是量测量对于状态量的修正权重。
$z_{k}$ 为观测量，多为传感器测量值或其等价值。
$H_{k}$ 为量测矩阵，描述量测值与状态值之间的关系。
$R_{k}$ 为量测噪声阵，描述传感器的测量噪声。完整的卡尔曼滤波算法流程如下图所示，
导航系统设计专题（三）—卡尔曼滤波算法入门该流程图较清晰地阐述了整个卡尔曼滤波算法的流程：

确定系统的状态转移矩阵 $F_{k}$ 与量测矩阵 $H_{k}$ 。
确定协方差矩阵初值 $P_{0}$ 与状态量初值 $x_{0}$ .
更新卡尔曼增益 $K$ 。
根据量测向量 $z_{k}$ 、卡尔曼增益 $K$ 以及量测量 $z_{k}$ ，修正状态量，得到该更新周期的状态估计值 $\hat x_{k}^{'}$ 。
更新协方差矩阵，得到 $P_{k}^{'}$ 。
根据状态转移矩阵，递推状态方程，预测下一周期状态量 $\hat x_{k}$ 。
根据状态转移矩阵，递推协方差矩阵，预测下一周期协方差阵 $P_{k}$ 。

匀速直线运动的MATLAB卡尔曼滤波估计实例

卡尔曼滤波算法的核心在于状态更新与量测更新两个部分，这里以匀速直线运动为例，建立运动方程，进而搭建对应的卡尔曼滤波器。本案例中，假设物体以2.5
$m/s$ 的速度，匀速运动，初速度为0，初始位置为6
$m$ 处。由此，可以建立对应的运动方程：
对于该线性系统，我们观测的状态量为：
导航系统设计专题（三）—卡尔曼滤波算法入门根据运动学方程，易得：
因此，状态转移矩阵
$F_{k}=\begin{bmatrix} 1&\Delta t\\ 0&1 \end{bmatrix}\\$ 本案例中，
$\Delta t$ 取为1秒。再来看量测方程：
$z_{k}=H_{k}x_{k}+V_{k}\\$ 本案例中，唯一的观测量为位置
$\vec p_{k}$ 易得：
$\begin{bmatrix} \vec p_{k}\\ \vec v_{k} \end{bmatrix} = \begin{bmatrix} 1 &0 \end{bmatrix} \begin{bmatrix} p_{k}\\ v_{k} \end{bmatrix}\\$ 由此可得，量测矩阵
$H_{k}$ ：
$H_{k}=\begin{bmatrix} 1 &0 \end{bmatrix}\\$ 一般而言，协方差阵的初值取一个较大值，本案例取值如下：
$P_{0}=\begin{bmatrix} 100&0\\ 0&100 \end{bmatrix}\\$ 状态量初值如下：
$x_{0}=\begin{bmatrix} 6\\ 0 \end{bmatrix}\\$ 本案例中，我们认为模型准确度非常高，因此，
$Q_{k}$ 取值为0，量测噪声
$R$ 取25。下面，我们将通过MATLAB的Simulink搭建系统模型，生成匀速直线运动的一组观测数据。打开MATLAB后，在命令窗中输入”simulink”，打开仿真模块，如下图所示。
导航系统设计专题（三）—卡尔曼滤波算法入门然后，我们按照上述的匀速直线运动模型，搭建对应的仿真模块，并叠加幅值为5
$m$ 的随机噪声。这里，我们分别将叠加随机噪声的量测输出x_in与x_real_in加载到工作空间，如图所示。
导航系统设计专题（三）—卡尔曼滤波算法入门在得到仿真量测数据后，我们就可以开始验证卡尔曼滤波算法的效果了。下面为MATLAB仿真代码：

close all;
F = [1 1
     0 1];
H = [1 0
     0 1];

x0 = [6 0]';
p0 = [100 0
      0   100];
  
Q = [0 0
     0 0];
 
R = [25 0
     0  1];
 
length = size(x_in.Time);
length = length(1,1); 

x_k = [];
p_k = [];
k_k = [];

plot_x = [];
x_k = x0;
p_k = p0;
for i = 1 : length
% % 状态方程更新
    x_k = F * x_k;
    p_k = F * p_k * F' + Q;    
% % 量测方程更新
    k_k = (p_k * H') * inv(H * p_k * H' + R); 
    z = [x_in.Data(i) 2.5]';
    x_k = x_k + k_k * (z - H * x_k);
    p_k = p_k - k_k * H * p_k;    
    plot_x(i) = x_k(1);         
end

figure(1)
plot(x_in.Data,'r');
hold on;
plot(plot_x,'g');
hold on;
plot(x_real_in.Data,'b')
grid on;

其仿真效果如下图所示，蓝色曲线为系统模型输出的真值，绿色为卡尔曼滤波输出的估计值，红色为叠加量测噪声之后的观测值。当量测噪声取值合适时，卡尔曼滤波的估计值能够较好地跟踪匀速直线运动这类线性系统的模型输出。
导航系统设计专题（三）—卡尔曼滤波算法入门当量测噪声取值过大时，卡尔曼滤波器的收敛速度明显变慢，这是因为量测量对于状态量修正的权重变低，如下图所示：
反之，当量测噪声取值较小时，卡尔曼滤波器的收敛速度较快，此时，卡尔曼滤波器的估计值接近于量测值。

总结

本篇主要介绍了卡尔曼滤波器的实现过程，通过一个简单的线性系统例子，在MATLAB环境下搭建了卡尔曼滤波器，并对其参数整定进行了一定的解释。然而，自然界中的对象往往不是线性的。那么，卡尔曼滤波器是否也能够适用于这些非线性对象呢？下一篇中，我们将针对非线性对象进行卡尔曼滤波算法仿真，并针对多旋翼飞行器导航系统设计中最常用的扩展卡尔曼滤波算法（EKF）进行详解。

作者简介： 一个被Coding耽误的无人机算法工程师，控制、导航略懂一二，热衷技术，喜欢乒乓、音乐、电影，欢迎交流。 知乎：@遥远的乌托邦 GitHub: https://github.com/DistantUtopia 微信公众号：@遥远的乌托邦