Multilateration Track smoothing using Kalman Filter?

Question

I have the following Problem regarding aircraft tracking via multilateration: We managed to implemented some algorithms to calculate aircraft Positions based on multilateration using 3 or 4 receivers. The problem is, that the track looks quite "shaky" (Please See the picture in the link, sorry i could not add it here because of reputation.)

GPS and Mlat Track

The green line is the true GPS track of an aircraft on Ground, orange is the computed track by multilateration (Time Difference of Arrival, roughly one to two positions per second). The GPS Track is just for comparison, it might not be available in future.

What would be a good way to smooth the track? I stumbled over Kalman Filters. I am not a mathematician or have any experience in robotics or the like. Math at this level is extremely hard to non-understandable for me (I am glad i managed the multilateration quite ok). The Track is computed by closed-form algorithm. Maybe switching to an iterative algorithm also might help?

So, would it make sense to implement a Kalman Filter on the resulting Multilateration Position itself? Or maybe already on the TDOA values before actually calculating the position? The shaky look results in minor measurement errors at the receivers site, so it might help to smooth/filter the TDOA values itself.

But then still I need a very sophisticated update model, since the aircraft is moving and the TDOA values depend one true aircraft position. I could imagine, the model for smoothing a Track and smoothing the TDOA values itself is quite similar. if not the same.

We do all that in Java, so maybe there is some library available with which we could start without doing all the math again?

Answer 1

This is a classical tracking problem, on which you'll find tons of scientific literature with a lot of different approaches. The bad news is, if you can't find a library which does the job for you, you'll have to look into the math.

Utilizing a Kalman Filter goes into the right direction, since can it estimate a state (position, velocity) from indirect measurement data. Since your multiliterations are a non-linear mapping for the measurement data, you need a non-linear estimator.

My standard recommendation for such problems is the Unscented Kalman Filter because of its (relative) algorithmic simplicity and its high robustness. It can also take care of your multiliteration, because multiple different measurement within one time step are allowed. As for the Kalman Filter you'll also a need a motion model - a simple (linear) one might do the job, since I assume you're tracking normal planes (not highly maneuverable jet fighters). Unfortunately I'm not aware of any appropriate implementation - for instructions how to implement one efficiently, read (the math behind it is not trivial):

Merwe, R. V. D. & Wan, E. A. The square-root unscented Kalman filter for state and parameter-estimation in International Conference on Acoustics, Speech, and Signal Processing, 2001, 3461-3464

For a (low accuracy) quick and dirty solution, implement a FIR Low-path Filter for each dimension. You'll find tools on the web (e.g. here), which can generate code for you.

Answer 2

After having a quick look at the wikipedia entry for the kalman filter, this seems quite promising.

Here is another suggestion you can consider:

Due to previous measurements you can estimate position, speed and direction of the plane. So for any new measurement, you can calculate if the forces required to move the plane to that position are realistic. This way you can sort out measurements with a large error.