看过很多关于卡尔曼滤波的资料，发现很多资料写的都很晦涩，初学者很难看懂。在网上找了很多资料之后，发现了这篇博文讲的非常清晰易懂，特此翻译记录，以备后用。另外，本人也检索到上有篇作者做了同样的工作，但这个工作中公式摆放比较杂乱，部分翻译不确切，本文也参考了其中的部分翻译。为保证翻译的原滋原味，以下均用第一人称表述。
我不得不说一说卡尔曼滤波，因为它能做到的事情简直令人惊叹！
很可惜的是，很少有软件工程师和科学家对此有深入了解。这是令人很不解的，因为卡尔曼滤波是如此通用且强大的工具，它能在不确定情况下融合信息。有时，它准确提取信息的能力是令人难以置信的！如果我讲的太多了，那么请看一下之前发布的视频，其中演示了一个利用卡尔曼滤波观察自由浮动物体的速度来确定它的方向。能力真强大呀！
你可以在任何含有不确定信息的动态系统中的使用卡尔曼滤波，对系统的下一步动作做出有根据的猜测。即使混乱的真实世界伴随着各种干扰，卡尔曼滤波总是能清楚知道真实世界发生了什么，而且它可以利用各种你不会想到的奇特现象进行关联！
卡尔曼滤波对于持续变化的系统是理想的选择。由于卡尔曼滤波除了记忆前一个状态而不需要保留其他的历史记忆信息，因此卡尔曼滤波具有轻量化的特点，运行速度非常快，非常适合实时问题和嵌入式系统。
在Google上找到的大部分关于卡尔曼滤波的数学描述是晦涩难懂的。真糟糕，因为卡尔曼滤波能用简单容易的方式所理解的。因此，本文将尝试用许多清晰、美观的图片来阐明它。阅读本文的前提很简单，你仅仅需要对基本的概率论和矩阵知识有所了解。
本文将从一个简单的例子开始，说明卡尔曼滤波可以解决的问题。但如果你想直接接触精美的图片和数学，可以随时往下翻。
举一个简单的小例子：你已经做了一个能在丛林中随意行走的小机器人，机器人需要时刻定位才能使他导航。 将机器人的运动状态表示为 x k → \overrightarrow{x_{k}} xk ，其仅仅包含了位置和速度： x k → = ( p ⃗ , v ⃗ ) \overrightarrow{x_{k}}=(\vec{p}, \vec{v}) xk =(p ,v ) 请注意，这个状态只是关于该系统基本属性的一堆数字，它可以是任何其它东西。在我们的例子中是位置和速度，但它可以是油箱中液体量的数据、汽车引擎的温度、用户手指在触控板上的位置或者任何你需要跟踪的信号。
我们的机器人也有GPS传感器，精确大约10米，但它需要更精确地知道自己的位置。在树林中有很多沟壑和悬崖，如果机器人的误差超过几英尺，它可能会从悬崖上掉下去。因此仅依赖GPS进行定位是远远不够的。
或许我们还知道机器人是如何移动的额外信息，如：发送给车轮电机的指令，如果在没有任何干扰情况下朝一个方向前进，下一刻它可能会继续朝同一方向前进。当然，它对自己的运动也不会完全掌握，如：它可能会受到风的冲击；车轮可能会打滑；或者在崎岖不平的地形上滚动。所以轮子转动的数量无法准确地代表机器人实际行走了多远。
GPS传感器告诉我们一些关于状态的信息，我们的预测告诉了机器人是如何移动的，但只是间接的，并且伴随着不确定性。
但是，如果我们利用所有可获取到的信息，我们能得到一个比这两个估计本身更好的答案吗?当然，答案是肯定的，这就是卡尔曼滤波器的作用。
我们继续上一个例子，机器人仅仅包含一个位置和速度的简单状态。
x ⃗ = [ p v ] \vec{x}=\begin{bmatrix} p \\ v \end{bmatrix} x =[pv]
我们不知道实际的位置和速度是什么，它们之间有很多种可能的组合，但其中一些的可能性要大于其他部分：
卡尔曼滤波器假设两个变量（例子中为位置和速度）都是随机变量且服从高斯分布的。每个变量的均值为 μ \mu μ，它表示随机分布的中心位置，即机器人最可能的状态，不确定性用方差 σ 2 \sigma^2 σ2表示：
在上图中，位置和速度是不相关的，这意味着一个变量的状态不能推测出其他变量的状态。
下面的例子更有趣：位置和速度是呈相关性的。观察特定位置的可能性取决于当时的速度：
这种情况可能会出现。例如：根据一个旧的位置估计一个新的位置，如果速度很快，可能会走得更远，所以位置会更远。如果走得很慢，那么就不会走的很远。
这种关系非常重要，因为它给我们提供了更多的信息：一个测量值告诉我们其他测量值可能是什么。这就是卡尔曼滤波的目的，尽可能地在多个不确定的测量数据中中提取更多信息！
这种相关性可以用协方差矩阵来表示。简而言之，矩阵的每个元素 Σ i j \Sigma_{ij} Σij是第 i i i个状态变量和 j j j个状态变量之间的相关程度。（由于协方差矩阵是对称的，也就是交换 i i i和 j j j不会影响最终的结果)。协方差矩阵往往用“ Σ \Sigma Σ”来表示，其中的元素则表示为“ Σ i j \Sigma_{ij} Σij”。
我们基于高斯分布来建立状态变量，所以在时间 k k k需要两条信息：最佳估计 x ^ k \hat{\mathbf{x}}_{\mathbf{k}} x^k（即均值 μ \mu μ）；协方差矩阵 P k \rm{P_k} Pk。 x ^ k = [ position velocity ] P k = [ Σ p p Σ p v Σ v p Σ v v ] (1) \begin{array}{l} \hat{\mathbf{x}}_{\mathbf{k}} = \begin{bmatrix} \text{position} \\ \text{velocity} \end{bmatrix} \\ \rm{P_k} = \begin{bmatrix} \Sigma_{pp} & \Sigma_{pv} \\ \Sigma_{vp} & \Sigma_{vv} \end{bmatrix} \end{array} \tag{1} x^k=[positionvelocity]Pk=[ΣppΣvpΣpvΣvv](1) 在这里只使用了位置和速度，但是该状态可以包含任意数量的变量（任何需要表示的信息），这对于处理其他问题是非常有作用。
接下来，我们需要某种方式来知道当前状态（时刻 k − 1 k-1 k−1）并预测下一个时刻 k k k的状态。记住，我们不知道对下一状态的所有预测中哪个是“真实的”，我们的预测函数也不关心。它适用于所有的情况，并给出了一个新的高斯分布：
用矩阵 F k \mathbf{F_k} Fk表示这个预测过程：
它将我们原始估计中的每个点都移动到了一个新的预测位置，如果原始估计是正确的话，这个新的预测位置就是系统下一步会移动到的位置。
那我们又如何用矩阵来预测下一个时刻的位置和速度呢？下面用一个基本的运动学公式来表示： p k = p k − 1 + Δ t v k − 1 v k = v k − 1 \begin{array}{l} p_k=p_{k-1}+\Delta t v_{k-1} \\ v_k=v_{k-1} \end{array} pk=pk−1+Δtvk−1vk=vk−1
用矩阵的表示是： x ^ k = [ 1 Δ t 0 1 ] x ^ k − 1 = F k x ^ k − 1 (2, 3) \begin{aligned} \hat{ \mathbf{x}}_{k} &=\left[\begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array}\right] \hat{\mathbf{x}}_{k-1} \tag{2, 3} \\ &=\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} \end{aligned} x^k=[10Δt1]x^k−1=Fkx^k−1(2, 3)
现在，我们有了一个预测矩阵，该矩阵提供了下一个状态，但是我们仍然不知道如何更新协方差矩阵。
这也就是为什么我们需要另一个公式的原因。如果我们将分布中的每个点乘以矩阵 A \rm{A} A，那么其协方差矩阵 Σ \Sigma Σ会变成什么？
很简单，下面就是这个公式： Cov ( x ) = Σ Cov ( A x ) = A Σ A T (4) \begin{aligned} \operatorname{Cov}(x) &=\Sigma \\ \operatorname{Cov}(\mathbf{A} x) &=\mathbf{A} \Sigma \mathbf{A}^{T} \tag{4} \end{aligned} Cov(x)Cov(Ax)=Σ=AΣAT(4)
下一步联合公式(4)和(3)： x ^ k = F k x ^ k − 1 P k = F k P k − 1 F k T (5) \begin{array}{l} \hat{\mathbf{x}}_{k}=\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} \\ \mathbf{P}_{k}=\mathbf{F}_{\mathbf{k}} \mathbf{P}_{k-1} \mathbf{F}_{k}^{T} \tag{5} \end{array} x^k=Fkx^k−1Pk=FkPk−1FkT(5)
不过，我们并没有捕捉到所有的信息，可能会有一些与状态本身无关的变化（外部世界的改变可能会影响到这个系统）。
例如，当模拟一列火车的状态模型时，列车驾驶员可能会踩下油门，导致列车加速。类似地，在上述的机器人例子中，导航软件可能会发出一个命令让车轮转动或停止。如果我们知道这个世界上正在发生什么的其他信息，我们可以把它填充到一个叫做 u k ⃗ \vec{u_k} uk 的向量中，用它做一些事情，然后把它加入我们的预测中进行修正。
假设我们知道由于油门设置或控制命令而产生的预期加速度 a a a。根据运动学公式我们得到： p k = p k − 1 + Δ t v k − 1 + 1 2 a Δ t 2 v k = v k − 1 + a Δ t \begin{array}{l} p_{k}=p_{k-1}+\Delta t v_{k-1}+\frac{1}{2} a \Delta t^{2} \\ v_{k}=\quad v_{k-1}+a \Delta t \end{array} pk=pk−1+Δtvk−1+21aΔt2vk=vk−1+aΔt
其矩阵形式为： x ^ k = F k x ^ k − 1 + [ Δ t 2 2 Δ t ] = F k x ^ k − 1 + B k u k → (6) \begin{aligned} \hat{\mathbf{x}}_{k} &=\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1}+\left[\begin{array}{c} \frac{\Delta t^{2}}{2} \\ \Delta t \end{array}\right] \\ &=\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1}+\mathbf{B}_{k} \overrightarrow{\mathbf{u}_{k}} \tag{6} \end{aligned} x^k=Fkx^k−1+[2Δt2Δt]=Fkx^k−1+Bkuk (6)其中 B k \rm{B_k} Bk称为控制矩阵（control matrix）， u k ⃗ \vec{u_k} uk 是控制向量（control vector）（对于没有外部控制变量的简单系统而言，可以忽略掉这些）。
让我们再增加一个细节。如果我们的预测不是一个100%准确的模型，那会发生什么？
如果状态是基于自身的属性发展的，那么一切都是好的。如果状态是以外力为基础发展起来的，只要我们知道这些外在因素是什么，一切还在掌握之中。
但是对于我们不知道的外在因素呢?例如：如果我们在追踪一个四轴飞行器，它可能会受到风的冲击；如果我们追踪一个带轮子的机器人，轮子可能会打滑，或者地面上的颠簸会让它减速。我们无法跟踪这些东西，如果这些发生了，我们的预测可能会出错，因为我们没有考虑这些额外的因素。
我们可以通过在每个预测步骤之后添加一些新的不确定性，来建立与“世界”（包括我们没有跟踪的事物）相关的不确定性模型：
最初估计的每个状态都有可能移动的范围。高斯分布非常适合这个场景， x ^ k − 1 \hat{x}_{k-1} x^k−1中的每一点点都被移动到一个高斯分布内的某个地方协方差为 Q k \rm{Q_k} Qk。另一种说法是，我们用协方差 Q k \rm{Q_k} Qk来处理未跟踪的噪声的影响。
这会产生一个新的高斯分布，因此也有了一个不同的协方差，但是均值是相同的。 只需添加 Q k \rm{Q_k} Qk即可得到扩展的协方差，以下为预测步骤完整的表达式： x ^ k = F k x ^ k − 1 + B k u k → P k = F k P k − 1 F k T + Q k (7) \begin{aligned} \hat{\mathbf{x}}_{k} &=\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1}+\mathbf{B}_{k} \overrightarrow{\mathbf{u}_{k}} \\ \mathbf{P}_{k} &=\mathbf{F}_{\mathbf{k}} \mathbf{P}_{k-1} \mathbf{F}_{k}^{T}+\mathbf{Q}_{k} \end{aligned} \tag{7} x^kPk=Fkx^k−1+Bkuk =FkPk−1FkT+Qk(7)
换句话说，新的最优估计的预测来自两个方面，即先前的最优估计加上已知外部控制量的修正。
新的不确定性是根据旧的不确定性，加上一些来自环境的附加不确定性来预测的。
好，这很简单。我们通过 x ^ k \rm\hat{x}_k x^k和 P \rm{P} P k _k k对系统的位置有一个模糊的估计，当我们从传感器得到一些数据时会发生什么?
我们可能有几个传感器，这些传感器可以向我们提供有关系统状态的信息。 就目前而言，衡量什么无关紧要，也许一个测量位置，另一个测量速度。 每个传感器都告诉我们有关状态的一些间接信息，换句话说，传感器在状态下运行并产生一组数据。 注意，传感器数据的单位和比例可能与我们所追踪的状态的单位和比例不一样。你也许能猜到这是怎么回事，我们将用矩阵 H \rm{H} H k _k k对传感器进行建模。
我们可以用通常的方式找出我们希望看到的传感器读数的分布： μ ⃗ expected = H k x ^ k Σ expected = H k P k H k T (8) \begin{array}{l} \vec{\mu}_{\text {expected }}=\mathbf{H}_{k} \hat{\mathbf{x}}_{k} \\ \mathbf{\Sigma}_{\text {expected }}=\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T} \end{array} \tag{8} μ expected =Hkx^kΣexpected =HkPkHkT(8)
卡尔曼滤波器最适合做的一件事是处理传感器噪声。 换句话说，我们的传感器至少有些不可靠，并且我们原始估计中的每个状态都可能导致传感器读数在一定范围内。 从我们观察到的每一个读数，我们可以猜测系统处于一个特定的状态。但由于存在不确定性，一些状态比其他状态更有可能产生我们看到的读数： 我们将这种不确定性（即传感器噪声）的协方差称为 R \rm{R} R k _k k。这个分布的平均值等于我们观察到的读数，我们称之为 z k ⃗ \vec{z_k} zk 。
现在我们有了两个高斯分布：一个是在预测值附近，另一个是实际传感器读数附近。 我们必须尝试将基于预测状态（粉红色）的读数猜测与基于实际观察到的传感器读数（绿色）的猜测保持一致。
那么新的最可能的状态是什么？对于任何可能的读数 ( z 1 , z 2 ) (z1,z2) (z1,z2)，有两个相关的概率：（1）传感器的测量值；（2）前一个状态的估计值。
如果这两个概率都知道，只需要把它们相乘就能够得到最后得结果： 剩下的重叠部分，即两个斑点都亮、可能相同的区域，比我们之前的任何一个估算值都精确得多。 这种分布的平均值就是两个估计最有可能的值，是给定所有信息的最佳估计。
瞧！这个重叠的区域看起来像另一个高斯分布。 如你所见，把两个具有不同均值和方差的高斯分布相乘，你会得到一个新的具有独立均值和方差的高斯分布！下面用公式讲解。
先以一维高斯分布来分析比较简单点，具有方差 σ 2 \sigma^2 σ2和 μ \mu μ的高斯曲线可以用下式表示： N ( x , μ , σ ) = 1 σ 2 π e − ( x − μ ) 2 2 σ 2 (9) \mathcal{N}(x, \mu, \sigma)=\frac{1}{\sigma \sqrt{2 \pi}} e^{-\frac{(x-\mu)^{2}}{2 \sigma^{2}}} \tag{9} N(x,μ,σ)=σ2π 1e−2σ2(x−μ)2(9)
如果把两个服从高斯分布的函数相乘会得到什么呢？
N ( x , μ 0 , σ 0 ) ⋅ N ( x , μ 1 , σ 1 ) = ? N ( x , μ ′ , σ ′ ) (10) \mathcal{N}\left(x, \mu_{0}, \sigma_{0}\right) \cdot \mathcal{N}\left(x, \mu_{1}, \sigma_{1}\right) \stackrel{?}{=} \mathcal{N}\left(x, \mu^{\prime}, \sigma^{\prime}\right) \tag{10} N(x,μ0,σ0)⋅N(x,μ1,σ1)=?N(x,μ′,σ′)(10)
将式（9）代入到式（10）中（注意重新归一化，使总概率为1）可以得到： μ ′ = μ 0 + σ 0 2 ( μ 1 − μ 0 ) σ 0 2 + σ 1 2 σ ′ 2 = σ 0 2 − σ 0 4 σ 0 2 + σ 1 2 (11) \begin{aligned} \mu^{\prime} &=\mu_{0}+\frac{\sigma_{0}^{2}\left(\mu_{1}-\mu_{0}\right)}{\sigma_{0}^{2}+\sigma_{1}^{2}} \\ \sigma^{\prime 2} &=\sigma_{0}^{2}-\frac{\sigma_{0}^{4}}{\sigma_{0}^{2}+\sigma_{1}^{2}} \end{aligned} \tag{11} μ′σ′2=μ0+σ02+σ12σ02(μ1−μ0)=σ02−σ02+σ12σ04(11)
将式（11）中的两个式子相同的部分用 k \mathbf{k} k表示： k = σ 0 2 σ 0 2 + σ 1 2 (12) \mathbf{k}=\frac{\sigma_{0}^{2}}{\sigma_{0}^{2}+\sigma_{1}^{2}} \tag{12} k=σ02+σ12σ02(12)
μ ′ = μ 0 + k ( μ 1 − μ 0 ) σ ′ 2 = σ 0 2 − k σ 0 2 (13) \begin{aligned} \mu^{\prime} &=\mu_{0}+\mathbf{k}\left(\mu_{1}-\mu_{0}\right) \\ \sigma^{\prime 2} &=\sigma_{0}^{2}-\mathbf{k} \sigma_{0}^{2} \end{aligned} \tag{13} μ′σ′2=μ0+k(μ1−μ0)=σ02−kσ02(13)
下面进一步将式（12）和（13）写成矩阵的形式，如果 Σ \Sigma Σ表示高斯分布的协方差， μ ⃗ \vec{\mu} μ 表示每个维度的均值，则：
K = Σ 0 ( Σ 0 + Σ 1 ) − 1 (14) \mathbf{K}=\Sigma_{0}\left(\Sigma_{0}+\Sigma_{1}\right)^{-1} \tag{14} K=Σ0(Σ0+Σ1)−1(14)
μ ⃗ ′ = μ 0 → + K ( μ 1 → − μ 0 → ) Σ ′ = Σ 0 − K Σ 0 (15) \begin{array}{l} \vec{\mu}^{\prime}=\overrightarrow{\mu_{0}}+\mathbf{K}\left(\overrightarrow{\mu_{1}}-\overrightarrow{\mu_{0}}\right) \\ \Sigma^{\prime}=\Sigma_{0}-\mathbf{K} \Sigma_{0} \end{array} \tag{15} μ ′=μ0 +K(μ1 −μ0 )Σ′=Σ0−KΣ0(15) 矩阵 K \mathbf{K} K称为卡尔曼增益，下面将会用到。放松！马上就要结束了！
我们有两个高斯分布，预测部分 ( μ 0 , Σ 0 ) = ( H k x ^ k , H k P k H k T ) \left(\mu_{0}, \Sigma_{0}\right)=\left(\mathbf{H}_{k} \hat{\mathbf{x}}_{k}, \mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T}\right) (μ0,Σ0)=(Hkx^k,HkPkHkT)和测量部分 ( μ 1 , Σ 1 ) = ( z k → , R k ) \left(\mu_{1}, \Sigma_{1}\right)=\left(\overrightarrow{\mathbf{z}_{k}}, \mathbf{R}_{k}\right) (μ1,Σ1)=(zk ,Rk)，将他们放到式(15)中算出它们之间的重叠部分：
H k x ^ k ′ = H k x ^ k + K ( z k → − H k x ^ k ) H k P k ′ H k T = H k P k H k T − K H k P k H k T (16) \begin{aligned} \mathbf{H}_{k} \hat{\mathbf{x}}_{k}^{\prime} &=\mathbf{H}_{k} \hat{\mathbf{x}}_{k} &+\mathbf{K}\left(\overrightarrow{\mathrm{z}_{k}}-\mathbf{H}_{k} \hat{\mathbf{x}}_{k}\right) \\ \mathbf{H}_{k} \mathbf{P}_{k}^{\prime} \mathbf{H}_{k}^{T} &=\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T} &-\mathbf{K} \mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T} \end{aligned} \tag{16} Hkx^k′HkPk′HkT=Hkx^k=HkPkHkT+K(zk −Hkx^k)−KHkPkHkT(16)
由式（14）可得卡尔曼增益为： K = H k P k H k T ( H k P k H k T + R k ) − 1 (17) \mathbf{K}=\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T}\left(\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T}+\mathbf{R}_{k}\right)^{-1} \tag{17} K=HkPkHkT(HkPkHkT+Rk)−1(17)
将式（16）和式（17）的两边同时左乘矩阵的逆（注意 K \mathbf{K} K里面包含了 H k \mathbf{H}_{k} Hk）将其约掉，再将式（16）的第二个等式两边同时右乘矩阵 H k T \mathbf{H}_{k}^{T} HkT的逆得到以下等式： x ^ k ′ = x ^ k + K ′ ( z k → − H k x ^ k ) P k ′ = P k − K ′ H k P k (18) \begin{aligned} \hat{\mathbf{x}}_{k}^{\prime} &=\hat{\mathbf{x}}_{k}+\mathbf{K}^{\prime}\left(\overrightarrow{\mathrm{z}_{k}}-\mathbf{H}_{k} \hat{\mathbf{x}}_{k}\right) \\ \mathbf{P}_{k}^{\prime} &=\mathbf{P}_{k^{-}} \mathbf{K}^{\prime} \mathbf{H}_{k} \mathbf{P}_{k} \end{aligned} \tag{18} x^k′Pk′=x^k+K′(zk −Hkx^k)=Pk−K′HkPk(18)
K ′ = P k H k T ( H k P k H k T + R k ) − 1 (19) \mathbf{K}^{\prime}=\mathbf{P}_{k} \mathbf{H}_{k}^{T}\left(\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T}+\mathbf{R}_{k}\right)^{-1} \tag{19} K′=PkHkT(HkPkHkT+Rk)−1(19) 上式给出了完整的更新步骤方程。 x ^ k \hat{\mathbf{x}}_{k} x^k就是新的最优估计，我们可以将它和 P k \mathbf{P}_{k} Pk放到下一个预测和更新方程中不断迭代。
对于上述所有的数学公式，你仅仅需要实现公式（7）、（18）和（19）。（如果你忘记了上述公式，你也能从公式（4）和（5）重新推导。）
这将允许你精确地建模任何线性系统。对于非线性系统，需要用到扩展卡尔曼滤波，区别在于EKF多了一个把预测和测量部分进行线性化的过程。
