卡尔曼测量的2-D距离

Question

我设计了一个简单的3态（x，x点，x点点）卡尔曼滤波器，它通过测量x和x点进行更新。滤波器跟踪x / x点平面上的峰值。在每次迭代中，我在表面上检测到许多峰，每个峰都有一个x / x点测量对。

我需要选择最接近当前状态的峰值作为下一次滤波器迭代的输入。目前我只是找到x和x点尺寸的差异，缩放它们以使它们在尺度上相似，然后计算2-D距离。这对我来说一直很好，但我知道这不是计算距离的“正确”方法。

有没有办法使用卡尔曼变量来确定哪个测量值最接近当前估计值？

不确定这属于哪里，所以我将它发布在堆栈，dsp，数学和统计信息交换中。

Answer 1

你是正确的方式。假设您将当前时间步的状态向量设为mu：

和相应的标准偏差：

和当前测量m：

你可以定义类似“存在概率”的东西（如果你的状态向量是x和y坐标那么就是这样）。那个公式不再那么容易了。

二维高斯分布的“包含所有内容”公式如下： （from Wikipedia） 假设你的变量是独立的（x的高值没有说明x_dot的值），它简化了这个：

这对您来说可能并不简单。但它真的只是键入数字。现在忍受我：这是一个概率密度。当估计接近当前状态时，它是最高的。这不是很方便，因为最低值为零。一个遥远的点得到一个接近零的值，但距离十倍的点也是如此。你就像一个表达这一点的衡量标准。

这就是对数可能性发挥作用的地方：

虽然可能并不明显，但这会将概率W转换为二次成本函数K.距离越大，成本越高。 （指数取消ln）

您现在可以计算每对（m | x）的成本并获得矩阵。然后将此矩阵输入linear assignment problems的求解器，如munkres算法或this one on FX。

输出是一个赋值矩阵，其中包含相应对的矩阵。

请注意：包括计算中的差异可能是有益的，但请确保它们是正确的。如果它们接近零则它们是错误的，如果它们跨越测量的整个范围则它们太大。

Answer 2

我考虑了一下（发布的力量），并且决定了缩放x和x点的最佳方法是通过标准偏差，在协方差矩阵中进行跟踪。

errX = (x_n - x_n-1)/sqrt(var(x));
errXdot = (xdot_n - xdot_n-1)/sqrt(var(xdot));
err2D = sqrt(errX^2 + errXdot^2);