热文推荐：MIT雾天识别系统

卡尔曼滤波器是传感器融合工程师用于自动驾驶汽车的工具。想象一下，你有一个雷达传感器，告诉你另一辆车距离15米，一个激光传感器说车辆距离20米。你如何协调这些传感器测量？这就是卡尔曼滤波器的功能。卡尔曼滤波在自动驾驶汽车上的应用十分广泛，笔者搜集并整理了几篇卡尔曼滤波的文章，陆续带给大家，希望能给大家的研究一些帮助。本文为一段关于卡尔曼滤波有趣的对话，清晰把卡尔曼滤波的知识讲明白了。

卡尔曼滤波器讨论（作者：Harveen Singh）

最近我遇到了一位同事拉里，他是一位年轻的开发人员，对能够成为自动驾驶汽车行业的一员感到非常兴奋。他问我关于卡尔曼滤波器的理解。所以这里就有了下面的谈话。

拉里：什么是卡尔曼滤波器？

我：卡尔曼滤波器是一种有助于预测值的工具。

拉里：那太酷了！意味着它是某种占卜功能？
我：呃，不是。这是一个迭代的数学过程，它使用一组方程和连续的数据输入来快速估计我们感兴趣的与该物体相关的值。基本上都是快速数学！

拉里：我在数学上并不擅长！也许这听起来有点荒唐。你能继续吗？
我：好吧，第一件事，卡尔曼过滤器使用gaussians或正态分布。

拉里：正态分布？
我：在连续图形中，数据可以以不同的方式展开：它可以传播到左侧，也可以传播到右侧或混乱。

图1.左倾斜和右倾斜分布的示例

但是有很多情况下，数据趋向于左右中间值，没有左偏差或右偏差，所得分布称为正态分布或高斯或钟形曲线。

图2.正态分布的例子，看起来像一个钟，因此钟形曲线。

拉里：明白了！正如你指出的那样，这是一条连续的曲线，而不是离散值。我怎么知道掷骰子掷到5点的概率？
我：好，问题不错，但是要表示像骰子获得概率那样的分布，我们使用二项分布，因为它是离散值。在正态分布中，您需要定义一个范围。说，在4.5到5.5厘米之间下雨的可能性是多少。因此，在这种情况下，我们有一个正态分布，我们标记点，并计算这些点下的面积，这给了我们相应的概率。

拉里：哦！那么完整图的面积将为1，因为概率可以有最大值1？
我：必须的，另外平均数，中位数和模式在这种分布情况下都是相等的。

拉里：但你没有用数学术语表示高斯？
我：所以，为了定义高斯，我们基本上有两件事情——均值和方差。您明显知道的平均值（μ），方差（σ²）基本上表示数字分布的数量以及平均数量。标准偏差（σ）只是公式给出的方差（σ²）的平方根：

拉里：那么与卡尔曼滤波器有关的高斯如何？
我：卡尔曼滤波器中的高斯表示我们的预测中的噪声/误差/不确定性的预测值，通常称为方差。预测值以平均值为中心，高斯宽度表示我们的值的不确定性。基本上它说明了我们有多少确定的价值是真实的。高斯的宽度表示更多的不确定性。

拉里：哦！你在这里玩了一些多态游戏。你刚才提到这是一个迭代过程？
我：是的，它基本上是一个两步过程——
1.预测
2.更新

在预测中，我们只是根据初始值预测称为预测值的新值，然后根据系统中存在的各种过程噪声预测我们预测中的不确定性/误差/方差。

在更新中，我们考虑来自设备的实际测量结果，我们称之为测量值。这里我们计算我们的预测值和测量值之间的差异，然后通过计算卡尔曼增益来决定保留哪个值。然后，我们根据Kalman Gain做出的决定计算新值和新的不确定性/误差/方差。这些计算值将最终成为我们的卡尔曼滤波器在迭代1中完成的预测。

更新步骤的输出再次馈入预测状态，并且循环继续，直到我们的预测值和实际值之间的误差/不确定性倾向于收敛到零。

拉里：那很快。你能解释任何例子或流程图？
我：好吧，从现在起，这些符号将被遵循：

x – >平均
P – >方差

图3.卡尔曼滤波器的粗略流程图

拉里：看起来很直观！但Kalman Gain到底是什么？
Me：卡尔曼增益是一个决定预测值和测量值应有多少权重的参数。这是一个决定我们的实际值是否接近预测值或测量值的参数。

拉里：但它如何知道如何相信预测值或实际值？
我：它检查我的朋友的不确定性/错误。

K =预测中的误差/（预测误差+测量误差）

K的取值范围为0到1。如果我们的测量误差很大，K接近0，这意味着我们的预测值接近实际值。如果我们预测的误差很大，K更接近1，这意味着我们的测量值更接近实际值。

拉里：好的，明白了！我现在变得很兴奋。你现在可以详细阐述方程吗？
我：当然。我会在预测和更新步骤中为您写下不同的内容。请记住这些公式适用于二维空间。

图4.预测步骤的等式

图5.更新步骤的等式

拉里：在这一步，可能我们应该结束我们的讨论。什么是F，B，H？有各种各样的变量搞乱了。你说这就是为了卡尔曼过滤器，我了解功能，但为什么现在如何解？
我：不要惊慌。我现在将详细解释一切。

假设我们想要根据来自不同传感器的测量结果来预测汽车的位置和速度。

x – >包含位置和速度的平均状态向量。
P – >协方差矩阵（表示误差）。

x向量

预测步骤

公式1-：

x’= Fx +B.μ+ν

x’ – >预测值
F – >状态转移矩阵
B – >控制输入矩阵
μ – >控制向量
ν – >过程噪声

F矩阵

F是将矩阵从一种形式转换为另一种形式所需的状态转移矩阵或自适应矩阵。例如，假设我们有一个模型来预测未加速的对象的位置和速度。所以在这种情况下，时间增量t之后的新p和v被赋予为：

p’= p +vΔt

v’= v

所以在这种情况下，F矩阵将是：

F矩阵

B矩阵

B是控制输入矩阵，表示由于内部或任何外力而引起的物体状态变化。例如：重力或对物体的摩擦力。

为什么B.μ= 0？

大多数情况下，在自动驾驶汽车中，控制乘积矢量的值等于零，因为我们无法模拟作用于汽车物体的外力。

ν

这是过程中的噪声。我们添加可能存在于频道中的随机噪声，以使我们的预测有点正确。

公式2-：

P’= FPF + Q

P’ – >预测协方差
Fᵀ – >状态转移矩阵
Q – >噪音的转置

Q矩阵

我们假设物体改变方向或者可能加速或减速。所以在一段时间Δt之后，我们的不确定度增加了Q因子，这又是噪声。所以我们在噪音技术上添加噪音。

所以在预测步骤中，我们得到两个预测值x’和P’。

更新步骤

公式1：

y = z-Hx’

z – >实际测量值
H – >状态转移矩阵
x’ – >预测值
y – >测量值与实际值之间的差值

Z

这是来自传感器的实际测量值。

H

这又是一个状态转换矩阵。使用H，我们可以丢弃我们不需要的状态变量的信息。从技术上来说，H在做与预测步骤中F相同的工作。

等式2：

S =HP’Hᵀ+ R
K =P’HᵀS – 1

R->测量噪声
K->卡尔曼增益
S->总误差
S -1 – > S的倒数

R

R表示测量中的噪声。什么？那么这些设备不是100％准确的？是的，在这个世界上没有什么是完美的，甚至没有衡量价值的设备。所有设备都有一个由制造商给出的R参数的预定义值，该值在整个周期内始终保持不变。

K

我们在这里有一个复杂的等式，但它非常简单。我们正在计算卡尔曼增益K，之前给出的公式。

S

这是系统中的总错误。我们的预测误差加上测量误差。

那么为什么如此复杂的K方程与公式中的早期相比简单呢？
这是因为我们没有对矩阵进行划分的概念。因此，我们选择首先计算总误差，然后将预测误差与总误差的倒数相乘。

等式3：

x = x’+ Ky
P =（I-KH）P’

最后一步

这是我们根据卡尔曼增益所做的计算更新x和P的最后一步。注意：对于LHS，我们有x和P，而不是x’和P’，因为我们现在将x和P设置为下一个预测步骤，因此我们需要找到它们的值。

拉里：好的，这需要一些时间来消化！我必须通过什么额外的资源才能完全理解它？
我：Michel van Biezen通过他的YouTube频道为卡尔曼滤波器做出的杰出贡献。观看他的视频以获取更多示例和见解。

（https://www.youtube.com/channel/UCiGxYawhEp4QyFcX0R60YdQ）

总结所有方程式：

我的同事拉里对于了解卡尔曼滤波器的工作非常兴奋，但他真的能理解非线性和扩展卡尔曼滤波器的概念吗？

拉里：我知道卡尔曼滤波器，现在我可以预测和更新，我基本知道一个重要的预测工具。

我：你能告诉我什么是我们在阅读卡尔曼滤波器时做出的假设吗？

拉里：你什么意思？你只是说卡尔曼滤波器只能使用高斯滤波器。而已？不是吗？
我：那是对的。上一篇文章中隐藏的另一个重要假设是线性函数。所以这两个假设是：
1.卡尔曼滤波器将始终与高斯分布一起工作。
2.卡尔曼滤波器将始终与线性函数一起工作。

拉里：天啊！线性函数从哪里出现？
Me：使用线性函数，我的意思是预测和更新步骤都只包含线性函数。如果你仔细观察所有的方程，他们已经在那里了。

线性函数看起来像这样：

另一方面，非线性函数如下所示：

因此，从这些图中观察，直线方程是线性函数，而cos函数是非线性函数。

拉里：是的，没关系。我们的方程没有任何角度，所以它们看起来只是线性的。那么KF现在的问题是什么？

我：大多数现实世界问题都涉及非线性函数。在大多数情况下，系统正在研究某个方向并在另一个方向进行测量。这涉及角度和正弦，余弦函数，它们是非线性函数，然后导致问题。

拉里：嗯，但是非线性函数又是如何产生问题的呢？
我：如果你喂一个线性函数的高斯，那么输出也是一个高斯函数。

如果喂入具有非线性函数的高斯，则输出不是高斯。非线性函数导致非高斯分布。

因此，如果我们应用非线性函数，它不会最终成为高斯分布，我们无法再应用卡尔曼滤波器。非线性破坏了高斯分布，计算均值和方差是没有意义的。

拉里：哦，不，在这种情况下，我们的卡尔曼滤波器现在已经损坏。那么解决方案是什么？
我：你能想到的最微不足道的解决方案是什么？

拉里：我？嗯。我会说只使用线性函数：D
我：即使它没有意义，但这正是解决方案。

拉里：什么？你是说我是对的？怎么来的？
我：是的，你是对的。我们只会使用线性函数。

拉里：那么你说的那些因素，罪恶功能呢？他们仍然是非线性的，对吧？
我：绝对。它们是非线性的，但我们将通过近似将它们变为线性。在这里，我们将借助泰勒级数的强大工具，帮助我们获得非线性函数的线性逼近。应用近似值后，我们得到的是扩展卡尔曼滤波器。

拉里：新工具不断涌现！泰勒是如何工作的？
我：在这一点上，我们有一个观点并且执行一系列衍生工具。在EKF的情况下，我们在非线性曲线上采用高斯平均值，并执行一系列的导数来逼近它。

假设我们想在x = 0时近似sin（x）。
假设我们想要找到一个多项式函数P（x）= c_0 + c_1 * x + c_2 *x²+ c_3 *x³来近似sin（x）。所以我们需要找出c_0，c_1，c_2和c_3的值。

在x = 0时，sin（x）= 0，P（x）= c_0 + 0 + 0如果我们的逼近必须比sin（x）更接近一点，那么sin（x）的值必须相等到在x = 0时P（x）的值。所以，c_0 = 0

如果我们的近似在x = 0
时具有与sin（x）相同的切线斜率也是好的。sin(x) at x=0
At x=0, d sin(x)/dx = cos(x) cos(0) = 1
d P(x)/dx = c_1 + 2*c_2*x + 3*c_3*x²= c_1 + 0 + 0

如果我们的近似必须精确，则P（x）的导数值必须等于x X = 0。所以c_1 = 1

继续…我们可以发现sin（x）= x – x³的近似值sin(x) = x − x³. 3! + x⁵. 5! − x⁷. 7! + x⁹. 9!…

拉里：很酷！这真的很酷。但是这又会给出一条非线性的曲线，我们是不是只对线性化感兴趣？
我：我们对线性化感兴趣，所以我们只对泰勒级数的一阶导数感兴趣。对于每个非线性函数，我们只需在均值附近绘制一条切线并尝试线性逼近函数。

拉里：嗯。好。KF只适用于线性函数，但在现实生活中，我们有非线性函数来破坏我们的高斯函数，所以我们尝试用泰勒级数线性逼近这些函数，并将其纳入扩展卡尔曼滤波器。对？
我：绝对的。

拉里：我想知道这是如何影响我们为卡尔曼滤波器编写的方程，但在此之前，所有提供数据的传感器都是如此？
我：假设我们有两个传感器LIDAR和RADAR。LIDAR以笛卡尔坐标系的形式为我们提供距离。另一方面，RADAR为我们提供了极坐标系统中的距离和速度。

Lidar => {px, py}
Radar =>{ρ, Φ, ρ_dot}

px，py – >笛卡尔坐标系中物体的坐标
ρ – >是到物体的距离
Φ – >是ρ和x轴之间的逆时针角度
ρ_dot – >是ρ的变化x轴总是在汽车前进的方向。

拉里：我们可以从两个传感器获取数据吗？
我：当然，必须的。从不同传感器获取数据并将它们组合在一起称为传感器融合。

拉里：好的，据我所知，来自雷达的测量结果是非线性的，因为它们涉及角度。现在我有兴趣了解扩展卡尔曼滤波器的方程！
我：对！当然。

预测步骤

x′ = F.x + B.μ + ν

P′ = FPFᵀ + Q

预测步骤与卡尔曼滤波完全相同。数据是来自激光雷达还是雷达并不重要，预测步骤完全相同。

更新步骤（仅在EKF情况下，即来自雷达的非线性测量）

公式1：

y = z – h（x’）

z – >极坐标中的实际测量
h – >函数，指定我们的速度和位置如何映射到极坐标
x’ – >预测值
y – >测量值和实际值之间的差异

H（X’）

这是一个函数，它指定了笛卡尔坐标系中的预测值与极坐标系之间的映射关系。这种映射是必需的，因为我们在笛卡尔坐标系中预测，但是我们的来自传感器的测量值（z）是在极坐标系中。

图7.笛卡尔和极坐标之间的映射

等式2：

S= HⱼP′Hⱼᵀ + R
K= P′HⱼᵀS⁻¹

R – >测量噪声
K – >卡尔曼增益
S->总误差
S
？ – >SH的倒数- >雅可比矩阵

Hⱼ

Hⱼ是雅可比矩阵。雅可比矩阵是我们刚刚在泰勒级数中讨论的一阶导数。既然我们在这里处理矩阵，我们需要以矩阵的形式找到差分。

J_kl = d F_k / dX_l

J_kl是雅可比矩阵的k，l元素，F_k是向量函数F的第k个元素，并且X_1是向量变量X的第l个元素。

这里F_k = {ρ, Φ, ρ_dot}，X_1 = {px，py，vx，vy}

因为在RADAR的情况下，我们有4个测量值，2个距离和2个速度。

图8.雅可比矩阵

等式3：

x = x′ + K.y
P = (I- KHⱼ)P′

拉里：哦，明白了！因此，在LIDAR的情况下，我们将应用卡尔曼滤波器，因为来自传感器的测量结果是线性的。但是在雷达的情况下，我们需要应用扩展卡尔曼滤波器，因为它包含非线性的角度，因此我们使用称为雅可比矩阵（Hⱼ）的泰勒级数的一阶导数来近似非线性函数。然后我们使用h（x’）将我们的笛卡尔空间转换为极坐标空间，最后在KF的所有其他方程中Hj替换H。

我：10/10。谢谢！

当然，雅可比矩阵有点魔幻，因为它将非线性空间转换为线性空间。但相信我，这不是什么魔术，而是数学。