Tracking velocity of object that inconsistently reports its position

Question

My code tracks an object that exists in virtual 3D space.

I have no direct access to the object's position, but it reports its coordinates, every so often. The exact frequency with which the updates occur is not exactly known or reliable, but I know it usually happens every X milliseconds.

Additionally, I cannot know when updates occur. Instead, the code is limited to a single function that runs many times a second (think ~60 times a second, but it should work reasonably well with more or less executions, too, and the rate can change during runtime). Every time the function runs, it has access to the object's last reported coordinates, as well as the exact current time (but not the time when the coordinates were reported).

The function must return its best estimate of what the object's current velocity and acceleration are.

The estimation is not trivial. For example, reading the exact same position twice in a row may mean the object stopped moving, or it may simply mean the moving object hasn't updated its position yet.

Here's a python/pseudocode example of a possible "solution," but not a very good one:

USUAL_UPDATE_RATE = X
STOPPING_THRESHOLD = USUAL_UPDATE_RATE * 1.2

lastTimePosition = None
lastVelocity = None
lastAcceleration = None

def update(time, position):
    # First time running, can't estimate yet
    if lastTimePosition is None:
        lastTimePosition = (time, position)
        return (0, 0)

    lastTime, lastPosition = lastTimePosition
    deltaTime = time - lastTime

    # Ignore duplicate positions if not enough time has passed
    if lastPosition == position and deltaTime < STOPPING_THRESHOLD:
        return (lastVelocity, lastAcceleration)

    # Calculate velocity
    velocity = distance(position, lastPosition) / deltaTime

    # If there's a previous velocity known, calculate acceleration
    if lastVelocity is not None:
        acceleration = (velocity - lastVelocity) / deltaTime
    else:
        acceleration = 0

    # Save new values
    lastTimePosition = (time, position)
    lastVelocity = velocity
    lastAcceleration = acceleration

    return (velocity, acceleration)

The result is not good enough becuase the calculated velocity and acceleration values will vary wildly from call to call, even when the real velocity or acceleration haven't changed. I also tried averaging multiple velocities and accelerations together, but even then there is too much variation, and when averaging too many values, the results take too long to reflect the true values.

Given the limiting circumstances, what algorithm would give me the most accurate estimations, as close to real-time as possible?

Thanks in advance.

Answer 1

I'm one of the developers of a general trajectory analysis python library and we had a similar issue recently when estimating the velocity of a trajectory by several methods.

Algorithm

There is luckly an algorithm that can differentiate any function in a specific point at any degree using some given neighbours points and they don't even have to be equally spaced. The best of all is that you can even define the accuracy of the approximation (this of course is by giving more points).

This is the article where the algorithm is presented (here the link to the pdf directly). It has some dense math and some tricky notations, but it does an excelent job.

Limitations

You said the reported time might not be the extact time for the reported coordinates. Given that the time spasing is so low I think this is not a big problem (it depends on the accuracy you might want). Also, if you use this algorithm, to reduce noise I think it would be a good idea to subsample the trajectories. Instead of taking the points p, p-1, p-2, ... (where p is the current point) you could take p, p-k, p-2k, ... as inputs to the algorithm.

About the library

We don´t have covered life updating trajectories yet in the library, but you can create a new trajectory each time (maybe only using the last few points) but this might be not optimal. Althow this limitation (that surelly will be tackle in the future), I encourage you to use our library which is intended to be use with all kind of trajectories despite their nature in an easy way. There you can specify the velocity estimation method in a few lines. This is a project in work right now and we will be adding new features in these days.

We also have an expample repository which contains a notebook dedicated only to analyse the diferent velocity estimation methods and how they work. You can use this along with the offitial documentation.

IMPORTANT NOTE: this project is in an alpha stage still, and causally the next release will have somename changes specifically in this velocity method selection (we are making it general so it can be used too for aceleration estimation). Stay tuned if you use it.