详解卡尔曼滤波（Kalman Filter）原理

前言

看过很多关于卡尔曼滤波的资料，发现很多资料写的都很晦涩，初学者很难看懂。在网上找了很多资料之后，发现了这篇博文讲的非常清晰易懂，特此翻译记录，以备后用。为保证翻译的原滋原味，以下均用第一人称表述。

背景

我不得不说一说卡尔曼滤波，因为它能做到的事情简直令人惊叹。

很可惜的是，很少有软件工程师和科学家对此有深入了解。这让我感到很沮丧，因为卡尔曼滤波是如此通用且强大的工具，它能在不确定情况下组合信息。有时，它提取准确信息的能力似乎几乎是不可思议的。如果听起来我讲的太多了，那么请看一下之前发布的视频，其中演示了一个利用卡尔曼滤波观察*浮动物体的速度来确定它的方向。能力真强大！

卡尔曼滤波是什么？

你可以在任何含有不确定信息的动态系统中的使用卡尔曼滤波，对系统的下一步动作做出有根据的猜测。即使伴随着各种干扰，卡尔曼滤波总是能指出真实世界发生的情况。它可以利用怪异现象之间的关联，你可能不会想到利用这些关联！

卡尔曼滤波对于持续变化的系统是理想的选择。由于卡尔曼滤波除了记忆前一个状态而不需要保留其他的历史记忆信息，因此卡尔曼滤波具有轻量化的特点，运行速度非常快，非常适合处理实时的问题和嵌入式系统。

你在Google上找到的大部分关于卡尔曼滤波的数学描述是晦涩难懂的。那是非常糟糕的状况！因为卡尔曼滤波能被简单和容易的方式所理解的。因此，本文是一个非常不错的文章主题，本文将尝试用许多清晰、美观的图片来阐明它。 本文的前提很简单，你仅仅需要对概率和矩阵有基本的了解。

我们能用卡尔曼滤波做什么？

本文将从一个简单的例子开始，说明卡尔曼滤波可以解决的问题。但如果你想直接接触精美的图片和数学，请随时跳转。

举一个简单的小例子：你已经做了一个能在丛林中游走的小机器人，为确保其能导航，机器人需要知道它所在的位置。

将机器人的运动状态表示为 x k ⃗ \vec{x_k} xk​ ​，其仅仅包含了位置和速度：

x k ⃗ = ( p ⃗ , v ⃗ ) \vec{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−1​vk​=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​=Fk​x^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​=Fk​x^k−1​Pk​=Fk​Pk−1​FkT​​(5)

外部控制变量

不过，我们并没有捕捉到所有的信息，可能会有一些与状态本身无关的变化（外部世界的改变可能会影响到这个系统）。

例如，当模拟一列火车的状态模型时，列车驾驶员可能会踩下油门，导致列车加速。类似地，在上述的机器人例子中，导航软件可能会发出一个命令让车轮转动或停止。如果我们知道这个世界上正在发生什么的其他信息，我们可以把它填充到一个叫做 u k ⃗ \vec{u_k} uk​ ​的向量中，用它做一些事情，然后把它加入我们的预测中进行修正。

假设我们知道由于油门设置或控制命令而产生的预期加速度$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​+21​aΔ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​​=Fk​x^k−1​+[2Δt2​Δt​]=Fk​x^k−1​+Bk​uk​ ​​(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^k​Pk​​=Fk​x^k−1​+Bk​uk​ ​=Fk​Pk−1​FkT​+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 ​=Hk​x^k​Σexpected ​=Hk​Pk​HkT​​(8)

卡尔曼滤波器最适合做的一件事是处理传感器噪声。 换句话说，我们的传感器至少有些不可靠，并且我们原始估计中的每个状态都可能导致传感器读数在一定范围内。

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

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

因此，现在我们有两个高斯分布：一个是经过转换得预测平均值，另一个是得到的实际传感器读数。

我们必须尝试将基于预测状态（粉红色）的读数猜测与基于实际观察到的传感器读数（绿色）的猜测保持一致。

那么新的最可能的状态是什么？对于任何可能的读数 ( z 1 , z 2 ) (z1,z2) (z1,z2)，有两个相关的概率：（1）传感器测量值 z k ⃗ \vec{z_k} zk​ ​是 ( z 1 , z 2 ) (z1,z2) (z1,z2)测量误差的概率；（2）前一个状态得估计值 ( z 1 , z 2 ) (z1,z2) (z1,z2)。

如果这两个概率都知道，只需要把它们相乘就能够得到最后得结果：

剩下的重叠部分，即两个斑点都亮、可能相同的区域，比我们之前的任何一个估算值都精确得多。 这种分布的平均值就是两个估计最有可能的值，是给定所有信息的最佳估计。

瞧！这个重叠的区域看起来像另一个高斯分布。

　　
  如你所见，把两个具有不同均值和方差的高斯分布相乘，你会得到一个新的具有独立均值和方差的高斯分布！下面用公式讲解。

联合高斯分布

  先以一维高斯分布来分析比较简单点，具有方差 σ 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π ​1​e−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​)=(Hk​x^k​,Hk​Pk​HkT​)和测量部分 ( μ 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} Hk​x^k′​Hk​Pk′​HkT​​=Hk​x^k​=Hk​Pk​HkT​​+K(zk​ ​−Hk​x^k​)−KHk​Pk​HkT​​(16)

由式（14）可得卡尔曼增益为：
K = H k P k H k T ( H k P k H k T + R k ) − 1 \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} K=Hk​Pk​HkT​(Hk​Pk​HkT​+Rk​)−1

将式（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​ ​−Hk​x^k​)=Pk−​K′Hk​Pk​​(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′=Pk​HkT​(Hk​Pk​HkT​+Rk​)−1(19)

上式给出了完整的更新步骤方程。 x ^ k \hat{\mathbf{x}}_{k} x^k​就是新的最优估计，我们可以将它和 P k \mathbf{P}_{k} Pk​放到下一个预测和更新方程中不断迭代。

总结

对于上述所有的数学公式，你仅仅需要实现公式（7）、（18）和（19）。（如果你忘记了上述公式，你也能从公式（4）和（5）重新推导。）

总结

对于上述所有的数学公式，你仅仅需要实现公式（7）、（18）和（19）。（如果你忘记了上述公式，你也能从公式（4）和（5）重新推导。）

这将允许你精确地建模任何线性系统。对于非线性系统，需要用到扩展卡尔曼滤波，区别在于EKF多了一个把预测和测量部分进行线性化的过程。