可视化和调试EKF

我目前正在调试和调整EKF（扩展卡尔曼滤波器）。任务是经典的移动机器人姿态跟踪，其中地标是AR标记。

有时，我很惊讶某些度量如何影响估计。当我查看并计算涉及的数字和矩阵时，我可以算出执行更新步骤的方式，发生的原因和原因，但这非常繁琐。

因此，我想知道是否有人在使用某种技术，技巧或聪明的可视化来更好地了解EKF更新步骤中正在发生的事情？

更新＃1（将更具体，并显示我所想到的第一近似值）

我正在寻找的是以某种方式可视化一个更新步骤的方式，这种方式可以让我感觉到测量的每个组成部分如何影响状态的每个组成部分。

我的第一个想法是绘制测量值及其预测值以及从K矩阵中提取的一些矢量。来自K的向量表示创新向量（测量-测量预测，未绘制）将如何影响状态的每个分量。

目前，我正在使用状态为2D姿势（x，y，角度）且测量值也是2D姿势的EKF。

在所附的图片（在新页面/标签中打开以查看完整的分辨率）中，（缩放的）矢量K（1,1：2）（MATLAB语法从3x3矩阵中提取子矩阵）应该给出一个思路： EKF状态的分量将随着当前创新向量，K（2,1：2）的变化而变化，依此类推。在此示例中，创新向量具有相对较大的x分量，并且与向量K（2,1：2）-状态的第二个分量（y坐标）变化最大。

该图中的一个问题是，它没有感觉到创新矢量的第三分量（角度）如何影响状态。状态的第一部分增加了一点，这与K（1：1：2）所表明的相反-创新的第三部分导致了这种情况，但是目前我无法形象地看到。

第一个改进是可视化创新的第三部分如何影响国家。然后，最好添加协方差数据以了解如何创建K矩阵。

更新＃2现在，该图在状态空间中具有矢量，这些矢量显示了测量的每个分量如何改变位置。从该图可以看出，测量的第三部分会最大程度地改变状态。

可视化测量效果（对我而言）的一种非常有用的方法是绘制每次测量之前和之后的机器人状态（均值，带有协方差椭圆）。然后，获取测量的各个组成部分（轴承，AR标记的范围），并分别应用它们以得到相应的感觉。

去做这个：

$a,b$$\theta$$[0, 2\pi]$

跟踪先前假设，测量状态和后验假设的协方差通常足以确定EKF方程是否被正确应用。

祝您好运，不要经常更新您的问题。相反，请提出新的问题。

通常要做的是绘制状态变量随时间及其3σ间隔的情况。此间隔缩小的点是更新，您可以在其中注释涉及的测量源。

除了应检查的实现中的错误（不仅是错误的方程式，而且还有数值不稳定的方程式）之外，更新的效果仅直接受到“预期”和“测量”之间的差异以及它们各自的不确定性的影响。因此，您可能想找出一种方法来根据第一个绘图中的时间进度可视化这种平衡。