Attitude estimation for pi3 hat

Now that the IMU is functioning, my next step is to use that to produce an attitude estimate.  Here, I dusted off my unscented Kalman filter based estimator from long ago, and adapted it slightly to run on an STM32.  As before, I used a UKF instead of the more traditional EKF not because of its superior filtering performance, but because of the flexibility it allows with the process and measurement functions.  Unlike the EKF, the UKF is purely numerical, so no derivation of Jacobians is necessary.  It turns out that even an STM32 has plenty of processing power to do this for things like a 7 state attitude filter.

One problem I encountered, was by default I have been building everything for the STM32 with the “-Os” optimization level.  Unfortunately, with Eigen linear algebra routines, that is roughly 4x slower than “-O3”.  Doubly unfortunate, just using copts at the rule level or --copts on the command line didn’t work.  bazel doesn’t let you control the order of command line arguments very well, and the -Os always ended up *after* any of the additional arguments I tried to use to override.  To get it to work, I had to navigate some bazel toolchain mysteries in rules_mbed in order to allow build rules to specify if they optionally want the higher optimization instead of optimizing for size.  I’m pretty sure this is not exactly what the with_features mechanism in toolchain’s feature rule is for, but it let me create a feature called speedopt which turns on -O3 and turns off -Os.  The final result is at rules_mbed/530fae6d8

To date, I’ve only done some very zeroth order performance optimization.  I spent 15 minutes parameter tuning, making sure that the covariances updated to approximately the correct levels and  I added a simple filter to reject accelerometer updates during dynamic motion.  I did just enough runtime performance to get an update down to around 300us, which is just fine for a filter intended to run at 1kHz.  More will remain as future work.

Here’s a plot from a quick sanity check, where I manually rolled the device in alternating directions, then pitched it in alternating directions.  (When pitching, it was on a somewhat springy surface, thus the ringing).

20200305-attitude-validation

The pitch and roll are plenty smooth, although they look to perhaps not returning exactly to their original position.  At some point, I will do a more detailed qualification to dial in the performance.

 

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s