Smoothing a particle filtered 2D trajectory

Question

I am currently developing a very basic particle filter for a 2D robot localization task.

My process is defined by a really simple velocity / steering angle based motion model. I am re-weighting the particles according to several sensors (including GPS), which I assume are not giving me independent samples and probably inhibit systematic errors. I am using roughly 1000 particles, which I'm resampling from every couple iterations.

After each reweighting step, the current position of the robot is determined using the weighted particle mean. Obviously, due to the reweighting and resampling, this approach yields a highly non-smooth trajectory, that is not suitable for my application.

What would be an appropriate thing to do in such a scenario?
Can I remodel the particle filter in some way?
Should I apply some smoothing algorithm? (eg. particle smoothing?)

Thank you for your suggestions!

Answer 1

First a word about the GPS error, then smoothing:

1) The error in the GPS has fat tails: since you're using particle filtering, you can get non gaussian densities for free, so use it. But more problematically, as you suspect, it is strongly autocorrelated. Worse, the output of your GPS receiver is actually the output of a ... filter working on the raw GPS signal. So the error of the GPS will depend on whether you're moving for instance, or how much you've moved in the past. It's messy. Ideally you'd want to filter directly the raw GPS signal, but you would be foregoing a nice, hardware implemented filter and you may not afford the extra computational burden.

2) Here are three ideas

• Increase the number of particles. If the estimate of the mean position of your robot is visibly noisy, with a mean reverting noise, it means the filter is forgeting something.

• Instead of treating particles as Diracs, treat them as multivariate normals and approximate the covariance matrix during the prediction and filtering steps (each particle acts like a local Unscented Kalman Filter)

• Do Kalman filtering on the output of your model to smooth out the noise, learn the parameters of the filter from historical data using the EM algorithm.