理解卡尔曼滤波器,第4部分:最优状态估计算法
从系列中:理解卡尔曼滤波器
Melda Ulusoy, MathWorks
发现实现卡尔曼滤波算法所需的一组方程。您将学习如何执行卡尔曼滤波算法的预测和更新步骤,并且您将看到卡尔曼增益如何合并预测状态估计(先验状态估计)和测量,以便计算新的状态估计(后ori状态估计)。
下载这个虚拟实验室通过互动练习学习线性和扩展卡尔曼滤波器设计。
在本视频中,我们将讨论实现卡尔曼滤波算法所需的一组方程。让我们回顾一下之前视频中介绍的例子。为了赢得大奖,你参加了一个比赛。你被要求设计一辆自动驾驶汽车,需要在100个不同的地形上行驶1公里。
在每次试车中,赛车必须尽可能靠近终点线停车。在比赛结束时,计算每个车队的平均试车位置,在最接近1公里的平均试车位置误差方差最小的车主将获得大奖。
在该示例中,我们还展示了单状态系统的汽车动力学和汽车模型,并讨论了过程噪声和测量噪声以及协方差。最后,我们说过你可以通过使用卡尔曼滤波器来赢得比赛,卡尔曼滤波器以最小的方差计算出对汽车位置的最优、无偏估计。
这个最优估计是通过将预测和测量概率函数相乘,缩放结果,并计算得到的概率密度函数的平均值来找到的。在计算上,这两个概率密度函数的乘法与这里所示的离散卡尔曼滤波方程有关。
这听起来耳熟吗?它看起来和我们在之前的视频中讨论过的状态观测器方程相似吗?实际上,卡尔曼滤波器是一种状态观测器,但它是为随机系统设计的。下面是卡尔曼滤波方程与我们讨论过的概率密度函数的关系。
第一部分通过使用来自前一个时间步和当前输入的状态估计来预测当前状态。请注意,这两个状态估计彼此不同。我们用这个符号来表示预测的状态估计。这也被称为先验估计,因为它是在进行当前测量之前计算的。
现在我们可以把方程写成这样。方程的第二部分使用测量值并将其纳入预测以更新先验估计。我们称这个结果为后验估计。你想赢大奖,对吧?然后这些是你需要在你的车CCU上运行的方程式。
看起来有点吓人?如果我们把一切都颠倒过来呢?变化不大,不是吗?好的,我们会一步一步地复习算法方程。卡尔曼滤波是一个两步过程。让我们先从预测部分开始。在这里,系统模型用于计算先验状态估计和误差协方差p。
对于我们的单状态系统,p是先验估计的方差,它可以被认为是估计状态不确定性的度量。这个方差来自于过程噪声和不确定的x帽k减1的传播。
在算法的最开始,x帽和p的k - 1值来自于它们的初始估计。该算法的第二步使用在预测步骤中计算的先验估计并更新它们以找到状态和误差协方差的后验估计。
卡尔曼增益的计算使后验误差协方差最小化。让这个柱状图代表x帽k的计算。通过加权修正项,卡尔曼增益决定了测量值和先验估计对x帽k的计算有多重要。
如果测量噪声小,测量值更可信,并且比先验状态估计更有助于计算x帽k。在相反的情况下,当先验估计的误差很小时,先验估计更可信。x ^ k的计算主要来自于这个估计。
我们也可以用两个极端的例子来证明这一点。假设在第一种情况下,测量协方差为0。为了计算卡尔曼增益,我们取r趋于0时的极限。我们把r代入0,看到这两项互相抵消了。
当r趋于0时,卡尔曼增益趋于c的逆,在我们的系统中等于1。把k, c的逆,代入后验集估计,得到x帽k = yk。所以计算只来自于预期的测量。
现在,如果我们更新我们的图表,我们可以用一个脉冲函数来显示测量值,就是这条橙色的垂直线。注意,测量中的方差为0,因为r趋于0。我们发现后验估计等于测量值,所以我们可以用相同的脉冲函数来表示。
另一方面,如果先验误差协方差接近0,则卡尔曼增益形式为0。因此,这一项对x帽k的贡献被忽略,x帽k的计算来自先验状态估计。在这些图上,我们将用一个具有零方差的脉冲函数来表示先验状态估计。
由于后验估计等于先验估计,我们用相同的脉冲函数来表示。一旦我们计算了更新的方程,在下一个时间步中,后验估计被用来预测先验估计,算法重复自己。
注意,要估计当前状态,算法不需要所有过去的信息。它只需要从前一个时间步和当前测量中估计的状态和误差协方差矩阵。这就是为什么卡尔曼滤波器是递归的。现在你知道了实现卡尔曼滤波算法所需的一组方程,当你赢得比赛时,你打算如何处理大奖呢?
如果你不能决定,这里有一个建议。请注意,卡尔曼滤波器也被称为传感器融合算法。所以你可以买一个额外的传感器,比如IMU,然后进行实验,看看使用多个传感器是否会提高你的自动驾驶汽车的估计位置。如果你有两次测量,y、c和k矩阵的维数将会改变,如图所示。
但基本上,你还是要遵循相同的逻辑来计算最佳状态估计。在图中,我们将有一个来自IMU的测量的概率密度函数。这一次,我们将把三个概率密度函数相乘来找到对汽车位置的最佳估计。
到目前为止,我们有一个线性系统,但是如果你有一个非线性系统并且想要使用卡尔曼滤波器呢?在下个视频中,我们将讨论非线性状态估计器。
您也可以从以下列表中选择一个网站:
如何获得最佳的网站性能
选择中国站点(中文或英文)以获得最佳站点性能。其他MathWorks国家站点没有针对您所在位置的访问进行优化。