# Smoothing tracks with a Kalman filter

**URL:** <https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209>\
**Category:** General Usage\
**Tags:** question, kalman, smoothing\
**Created:** [May 14, 2019, 2:39pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209 "2019-05-14T14:39:37Z")\
**Posts on this page:** 20\
**Page:** 1

<div class="post-metadata">

**Author:** ![yakir12](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/yakir12/32/297_2.png) [@yakir12](https://discourse.julialang.org/u/yakir12)\
**Post date:** [May 14, 2019, 2:39pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/1 "2019-05-14T14:39:37Z")

</div>

Hi all,  
I have videos of dung beetles rolling balls of dung:  
 ![a](https://global.discourse-cdn.com/julialang/original/3X/7/9/79a9255c4fc79677249fce7321c8c70f3df46431.gif)  
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):

 ![screenshot_2019-05-14_15%3A47%3A41](https://global.discourse-cdn.com/julialang/original/3X/3/a/3a98ec04c77ab22df1c1df0292474a32581d4f13.png)  
**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:

- [How to Kalman filter](https://discourse.julialang.org/t/how-to-kalman-filter/8745)
- [Help implement Kalman filter](https://discourse.julialang.org/t/help-implement-kalman-filter/8718)
- [My Julia workflow](https://discourse.julialang.org/t/my-julia-workflow/23815)

⋆ 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.

---

<div class="post-metadata">

**Author:** ![baggepinnen](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/baggepinnen/32/693_2.png) [@baggepinnen](https://discourse.julialang.org/u/baggepinnen)\
**Post date:** [May 14, 2019, 3:05pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/2 "2019-05-14T15:05:20Z")

</div>

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.

---

<div class="post-metadata">

**Author:** ![yakir12](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/yakir12/32/297_2.png) [@yakir12](https://discourse.julialang.org/u/yakir12)\
**Post date:** [May 14, 2019, 3:10pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/3 "2019-05-14T15:10:16Z")

</div>

> [@baggepinnen](#):
>
> using a particle smoother.

Do you mean with your new MonteCarloMeasurements.jl?

---

<div class="post-metadata">

**Author:** ![baggepinnen](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/baggepinnen/32/693_2.png) [@baggepinnen](https://discourse.julialang.org/u/baggepinnen)\
**Post date:** [May 14, 2019, 3:13pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/4 "2019-05-14T15:13:59Z")

</div>

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](https://github.com/baggepinnen/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.

---

<div class="post-metadata">

**Author:** ![yakir12](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/yakir12/32/297_2.png) [@yakir12](https://discourse.julialang.org/u/yakir12)\
**Post date:** [May 14, 2019, 3:20pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/5 "2019-05-14T15:20:24Z")

</div>

> [@baggepinnen](#):
>
> I developed [LowLevelParticleFilters.jl](https://github.com/baggepinnen/LowLevelParticleFilters.jl) to solve these kind of problems

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!

---

<div class="post-metadata">

**Author:** ![baggepinnen](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/baggepinnen/32/693_2.png) [@baggepinnen](https://discourse.julialang.org/u/baggepinnen)\
**Post date:** [May 14, 2019, 3:21pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/6 "2019-05-14T15:21:21Z")

</div>

> [@yakir12](#):
>
> Maybe if it works, we can add it as an example to your `example_lineargaussian.jl`

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

---

<div class="post-metadata">

**Author:** ![yakir12](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/yakir12/32/297_2.png) [@yakir12](https://discourse.julialang.org/u/yakir12)\
**Post date:** [May 14, 2019, 3:22pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/7 "2019-05-14T15:22:48Z")

</div>

> [@baggepinnen](#):
>
> If you run into problems

When.

---

<div class="post-metadata">

**Author:** ![ExpandingMan](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/expandingman/32/866_2.png) [@ExpandingMan](https://discourse.julialang.org/u/ExpandingMan)\
**Post date:** [May 14, 2019, 3:37pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/8 "2019-05-14T15:37:59Z")

</div>

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.

REVEAL YOUR SECRETS GREAT BEAST!

😆🪲💩

(really appreciate the gif btw)

---

<div class="post-metadata">

**Author:** ![tkoolen](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/tkoolen/32/1603_2.png) [@tkoolen](https://discourse.julialang.org/u/tkoolen)\
**Post date:** [May 14, 2019, 4:15pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/9 "2019-05-14T16:15:50Z")

</div>

You could also try an extended Kalman filter, [Extended Kalman filter - Wikipedia](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.

---

<div class="post-metadata">

**Author:** ![baggepinnen](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/baggepinnen/32/693_2.png) [@baggepinnen](https://discourse.julialang.org/u/baggepinnen)\
**Post date:** [May 14, 2019, 4:30pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/10 "2019-05-14T16:30:08Z")

</div>

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

> [@yakir12](#):
>
> ⋆ 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°.

---

<div class="post-metadata">

**Author:** ![Ronis\_BR](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/ronis_br/32/50999_2.png) [@Ronis\_BR](https://discourse.julialang.org/u/Ronis_BR)\
**Post date:** [May 14, 2019, 4:44pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/11 "2019-05-14T16:44:13Z")

</div>

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.

---

<div class="post-metadata">

**Author:** ![yakir12](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/yakir12/32/297_2.png) [@yakir12](https://discourse.julialang.org/u/yakir12)\
**Post date:** [May 14, 2019, 4:50pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/12 "2019-05-14T16:50:17Z")

</div>

> [@baggepinnen](#):
>
> some probability that the beedle does a 180

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.

> [@Ronis\_BR](#):
>
> when the velocity is very small

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!!!

---

<div class="post-metadata">

**Author:** ![tkoolen](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/tkoolen/32/1603_2.png) [@tkoolen](https://discourse.julialang.org/u/tkoolen)\
**Post date:** [May 14, 2019, 5:11pm UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/13 "2019-05-14T17:11:53Z")

</div>

> [@baggepinnen](#):
>
> if there is some probability that the beedle does a 180, an EKF might struggle

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.

---

<div class="post-metadata">

**Author:** ![asprionj](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/asprionj/32/6856_2.png) [@asprionj](https://discourse.julialang.org/u/asprionj)\
**Post date:** [May 17, 2019, 9:24am UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/14 "2019-05-17T09:24:20Z")

</div>

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.

---

<div class="post-metadata">

**Author:** ![yakir12](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/yakir12/32/297_2.png) [@yakir12](https://discourse.julialang.org/u/yakir12)\
**Post date:** [May 17, 2019, 9:43am UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/15 "2019-05-17T09:43:17Z")

</div>

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.

---

<div class="post-metadata">

**Author:** ![asprionj](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/asprionj/32/6856_2.png) [@asprionj](https://discourse.julialang.org/u/asprionj)\
**Post date:** [May 17, 2019, 11:52am UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/16 "2019-05-17T11:52:41Z")

</div>

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…

---

<div class="post-metadata">

**Author:** ![sdwfrost](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/sdwfrost/32/2831_2.png) [@sdwfrost](https://discourse.julialang.org/u/sdwfrost)\
**Post date:** [May 27, 2020, 4:58am UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/17 "2020-05-27T04:58:16Z")

</div>

Are there any updates on this? The crawl package in R ([GitHub - NMML/crawl: R package for modeling animal movement with continuous-time correlated random walk models](https://github.com/NMML/crawl)) and its state-switching cousin ([GitHub - TheoMichelot/MScrawl: State-switching continuous-time correlated random walks](https://github.com/TheoMichelot/MScrawl)) seem to be what you need, but I’d be interested if there were any Julia solutions out there.

---

<div class="post-metadata">

**Author:** ![baggepinnen](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/baggepinnen/32/693_2.png) [@baggepinnen](https://discourse.julialang.org/u/baggepinnen)\
**Post date:** [May 27, 2020, 6:56am UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/18 "2020-05-27T06:56:53Z")

</div>

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

> <https://github.com/baggepinnen/LowLevelParticleFilters.jl/blob/yakir12-patch-1/src/beetle_example.jl>

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

---

<div class="post-metadata">

**Author:** ![yakir12](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/yakir12/32/297_2.png) [@yakir12](https://discourse.julialang.org/u/yakir12)\
**Post date:** [May 27, 2020, 6:58am UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/19 "2020-05-27T06:58:53Z")

</div>

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](https://github.com/kbarbary/Dierckx.jl)). But this is a very cool way of doing it.

---

<div class="post-metadata">

**Author:** ![mschauer](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/mschauer/32/13946_2.png) [@mschauer](https://discourse.julialang.org/u/mschauer)\
**Post date:** [May 27, 2020, 8:39am UTC](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209/20 "2020-05-27T08:39:39Z")

</div>

Can you share a trajectory to play with?

[Next page](https://discourse.julialang.org/t/smoothing-tracks-with-a-kalman-filter/24209.md?page=2)