[nj5rq]

The last code block has a typo:

    #define FRAMERATE = 100.0

Should probably be:

    #define FRAMERATE 100.0

[niborgen]

Right, sorry! Corrected now.

[sdefinit]

> Turns out, all the ‘update’ quantities were zero, resulting in the sluggish behavior of the filter.

I think you also meant the predict quantities were zero, not the update!

[niborgen]

The prediction quantities were indeed zero, as they get discretized by multiplying with the sampling time. You're right that some discretized part of the update quantities were also zero, resulting in an identity 'A' matrix. I'll correct it, thanks!