原文：多维卡尔曼滤波 (kalmanfilter.net)

前言

基本背景

状态外推方程

示例 - 飞机 - 无控制输入

示例 - 带控制输入的飞机

示例 – 坠落物体

状态外推方程维度

线性时不变系统

线性动态系统建模

状态外推方程的推导

状态空间表示形式

示例 - 等速运动体

高阶动态系统建模

示例 - 恒加速度运动体

示例：质量弹簧阻尼系统

求解微分方程

无输入变量的动态系统

具有输入变量的动态系统

协方差外推方程

无过程噪声的估计不确定度

构造过程噪声矩阵 Q

离散噪声模型

连续噪声模型

选择哪种型号？

测量公式

观察矩阵 H

缩放

状态选择

状态组合

测量方程尺寸

中期摘要

预测方程

状态外推方程

协方差外推方程

辅助方程

测量公式

协方差方程

状态更新公式

状态更新公式维度

协方差更新方程

协方差更新方程推导

卡尔曼增益

卡尔曼增益方程推导

简化协方差更新公式

总结

例子

示例 9 – 车辆位置估计

状态外推方程

协方差外推方程

测量公式

卡尔曼增益

状态更新公式

协方差更新方程

数值示例

示例分析

示例 10 – 火箭高度估计

状态外推方程

协方差外推方程

测量公式

卡尔曼增益

状态更新公式

协方差更新方程

数值示例

示例分析

前言

阅读“一维卡尔曼滤波”部分后，您应该熟悉卡尔曼滤波的概念。在本节中，我们推导出多维（多变量）卡尔曼滤波方程。

本教程部分介绍线性卡尔曼滤波器（LKF）。LKF假设系统动力学是线性的。

到目前为止，我们已经处理了一维过程，例如估计液体温度。但许多动态过程具有两个、三个甚至更多的维度。

例如，描述飞机在空间中位置的状态向量是三维的：

描述飞机位置和速度的状态向量是六维的：

描述飞机位置、速度和加速度的状态向量是九维的：

假设一个恒定加速度动力学模型，我们可以通过九个运动方程来描述时间\（n\）处的外推飞机状态：

通常的做法是用矩阵形式的单个方程描述多维过程。

首先，写出所有这些方程是非常累人的;用矩阵表示法表示它们更短，更优雅。

其次，计算机在矩阵计算方面非常高效。以矩阵形式实现卡尔曼滤波可缩短计算运行时间。

以下章节以矩阵形式描述卡尔曼滤波方程。当然，理论部分之后是完全求解的数值示例。

最后一章包括两个数值示例。在第一个例子中，我们设计了一个没有控制输入的六维卡尔曼滤波器。在第二个示例中，我们设计了一个带有控制输入的二维卡尔曼滤波器。

基本背景

状态外推方程

我想描述的第一个卡尔曼滤波方程是状态外推方程。

使用状态外推方程，我们可以根据对当前状态的了解来预测下一个系统状态。它将状态向量从现在（时间步长 n ）外推到未来（时间步长 n + 1 ）。

状态外推方程描述了动态系统的模型。在文献中，它也被称为：

预测变量方程
过渡方程
预测方程
动态模型
状态空间模型

矩阵表示法中状态外推方程的一般形式为：

其中：

$\boldsymbol{\hat{x}}_{n+1,n}$	是时间步长 \（ n + 1 \）处的预测系统状态向量
$\boldsymbol{\hat{x}}_{n,n}$	是时间步长 \（ n \）处的估计系统状态向量
$\boldsymbol{u}_{n}$	是控制变量或输入变量 - 系统的可测量（确定性）输入
$\boldsymbol{w}_{n}$	是过程噪声或干扰 - 影响状态的不可测量输入
$F$	是一个状态转换矩阵
G	是控制矩阵或输入转换矩阵（将控件映射到状态变量）

注意：在文献中，状态转移矩阵 \（ F \）有时用希腊字母 \（ \Phi \）表示。

下图提供了状态外推方程的示意图说明。

状态变量可能表示我们想知道的系统属性。

例如，移动的车辆具有三个属性：位置、速度和加速度。

您可能会问自己，哪些属性是状态变量，哪些属性是系统的输入？

移动机械系统具有位置、速度、加速度和阻力等属性。
作用在系统上的力应被视为外部强迫函数，即控制状态矢量（恒定加速度情况下的位置和速度）的系统的输入。
牛顿第二定律告诉我们 F = ma 。因此，我们可以将加速度视为系统的外部输入。
位置和速度是感兴趣的主要状态变量。

例如，在弹簧系统中，施加到弹簧 F（t）的力是输入 u（t），而弹簧位移 x（t）是系统状态。

对于下落的物体，输入是重力 $F_{g}$ 和阻力 $F_{drag}(t)$ ，而物体高度 h（t）和速度v（t）是系统状态。

注意：过程噪声\（w_{n} \）通常不会直接出现在感兴趣的方程中。相反，该术语用于模拟协方差外推方程中的不确定性。

让我们看一下状态外推方程的几个例子。

示例 - 飞机 - 无控制输入

在此示例中，我们定义了飞机的状态外推方程，假设一个恒定的加速度模型。

在此示例中，没有控件输入。

我们将假设没有控制输入。我们将在下一个示例中看到控件输入。

$\boldsymbol{u}_{n} = 0$

考虑一架飞机在三维空间中以恒定加速度移动。描述笛卡尔坐标系中估计的飞机位置、速度和加速度的状态向量（ x，y，z ）为：

注意：不要混淆估计的状态向量 $\boldsymbol{\hat{x}_{n}}$ （粗体字体）和 x 轴处的估计飞机位置，该轴由 $\hat{x}_{n}$ 表示（法线字体）。这两个变量在文献中用同一个字母表示，我试图保持一致。

状态转换矩阵 F 为：

状态外推方程为：

矩阵乘法结果：

示例 - 带控制输入的飞机

此示例与前面的示例类似，但现在我们有一个传感器连接到飞行员的控件，因此我们根据飞行员的命令获得了有关飞机加速的其他信息。

描述笛卡尔坐标系中估计的飞机位置和速度的状态向量 $\hat{x}_{n}$ 为：

描述笛卡尔坐标系（ x，y，z）中对照向量的飞机加速度的控制向量 $u_{n}$ 为：

状态转换矩阵 F 为：

控制矩阵 G 为：

状态外推方程为：

示例 – 坠落物体

考虑一个自由落体的物体。状态向量包括高度 h 和对象的速度 $\dot{h}$ ：

状态转换矩阵 F 为：

控制矩阵 G 为：

输入变量 $\boldsymbol{u}_{n}$ 为：

其中 g 是引力加速度。

我们没有测量加速度的传感器，但我们知道对于下落物体，加速度等于g 。

状态外推方程如下所示：

矩阵乘法产生以下结果：

状态外推方程维度

下表指定了状态外推方程变量的矩阵维度：

变量	描述
\ x	状态向量
\ F	状态转换矩阵
u	输入变量
\ G	控制矩阵
w	过程噪声矢量

线性时不变系统

线性卡尔曼滤波器假设LTI（线性时不变）系统模型。

那么，什么是“线性”，什么是“时不变”？

线性系统由方程组描述，其中变量永远不会彼此相乘，而只是与常数相加，然后相加。线性系统用于描述变量之间的静态和动态关系。

线性系统是其输出函数 $\mathcal{F}$ 满足以下等式的系统：

其中：

a 和 b 是常数实数

g 和 h 是自变量 t 的任何任意函数

线性系统遵循两个基本规则：

您可以“分解”恒定乘法比例因子（上面的a 和 b ）。
系统对输入总和的响应分别是对每个输入的响应总和。

时不变系统具有不是时间直接函数的系统函数。

让我们以增益为（ G = 10 ）的放大器为例。

这个系统是时不变的。尽管系统的输出随时间变化，但系统功能与时间无关。

时不变系统是指输入序列中的时间延迟（或移位）导致系统输出序列中的等效时间延迟的系统。

线性动态系统建模

嗯，这很容易描述飞机的动态模型。我想你从高中就熟悉牛顿的运动方程。

本章概括了任何线性动态系统的动态模型推导。以下描述包括积分方程和微分方程。

本章是本教程中最具挑战性的章节。理解卡尔曼滤波原理不需要它。如果您对这个数学感到不舒服 - 请随时跳过本章。

就我而言，我试图使我的解释尽可能简单易懂，当然，我提供了现实生活中的例子。

状态外推方程的推导

我们的目标是以以下形式推导出状态外推方程：

为此，我们需要对动态系统进行建模。换句话说，要弄清楚动态系统的状态空间表示。以下两个方程是 LTI 系统的状态空间表示：

哪里：

x	是状态向量
y	是输出向量
A	是系统的动态矩阵
B	是输入矩阵
C	是输出矩阵
D	是馈通矩阵

要找到状态转移矩阵 F 和输入转换矩阵 G ，我们需要求解状态空间微分方程。

下图总结了状态外推方程推导的过程。

状态空间表示形式

您可能想知道为什么状态空间表示形式必须采用以下形式：

许多求解微分方程的计算机软件包都需要这种表示。

描述状态空间表示的最佳方式是通过示例。

示例 - 等速运动体

由于没有施加到身体上的外力，因此系统没有输入：

状态空间变量 x（t）是物体的位移 p（t）和速度v（t）。

注意：为了避免状态向量 x（t）（粗体字体）和沿 x 轴的身体位置（由 x（t））表示（正常面字体），我用 p（t）表示身体位置。

我们得到了微分形式的第一个方程。

动态系统输出 y（t）是主体位移 p（t），它也是状态变量。

我们完成了。

高阶动态系统建模

许多动态系统模型由高阶微分方程描述。微分方程的阶数是微分方程中最高导数的数。

为了解决高阶方程，我们应该通过定义新变量并将它们代入最高阶项来将其简化为一阶微分方程。

微分方程阶的简化算法

一般的 n -th 阶线性微分方程可以表示为 n 一阶微分方程组。

动态系统的高阶控制方程如下所示：

控制方程完全表征了系统的动态状态。

减少方程阶数：

隔离最高阶导数：

y 及其第一个 n - 1 导数是这个系统的状态。

2.定义一个新变量 $x_{1}$ ：

设置 $x_{1} = y$ ，我们写：

现在，函数 $x_{i}(t)$ 是状态变量。

3.取状态变量的导数：

4. 将隔离的 $\frac{d^{n}y}{dt^{n}}$ 项（参见步骤 1）代入最后一个等式：

5. 使用向量矩阵表示法表示生成的方程组：

就是这样。我们得到了以下形式的状态空间方程：

其中：

让我们看两个例子。

示例 - 恒加速度运动体

在这个例子中，有一个外力施加到身体上。

具有恒定加速度的运动物体的控制方程是牛顿第二定律：

其中：

p 是身体位置位移

m 是体重

F 是施加在身体上的外力

我们将把降阶算法应用于控制方程。

隔离最高阶导数：

2.定义新变量 $x_{1}$ 和 $x_{2} \$ ：

$x_{1}$ 和 $x_{2} \$ 是状态变量。

3. 取状态变量的导数

4.在最后一个等式中插入隔离的 $\ddot{p}$ 项（请参阅步骤 1）：

5.使用向量矩阵表示法表示生成的方程组：

请记住 $x_{2} = \dot{p}$ 。用 v 表示 $x_{2}$ 会更有意义，因为 $x_{2}$ 是体速。

我们可以将上面的等式重写如下：

或者

其中 a 是由施加的力 F 产生的身体加速度。

我们有一个公式，形式如下：

状态空间变量 x（t）是物体的位移 p（t）和速度 v（t）。

动态系统输出 y（t）是主体位移 p（t），它也是状态变量的一个元素。

示例：质量弹簧阻尼系统

质量弹簧阻尼器系统包括三个基本要素：

质量 - 惯性元件
弹簧 - 弹性元件
阻尼器 - 摩擦元件

每个元素都有以下两种可能的能量行为之一：

存储提供给它的所有能量
通过“摩擦”效应将所有能量耗散成热量

质量将能量存储为动能。

当弹簧从其原始长度压缩时，它将能量存储为势能。

阻尼器以热量的形式耗散能量。

这三个组件：质量、弹簧和阻尼器，可以在一般意义上模拟任何动态响应情况。

该系统的力图如下所示。

弹簧力与质量的位置位移 p 成正比。

粘性阻尼力与质量的速度 $\dot{p}$ 成正比。

牛顿第二定律指出：

我们继续求和力并应用牛顿第二定律：

其中：

\（ p \）是身体位置位移

\（ m \）是体重

\（ F \）是施加在身体上的外力

\（ k \）是弹簧常数

\（ c \）是阻尼系数

该方程是一个控制方程，可以完全表征系统的动态状态。

我们将把降阶算法应用于控制方程。

更多示例

您可以在 ShareTechnote 网站上找到许多状态空间建模的精美示例。

求解微分方程

请记住，对于我们的卡尔曼滤波模型，我们需要确定以下形式的状态外推方程：

为了达到这个目标，我们将求解描述状态空间表示的微分方程。

我们可以使用计算机软件来解决微分方程或自己做。

让我们看看如何求解微分方程。

无输入变量的动态系统

没有外部输入的LTI动态系统可以用一阶微分方程来描述：

其中 A 是一个系统动力学矩阵。

我们的目标是找到状态转换矩阵 F。

我们需要求解微分方程来找到 F 。

在单个维度中，微分方程如下所示：

整合双方可产生以下结果：

求解积分：

同样，在多维情况下，对于：

解决方案是：

我们找到了状态转换矩阵 F :

$e^{\boldsymbol{A}t}$ 是一个矩阵指数。

矩阵指数可以通过泰勒级数展开计算：

因此：

其中 $\boldsymbol{I}$ 是一个单位矩阵。

示例续 - 等速运动体

现在我们可以找到状态转换矩阵F对于等速运动方程。

以下一组微分方程可以描述恒定速度动力学模型。

以矩阵形式：

让我们计算 ${A}^{2}$

由于 $\boldsymbol{A}^{2} = 0$ ， $\boldsymbol{A}$ 的更高幂也等于 0。

现在，我们可以找到恒定速度模型的状态转换矩阵 $\boldsymbol{F}$ ：

具有输入变量的动态系统

对于零阶保持采样，假设输入是分段常数，状态空间方程的一般解形式为：

由以下给出：

如果我们删除输入变量 $\boldsymbol{u(t)} = 0$ ，我们得到上一章中得出的解决方案。

我不打算证明这一点——你可以在汤姆·M·阿波斯托尔的“微积分”教科书（第二版）、定理 8.3 或任何其他微积分教科书中找到证明。

现在让我们求解恒加速度运动体的状态空间方程和质量-弹簧-阻尼器系统示例。

示例续 - 恒加速度运动体

回想一下，恒定加速度运动体的状态空间表示为：

其中：

让我们求解等式：

结果 F

我们已经解决了

查找 G

$\int _{0}^{ \Delta t}e^{\boldsymbol{A}t}dt$ 的一般公式是幂级数：

$\boldsymbol{A}$ 的较高幂也等于 0。

现在，我们可以编写状态外推方程：

示例续 - 质量弹簧阻尼系统

回想一下，质量弹簧阻尼器系统的状态空间表示为：

在这个例子中，矩阵指数的计算并不容易，因为 A 的高幂不为零。

此微分方程的解超出了本教程的范围。

协方差外推方程

我假设读者已经熟悉协方差外推（预测）的概念。我们已经在“一维卡尔曼滤波”部分中遇到了协方差外推方程（或预测变量协方差方程）。在本节中，我们用矩阵符号推导出卡尔曼滤波协方差外推方程。

协方差外推方程的一般形式由下式给出：

$\boldsymbol{P}_{n,n}$ 是当前状态的估计值（协方差矩阵）的不确定性

$\boldsymbol{P}_{n+1,n}$ 是下一个状态的预测（协方差矩阵）的不确定性

F 是我们在“线性动态系统建模”部分中推导出的状态转换矩阵

Q 是过程噪声矩阵

无过程噪声的估计不确定度

假设过程噪声等于零 Q=0，则：

推导相对简单。我在“基本背景II”部分中展示了：

其中向量 x 是系统状态向量。

因此：

根据状态外推方程：

因此：

应用矩阵转置属性： $\boldsymbol{(AB)}^T = \boldsymbol{B}^T \boldsymbol{A}^T$

构造过程噪声矩阵 Q

如您所知，系统动态描述如下：

其中 $\boldsymbol{w}_{n}$ 是时间步长 n 处的进程噪声。

我们已经在“一维卡尔曼滤波器”部分讨论了过程噪声及其对卡尔曼滤波器性能的影响。在一维卡尔曼滤波器中，过程噪声方差用 q 表示。

在多维情况下，过程噪声是一个协方差矩阵，用 $\boldsymbol{Q}$ 表示。

我们已经看到，过程噪声方差对卡尔曼滤波器的性能有关键影响。太小的 q 会导致滞后错误（see Example 7）。如果 q 值太高，卡尔曼滤波器将遵循测量值（see Example 8）并产生噪声估计值。

过程噪声可以在不同的状态变量之间独立。在这种情况下，过程噪声协方差矩阵 $\boldsymbol{Q}$ 是对角矩阵：

过程噪声也可能取决于。例如，恒定速度模型假定加速度为零（a=0）。但是，加速度 $\sigma^{2}_{a}$ 的随机方差会导致速度和位置的方差。在这种情况下，过程噪声与状态变量相关。

环境过程噪声有两种模型。

离散噪声模型
连续噪声模型

离散噪声模型

离散噪声模型假设噪声在每个周期不同，但在周期之间是恒定的。

对于等速度模型，过程噪声协方差矩阵如下所示：

我们用模型的随机加速度方差来表示位置和速度方差和协方差： $\sigma^{2}_{a}$ 。

我们使用“基本背景II”部分中的期望算术规则推导出矩阵元素。

现在我们可以将结果代入 $\boldsymbol{Q}$ 矩阵：

有两种方法可以更快地构造 $\boldsymbol{Q}$ 矩阵。

使用状态转换矩阵进行投影

如果动态模型不包含控制输入，我们可以使用状态转换矩阵在动态模型上投影加速度 $\sigma^{2}_{a}$ 的随机方差。

让我们定义一个矩阵 $\boldsymbol{Q}_{a}$ ：

过程噪声矩阵为：

对于运动模型， $\boldsymbol{F}$ 矩阵由下式给出：

使用控制矩阵进行投影

如果动态模型包含控制输入，我们可以更快地计算 $\boldsymbol{Q}$ 矩阵。我们可以使用状态转移矩阵在动态模型上投影加速度 $\sigma^{2}_{a}$ 的随机方差。

其中 $\boldsymbol{G}$ 是控制矩阵（或输入转换矩阵）。

对于运动模型， $\boldsymbol{G}$ 矩阵由下式给出：

您可以使用上述方法来构造离散 $\boldsymbol{Q}$ 矩阵。

连续噪声模型

连续模型假设噪声随时间连续变化。

为了推导出连续模型 $\boldsymbol{Q}_{c}$ 的过程噪声协方差矩阵，我们需要随着时间的推移对离散过程噪声协方差矩阵 $\boldsymbol{Q}$ 进行积分。

选择哪种型号？

在回答此问题之前，您需要为过程噪声方差选择正确的值。您可以使用随机统计公式进行计算，也可以根据您的工程实践选择一个合理的值（最好）。

在雷达世界中， $\sigma^{2}_{a}$ 取决于目标特性和模型完整性。对于机动目标，如飞机， $\sigma^{2}_{a}$ 应该相对较高。对于非机动目标，如火箭，您可以使用较小的 $\sigma^{2}_{a}$ 。模型完整性也是选择过程噪声方差的一个因素。如果您的模型包含空气阻力等环境影响，则过程噪声随机性的程度较小，反之亦然。

选择合理的过程噪声方差值后，应选择噪声模型。它应该是离散的还是连续的？

这个问题没有明确的答案。我建议尝试这两种模型，并检查哪一个在卡尔曼滤波器上表现更好。当 $\Delta t$ 非常小时，可以使用离散噪声模型。当 $\Delta t$ 较高时，最好使用连续噪声模型。

测量公式

到目前为止，我们已经处理了未来。我们推导出了两个卡尔曼滤波预测方程：

状态外推方程
协方差外推方程

从现在开始，我们将处理现在。让我们从测量方程式开始。

在“一维卡尔曼滤波器”部分，我们用 $z_{n}$ 表示测量值。

测量值表示除测量设备引起的随机测量噪声 $v_{n}$ 之外的真实系统状态。

每次测量的测量噪声方差 $r_{n}$ 可以是恒定的 - 例如，精度为 0.5kg（标准偏差）的秤。另一方面，每次测量的测量噪声方差 $r_{n}$ 可能不同 - 例如，精度为 0.5%（标准偏差）的温度计。在后一种情况下，噪声方差取决于测量的温度。

矩阵形式的广义测量方程由下式给出：

$\boldsymbol{z}_{n}$	是测量向量
$\boldsymbol{x}_{n}$	是真实的系统状态（隐藏状态）
$\boldsymbol{v}_{n}$	是一个随机噪声矢量
H	是一个观察矩阵

观察矩阵 H

在许多情况下，测量值不是所需的系统状态。例如，数字电温度计测量电流，而系统状态是温度。需要将系统状态（输入）转换为测量（输出）。

观察矩阵 H 的目的是使用线性变换将系统状态转换为输出。以下章节包括观察矩阵用法的示例。

缩放

测距仪向目的地发送信号并接收反射回波。测量是信号发送和接收之间的时间延迟。系统状态是范围。

在这种情况下，我们需要执行缩放：

其中：

c 是光速

$\boldsymbol{x}_{n}$ 是范围

$\boldsymbol{z}_{n}$ 是测量的时间延迟

状态选择

有时某些状态是测量的，而其他状态则不是。例如，五维状态向量的第一、第三和第五个状态是可测量的，而第二和第四个状态是不可测量的：

状态组合

有时可以测量状态的某种组合，而不是每个单独的状态。例如，三角形边的长度可能是状态，并且只能测量总周长：

测量方程尺寸

下表指定了测量公式变量的矩阵维度：

中期摘要

这是一个停下来做一个简短总结的好地方。在进一步讨论之前，我想总结一下我们迄今所学到的情况。

您还记得“一维卡尔曼滤波部分”（如果您不记得了，请再次查看），卡尔曼滤波计算基于五个方程。

两个预测方程：

状态外推方程 - 根据已知的当前估计预测或估计未来状态。
协方差外推方程 - 预测中不确定性的度量。

两个更新公式：

状态更新方程 - 根据已知的过去估计和当前测量估计当前状态。
协方差更新方程 - 估计中不确定性的度量。

卡尔曼增益方程 – 计算更新方程所必需的。卡尔曼增益是测量和过去估计的“加权”参数。它定义了过去估计的权重和估计当前状态的测量权重。

到目前为止，我们已经学习了矩阵符号中的两个预测方程和计算主方程所需的几个辅助方程。

预测方程

状态外推方程

矩阵表示法中状态外推方程的一般形式为：

$\boldsymbol{\hat{x}}_{n+1,n}$	是时间步长 \（ n + 1 \）处的预测系统状态向量
$\boldsymbol{\hat{x}}_{n,n}$	是时间步长 \（ n \）处的估计系统状态向量
$\boldsymbol{u}_{n}$	是控制变量或输入变量 - 系统的可测量（确定性）输入
$\boldsymbol{w}_{n}$	是过程噪声或干扰 - 影响状态的不可测量输入
F	是状态转移矩阵
G	是控制矩阵或输入转换矩阵（将控件映射到状态变量）

协方差外推方程

协方差外推方程的一般形式由下式给出：

$\boldsymbol{P}_{n,n}$	是当前状态估计的不确定性（协方差）矩阵
$\boldsymbol{P}_{n+1,n}$	是下一个状态估计（预测）的不确定性（协方差）矩阵
F	是我们在“线性动态系统建模”部分中推导出的状态转换矩阵
Q	是过程噪声矩阵

辅助方程

测量公式

矩阵形式的广义测量方程由下式给出：

协方差方程

对应于过程和测量噪声的术语 $\boldsymbol{w}$ 和 $\boldsymbol{v}$ 通常不会直接出现在计算中，因为它们是未知的。

相反，这些项用于模拟方程本身中的不确定性（或噪声）。

所有协方差方程都是协方差矩阵，形式为：

即，对平方误差的期望。有关更多详细信息，请参阅基本背景 II 部分。

测量不确定度

测量不确定度由下式给出：

\ $\boldsymbol{R}_{n}$	是测量值的协方差矩阵
\ $\boldsymbol{v}_{n}$	是测量误差

过程噪声不确定性

过程噪声不确定度由下式给出：

\ $\boldsymbol{Q}_{n}$	是过程噪声的协方差矩阵
\ $\boldsymbol{w}_{n}$	是过程噪声

估计不确定性

估计不确定性由下式给出：

\ $\boldsymbol{P}_{n,n}$	是估计误差的协方差矩阵
\ $\boldsymbol{e}_{n}$	是估计误差
\ $\boldsymbol{x}_{n}$	是真实的系统状态（隐藏状态）
\ $\boldsymbol{\hat{x}}_{n,n}$	是时间步长 \（ n \）处的估计系统状态向量

状态更新公式

此页面是本教程中最短的页面。我在“\（ \alpha -\beta -\gamma \）滤波器”部分和“一维卡尔曼滤波器”部分中提供了对状态更新方程的广泛描述。

矩阵形式的状态更新方程由下式给出：

\ $\boldsymbol{\hat{x}}_{n,n}$	是时间步长 \（ n \）处的估计系统状态向量
\ $\boldsymbol{\hat{x}}_{n,n-1}$	是时间步长 \（ n - 1 \）处的预测系统状态向量
\ $\boldsymbol{K}_{n}$	是卡尔曼增益
\ $\boldsymbol{z}_{n}$	是一种度量
\H	是一个观察矩阵

您应该熟悉状态更新方程的所有分量，但矩阵表示法中的卡尔曼增益除外。我们将在以下章节中推导出卡尔曼增益。

您应该注意尺寸。例如，如果状态向量有 5 个维度，而只有 3 个维度是可测量的（第一、第三和第五个状态）：

观察矩阵将是一个 $3 \times 5$ 矩阵：

创新( $\boldsymbol{z}_{n} - \boldsymbol{H \hat{x}}_{n,n-1}$ )产生

卡尔曼增益维度应为 5X 3 。

状态更新公式维度

下表指定了状态更新公式变量的矩阵维度：

协方差更新方程

协方差更新方程由下式给出：

\ $\boldsymbol{P}_{n,n}$	是当前状态估计的不确定性（协方差）矩阵
\ $\boldsymbol{P}_{n,n-1}$	是当前状态的先验估计不确定性（协方差）矩阵（在前一个状态预测）
$\boldsymbol{K}_{n}$	是卡尔曼增益
\H	是观察矩阵
\ $\boldsymbol{R}_{n}$	是测量不确定度（测量噪声协方差矩阵）
\ I	是一个单位矩阵( $n \times n \$ 方阵，主对角线上有 1，其他地方有零）

协方差更新方程推导

本节包括协方差更新方程推导。你们中的一些人可能会觉得它太详细了，但另一方面，它会帮助其他人更好地理解。

如果您不关心派生，可以跳到下一个主题。

对于推导，我使用以下四个等式：

我们推导出当前估计不确定性 $\boldsymbol{P}_{n,n}$ 作为卡尔曼增益 $\boldsymbol{K}_{n}$ 的函数。

卡尔曼增益

最后一个方程是卡尔曼增益方程。

矩阵符号中的卡尔曼增益由下式给出：

\ $\boldsymbol{K}_{n}$	是卡尔曼增益
\ $\boldsymbol{P}_{n,n-1}$	是当前状态的先验估计不确定性（协方差）矩阵（在上一步预测）
\ H	是观察矩阵
\ $\boldsymbol{R}_{n}$	是测量不确定度（测量噪声协方差矩阵

卡尔曼增益方程推导

本章包括卡尔曼增益方程的推导。如果您不关心派生，可以跳到下一个主题。

首先，让我们重新排列协方差更新方程：

卡尔曼滤波是最佳滤波。因此，我们寻求一个卡尔曼增益，以最小化估计方差。

为了最小化估计方差，我们需要最小化协方差矩阵 $\boldsymbol{P}_{n,n}$ 的主对角线（从左上角到右下角）。

方阵主对角线之和就是矩阵的迹线。因此，我们需要最小化 tr（ $\boldsymbol{P}_{n,n}$ ）。为了找到产生最小值所需的条件，我们将 $\boldsymbol{P}_{n,n}$ 的跟踪与 $\boldsymbol{K}_{n}$ 进行区分，并将结果设置为零。

简化协方差更新公式

在许多教科书中，您可以找到协方差更新方程的简化形式：

要推导协方差更新方程的简化形式，请将卡尔曼增益方程代入协方差更新方程。

警告！这个等式更优雅，更容易记住，并且在许多情况下表现良好。然而，计算卡尔曼增益时的微小误差（由于舍入）可能会导致巨大的计算误差。减法 $\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H}$ 由于浮点误差，可能会导致非对称矩阵。计算卡尔曼增益时的微小误差（由于舍入）可能导致巨大的计算误差。减法由于浮点误差，可能导致非对称矩阵。！！

总结

我们已经用矩阵符号推导出了所有五个卡尔曼滤波方程。让我们把它们放在一个页面上。

卡尔曼滤波在“预测-校正”循环中运行，如下图所示。

初始化后，卡尔曼滤波器将在下一步预测系统状态。它还提供了预测的不确定性。

收到测量值后，卡尔曼滤波会更新（或校正）预测和当前状态的不确定性。卡尔曼滤波器还可以预测以下状态，依此类推。

下图提供了卡尔曼滤波操作的完整图片。

下表描述了所有卡尔曼滤波方程。

下表总结了符号（包括文献中发现的差异）和尺寸。

尺寸表示法：

\ $n_{x}$ 是状态向量中的多个状态
\ $n_{z}$ 是许多测量状态
\ $n_{u}$ 是输入变量的多个元素

例子

这是多元卡尔曼滤波章节的最后一部分。

它包括两个数值示例。在第一个例子中，我们设计了一个没有控制输入的六维卡尔曼滤波器。在第二个示例中，我们设计了一个带有控制输入的二维卡尔曼滤波器。

示例 9 – 车辆位置估计

在下面的示例中，我们使用所学的材料实现多元卡尔曼滤波器。

在此示例中，我们想估计车辆在 XY 平面上的位置。

车辆有一个车载位置传感器，可报告系统的 \（ X \）和 \（ Y \）坐标。

我们假设恒定的加速度动态。

状态外推方程

首先，我们推导出状态外推方程。您还记得，矩阵表示法中状态外推方程的一般形式是：

\ $\boldsymbol{\hat{x}}_{n+1,n}$	是时间步长 \（ n + 1 \）处的预测系统状态向量
\ $\boldsymbol{\hat{x}}_{n,n}$	是时间步长 \（ n \）处的估计系统状态向量
\ $\boldsymbol{u}_{n}$	是一个控制变量
\ $\boldsymbol{w}_{n}$	是过程噪声
\F	是状态转移矩阵
\G	是控制矩阵

在此示例中，没有控制变量 $\boldsymbol{u}$ ，因为没有控制输入。

对于此示例，状态外推方程可以简化为：

系统状态 $\boldsymbol{x}_{n}$ 由下式定义：

时间 \（ n + 1 \）的外推车辆状态可以用以下方程组来描述：

以矩阵形式：

协方差外推方程

协方差外推方程的一般形式由下式给出：

Pn，n	是当前状态估计的协方差矩阵
Pn+1，n	是下一个状态估计（预测）的协方差矩阵
F	是我们在“线性动态系统建模”部分中推导的状态转换矩阵
Q	是过程噪声矩阵

估计协方差矩阵为：

矩阵主对角线上的元素是估计的方差：

$p_{x}$ 是 X 坐标位置估计的方差pxX
$p_{\dot{x}}$ 是 X 坐标速度估计的方差px˙˙X
$p_{\ddot{x}}$ 是 X 坐标加速度估计的方差px¨X
$p_{y}$ 是 Y 坐标位置估计的方差pyY
$p_{\dot{y}}$ 是 Y 坐标速度估计的方差py˙Y
$p_{\ddot{y}}$ 是 Y 坐标加速度估计的方差py¨Y
非对角线条目是协方差

我们假设 X 轴和 Y 轴中的估计误差不相关，因此互项可以设置为零。

我们已经导出了状态转换矩阵 F 。现在我们将推导出过程噪声 Q 矩阵。

过程噪声矩阵

我们假设一个离散噪声模型 - 每个时间样本的噪声不同，但在时间样本之间是恒定的。

二维恒加速度模型的过程噪声矩阵如下所示：

我们已经推导出了恒加速度运动模型的 Q 矩阵。我们示例中的 Q 矩阵为：

其中：

$\Delta t$ 是连续测量之间的时间
$\sigma_{a}^{2}$ 是加速度的随机方差

现在我们可以为我们的示例写下协方差外推方程

测量公式

矩阵形式的广义测量方程由下式给出：

zn	是测量向量
xn	是真实的系统状态（隐藏状态）
vn	随机噪声矢量
H	是观察矩阵

测量仅为我们提供车辆的 X 和 Y 坐标。So：

$\boldsymbol{z}_{n}$ 的维度是 2X1， $\boldsymbol{x}_{n}$ 的维度是 $6 \times 1$ 。因此，观察矩阵 $\boldsymbol{H}$ 的维数应为 2 X 6 。

测量不确定度

卡尔曼增益

矩阵符号中的卡尔曼增益由下式给出：

我们已经推导出了卡尔曼增益的所有构建块：

状态更新公式

矩阵形式的状态更新方程由下式给出：

我们已经定义了状态更新方程的所有构建块。

协方差更新方程

协方差更新方程由下式给出：

我们已经定义了协方差更新方程的所有构建块。

数值示例

现在我们准备求解数值示例。让我们假设一辆车辆以恒定的速度沿 X 方向直线移动。行驶400米后，车辆右转，转弯半径为300米。在转弯操作期间，车辆会因圆周运动（角加速度）而加速。

下图描述了车辆的运动。

测量周期： Δt=1s
随机加速度标准差： \ $sigma_{a} = 0.2\frac{m}{s^{2}}$
测量误差标准差： $\sigma_{x_{m}} = \sigma_{y_{m}} = 3m$
状态转换矩阵 F 将为：

过程噪声矩阵 Q 将为：

测量协方差 R 将为：

下表包含一组 35 个噪声测量值：

迭代零

初始化

我们不知道车辆位置;我们将初始位置、速度和加速度设置为 0

由于我们的初始状态向量是一个猜测，因此我们设置了一个非常高的估计不确定性。高估计不确定度通过为测量提供高权重而导致高卡尔曼增益。

预测

现在我们可以根据初始化值预测下一个状态。

第一次迭代

步骤 1 - 测量

测量值：

步骤 2 - 更新

卡尔曼增益计算：

如您所见，位置的卡尔曼增益为 0.9921，这意味着第一次测量的权重明显高于估计的权重。估计的权重可以忽略不计。

估计当前状态：

更新当前估计协方差：

步骤 3 - 预测

我们的预测不确定性仍然很高。

第二次迭代

步骤 1 - 测量

测量值：

步骤 2 - 更新

卡尔曼增益计算：

估计当前状态：

更新当前估计协方差：

步骤 3 - 预测

我们的预测不确定性仍然很高。

此时，跳转到最后一个卡尔曼滤波迭代是合理的。

第三十五次迭代

步骤 1 - 测量

测量值：

步骤 2 - 更新

卡尔曼增益计算：

该位置的卡尔曼增益已收敛到 0.56，这意味着测量和估计权重几乎相等。

估计当前状态：

更新当前估计协方差：

此时，位置方差 $p_{x} = p_{y} = 5$ ，这意味着估计的标准差为 $\sqrt{5}m$ 。

步骤 3 - 预测

示例分析

下图显示了卡尔费休的位置和速度估计性能。

左侧的图表比较了车辆位置的真实值、测量值和估计值。右侧的两个图表比较了 x 轴速度和 y 轴速度的真实值、测量值和估计值。

如您所见，卡尔曼滤波器成功跟踪车辆。

让我们放大车辆运动的线性部分和转弯机动部分。

图上的圆圈表示 95% 置信度椭圆。由于 x 轴和 y 轴的测量误差相等，因此置信度椭圆是一个圆。

当车辆沿直线行驶时，加速度恒定且等于零。然而，在转弯机动期间，由于圆周运动 - 角加速度，车辆会经历加速。

回想一下物理学入门，角加速度是 $\alpha = \frac {\Delta V}{R \Delta t}$ ，其中 $\Delta t$ 是时间间隔，\Delta V 是时间间隔内的速度差，R 是圆半径。

尽管角加速度是恒定的，但 x 轴和 y 轴上的角加速度投影不是恒定的，因此 $\ddot{x}$ 和 $\ddot{ y}$ 不是恒定的。

我们的卡尔曼滤波器专为恒加速度模型而设计。尽管如此，由于正确选择了 $\sigma_{a}^{2}$ 参数，它成功地跟踪了机动车辆。

我想鼓励读者在软件中实现这个例子，看看 $\sigma_{a}^{2}$ 或 $\boldsymbol{R}$ 的不同值如何影响实际的卡尔曼滤波精度、卡尔曼增益收敛性和估计不确定性。

示例 10 – 火箭高度估计

在此示例中，我们估计火箭的高度。火箭配备了一个提供高度测量的机载高度计。除了高度计外，火箭还配备了一个加速度计，用于测量火箭的加速度。

加速度计用作卡尔曼滤波器的控制输入。

我们假设恒定的加速度动态。

加速度计不感知重力。静止在桌子上的加速度计向上测量 1g，而自由落体的加速度计测量零加速度。因此，我们需要从每个加速度计测量中减去重力加速度常数g。

时间步长 n 的加速度计测量值为：

$\ddot{x}$ 是对象的实际加速度（对象位置的二阶导数）
g是重力加速度常数; g = -9.8 $\frac{m}{s^{2}}$
ε 是加速度计测量误差

状态外推方程

矩阵表示法中状态外推方程的一般形式为：

在这个例子中，我们有一个控制变量 u ，它基于加速度计的测量。

系统状态 $\boldsymbol{x}_{n}$ 由下式定义：

$x_{n}$ 是时间 n 的火箭高度
$\dot{x}_{n}$ 是时间 n 的火箭速度

我们可以将状态外推方程表示如下：

在上式中：

协方差外推方程

协方差外推方程的一般形式为：

矩阵形式的估计协方差为：

矩阵主对角线的元素是估计的方差：

$p_{x}$ 是高度估计的方差
$p_{\dot{x}}$ 是速度估计的方差
非对角线条目是协方差

我们已经导出了状态转换矩阵 F。现在我们将推导出过程噪声 Q 矩阵。

过程噪声矩阵

我们假设一个离散噪声模型 - 每个时间样本的噪声不同，但在时间样本之间是恒定的。

恒加速度模型的过程噪声矩阵如下所示：

$\Delta t$ 是连续测量之间的时间
$\epsilon^{2}$ 是加速度计测量中的随机方差

在前面的示例中，我们使用系统的加速度随机方差 $\sigma_{a}^{2}$ 作为过程噪声矩阵的乘数。但在这里，我们有一个加速度计来测量系统的随机加速度。加速度计误差v远低于系统的随机加速度;因此，我们使用 $\epsilon^{2}$ 作为过程噪声矩阵的乘数。

它使我们的估计不确定性大大降低！

现在我们可以为我们的示例写下协方差外推方程：

注意：根据“构造过程噪声矩阵”一章，恒速模型的 Q 矩阵的大小应为2 X 2，恒加速度模型的Q 矩阵的大小应为3 X 3。在此示例中，加速度由控制输入处理（因此，它不是 F 矩阵的一部分，并且 Q 矩阵为 2 X 2 ）。如果没有外部控制输入，过程矩阵将为 2 X 2 。

测量公式

矩阵形式的广义测量方程由下式给出：

测量仅提供火箭的高度： $\boldsymbol{z}_{n} = [ x_{n,measured} ]$

$\boldsymbol{z}_{n}$ 的维数是 $1 \times 1$ ，而 $\boldsymbol{x}_{n}$ 的维数是 $2 \times 1$ ，所以观察矩阵 $\boldsymbol{H}$ 的维数是 $1 \times 2$ 。

测量不确定度

卡尔曼增益

矩阵符号中的卡尔曼增益由下式给出：

我们已经推导出了卡尔曼增益的所有构建块：

状态更新公式

矩阵形式的状态更新方程由下式给出：

我们已经定义了状态更新方程的所有构建块。

协方差更新方程

协方差更新方程由下式给出：

我们已经定义了协方差更新方程的所有构建块。

数值示例

让我们假设一个具有恒定加速度的垂直助推火箭。火箭配备了一个提供高度测量的高度计和一个用作控制输入的加速度计。

测量周期： Δt=0.25s
火箭加速： $\ddot{x} = 30\frac{m}{s^{2}}$
高度计测量误差标准偏差： $\sigma_{x_{m}} = 20m$
加速度计测量误差标准偏差： $\epsilon = 0.1\frac{m}{s^{2}}$
状态转换矩阵 $\boldsymbol{F}$ 将为：

控制矩阵 $\boldsymbol{G}$ 将是：

过程噪声矩阵 $\boldsymbol{Q}$ 将为：

测量方差 $\boldsymbol{R}$ 为：

下表包含一组高度计和加速度计的 30 个噪声测量值：

迭代零

初始化

我们不知道火箭的位置;我们将初始位置和速度设置为 0。

我们也不知道火箭的加速度，但我们可以假设它大于零。让我们假设：

由于我们的初始状态向量是一个猜测，因此我们设置了一个非常高的估计不确定性。高估计不确定性导致高卡尔曼增益，从而为测量提供高权重。

预测

现在我们可以根据初始化值预测下一个状态。

第一次迭代

步骤 1 - 测量

测量值：

步骤 2 - 更新

卡尔曼增益计算：

估计当前状态：

更新当前估计协方差：

我们的预测不确定性仍然很高。

第二次迭代

步骤 1 - 测量

测量值：

步骤 2 - 更新

卡尔曼增益计算：

估计当前状态：

更新当前估计协方差：

步骤 3 - 预测

此时，跳转到最后一个卡尔曼滤波迭代是合理的。

第三十次迭代

步骤 1 - 测量

测量值：

步骤 2 - 更新

卡尔曼增益计算：

高度的卡尔曼增益收敛到0.12，这意味着估计权重远高于测量权重。

估计当前状态：

更新当前估计协方差：

此时，高度方差 $p_{x} = 49.3$ ，这意味着估计的标准差为 $\sqrt{49.3}m = 7.02m$ （请记住测量的标准差为 20m）。

步骤 3 - 预测

示例分析

下图比较了火箭高度的真实值、测量值和估计值。

我们可以看到良好的钦哲基金会跟踪性能和收敛性。

下图比较了火箭速度的真实值、测量值和估计值。

将估计值收敛到真正的火箭速度大约需要 2.5 秒。

一开始，估计的高度受到测量值的影响，并且由于测量值非常嘈杂，因此与真实的火箭高度不太一致。

但随着KF收敛，噪声测量的影响较小，估计的高度与真实高度完全一致。

在这个例子中，我们没有任何导致加速度变化的动作，但如果我们有，控制输入（加速度计）将更新状态外推方程。

卡尔曼滤波器简介——多维卡尔曼滤波

前言

基本背景

状态外推方程

示例 - 飞机 - 无控制输入

示例 - 带控制输入的飞机

示例 – 坠落物体

状态外推方程维度

线性时不变系统

线性动态系统建模

状态外推方程的推导

状态空间表示形式

示例 - 等速运动体

高阶动态系统建模

示例 - 恒加速度运动体

示例：质量弹簧阻尼系统

求解微分方程

无输入变量的动态系统

具有输入变量的动态系统

协方差外推方程

无过程噪声的估计不确定度

构造过程噪声矩阵 Q

离散噪声模型

连续噪声模型

选择哪种型号？

测量公式

观察矩阵 H

缩放

状态选择

状态组合

测量方程尺寸

中期摘要

预测方程

状态外推方程

协方差外推方程

辅助方程

测量公式

协方差方程

状态更新公式

状态更新公式维度

协方差更新方程

协方差更新方程推导

卡尔曼增益

卡尔曼增益方程推导

简化协方差更新公式

总结

例子

示例 9 – 车辆位置估计

状态外推方程

协方差外推方程

测量公式

卡尔曼增益

状态更新公式

协方差更新方程

数值示例

示例分析

示例 10 – 火箭高度估计

状态外推方程

协方差外推方程

测量公式

卡尔曼增益

状态更新公式

协方差更新方程

数值示例

示例分析

相关文章