Iteratively Smoothing Changes in Position

Question

I have an entity which has noise in its movement. The entity is heading straight toward a target entity and I am trying to estimate how long it will take for it to reach its goal.

I am trying to estimate the velocity of the entity by looking at its position history.

I have a History<Tuple<double,Vector2D> which has the last N positions and at what time I got that position. The positions generally come in very consistently at 1 second intervals.

I tried a few home made formulas where xtk[n] is the component (x or y) at [n] seconds ago:

       private double GetFirstOrderVelocity(double xtk, double xtk1, double h)
        {
            return (xtk - xtk1)/h;
        }

        private double GetSecondOrderVelocity(double xtk, double xtk2, double h)
        {
            return (xtk - xtk2) / (h*2);
        }

        private double GetThirdOrderVelocity(double xtk, double xtk1, double xtk2, double xtk3, double h)
        {
            return (xtk + xtk1 - xtk2 - xtk3) / (h * 4);
        }

        private double GetFourthOrderVelocity(double xtk, double xtk1, double xtk3, double xtk4, double h)
        {
            return (xtk + (2 * xtk1) - (2 * xtk3) - xtk4) / (h * 8);
        }

Where his always 1 since they come in at 1 second intervals.

The fourth order helped, but I would like to know if there is a better and more general way to estimate velocity from previous positions? Something that is iterative so that if I need stronger smoothing, I just need to increase a counter, which would probably use more of the history and would trade off responsiveness for smoothness. The problem is right now the time to position is very jumpy and logically if something is heading right for a target, with enough samples we can start to fairly accurately estimate how long until it gets there.

score 1 · Accepted Answer · answered May 06 '16 at 17:12

Iterative. Keep two exponential decaying averages with different decay rates, then project by comparing them.

The idea is this. If 0 < k < 1 then a decaying average can be computed by:

average = (1-k)*prev_average + k*observation

You should do your own numeric experiments to be sure I didn't make a silly mistake. But if your path is linear, this average will converge to something a lot like the average of the last 1/k observations, which represents your best guess as to where it was 1/(2*k*T) seconds ago. So if you have 2 of these then you'll have 2 smoothed out measurements of where it should have been. From those you can project the average velocity, and from that at either position you can estimate the arrival time.0

You will have to play around with it to find two constants that work for your data set.

Iteratively Smoothing Changes in Position

1 Answers1