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

3 年前 · 来自专栏多旋翼无人机技术开发深度剖析

浙江大学空天信息技术硕士

前言

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

卡尔曼滤波算法实现

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

状态更新方程

\hat x_{k}=F_{k}\hat x_{k-1}+B_{k}u_{k}\\ P_{k}=F_{k}P_{k-1}F_{k}^{T}+Q_{k}\\

量测更新方程

\hat x_{k}^{'}=\hat x_{k}+K(z_{k} - H_{k}\hat x_{k})\\ P_{k}^{'}=P_{k}-KH_{k}P_{k}\\ K=P_{k}H_{k}^{T}(H_{k}P_{k}H_{k}^{T}+R_{k})^{-1}\\

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 处。

由此，可以建立对应的运动方程：

p(t)=2.5v(t)+6\\

对于该线性系统，我们观测的状态量为：

x_{k}= \begin{bmatrix} p_{k}\\ v_{k} \end{bmatrix}\\

根据运动学方程，易得：

\begin{bmatrix} p_{k}\\ v_{k} \end{bmatrix} = \begin{bmatrix} 1&\Delta t\\ 0&1 \end{bmatrix} \begin{bmatrix} p_{k-1}\\ v_{k-1} \end{bmatrix}\\

因此，状态转移矩阵 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);         
figure(1)