卡尔曼滤波工作原理详解
时间：2022-12-26 20:00:00
详细说明卡尔曼滤波工作原理
我在网上看到很多博客翻译 How a Kalman filter works, in pictures, 本文对卡尔曼滤波进行了非常有趣的解释，. 但是目前很多翻译文章中的公式都很凌乱， 所以我决定重新整理格式. 此外, 也有很多公式，有的博主跟着原博客翻译， 解释的部分很少. 我参考了资料，做了进一步的解释. 所以, 总体而言, 降低阅读难度，提高阅读体验.
我必须告诉你关于卡尔曼滤波器的事情，因为它的功能非常惊人。
令人惊讶的是，几乎没有软件工程师和科学家真正理解卡尔曼滤波器，这让我很难过，因为它是一个通用而强大的工具，在不确定性的情况下组合信息。有时，它提取准确信息的能力似乎很神奇——如果我说得太多，看看之前发布的视频，我演示了一个卡尔曼滤波器，通过观察自由漂浮物体的速度来计算它的方向。太棒了！
什么是卡尔曼滤波器？
对于这个过滤器，我们几乎可以得出这样的结论：只要有不确定信息的动态系统，卡尔曼过滤器就可以对系统下一步要做什么做出基本的猜测。即使有噪声信息干扰，卡尔曼过滤器通常也能很好地找出发生了什么，并找出现象之间不易察觉的相关性。
你可以在任何关于动态系统的不确定信息的地方使用卡尔曼滤波器，你可以猜测下一步该做什么。即使现实中的噪音干扰了你猜测的理想运动状态，卡尔曼滤波器通常也能很好地推断出实际情况。它还可以利用奇怪现象之间的关系，你可能不会考虑使用它！
卡尔曼滤波器是持续变化系统的理想选择。它的优点是轻量化， 很少消耗内存(除了以前的状态，不需要保存任何历史记录)，速度常适合实时问题和嵌入式系统。
在谷歌上的大多数地方，实现卡尔曼滤波器的数学看起来都非常晦涩难懂。这太令人糟心了，因为如果你以正确的方式看待它，卡尔曼滤波器实际上是超级简单和容易理解的。因此，本文是一篇关于卡尔曼滤波主题很好的文章，我将尝试用许多清晰、漂亮的图片和颜色来阐明它。前提很简单: 你所需要的只是对概率和矩阵的基本理解。
我将从一个简单的例子开始，介绍卡尔曼滤波器可以解决的问题，但是如果你想直接看到精美的图片和数学知识，请随时跳转。
卡尔曼滤波器能做什么？
以玩具机器人为例：如果你开发了一个可以自由行走的小机器人，机器人需要实时了解他们的位置信息，以实现导航功能。
决定机器人的状态是什么？ 也就是说，机器人在一定时间和位移后的位置. 因此, 将机器人的运动状态表示为 x k ? \vec{x_k} xk , 它只包含位置和速度：
x k ? = ( p ? , v ? ) \vec{x_k} = (\vec{p}, \vec{v}) xk=(p,v)
注意，状态只是有关系统底层配置的数字列表；它可能是任何东西, 可以是有关油箱中液体量、汽车发动机温度、用户手指在触摸板上的位置的数据，也可以是您需要跟踪的任意数量的数据。为了方便讲解, 在我们的小机器人的示例中，状态的变化仅仅由位置和速度决定。
从外部观测条件来看, 我们为机器人配置了一个GPS传感器，精确到10米左右，这对于一般的精度来说够用了, 但是机器人需要知道自己精确的的位置。因为在树林里有很多沟壑和悬崖，如果机器人出了几英尺的差错，它可能会从悬崖上掉下来。所以GPS提供的信息还不够充分。
从机器人自身状态改变来看, 我们可能还知道一些关于机器人如何移动的信息：例如机器人知道发送给车轮马达的命令，它知道如果它朝一个方向前进，并且没有任何干扰，那么在下一瞬间它可能会沿着同一方向走得更远。但是，由于存在外部因素的干扰, 它并不完全了解自己的运动：它可能会受到风的冲击，车轮可能会稍微打滑，或者在崎岖不平的地形上滚动；因此，车轮转动的周数可能无法准确表示机器人实际行驶的距离，因此预测也不完美。
因此, 关于机器人状态而言, 现在有GPS提供的观测值和小车关于自身运动的预测值:
- GPS 传感器可以告诉我们一些关于状态的信息，但只是间接的，带有一些不确定性或不精确性。
- 我们的预测是关于机器人是如何移动的，但只是间接的，也带有一些不确定性或不精确性。
但是，如果我们使用所有可用的一系列不精确的信息，我们能通过 数据融合的方式得到一个比任何单独一个估计都更好的答案吗？当然答案是肯定的，这就是卡尔曼滤波器的作用。
如何从卡尔曼滤波的角度看待问题
我们将继续前面机器人导航的例子, 仅仅使用一个只有位置和速度的简单状态。
x ⃗ = [ p v ] \vec{x} = \begin{bmatrix} p\\ v \end{bmatrix} x=[pv]
我们不知道实际的位置 p p p 和速度 v v v 是什么；下图横坐标是速度, 纵坐标是位置. 关于速度和位置组合的情况, 他们可能会有一些可能的状态(下图中白色部分). 有一系列可能的位置和速度组合可能是正确的，但其中一些状态比其他的状态更可能：哪些状态更有可能呢? 卡尔曼滤波能够告诉我们.
卡尔曼滤波器假设两个变量（我们的例子中是: 位置 p p p和速度 v v v）都是随机和服从高斯分布的。每个变量都有一个均值 μ \mu μ，它是随机分布的中心（机器人最可能的状态），还有一个表示不确定性的方差 σ 2 \sigma^2 σ2：
在上图中，位置和速度是不相关的，这意味着一个变量的状态不会告诉你另一个变量可能是什么。
实际生活中, 位置和速度完全随机不现实, 位置和速度是存在相关性的. 下面的例子显示了更有趣的事情：位置和速度是相关的。观测小车到达特定位置的可能性取决于速度：
可能会出现这种情况，我们根据一个旧的位置估计一个新的位置。如果我们的速度很快，我们可能会走得更远，所以我们的位置会更远。如果我们行动缓慢，我们就不会走那么远。
这种关系确实很重要，因为它给了我们很多信息：一个测量值告诉我们其他测量可能是什么。这就是卡尔曼滤波器的目标，我们希望尽可能多地从不确定的测量中提取信息！
这种相关性被称为协方差矩阵。简言之，矩阵的每个元素 Σ i j \Sigma_{ij} Σij 是第 i i i 个状态变量和第 j j j 个状态变量之间的关联度。（您可以了解到协方差矩阵是对称的，这意味着交换 i i i 和 j j j 并不重要）。协方差矩阵通常标记为 Σ \mathbf{\Sigma} Σ，其中的元素为 Σ i j \Sigma_{ij} Σij 。
用矩阵描述问题
将我们对状态的了解建模为一个高斯斑点(Gaussian blob), 也就是基于高斯分布来建立状态变量，因此我们需要在时间 k k k 时获得两条信息：我们将我们的最佳估计称为 x ^ k \mathbf{\hat{x}_k} x^k（平均值，依然称为 μ \mu μ）及其协方差矩阵 P k \mathbf{P_k} Pk 。
x ^ k = [ position velocity ] P k = [ Σ p p Σ p v Σ v p Σ v v ] (1) \mathbf{\hat{x}}_k = \begin{bmatrix} \text{position}\\ \text{velocity} \end{bmatrix}\\ \mathbf{P}_k = \begin{bmatrix} \Sigma_{pp} & \Sigma_{pv} \\ \Sigma_{vp} & \Sigma_{vv} \\ \end{bmatrix} \tag {1} x^k=[positionvelocity]Pk=[ΣppΣvpΣpvΣvv](1)
（当然，我们在本文中只使用位置和速度，但记住状态可以包含任意数量的变量，并表示您想要的任何内容，这对于处理其它问题是很有用的）。
接下来，我们需要一些方法来查看 当前状态（在时刻 k − 1 k-1 k−1）并预测下一个时刻 k k k 的状态。记住，我们不知道哪个状态是 真实 状态，但我们的预测函数并不关心。它适用于所有情况，并为我们提供了一个新的分布：
我们可以用一个矩阵 F k F_k Fk 来表示这个预测步骤
它将我们原始估计中的每个点都移动到了一个新的预测位置，如果原始估计是正确的话，这个新的预测位置就是系统下一步会移动到的位置。
那我们又如何用矩阵来预测未来下一个时刻的位置和速度呢？下面用一个基本的运动学公式来表示(再次提醒: 此次示例中仅仅使用了 位置 p p p 和 速度 v v v), 假设速度是平均的, 并没有加速度噢：
p k = p k − 1 + Δ t v k − 1 v k = v k − 1 \begin{array}{c} \\ \color{deeppink}{p_k} &= \color{royalblue}{p_{k-1}} \color{black}+ \Delta t \ \color{royalblue}{v_{k-1}} \\ \color{deeppink}{v_k} &= \qquad\qquad\ \ \color{royalblue}{v_{k-1}} \end{array} pkvk=pk−1+Δt vk−1= vk−1
可能突然跳出来公式让人难以适应, 因此我就这个初中水平的公式稍微讲一下:
假设时间间隔 Δ t \Delta{t} Δt 足够小, 然后:
- 当前时刻位置=上一时刻位置+时间间隔*平均速度(平均高速度≈上一时刻速度),
- 当前时刻速度≈上一时刻速度
用矩阵表示一下:
首先, 根据前文得到:
x ^ k − 1 = [ p ^ k − 1 v ^ k − 1 ] \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \color{black} = \begin{bmatrix} \hat{p}_{k-1}\\ \hat{v}_{k-1} \end{bmatrix} \\ x^k−1=[p^k−1v^k−1]
我们假设存在 预测矩阵 和 k − 1 k-1 k−1 时刻,也就是上一时刻的状态 x ^ k − 1 \color{royalblue}{\mathbf{\hat{x}}_{k-1}} x^k−1, 那么:
x ^ k = [ 预 测 矩 阵 ] x ^ k − 1 = [ 1 Δ t 0 1 ] [ p ^ k − 1 v ^ k − 1 ] \color{deeppink}{\mathbf{\hat{x}}_k} \color{black} = \begin{bmatrix} 预测矩阵 \end{bmatrix} \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \color{black} \\ \color{black} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \color{royalblue}\begin{bmatrix} \hat{p}_{k-1}\\ \hat{v}_{k-1} \end{bmatrix} \\ x^k=[预测矩阵]x^k−1=[10Δt1][p^k−1v^元器件数据手册、IC替代型号，打造电子元器件IC百科大全！
