申明

  本博客参考了b站up主“华南小虎队”的卡尔曼滤波教学视频以及Lauszus Kristian Sloth Lauszus的卡尔曼滤波代码（c++实现），以及关于该代码示例中的博客教程。

一、什么是卡尔曼滤波

1.1 卡尔曼滤波的使用场景

  卡尔曼滤波是一种最优估计的算法，我们可以把他理解成一个矫正算法，或者是一种插值算法。卡尔曼滤波可以根据我们对当前任务场景的运动学建模方程以及前一时刻的状态来估计下一时刻的状态。卡尔曼滤波被广泛应用于定位导航场景中，比如疾行的汽车在隧道中穿行时，由于我们的卫星定位系统会被隧道的山体所屏蔽信号，因此，我们无法通过卫星定位的方式来直观的定位汽车在隧道中的位置，这个时候我们就可以将当前的一些状态量通过传感器获得，比如当前速度以及当前位置等作为状态量传入我们的状态方程中来进行预测，并通过卡尔曼滤波来矫正我们的预测结果。
  在机器人控制场景中，也可以使用卡尔曼滤波来赋能，对于机器人发出的控制指令往往是离散的，这样会使得我们的机器人在执行动作时显得比较“生硬”，此时我们就可以使用卡尔曼滤波来对机器人进行状态的预测来插值，使得机器人的运动变得更加丝滑。当然卡尔曼滤波也不仅局限于以上两种场景，这只是我举得例子。

1.2 卡尔曼滤波的定义

  卡尔曼滤波的适用系统线性高斯系统（这里我们提到的为常规的kalman filter 而非UKF或者EKF）。线性系统即输入与输出满足齐次性和叠加性。高斯则是指外部噪声的概率分布满足正态分布的规律。以一个具有着初速度的小车来说，小车在行驶的过程中会受到地面对于轮胎的摩擦力，空气的阻力，这两者都可以算是影响小车最终位置的外部噪声，实际情况是，空气阻力以及地面的摩擦力都不是恒定的值，那么在卡尔曼滤波中，我们将其的分布假定为满足高斯正态分布的，因此我们的小车的位置最终也是满足高斯正态分布的，如下图所示：

二、卡尔曼滤波公式详解（无推导）

首先我们先明确一下参数：

• 过程噪声满足均值为0，方差为Q的高斯正态分布；
• 观测噪声满足均值为0，方差为R的高斯正态分布；

卡尔曼滤波简单可以总结为以下两个步骤：

• 使用上一时刻的最优结果来得到当前时刻的先验估计。
• 使用第一步的先验估计以及观测到的状态来对先验估计进行矫正得到这一时刻的最优结果。

具体来说，卡尔曼滤波可以分为两个阶段，一个是预测阶段，一个是更新阶段，如下图所示：

其中，最优估计的计算我们可以拆解为先验估计 + 增益 *（观测结果 - 先验估计的观测）。 也就是说，我们可以把它看成一个权重问题，即如果我们更相信观测结果（高精度的传感器），那么我们可以将卡尔曼增益的权重增大，来扩大观测结果与先验估计误差对最终估计的影响力，如果我们更相信预测的结果，那么使卡尔曼增益的值减小，来扩大先验估计对最终结果的影响力。具体的如何通过调节参数来改变卡尔曼增益的值，我们在下面会讲到。

三、卡尔曼滤波的简单应用