扩展卡尔曼滤波算法在陀螺仪姿态融合中的应用——上

本文基于知乎专栏《从全状态观测器到卡尔曼滤波器》系列文章总结而来，鉴于个人的水平有限，对于专栏文章中的推导过程和原理只是一知半解。为了快速地将卡尔曼滤波算法运用到项目当中，基于专栏的系列文章总结出了一部分简单的理解，可以通过本文粗浅的了解卡尔曼滤波器的基本组成结构以及运行的流程，对于原理想要有更深层次了解的建议通过专栏进一步学习。

《扩展卡尔曼滤波算法在陀螺仪姿态融合中的应用–上》简述了卡尔曼滤波器的基本组成结构以及运行流程，下一篇文章将结合大佬的开源卡尔曼代码理解卡尔曼滤波器C语言实现代码的逻辑以及运行流程。

一、概述

卡尔曼滤波器（观测器），在上个世纪五六十年代的时候提出的，到今天已经有六十年左右的时间，但卡尔曼滤波算法不管在控制、制导、导航或者通讯方面对数据的预测能力依然处在一个不可撼动的位置上。

观测增益确定方法的不同即是卡尔曼滤波器与现代控制理论中全状态观测器最核心的区别：状态观测器通过极点配置设计观测器增益，而卡尔曼滤波器通过结合量测噪声v与与系统噪声w估计误差的方差确定卡尔曼增益。

宏观上看，卡尔曼滤波器不仅仅是一个滤波器，更像是一种用于在变化的数据中去除噪声，对系统的未来做出预测的算法。亦可以理解为一个带有输入输出的系统。这个系统的输入是系统状态量的测量值（在本次应用中对应获取到的IMU数据），卡尔曼滤波器根据获取到的测量值估计系统的真实输出，同时给出最新估计值精度的大概范围，再输出为系统状态量的最优估计值（本次应用中对应小车姿态的四元数）。以此为一个周期，在重复的循环中对系统不断地测量并不断地给出最新的最优估计值，这样持续一段时间之后就能估计出系统一个非常准确的输出值。

二、卡尔曼滤波器的结构

为了便于移植到STM32F4上运行，需要将卡尔曼滤波器用C语言实现。而C语言运行的是一个离散时间的系统，所以在这里我们也采用离散时间形式学习卡尔曼滤波器的结构。

2.1 过程模型

卡尔曼滤波的过程模型与状态观测器的预测方程类似，不同的是卡尔曼滤波器的过程模型包含了系统噪声$\mathbf{w}$。

式中$\Gamma$为系统噪声驱动矩阵，${\mathbf{w}}_k$为系统噪声随机变量组成的列向量，${\mathbf{Q}}_k$为系统噪声$\mathbf{w}$的协方差矩阵。
$${\mathbf{x}}_k=F_k{\mathbf{x}}_{k-1}+B_k{\mathbf{u}}_{k-1}+{\Gamma }_{k-1}{\mathbf{w}}_{k-1},{\mathbf{w}}_k\sim N\left(0_{n\times 1},Q_k\right)$$

2.2 量测模型

卡尔曼滤波器量测模型中，${\mathbf{z}}_{\mathbf{k}}$为量测向量，${\mathbf{H}}_k$为量测矩阵，${\mathbf{v}}_k$为量测噪声随机变量组成的列向量，${\mathbf{R}}_k$为 量测噪声${\mathbf{v}}_k$的协方差矩阵。
$${\mathbf{z}}_k=H_k{\mathbf{x}}_k+{\mathbf{v}}_k,{\mathbf{v}}_k\sim N\left(0_{m\times 1},R_k\right)$$

2.3 先验估计

由于量测噪声${\mathbf{v}}_k$和系统噪声${\mathbf{w}}_k$是我们无法完全掌握的，因此我们在计算中无法得到真实的状态$\mathbf{x}$，只能得到其估计值$\hat{\mathbf{x}}$，即
$${\hat{\mathbf{x}}}_{k\mid k-1}=F_k{\hat{\mathbf{x}}}_{k-1\mid k-1}+B_k{\mathbf{u}}_{k-1}$$

2.4 后验估计（修正模型）

卡尔曼滤波器的修正模型类似于状态观测器的修正模型，唯一的区别是反馈增益的确定方法不同。

其中${\mathbf{K}}_k$为卡尔曼增益，不同于状态观测器通过极点配置确定增益，卡尔曼滤波器结合系统噪声与量测噪声确定增益。
$${\hat{\mathbf{x}}}_{k\mid k}={\hat{\mathbf{x}}}_{k\mid k-1}+K_k\left({\mathbf{z}}_k-H_k{\hat{\mathbf{x}}}_{k\mid k-1}\right)$$

2.5 卡尔曼增益

求解卡尔曼增益$K_k$的目的是为了使得估计值${\hat{\mathbf{x}}}_k$趋近于实际值${\mathbf{x}}_k$。由于本文主要目的是应用卡尔曼滤波算法，所以暂时忽略繁琐的求解过程，对求解过程感兴趣的朋友可以通过下面这一个系列专栏了解（卡尔曼增益求解过程在专栏中的第三篇文章）。最终可以得到使得$P_{k\mid k}$矩阵的迹最小的卡尔曼增益$K_k$。结果如下：
$$K_k=P_{k\mid k-1}{H}_k^T{\left(H_k{P}_{k\mid k-1}{H}_k^T+R_k\right)}^{-1}$$

三、卡尔曼滤波器的应用过程

了解完卡尔曼滤波器的结构（忽略推导过程），就可以开始着手卡尔曼滤波器的应用。接下来了解一下卡尔曼滤波器应用过程的具体步骤。

3.1 构建模型

分别建立过程模型和量测模型，其中，${\mathbf{w}}_k$和 ${\mathbf{v}}_k$ 是互不相关的高斯白噪声，他们的噪声方差矩阵分别为 ${\mathbf{Q}}_k$和 ${\mathbf{R}}_k$ 。

过程模型
$${\mathbf{x}}_k=F_k{\mathbf{x}}_{k-1}+B_k{\mathbf{u}}_{k-1}+{\Gamma }_{k-1}{\mathbf{w}}_{k-1},{\mathbf{w}}_k\sim N\left(0_{n\times 1},Q_k\right)$$
量测模型
$${\mathbf{z}}_k=H_k{\mathbf{x}}_k+{\mathbf{v}}_k,{\mathbf{v}}_k\sim N\left(0_{m\times 1},R_k\right)$$

3.2 状态初始化

状态初始化，顾名思义即在系统开始运行时初始化变量，给系统的各个变量赋初值。

当$\mathbf{k}=0$时，取${\mathbf{P}}_{0|0}={\mathbf{P}}_0$,${\hat{\mathbf{x}}}_{0|0}={\hat{\mathbf{x}}}_0$ 。
$$\{\begin{array}{l}{\hat{\mathbf{x}}}_0=\mathbf{E}\left({\mathbf{x}}_0\right)\\ P_0=\mathbf{E}\left[\left({\mathbf{x}}_0-\mathbf{E}\left({\mathbf{x}}_0\right)\right){\left({\mathbf{x}}_0-\mathbf{E}\left({\mathbf{x}}_0\right)\right)}^T\right]\end{array}$$

3.3 先验估计

$${\hat{\mathbf{x}}}_{k\mid k-1}=F_k{\hat{\mathbf{x}}}_{k-1\mid k-1}+B_k{\mathbf{u}}_{k-1}$$

3.4 先验误差协方差更新

$$P_{k\mid k-1}=F_k{P}_{k-1\mid k-1}{F}_k^T+Q_{k-1}$$

3.5 卡尔曼增益计算

$$K_k=P_{k\mid k-1}{H}_k^T{\left(H_k{P}_{k\mid k-1}{H}_k^T+R_k\right)}^{-1}$$

3.6 后验估计

$${\hat{\mathbf{x}}}_{k\mid k}={\hat{\mathbf{x}}}_{k\mid k-1}+K_k\left({\mathbf{z}}_k-H_k{\hat{\mathbf{x}}}_{k\mid k-1}\right)$$

3.7 后验误差协方差更新

$$P_{k\mid k}=\left(I-K_k{H}_k\right)P_{k\mid k-1}$$