了解卡尔曼滤波器3--最优状态估计算法及方程

最新推荐文章于 2024-07-24 22:13:06 发布

此时@此刻

最新推荐文章于 2024-07-24 22:13:06 发布

阅读量1.1k

点赞数

分类专栏：卡尔曼滤波文章标签：算法人工智能性能优化

本文链接：https://blog.csdn.net/qq_35611242/article/details/126422389

版权

卡尔曼滤波专栏收录该内容

6 篇文章 12 订阅

订阅专栏

本节，我们将讨论实现卡尔曼滤波算法所需的方程组。

让我们回顾一下我们在上一篇文章中介绍的例子，您参加了一个比赛，希望能赢得大奖。你需要设计一款自动驾驶汽车，要在100种不同地形上行驶1公里。在每次行驶任务中，汽车必须尽可能停靠终点线，在比赛结束时，计算每个队伍的平均最终位置, 具有最小方差而且平均位置最接近1公里的队伍将获得大奖。在那个例子中，我们还展示了汽车动力学以及单状态的汽车动力学模型, 我们还讨论了过程和测量噪声及其协方差。

最后，我们说到你可以通过使用卡尔曼滤波器来赢得比赛。它能计算最优无偏差汽车位置预估值, 而且方差最小。计算最优值首先将预测和测量概率函数相乘, 范围缩小, 然后计算得到的概率密度函数的均值。

从计算上说，这两个概率密度函数的乘法与这里所示的离散卡尔曼滤波器方程有关。还记得吗? 它看起来是不是像我们在之前的文章中讨论过的状态观测器方程? 实际上卡尔曼滤波器就是一种状态观测器, 但它是为随机系统设计的。这就是卡尔曼滤波器方程和我们讨论概率密度函数之间的关系。

第一部分预测当前状态，使用前一个时间步的估算状态以及当前的输入。请注意，这两个估算的状态并不同。我们将用这个标记（-）来表示这种预估状态值，这也称为预估值，因为它是在使用当前测量值之前计算的。

我们现在把这个等式重写成下面这样。等式的第二部分使用测量值, 代入方程来更新预估值。我们将结果称为后验估值。

你想赢得大奖，对吧? 那么下面这些是您需要在汽车的ECU上运行的公式。

看起来有点吓人? 好吧,我们来逐一讨论这些算法方程。卡尔曼滤波器分为两步。让我们首先从预测部分开始。系统模型用于计算状态预估值和误差协方差P。对于我们的单状态系统, P是状态预估值的方差，且把它当作预测状态中的不确定性的度量。这种不确定性来自过程噪声，和预估值^x(k-1) 的不确定性的影响。在算法的最开始, 预估值^x(k-1) 和P(k-1) 的值来自初始估计值。算法的第二步, 使用预测步骤中计算得到的预估值, 更新后的状态值及其误差协方差，调整卡尔曼增益, 使更新后的状态值误差协方差最小。

我们假设用下面右侧这个条表示估算值^x(k) 的计算。通过调整修正项，卡尔曼增益确定测量值和预估值对计算^x(k) 的影响程度。

如果测量噪音很小, 则测量值更可靠，则应对^x(k)的计算贡献更大。

在相反的情况下，预估值的误差很小, 则预估值更可信，所以^x(k)的计算更多的取决于预估值。

我们还可通过两种极端情况展示这个数学表达式。假设在第一种情况下，测量协方差趋R向于零。为了计算卡尔曼增益, 我们求K在R趋向于0的极值。我们将R替换为0，看到有两项相互抵消了。当R变为零时，卡尔曼增益接近C的倒数，在我们的系统中等于1。

将K=C^(-1)带入状态估算，得到^x(k)等于y(k), 因此计算结果仅取决于测量值,与预期一致。

另一方面，如果预估误差协方差接近于零, 则发现卡尔曼增益也为零。因此，带有卡尔曼增益的那一项对^x(k)的贡献为0, x(k)的计算完全来自于预估值。

我们计算了更新方程之后，在下一个时间步骤中，修正值用于预测新的预估值，继而重复。要注意的是，要估计当前状态, 算法不需要所有过去的信息。只需预估状态值和前一时间步的误差协方差矩阵，以及当前测量值。这就是卡尔曼滤波器递归的原因。

现在您知道了实现卡尔曼滤波算法所需的方程组，如果你赢得比赛，打算怎么处理大奖? 如果您无法决定,这有一个建议。注意,卡尔曼滤波器也称为传感器融合算法。因此,您可以购买额外的传感器，例如IMU，尝试使用多个传感器来提高您的自动驾驶汽车的位置估算精度。如果您有两个测量值, y，C和K矩阵的维度将如下式变化。