# 2-D Distance for Kalman measurements

I have designed a simple 3-state $[x\ \dot{x}\ \ddot{x}]$ Kalman filter which is updated by measurements for $x$ and $\dot{x}$. The filter is tracking a peak on a surface in the $x/\dot{x}$ plane. At each iteration, I detect a number of peaks on the surface, each with an $x/\dot{x}$ measurement pair.

I need to select the peak which is closest to the current state as input for the next filter iteration. At the moment I am simply finding the difference in the $x$ and $\dot{x}$ dimensions, scaling them so they are similar in scale, and then computing the 2-D distance. This has been working fine for me, but I know it is not the "right" way to calculate the distance.

Is there a way to use the Kalman variables to determine which measurement is closest to the current estimate?

Not sure where this belongs, so I have this posted in stack, dsp, math, and stats exchanges.

• The question is certainly welcome here, but it's generally Bad Form$^{\rm TM}$ to cross post. Let the mods (and users!) in the posted-to forum decide the right forum for it. – Peter K. Nov 5 '13 at 20:14
• Would it make any sense to use ALL of the $x/\dot{x}$ measurements, instead of just the closest to the present state? You could then just use the mean of all $x$ or $\dot{x}$ values rather than trying to find the closest at each time instant. – Peter K. Nov 5 '13 at 20:18
• @PeterK., I started in stack, and then two people commented and suggested math, stats, and dsp – David K Nov 5 '13 at 20:58

I thought about it a bit more (power of posting), and decided the best way to scale $x$ and $\dot{x}$ is by the standard deviation, which is being tracked within the covariance matrix.
errX = (x_n - x_n-1)/sqrt(var(x));