卡尔曼滤波实战 | 形象化理解卡尔曼滤（附源码）

一、概述

这篇文章会使用一个形象化的最小例子说明卡尔曼滤波器各个公式的推导。

如果你已经从纯数学公式推导过卡尔曼滤波器，那么这篇文章可以让你形象化地理解卡尔曼滤波器的精髓，即为什么利用[预测->更新]的循环可以得到对系统状态的估计？

如果你还在寻求对卡尔曼滤波器入门的讲解，这篇文章更可以让你建立对卡尔曼滤波器即直观又严谨的概念。如果要学习卡尔曼滤波器的完整公式化推导过程，可以查看另一篇文章：卡尔曼滤波器——基础知识(https://zhuanlan.zhihu.com/p/578920168)

二、最小例子

我们有一辆小车，小车上面装了加速度传感器，为了简便描述，该加速度传感器只会输出一个方向（设为x）上的加速度值。该加速度计每隔 时间输出小车在时刻的瞬时加速度。同时，有一个光学测量仪器可以在每一时刻测量光从原点传播到小车位置的时间。我们希望能够估计小车在每一时刻的速度和距离。

如何求解？

如果加速度计的测量是完美的，那么我们直接对加速度做积分就能得到速度，对速度再做一次积分就得到距离。但是在实际中，无论什么样的传感器的输出都是带有噪声的。因此，我们不能用该传感器的原始输出做积分。

三、卡尔曼滤波的两个步骤

卡尔曼滤波包含两个步骤：

1. 预测（prediction）—— Dynamic model

2. 更新（correction/measurment update）—— Observation model

所谓预测，就是用一个数学模型，根据当前的传感器输出，直接根据模型推导此时系统的状态。可以理解为一个方程的计算就行。

所谓的更新，就是在某些时刻或者每一时刻，获取一些系统的观测信息（可以同样是传感器的值），甚至是预测阶段中的同样的传感器的值，将其当作带噪声的真值，我们将这个值叫做测量值(measurment)。比较此刻预测的系统状态和测量的系统状态，对预测的系统状态进行修正。因此也叫测量更新（measurment update）

四、卡尔曼滤波的状态方程

状态方程

卡尔曼滤波算法假定系统在某一时刻的状态可以由上一时刻根据一个线性方程变化而来

其中

• 就是每一时刻系统的状态向量，N行1列

• 是该时刻系统的输入，M行1列

• 是系统随机噪声，N行1列；且该随机噪声是高斯噪声，均值为0，协方差矩阵为，N行N列

• 叫做状态转换矩阵，N行N列

• 称为控制矩阵，N行M列

PS:大写字母表示矩阵；小写加粗字母表示向量；小写字母表示变量