我设计了一个简单的三态 (x,x-dot,x-dot-dot) 卡尔曼滤波器，它通过 x 和 x-dot 的测量值进行更新。过滤器正在跟踪 x/x 点平面中的表面上的峰值。在每次迭代中，我检测到表面上的许多峰值，每个峰值都有一个 x/x 点测量对。
我需要选择最接近当前状态的峰值作为下一次滤波器迭代的输入。目前我只是在寻找 x 和 x-dot 尺寸的差异，缩放它们以使它们在比例上相似，然后计算二维距离。这对我来说一直很好，但我知道这不是计算距离的“正确”方法。
有没有办法使用卡尔曼变量来确定最接近当前估计的测量值？
不确定它属于哪里，所以我将它发布在堆栈、dsp、数学和统计交换中。
你走对了。假设您将当前时间步的状态向量设为 mu：
和相应的标准偏差：
和电流测量 m：
您可以定义诸如“存在概率”之类的东西（如果您的状态向量是 x 和 y 坐标，那就是这样）。这个公式不再那么容易了。
二维高斯分布的“所有内容”公式是这样的：（
来自维基百科）假设您的变量是独立的（x 的高值没有说明 x_dot 的值），它简化为：
这对您来说可能看起来并不简单。但这实际上只是输入数字。现在请耐心等待：这是一个概率密度。当估计接近当前状态时它是最高的。这不是很方便，因为最小值为零。一个远点的值接近于零，但十倍远的点也是如此。你就像一个表达这一点的度量。
这就是对数似然发挥作用的地方：
虽然这可能并不明显，但这会将概率 W 转换为二次成本函数 K。距离越大，成本越大。（指数与 ln 相消）
您现在可以计算每对 (m | x) 的成本并获得一个矩阵。然后将该矩阵输入到线性分配问题的求解器中，例如 munkres 算法或FX 上的这个。
输出是一个分配矩阵，其中有对应的对。
请注意：在计算中包含差异可能是有益的，但请确保它们是正确的。如果它们接近于零，它们是错误的，如果它们跨越整个测量域，它们就太大了。
我考虑了更多（张贴的力量），并决定缩放 x 和 x-dot 的最佳方法是通过标准偏差，这是在协方差矩阵中跟踪的。
errX = (x_n - x_n-1)/sqrt(var(x));
errXdot = (xdot_n - xdot_n-1)/sqrt(var(xdot));
err2D = sqrt(errX^2 + errXdot^2);