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.
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));
errXdot = (xdot_n - xdot_n-1)/sqrt(var(xdot));
err2D = sqrt(errX^2 + errXdot^2);