卡尔曼滤波工作原理详解

网上看到有不少的博客翻译 How a Kalman filter works, in pictures, 这篇文章中非常有趣的解释了卡尔曼滤波,. 但是目前很多翻译的文章中公式显示凌乱, 因此我决定重新整理一下格式. 此外, 也有很多公式有些博主也是跟着原博客翻译过来, 解释的部分少. 我参考了一下资料做了进一步的解释. 所以, 总体而言, 降低了阅读难度和提升了阅读体验.

我必须告诉你关于卡尔曼滤波器的事情，因为它的功能非常惊人。

令人意外的是，几乎没有软件工程师和科学家真正理解卡尔曼滤波器，这让我很难过，因为它是一个在存在不确定性的情况下组合信息的通用而强大的工具。有时，它提取准确信息的能力似乎很神奇——如果听起来我说得太多了，那么看看之前发布的视频，我演示了一个卡尔曼滤波器，通过观察自由漂浮物体的速度来计算其方向。真棒！

卡尔曼滤波是什么？

对于这个滤波器，我们几乎可以下这么一个定论：只要是存在不确定信息的动态系统，卡尔曼滤波就可以对系统下一步要做什么做出有根据的推测。即便有噪声信息干扰，卡尔曼滤波通常也能很好的弄清楚究竟发生了什么，找出现象间不易察觉的相关性。

你可以在任何关于某个动态系统的不确定信息的地方使用卡尔曼滤波器，你可以对系统下一步要做什么做出有根据的猜测。即使在现实中出现噪声并干扰了你所猜测的理想的运动状态，卡尔曼滤波器通常也能很好地推断出实际发生的情况。它还可以利用怪异现象之间的关联，而这一点你可能不会想到去利用！

卡尔曼滤波对于持续变化的系统是理想的选择。它的优点是轻量化, 消耗极少的内存（除了以前的状态之外，不需要保存任何历史记录），而且速度非常快，非常适合实时问题和嵌入式系统。

在谷歌上的大多数地方，实现卡尔曼滤波器的数学看起来都非常晦涩难懂。这太令人糟心了，因为如果你以正确的方式看待它，卡尔曼滤波器实际上是超级简单和容易理解的。因此，本文是一篇关于卡尔曼滤波主题很好的文章，我将尝试用许多清晰、漂亮的图片和颜色来阐明它。前提很简单: 你所需要的只是对概率和矩阵的基本理解。

我将从一个简单的例子开始，介绍卡尔曼滤波器可以解决的问题，但是如果你想直接看到精美的图片和数学知识，请随时跳转。

我们可以用卡尔曼滤波器做什么？

以一个玩具机器人为例：假如你研制了一个可以自由行走的小机器人，为了让它实现导航的功能，机器人需要实时的了解到知道自己所处的位置信息。

机器人的状态是有什么决定呢? 那就是机器人上一时刻所在的位置和经过一定时间和位移后新时刻的位置. 因此, 将机器人的运动状态表示为 xk⃗\vec{x_k}xk , 其仅仅包含了位置和速度:
xk⃗=(p⃗,v⃗)\vec{x_k} = (\vec{p}, \vec{v}) xk=(p,v)

注意，状态只是有关系统底层配置的数字列表；它可能是任何东西, 可以是有关油箱中液体量、汽车发动机温度、用户手指在触摸板上的位置的数据，也可以是您需要跟踪的任意数量的数据。为了方便讲解, 在我们的小机器人的示例中，状态的变化仅仅由位置和速度决定。

从外部观测条件来看, 我们为机器人配置了一个GPS传感器，精确到10米左右，这对于一般的精度来说够用了, 但是机器人需要知道自己精确的的位置。因为在树林里有很多沟壑和悬崖，如果机器人出了几英尺的差错，它可能会从悬崖上掉下来。所以GPS提供的信息还不够充分。

从机器人自身状态改变来看, 我们可能还知道一些关于机器人如何移动的信息：例如机器人知道发送给车轮马达的命令，它知道如果它朝一个方向前进，并且没有任何干扰，那么在下一瞬间它可能会沿着同一方向走得更远。但是，由于存在外部因素的干扰, 它并不完全了解自己的运动：它可能会受到风的冲击，车轮可能会稍微打滑，或者在崎岖不平的地形上滚动；因此，车轮转动的周数可能无法准确表示机器人实际行驶的距离，因此预测也不完美。

因此, 关于机器人状态而言, 现在有GPS提供的观测值和小车关于自身运动的预测值:

GPS 传感器可以告诉我们一些关于状态的信息，但只是间接的，带有一些不确定性或不精确性。
我们的预测是关于机器人是如何移动的，但只是间接的，也带有一些不确定性或不精确性。

但是，如果我们使用所有可用的一系列不精确的信息，我们能通过数据融合的方式得到一个比任何单独一个估计都更好的答案吗？当然答案是肯定的，这就是卡尔曼滤波器的作用。

如何从卡尔曼滤波的角度看待问题

我们将继续前面机器人导航的例子, 仅仅使用一个只有位置和速度的简单状态。

x⃗=[pv]\vec{x} = \begin{bmatrix} p\\ v \end{bmatrix} x=[pv]

我们不知道实际的位置 ppp 和速度vvv 是什么；下图横坐标是速度, 纵坐标是位置. 关于速度和位置组合的情况, 他们可能会有一些可能的状态(下图中白色部分). 有一系列可能的位置和速度组合可能是正确的，但其中一些状态比其他的状态更可能：哪些状态更有可能呢? 卡尔曼滤波能够告诉我们.

卡尔曼滤波器假设两个变量（我们的例子中是: 位置ppp和速度vvv）都是随机和服从高斯分布的。每个变量都有一个均值 μ\muμ，它是随机分布的中心（机器人最可能的状态），还有一个表示不确定性的方差 σ2\sigma^2σ2：

在上图中，位置和速度是不相关的，这意味着一个变量的状态不会告诉你另一个变量可能是什么。
实际生活中, 位置和速度完全随机不现实, 位置和速度是存在相关性的. 下面的例子显示了更有趣的事情：位置和速度是相关的。观测小车到达特定位置的可能性取决于速度：

可能会出现这种情况，我们根据一个旧的位置估计一个新的位置。如果我们的速度很快，我们可能会走得更远，所以我们的位置会更远。如果我们行动缓慢，我们就不会走那么远。

这种关系确实很重要，因为它给了我们很多信息：一个测量值告诉我们其他测量可能是什么。这就是卡尔曼滤波器的目标，我们希望尽可能多地从不确定的测量中提取信息！

这种相关性被称为协方差矩阵。简言之，矩阵的每个元素 Σij\Sigma_{ij}Σij 是第 iii 个状态变量和第 jjj 个状态变量之间的关联度。（您可以了解到协方差矩阵是对称的，这意味着交换 iii 和 jjj 并不重要）。协方差矩阵通常标记为 Σ\mathbf{\Sigma}Σ，其中的元素为 Σij\Sigma_{ij}Σij 。

用矩阵描述问题

将我们对状态的了解建模为一个高斯斑点(Gaussian blob), 也就是基于高斯分布来建立状态变量，因此我们需要在时间 kkk 时获得两条信息：我们将我们的最佳估计称为 x^k\mathbf{\hat{x}_k}x^k（平均值，依然称为 μ\muμ）及其协方差矩阵 Pk\mathbf{P_k}Pk 。

x^k=[positionvelocity]Pk=[ΣppΣpvΣvpΣvv](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−1k-1k−1）并预测下一个时刻 kkk 的状态。记住，我们不知道哪个状态是真实状态，但我们的预测函数并不关心。它适用于所有情况，并为我们提供了一个新的分布：

我们可以用一个矩阵 FkF_kFk 来表示这个预测步骤

它将我们原始估计中的每个点都移动到了一个新的预测位置，如果原始估计是正确的话，这个新的预测位置就是系统下一步会移动到的位置。

那我们又如何用矩阵来预测未来下一个时刻的位置和速度呢？下面用一个基本的运动学公式来表示(再次提醒: 此次示例中仅仅使用了位置 ppp 和速度vvv), 假设速度是平均的, 并没有加速度噢：

pk=pk−1+Δtvk−1vk=vk−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−1v^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−1k-1k−1 时刻,也就是上一时刻的状态 x^k−1\color{royalblue}{\mathbf{\hat{x}}_{k-1}}x^k−1, 那么:

x^k=[预测矩阵]x^k−1=[1Δt01][p^k−1v^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^k−1]
因此, 公式化后就是:
x^k=[1Δt01]x^k−1(2)\begin{array}{c} \color{deeppink}{\mathbf{\hat{x}}_k} \color{black} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \\ \end{array} \tag {2} x^k=[10Δt1]x^k−1(2)

x^k=Fkx^k−1(3)\color{deeppink}{\mathbf{\hat{x}}_k} \color{black} = \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \tag {3} x^k=Fkx^k−1(3)

我们现在有了 预测矩阵 Fk\mathbf{F}_kFk，它给出了我们的下一个状态，但我们仍然不知道如何更新协方差矩阵(提示: 公式1中)。

注意, 预测矩阵 Fk\mathbf{F}_kFk 中, 我们需要重点注意其中的各个量的意义, 也就是我们在前文中提到的影响状态变化的矩阵.
[1Δt01]\begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} [10Δt1]
其中 Δt\Delta tΔt 表示时间间隔, 因此实践中需要改为确切的时间间隔值.
其他几个量也是根据实际的需求进行更改.

前文中提到: 注意，状态只是有关系统底层配置的数字列表；它可能是任何东西, 可以是有关油箱中液体量、汽车发动机温度、用户手指在触摸板上的位置的数据，也可以是您需要跟踪的任意数量的数据。为了方便讲解, 在我们的小机器人的示例中，状态的变化仅仅由位置和速度决定。

这就是我们需要另一个公式的地方。如果我们用矩阵 A\color{firebrick}{\mathbf{A}}A 乘以分布中的每个点，那么它的协方差矩阵 Σ\SigmaΣ 会发生什么变化？

这是很简单的一步。我先抛出一个概率论公示的定理：

Cov(x)=ΣCov(Ax)=AΣAT(4)\begin{array}{c} Cov(x) = \Sigma\\ Cov(\color{firebrick}{\mathbf{A}} \color{black}x) = \color{firebrick}{\mathbf{A}} \color{black} \Sigma \color{firebrick}{\mathbf{A}}^ \color{black}T \end{array} \tag {4} Cov(x)=ΣCov(Ax)=AΣAT(4)

因此, 结合公式 4 和公式 3, 得到:

x^k=Fkx^k−1Pk=FkPk−1FkT(5)\begin{array}{c} \color{deeppink}{\mathbf{\hat{x}}_k} & = \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \\ \color{deeppink}{\mathbf{P}_k} & = \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \color{black} \mathbf{F}_k^T \end{array} \tag {5} x^kPk=Fkx^k−1=FkPk−1FkT(5)

外部影响/干扰

不过，我们还没有考虑到到所有影响因素。可能会有一些与系统本身无关的变化——外部世界可能会影响系统。

例如，如果模拟列车的运动状态，列车操作员可能会踩下油门，导致列车加速。类似地，在我们的机器人示例中，导航软件可能会发出转动车轮或停止的命令。如果我们知道这个关于世界上正在发生什么的额外信息，我们可以把它塞进一个叫做 uk⃗\color{darkorange}{\vec{\mathbf{u}_k}}uk 的向量中, 它是作为修正加入我们的预测中。

假设我们知道由于油门设置或控制命令而产生的预期加速度 a\color{darkorange}{a}a。根据基本运动学，我们得到：

初中物理位移和时间的关系:时刻k的位置=上一时刻位置+时间间隔∗上一时刻速度+12∗加速度∗时间间隔2x1=x0+v0t+12at2速度和时间的关系:时刻k的速度=上一时刻的速度+加速度+时间间隔v1=v0+at初中物理 \\ 位移和时间的关系: 时刻k的位置=上一时刻位置+时间间隔*上一时刻速度+\frac{1}{2}*加速度*时间间隔^2 \\ \begin{array}{c} x_1=x_0 + v_{0} t+\frac{1}{2} a t^{2} \end{array} \\ 速度和时间的关系: 时刻k的速度=上一时刻的速度 + 加速度 + 时间间隔\\ v_1=v_0+at 初中物理位移和时间的关系:时刻k的位置=上一时刻位置+时间间隔∗上一时刻速度+21∗加速度∗时间间隔2x1=x0+v0t+21at2速度和时间的关系:时刻k的速度=上一时刻的速度+加速度+时间间隔v1=v0+at

基于以上初中物理学知识, 得到:

pk=pk−1+Δtvk−1+12aΔt2vk=vk−1+aΔt\begin{array}{c} \color{deeppink}{p_k} &= & \color{royalblue}{p_{k-1}} & + {\Delta t}\color{royalblue}{v_{k-1}} + &\frac{1}{2} \color{darkorange}{a} \color{block} {\Delta t}^2 \\ \color{deeppink}{v_k} & = &\color{royalblue}{v_{k-1}} & + & \color{darkorange}{a} \color{block} {\Delta t} \end{array} pkvk==pk−1vk−1+Δtvk−1++21aΔt2aΔt

以矩阵的形式表示就是(对于公式 5 的完善)：

x^k=Fkx^k−1+[Δt22Δt]a=Fkx^k−1+Bkuk⃗(6)\begin{array}{c} \color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \color{black} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} \color{darkorange}{a} \\ &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \color{black} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}} \end{array} \tag {6} x^k=Fkx^k−1+[2Δt2Δt]a=Fkx^k−1+Bkuk(6)

Bk\mathbf{B}_kBk 被称为控制矩阵(control matrix),uk⃗\color{darkorange}{\vec{\mathbf{u}_k}}uk 是控制向量。（对于没有外部影响的非常简单的系统，可以忽略这些）。
在实践中, 如果添加了外部影响后，模型的准确率依然上不去，这又是为什么呢？
让我们再添加一个细节–外部干扰.

外部干扰

如果这些状态量是基于系统自身的属性或者已知的外部控制作用来变化的，则不会出现什么问题。

但是如果存在我们不知道的干扰呢？例如，如果我们跟踪一架四轮直升机，它可能会受到风的冲击。如果我们在跟踪一个带轮子的机器人，轮子可能会打滑，或者地面上的颠簸会使它减速。我们无法跟踪这些事情，如果发生任何这种情况，我们的预测可能会出现偏差，因为我们没有考虑这些额外的干扰。

通过在每个预测步骤后添加一些新的不确定性，我们可以对与 “外界”（即我们未跟踪的事物）相关的不确定性进行建模：

如上图所示, 我们将原始估计中的每个状态变量更新到新的状态后(紫色部分)，仍然服从高斯分布。我们可以认为 x^k−1\color{royalblue}{\mathbf{\hat{x}}_{k-1}}x^k−1 中的每个点被移动到协方差为 Qk\color{mediumaquamarine}{\mathbf{Q}_k}Qk , 并且服从高斯分布的取域。另一种说法是，我们将未跟踪的干扰信息视为具有协方差 Qk\color{mediumaquamarine}{\mathbf{Q}_k}Qk 的噪声。

　　
这产生了具有不同协方差（但是具有相同的均值）的新的高斯分布。换句话说, 也就是 这个紫色的高斯分布拥有和原分布相同的均值，但协方差不同。

我们通过简单地添加 Qk{\color{mediumaquamarine}{\mathbf{Q}_k}}Qk 得到扩展协方差(expanded covariance)，给出了预测步骤的完整表达式：

x^k=Fkx^k−1+Bkuk⃗Pk=FkPk−1FkT+Qk(7)\begin{array}{c} \color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \color{black} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}} \\ \color{deeppink}{\mathbf{P}_k} &= \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \color{black} \mathbf{F}_k^T + \color{mediumaquamarine}{\mathbf{Q}_k} \end{array} \tag {7} x^kPk=Fkx^k−1+Bkuk=FkPk−1FkT+Qk(7)

简而言之:

新的最佳估计是根据以前的最佳估计作出的预测，加上对已知外部影响的修正。
新的不确定性是从旧的不确定性中预测出来的，还有一些来自环境不确定性的额外干扰。

好吧，目前为止还很简单。我们对系统可能的位置有了一个模糊估计，由 x^k\color{deeppink}{\mathbf{\hat{x}}_k}x^k 和 Pk\color{deeppink}{\mathbf{P}_k}Pk 给出。

注意, 这仅仅是从车自身的状态进行估计, 我们还有传感器读数信息还没利用上.

当我们结合了从传感器获得一些数据时会发生什么？

那就是会让预测更加的精确.

用测量值来修正估计值

我们可能有几个传感器，可以提供系统状态的信息。就目前而言，他们衡量什么并不重要；也许一个读取位置，另一个读取速度。每个传感器都间接地告诉我们一些关于状态的信息——换句话说，传感器在一个状态下工作并产生一组读数。

请注意，读数的单位和刻度可能与我们跟踪的状态的单位和刻度不同。所以我们将用矩阵 HkH_kHk 对传感器建模.

注意: 这里的维度不一致的表述, 后面有助于理解 HkH_kHk

我们可以用一般的方法计算出传感器读数的分布：

μ⃗expected=Hkx^kΣexpected=HkPkHkT(8)\begin{array}{c} \begin{aligned} \vec{\mu}_{\text{expected}} &= \mathbf{H}_k \color{deeppink}{\mathbf{\hat{x}}_k} \\ \mathbf{\Sigma}_{\text{expected}} &= \mathbf{H}_k \color{deeppink}{\mathbf{P}_k} \color{black} \mathbf{H}_k^T \end{aligned} \end{array} \tag {8} μexpectedΣexpected=Hkx^k=HkPkHkT(8)

卡尔曼滤波器最擅长的一件事就是处理传感器噪声。换句话说，我们的传感器或多或少有点不可靠，原始估计中的每个状态都可以和一定范围内的传感器读数对应起来。也就是机器人小车的一个状态事实上可以产生多种读数.

从我们观察到的每个读数中，我们可能会猜测我们的系统处于特定的状态。但由于存在不确定性，一些状态比其他状态更接近我们看到的读数：

看到这句话, 你想起来什么? 没错, 前面我们说过类似的话.

我们将这种不确定性（即传感器噪声）的协方差称为 Rk\color{mediumaquamarine}{\mathbf{R}_k}Rk。这个分布的平均值等于我们观察到的读数，我们称之为 zk⃗\color{yellowgreen}{\vec{\mathbf{z}_k}}zk.

注意: 这里的分布均值是从传感器的读数中计算得到的, 协方差也是.

现在我们有两个高斯分布：一个围绕着我们的机器人小车状态转换预测的平均值，另一个围绕着我们从传感器中观察得到的实际传感器读数。

我们必须尝试根据预测状态（粉红色）对读数的猜测与根据实际观察到的传感器读数（绿色）之间找到最优解。

那么, 我们最有可能的新状态是什么？对于任何可能的读数 (z1,z2)(z_1,z_2)(z1,z2)，我们有两个相关的概率：
（1）在 (z1,z2)(z_1,z_2)(z1,z2)状态的时候, 传感器读数 zk⃗\color{yellowgreen}{\vec{\mathbf{z}_k}}zk 的（错误/正确）测量的概率.
（2）根据我们先前估计, 认为在 (z1,z2)(z_1,z_2)(z1,z2) 时刻我们应该看到的读数的概率。

如果我们想知道这两种情况都可能发生的概率，将这两个高斯分布相乘就可以了。

我们剩下的是重叠，两个斑点都很亮/很可能出现的区域。这比我们之前的估计要精确得多。这种分布的平均值是两种估计值最有可能出现的配置，因此，根据我们掌握的所有信息，它是对真实配置的最佳猜测。

嗯，这看起来像另一个高斯分布。

事实证明，当你将两个具有独立均值和协方差矩阵的高斯分布相乘时，你会得到一个具有自己均值和协方差矩阵的新高斯分布！还剩下什么问题呢? 那就是参数更新：必须有一个公式从旧的参数中得到新的参数！

融合高斯分布

简单起见, 首先从一维角度来看。方差为 σ2\sigma^2σ2 且平均值为 μ\muμ 的一维高斯贝尔曲线定义为：

N(x,μ,σ)=1σ2πe−(x–μ)22σ2(9)\begin{array}{c} \mathcal{N}(x, \mu,\sigma) = \frac{1}{ \sigma \sqrt{ 2\pi } } e^{ -\frac{ (x – \mu)^2 }{ 2\sigma^2 } } \end{array} \tag {9} N(x,μ,σ)=σ2π1e−2σ2(x–μ)2(9)

我们想知道两条高斯曲线相乘时会发生什么。下面的蓝色曲线表示两个高斯总体的（非标准化）交点：

N(x,μ0,σ0)⋅N(x,μ1,σ1)=?N(x,μ’,σ’)(10)\begin{array}{c} \mathcal{N}(x, \color{fuchsia}{\mu_0}, \color{deeppink}{\sigma_0}) \color{block} \cdot \mathcal{N}(x, \color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\sigma_1}) \color{block} \stackrel{?}{=} \mathcal{N}(x, \color{royalblue}{\mu’}, \color{mediumblue}{\sigma’}) \end{array} \tag {10} N(x,μ0,σ0)⋅N(x,μ1,σ1)=?N(x,μ’,σ’)(10)

您可以将等式（9）代入等式（10），并进行一些代数运算（重新规范化，使总概率为1）以获得：
μ’=μ0+σ02(μ1–μ0)σ02+σ12σ’2=σ02–σ04σ02+σ12(11)\begin{array}{c} \begin{aligned} \color{royalblue}{\mu’} &= \mu_0 + \frac{\sigma_0^2 (\mu_1 – \mu_0)} {\sigma_0^2 + \sigma_1^2}\\ \color{mediumblue}{\sigma’}^2 &= \sigma_0^2 – \frac{\sigma_0^4} {\sigma_0^2 + \sigma_1^2} \end{aligned} \end{array} \tag {11} μ’σ’2=μ0+σ02+σ12σ02(μ1–μ0)=σ02–σ02+σ12σ04(11)

将式（11）中的两个式子相同的部分用 k\color{purple}{\mathbf{k}}k 表示：
k=σ02σ02+σ12(12)\begin{array}{c} \color{purple}{\mathbf{k}} \color{black} = \frac{\sigma_0^2}{\sigma_0^2 + \sigma_1^2} \end{array} \tag {12} k=σ02+σ12σ02(12)

μ’=μ0+k(μ1–μ0)σ’2=σ02–kσ02(13)\begin{array}{c} \color{royalblue}{\mu’} &= \mu_0 + &\color{purple}{\mathbf{k}} (\mu_1 – \mu_0)\\ \color{mediumblue}{\sigma’}^2 &= \sigma_0^2 – &\color{purple}{\mathbf{k}} \sigma_0^2 \end{array} \tag {13} μ’σ’2=μ0+=σ02–k(μ1–μ0)kσ02(13)

记下你如何进行先前的评估，并添加一些内容以进行新的评估。看看这个公式有多简单！

多维情况下如何表示呢？好吧，让我们把方程（12）和（13）写成矩阵形式。如果 Σ\SigmaΣ 是高斯分布的协方差矩阵，则 μ⃗\vec{\mu}μ 其沿各轴的平均值，然后：

K=Σ0(Σ0+Σ1)−1(14)\begin{array}{c} \color{purple}{\mathbf{K}} \color{black} = \Sigma_0 (\Sigma_0 + \Sigma_1)^{-1} \end{array} \tag {14} K=Σ0(Σ0+Σ1)−1(14)

μ⃗’=μ0⃗+K(μ1⃗–μ0⃗)Σ’=Σ0–KΣ0(15)\begin{array}{c} \color{royalblue}{\vec{\mu}’} &= \vec{\mu_0} + &\color{purple}{\mathbf{K}} \color{black} (\vec{\mu_1} – \vec{\mu_0})\\ \color{mediumblue}{\Sigma’} &= \Sigma_0 – &\color{purple}{\mathbf{K}} \color{black} \Sigma_0 \end{array} \tag {15} μ’Σ’=μ0+=Σ0–K(μ1–μ0)KΣ0(15)

K\color{purple}{\mathbf{K}}K 是一个叫做Kalman增益(Kalman gain)的矩阵，下面我们马上就用它。

这里可能会有一丝丝的难. 光明就在前方, 放轻松, 我们马上就要结束了.

将所有公式整合起来

现在我们有两种分布：小车自身状态变化预测的测量值 (μ0,Σ0)=(Hkx^k,HkPkHkT)(\color{fuchsia}{\mu_0}, \color{deeppink}{\Sigma_0}) = (\color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k}, \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T})(μ0,Σ0)=(Hkx^k,HkPkHkT) 和观测的测量值(μ1,Σ1)=(zk⃗,Rk)(\color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\Sigma_1}) = (\color{yellowgreen}{\vec{\mathbf{z}_k}}, \color{mediumaquamarine}{\mathbf{R}_k})(μ1,Σ1)=(zk,Rk)。我们只需将它们插入方程（15）中，就可以找出它们的重叠部分：

Hkx^k’=Hkx^k+K(zk⃗–Hkx^k)HkPk’HkT=HkPkHkT–KHkPkHkT(16)\begin{array}{c} \begin{aligned} \mathbf{H}_k \color{royalblue}{\mathbf{\hat{x}}_k’} &= \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} & + & \color{purple}{\mathbf{K}} ( \color{yellowgreen}{\vec{\mathbf{z}_k}} – \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ \mathbf{H}_k \color{royalblue}{\mathbf{P}_k’} \color{black} \mathbf{H}_k^T &= \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} & – & \color{purple}{\mathbf{K}} \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} \end{aligned} \end{array} \tag {16} Hkx^k’HkPk’HkT=Hkx^k=HkPkHkT+–K(zk–Hkx^k)KHkPkHkT(16)

由式（14）可得卡尔曼增益为：
K=HkPkHkT(HkPkHkT+Rk)−1(17)\begin{array}{c} \color{purple}{\mathbf{K}} = \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} ( \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + \color{mediumaquamarine}{\mathbf{R}_k})^{-1} \end{array} \tag {17} K=HkPkHkT(HkPkHkT+Rk)−1(17)

我们可以在（16）和（17）中两边同时左乘矩阵的逆, 每一项前面去掉一个 HkH_kHk（注意，其中一个 Hk\mathbf{H}_kHk 隐藏在 K\color{purple}{\mathbf{K}}K 中），在 Pk’\color{royalblue}{\mathbf{P}_k’}Pk’ 方程的所有项末尾去掉一个 HkTH_k^THkT, 也就是等式两边同时右乘矩阵 HkT\color{deeppink} \mathbf{H}_k^THkT 的逆。

x^k’=x^k+K’(zk⃗–Hkx^k)Pk’=Pk–K’HkPk(18)\begin{array}{c} \color{royalblue}{\mathbf{\hat{x}}_k’} &= \color{fuchsia}{\mathbf{\hat{x}}_k} & + & \color{purple}{\mathbf{K}’} ( \color{yellowgreen}{\vec{\mathbf{z}_k}} – \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ \color{royalblue}{\mathbf{P}_k’} &= \color{deeppink}{\mathbf{P}_k} & – & \color{purple}{\mathbf{K}’} \color{deeppink}{\mathbf{H}_k \mathbf{P}_k} \end{array} \tag {18} x^k’Pk’=x^k=Pk+–K’(zk–Hkx^k)K’HkPk(18)

K’=PkHkT(HkPkHkT+Rk)−1(19)\begin{array}{c} \color{purple}{\mathbf{K}’} = \color{deeppink}{\mathbf{P}_k \mathbf{H}_k^T} ( \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + \color{mediumaquamarine}{\mathbf{R}_k})^{-1} \end{array} \tag {19} K’=PkHkT(HkPkHkT+Rk)−1(19)

上式给出了完整的更新步骤方程。
就这样！x^k’\color{royalblue}{\mathbf{\hat{x}}_k’}x^k’ 是我们新的最佳估计值，我们可以继续将它（连同 Pk’\color{royalblue}{\mathbf{P}_k’}Pk’ ）反馈到下一轮预测中，或者根据我们的喜好进行多次更新。

收尾

在上面所有的数学运算中，您只需要实现等式（7）、（18）和（19）。（或者，如果你忘记了这些，你可以从方程（4）和（15）中重新推导出一切。）
这将允许您精确地建模任何线性系统。对于非线性系统，我们使用扩展卡尔曼滤波器，其工作原理是简单地将预测和测量值的平均值线性化。（我以后可能会再写一篇关于EKF的文章）。
如果我的工作做得很好，希望其他人会意识到这些东西有多酷，并想出一个意想不到的新地方把它们付诸行动。

reference

@online{BibEntry2021Dec,
title = {{How a Kalman filter works, in pictures | Bzarg}},
year = {2021},
month = {12},
date = {2021-12-21},
urldate = {2021-12-21},
language = {english},
hyphenation = {english},
note = {[Online; accessed 21. Dec. 2021]},
url = {http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#mjx-eqn-kalunsimplified}
}

@online{BibEntry2021Dec,
title = {{详解卡尔曼滤波原理_清风莞尔的博客-CSDN博客_卡尔曼滤波原理}},
year = {2021},
month = {12},
date = {2021-12-21},
urldate = {2021-12-21},
language = {chinese},
hyphenation = {chinese},
note = {[Online; accessed 21. Dec. 2021]},
url = {https://blog.csdn.net/u010720661/article/details/63253509},
keywords = {卡尔曼滤波原理},
abstract = {{详解卡尔曼滤波原理　　在网上看了不少与卡尔曼滤波相关的博客、论文，要么是只谈理论、缺乏感性，或者有感性认识，缺乏理论推导。能兼顾二者的少之又少，直到我看到了国外的一篇博文，真的惊艳到我了，不得不佩服作者这种细致入微的精神，翻译过来跟大家分享一下，}}
}

@online{BibEntry2021Dec,
title = {{史上最详细的Kalman滤波解读与实践(python)_IT荻的博客-CSDN博客_pykalman}},
year = {2021},
month = {12},
date = {2021-12-26},
urldate = {2021-12-26},
language = {chinese},
hyphenation = {chinese},
note = {[Online; accessed 26. Dec. 2021]},
url = {https://blog.csdn.net/u010712012/article/details/90294295},
keywords = {pykalman},
abstract = {{项目课题当中有使用到Kalman滤波的算法思想，这里总结一下这个神奇算法的过程。什么是卡尔曼滤波？对于这个滤波器，我们几乎可以下这么一个定论：只要是存在不确定信息的动态系统，卡尔曼滤波就可以对系统下一步要做什么做出有根据的推测。即便有噪声信息干扰，卡尔曼滤波通常也能很好的弄清楚究竟发生了什么，找出现象间不易察觉的相关性。因此卡尔曼滤波非常适合不断变化的系统，它的优点还有内存占用较小（只需保留…}}
}