# Smoothing tracks with a Kalman filter

Hi all,
I have videos of dung beetles rolling balls of dung:

The pixel coordinates of the beeltes’ locations (x,y per time) have been extracted from these videos. Using the calibrations of these videos, the pixel coordinates were transformed to real-world coordinates (in cm). The resulting tracks are slightly jittery (mainly due to how these animals have been tracked, but also due to video quality, animal movement, terrain, etc):

I would therefore like to smooth these. We have tried splines, Savitzky-Golay, running window average, fitting polynomials, etc. While all these options have their merits/disadvantages, they do not take into consideration time/speed-limits – fast moving beetles don’t turn as quickly as slow ones, beetles have a (relatively) large turning radii (they can’t turn on a dime, well, maybe they can, but you get the drift), speeds can’t change too abruptly, etc. It’s for these reasons (and more⋆) that we want to try smoothing these tracks with a Kalman filter.

The beetles’ movement is complex: different species move differently, and a beetle that is holding a dung ball moves differently from one that doesn’t. In general terms, we can think of it like a tank, where either the left or right set of leg/s is pushing harder than the other set. What I’m looking for here is:

1. How would you describe/build a Kalman filter that would work that way?
2. Do you have any suggestions on how exactly to implement it that way?

Thanks a dung-load for any input!

Related discussions:

⋆ Ultimately we need to identify a “turning point” – a point where the beetle stops walking in a (more or less) straight path and turns (more or less) 180°. It seems logical that identifying such a point would be intrinsically more robust and rigorous via a Kalman filter.

6 Likes

The smoothing you want should be straight-forward to achieve, but perhaps not with a vanilla Kalman filter due to the nonlinear terms arising when you including the angle, i.e.,
x_{t+1} = x_t + \cos(\theta_t) v_t
y_{t+1} = y_t + \sin(\theta_t) v_t
v_{t+1} = v_t + e_t
\theta_{t+1} = \theta_t + w_t
where
e_t \sim N(0,\sigma_e), \quad w_t \sim N(0,\sigma_w)

The above is a reasonable model for the system, a double integrator, but does not limit the turning rate (can perhaps be included by an integration step on the heading angle). Including the heading angle as a state \theta_t makes the filtering problem nonlinear. It can still be solved fairly easily using a particle smoother. The model can be parameterized with things like “holding payload”, species etc. The parameters could be inferred from data using, e.g., the PMMH method.

3 Likes

Do you mean with your new MonteCarloMeasurements.jl?

No, that package does not solve a filtering problem. Particle filters are similar in some sense, but perform nonlinear filtering similar to Kalman filters. Kalman filters only really work when the dynamics are linear and the driving noise and measurement noise are Gaussian. If any of these condition fail to hold, you need to move to a non-linear filtering method, such as Extended Kalman filter (linearize the dynamics around current state), Unscented Kalman filter (unscented transform to propagate distributions followed by “re-Gaussification”) or the most general, particle filter (represent distributions using particle clouds).

Several julia packages for particle fitlering exists. I developed LowLevelParticleFilters.jl to solve these kind of problems, but it’s not the only package out there. This package also supports parameter estimation by PMMH or ML/MAP.

2 Likes

This looks really promising! I’ll try to implement it (with a toy example at first). Maybe if it works, we can add it as an example to your example_lineargaussian.jl.

Thanks!

That would be awesome! If you run into problems, drop a line here and I’ll have a look

1 Like

When.

1 Like

O, dung beetle! You live a life of such lowly toil yet you hold such a glamorous place in human society. First worshipped for thousands of years in the Lands of Black and Red (btw, does it bother anybody else that the ancient Egyptians thought that a ball of dung was an appropriate analogy for the SUN?), now modeled in Julia with your inscrutable meandering path smoothed by Kalman filters.

(really appreciate the gif btw)

12 Likes

You could also try an extended Kalman filter, https://en.wikipedia.org/wiki/Extended_Kalman_filter?wprov=sfla1, which doesn’t have the optimality guarantees of a normal Kalman filter, but often works quite well in practice for very smooth nonlinear dynamical models like the one @baggepinnen proposed.

2 Likes

Yes, the model I proposed is quite nice and smooth, but if there is some probability that the beedle does a 180, an EKF might struggle

The model of @baggepinnen seems fine. However, you probably will have problems when the velocity is very small (~0). In this case, the definition of angle will be unobservable (unless you can obtain from the movie which direction the beetle is pointing to). In this case, the linearization of EKF might fail. So, if you do not want to go with particle filters, I suggest to use UKF, which is a little more stable in those cases.

Btw, if you want to use a particle filter, look for Rao-Blackwellized Particle Filters, since your problem might be separated into linear and non-linear parts.

1 Like

It is guaranteed that the beetle will do a turn of anything between 90 and 180 degrees every track. The radius of said turn might vary.

This is also probable. I could though try to split the track into “moving” parts…

This is all great, but I’l openly admit that the exact implementation of all of these fine suggestions might be a bit beyond me. I’ll give it a solid try these next days though!!!

Agreed for a basic implementation. But there are designs that work on manifolds as well. You can use an overparameterization of rotation (the sine and cosine, instead of just the angle) as your ‘global’ estimate, and then calculate errors and updates as angle differences (local coordinates) centered around the current estimate. I’ve done something similar for a quaternion-based robot state estimator before, and the ODE integrator shipped with RigidBodyDynamics.jl works on the same principle.

1 Like

Two ideas:

• Make a coordinate transform from velocities in (x,y) to velocity v in current-facing direction and turning rate ω. Doesn’t this make detecting a turning point much simpler? (v ≈ 0 while ω ≠ 0)
• Apply a wavelet transform. I don’t know much about wavelets, but they should be quite good at extracting a pattern such as a standstill turn, also in “scaled” forms such as various turning rates and duration (and thus overall turning angle). If it works, there should be a single peak of the correlation, which would be the turning point.
1 Like

Yea, this is absolutely relevant to detecting the turning point.

Currently I’m working with @baggepinnen to fine tune a dynamics model for the particle filter with the ultimate goal of smoothing the track. Once we manage to get a nice behaving filter for these noisy tracks I can start working on the detection of the turning point. One hope is that the state data of the PF will yield some insight on where the turning point is and help detect it. But yea, once the track is smoothed and clean, many other methods for detecting the turning point should work just as fine.

I guess you could also try applying a wavelet transform to the raw / unfiltered track. After all, it also applies some sort of weighted averaging…

Are there any updates on this? The crawl package in R (https://github.com/NMML/crawl) and its state-switching cousin (https://github.com/TheoMichelot/MScrawl) seem to be what you need, but I’d be interested if there were any Julia solutions out there.

This is probably as far as we got with the particle-filter approach

As far as I can remember, it worked reasonable well, by @yakir12 has more insight into his final solution.

It worked really well. Adjusting it to your exact needs can be a bit involved, but worth it. Sadly, in my specific case, we ended up using a smoothing spline (from https://github.com/kbarbary/Dierckx.jl). But this is a very cool way of doing it.

Can you share a trajectory to play with?