EKF: Enhancements and Unit Tests

We have a very nice EKF that was written by Dale Schinstock from Kansas University that is used for navigation in our code. Generally it works quite well, although it is fairly sensitive to tuning parameters. I've discussed this somewhat in this post about the magnetometer and this about vertical control.

One thing that I haven't liked (and has slown me down for rolling out updates to the EKF) is that we don't really have any test coverage of it - especially for various initial conditions and systematic biases that might exist.

Unit tests

Recently I wrote a python wrapper that calls into the EKF. I've had something similar for Matlab in the past, but of course the problem with that is it requires a matlab license and cannot be easily integrated into a test environment. With this wrapper in hand, I wrote a series of uni tests (using the Python unittest platform). With this I can systematically test things like how rapidly it converges from various initial conditions given a set of inputs and measurements (with the option to visualize the results, but of course having this disabled when systematically testing things).

For example, I can simulate bias gyro inputs and verify that it tracks that bias correctly

test_gyro_bias

Or initialized at the totally wrong attitude, the EKF will converge to the correct one. Here, it takes longer than I would like and optimizing convergence times (through the EKF tuning) is on my list of things to do and then add worst case values to the test assertions.

test_init_bad_q_bias

When given a position different than the initial location, it quickly snaps to that location at the first update (because the initial position variance is quite high)

test_pos_offset

It is also possible to inject noise and verify that the system remains stable in this case

test_stable

Problematic cases

It can also expose certain problems with the current EKF. For example, if the mag isn't perfect (for example I shrink the z-axis measurement) that results in a biased attitude and the subsequently coupling bias into the velocity and position. In this case, the quad is meant to be sitting still at the origin (except for the mag). It also learns a bias (that isn't there) to explain why the attitude isn't making sense.

test_mag_offset

In a related problem, when simulating starting facing north and presenting mags consistent with facing East, it does reasonably but ends up picking up some roll/pitch errors that influence position:

test_face_west

Another issue is that a fairly small (e.g. 0.2 m/s^2) bias in the accelerometers can really bias the vertical velocity and altitude

test_accel_bias

Fourteen state EKF with mag-attitude decoupling

To address these shortcomings, I made two major changes to the EKF. The first, is to track the bias of accelerometer in the z-axis direction. The complementary filter used for altitude hold mode has such a feature and it proves quite useful for getting robust performance. We previously had a 16-state variant with biases on all three axes, but in my hands that is overkill and actually can be overparameterized - so there can be incorrect solutions to a given set of measurements that are not a good state.

The second was a bit trickier - to make the magnetometer values only influence the heading (in earth frame) without influencing roll and pitch. The solution to this is to pre-transform the measurement measurements by backing out the current estimate of roll and pitch. Then the predicted measurement is based only on the heading term. It ends up being quite a bit of math so I used matlab to rework all the equations and come up with an efficient covariance prediction matrix.

This combination ended up fixing those issues. For example, having a badly scaled (but facing the correct way) magnetometer reading ends up producing positively boring results, with no bias of the attitude:

test_mag_offset

Starting facing the wrong way just nicely rotates around to the correct heading without any errors in the attitude (and thus no position errors)

test_face_west

And having a biased accelerometer is nicely tracked and corrected for

test_accel_bias

At the same time, it still correctly identifies when there is a bias in the gyros and learns this without having a problem identifying zero accel bias. Note that the position and velocity drift while it learns the gyro bias and gets the attitude correct

[Note to self. 49549c3b9c682641d2461a90e176a2f3db7dc1b8 passes all these tests]

Tuning and optimizing convergence

Armed with a filter that can at least deal with some of the systematic errors we anticipate seeing on a multirotor (e.g. distorted mag fields, imperfect accel bias), the next step is to tune it to behave well in real life. There are a few sets of major parameters that define the EKF performance:

• gyro noise - Q[0..2], this is the amount of error expected between the gyro inputs and the change in state. increasing this variance will make the system trust the gyros less (and thus make things like the mags relatively more influential)
• accel noise - Q[3..5], this is the amount of error expected between the accel inputs and the change in velocity. Increasing this variance will make the velocity estimation trust the GPS and baro relatively more. It will also alter how the roll and pitch are estimated - basically tuning between trusting the gyros versus accels)
• bias walk - Q[6..9], how much the bias is expected to change during flight. Making this value smaller means the system will take longer to convergence when estimating the bias terms, but will be less likely to get random bias values when there is noise
• gps position noise - R[0..2], how much noise is expected from the GPS position measurements. setting this lower will trust the GPS more and setting it higher will trust the integrated velocity estimate more
• gps velocity noise - R[3..5], how much noise is expected from the GPS velocity measurements, setting this lower will trust the GPS more and setting it higher will trust the accel (and position) more
• mag noise - R[6..8], how much noise is expected from the mag. trusting this more will get a more accurate heading but be more susceptible to anything that distorts the mags

There is also a component of redundancy amongst these term, in that the covariances can all be scaled by a constant without changing the behavior of the state estimate. However, it is convenient to try and preserve the real units (position in m, velocity in m/s, etc) so the variance has meaningful units (true m^2 variance).

Bias convergence rates

One important parameter to converge quickly enough are the gyro bias terms. We try and initialize these at startup but still tracking changes in error is important and a critical function of the EKF. To test this I see how quickly the EKF learns the gyro bias (within 10%) after initialization. This takes multiple minutes to converge which is slower than we want in practice. Using this code we can increase the bias walk parameters for both the XY gyro, the Z gyro (which is affected differently since it gets corrections from mags instead of accels) and the Z accelerometer. The goal I wanted was for the gyros to converge within 10% of the correct value (when initialized at 10 deg/s off) within 30 seconds. For the accel bias I initialized with 1 m/s^2. After tweaking the variances this was achievable (without making it too fast -- thus tracking noise -- or failing any of the previous tests).

test_gyro_bias

Checking that it did fail any of the previous tests (including those with simulated noise) is critical because if the bias drifts around it can create very bad estimates.

Changing biases

An initial mismatch in the biases is also different than a change during flight. The initial covariance parameters also play into the former. The later is important to be fairly stable although should still track slower changes. This can be simulated by letting the filter run for 30 seconds and then introduce the bias.

changing test_gyro_bias

You can see changing the gyro bias causes the accel bias to briefly change. This is because the biased attitude makes the velocity and position have error, but it self corrects. These influences are unavoidable in a coupled filter like the EKF. Similarly, we can change the accelerometer bias which takes a bit longer to correct.

changing test_accel_bias

Replaying simulated flights

I also wrote a very simple simulator which mimics taking off and flying in circles (with a net drift) and passes that data plus noise to the real implementation of the EKF to make sure that it behaves correctly. Again, this can also be done with an initial bias added or incorrect state to verify that the system ends up at the correct state.

First for a simple flight without anything mismatched (except for sensor noise) the state estimates all do the correct things.

test_circle

Then we can test that it converges when initialized with the wrong attitude (the dotted line shows the real data):

test_bad_init_q

Or with either biased gyros or accels and check that it converges to the correct flight plan and bias values:

test_gyro_bias_circle

test_accel_bias_circle

This test is a really good one. Simulate the quad rocking pitch up and down while yawing. This is a good robust test on the mag attitude compensation. In fact I noticed at one point while doing this that I had an issue and the accel bias could wander a bit much. This led to me finding a bug in how the compensation was applied and fixing it. You can see below that the biases stay stable.

rock_and_turn

Replaying real flights

I've written about using python to analyze logs we collect here and here, With this code it is also possible to replay log data  a real flight. This is important because there are numerous places where the real world violations of the model might cause issues.

Here is a replay of a position hold flight comparing to the real data (or online estimates). You can see the heading well tracked the previous estimate. The position tracked the real data. And finally, the biases were fairly stable through the flight, although perhaps with a bit more oscillation in the gyro bias than I would like.

Flight tests

Of course all this simulation is all well and good, but what really matters is how well it performs. In practice the heading was nice and locked in and the annoying twitch in altitude that I had observed in the past when engaging position hold was gone.

Here is another flight. This was flying Seeing Spark with Sparky2 running this new INS and logging to the android app via a TauLink. I used the log files to generate a video overlay to better see the performance in time with the video. I was mostly flying in position hold mode and using the loiter feature.

The estimates for position look really good and track well with what the video shows the quad doing. Some of the time it drifted laterally but it pretty much always indicated that on the map, so this means it is most likely an issue of tuning rather than a problem with the filter. For vertical velocity it seemed to be generally in agreement with what the video showed. In forward flight, though, the altitude estimate did not seem to track as well - often indicating it was climbing while it was dropping. At the end of the maneuver the altitude estimate would drop rapidly to catch up. Interestingly, in a number of these occurrences it was my impression the climbrate plot was doing the correct thing (and contradicting the altitude estimate).

This disagreement where the velocity looks correct and the altitude goes the opposite way suggests that the accels are producing fairly good data but the baro is not. It's possible the airframe shape of seeing spark with the board on the inside of a cavity creates negative pressure when flying forward. This could cause it to think it is climbing while it is not. I need to try repeating this with another open frame.

Baro glitch

Here was another interesting moment. I was testing with Freedom on a QAV500 frame. I noticed in the logs some times when the baro put out REALLY weird values. Like shooting up 50m in a very short time. When analyzing the logs with video, something became really clear:

So the bottom right has this silly spiky pattern on altitude that is not real. Interestingly, they happen once every cycle while I'm spinning level. I realized, this is when the sun is hitting the baro. Definitely time to cover it.

Sparky 2.0 and TauLink

Update2: Sparky2 is available here http://www.hobbiesfly.com/taulabs-sparky2-0.html

I love Sparky. It's not too surprising, since I designed it to be everything I want in a flight controller. Simple, easy to solder, small, extremely capable, reasonably expandable.

I also really like Freedom, the more powerful cousin. The overo allows logging all the data, which has been hugely powerful. It has more connectivity which is great for having an external mag, spektrum, gps and OSD. The integrated radio is very convenient for connecting to Android. The obvious thing to do was try and fuse them together and get the best of both worlds.

My main goal with this project is something that is really easy and effective for doing POI tracking, follow me, as well as still just flying really well and being great for navigation.

I present:

Sparky 2.0 

Features

• Runs Tau Labs (of course ;-). Same great flight performance you've come to expect and standard features.
• Open source hardware! (also of course) 
• More powerful processor. Uses an STM32F4 running at 168 Mhz with increased memory and flash. This is useful for storing and flying longer waypoint sequences and running more complicated code like picoC scripting.
• More connectivity. (RCVR, Flexi, Main, I2C Aux). This is really useful if you want to have an external mag, gps, and spektrum receiver or other things like OSD.
• Flash chip which can be used to store flight plans, picoC code, or logging
• RFM22b radio can be used for telemetry or with openlrng (not implemented yet). The receiver antenna uses the same U.Fl. connector that Freedom has. I really like this because it is very small and you can easily an SMA break out cable or a whip antenna. It also has just the right amount of tension - strong enough to hold together in flight but in a crash it will disconnect and not damage the board. There is a small hole at the front to provide strain release for a whip antenna that you can just pass through and solder straight to the RFM22b for minimal configurations.
• OpenLRS support directly control from your OpenLRSng radio if you don't want a TauLink 
• Four buffered outputs to drive either LEDs (indicate flight status) or brushed motors. I'm really excited to have different colored LED strips turn on to indicate the direction it is facing relative to home.
• DAC output for VTX audio telemetry. I'd like to be able to some prerecorded audio sounds so the headphones can provide useful information for FPV.
• Dual analog input for voltage and current monitoring. The DAC output can also be used as an analog input.
• CAN Bus which is very useful for talking to external devices like the brushless gimbal to coordinate POI tracking or control the gimbal pitch from transmitter. Also CAN enabled ESCs are coming out which will allow serious improvements in flight quality.
• Full sensor suite of course. MS5611 baro and MPU-9250 combined gyro/accel/mag. I was really conflicted on switching from the MPU-9150 to the MPU-9250. However, being able to utilize an SPI bus which is much better handled on the processor side made it worthwhile.
• Optional single sided assembly - the necessary things to fly are all on the top of the board. The bottom has the CAN bus connector, RFM22b, antenna, external LED drivers. So if you just want a more powerful flight controller with more memory and CPU over Sparky it's still single sided.

Ground modem - TauLink

I also needed a ground side modem. This uses the same RFM22b module used on openlsrg as well as on the flight controller and uses an STM32F1. It was inspired by the PipXtreme (which also inspired the OPLink) done by Pip when we were with OP a few years ago, but is a bit smaller and easier to assemble. It's running code written by Pip, Brian Webb and myself. 

The modem has a standard SMA connector so can accept a wide selection of 433 Mhz antennas.  It uses micro USB and can be powered by a phone or tablet and works with the Tau Labs Android GCS. There is also a port that can serve as a receiver port for relaying signals from a transmitter or taking in PPM for a transmitter module. Finally, there is a serial port for connecting to a GPS. This is important for any tracking applications like POI mode since the accuracy of the phone GPS is not sufficient in my experience.

I also want to break out a few pins to a housing with buttons so the modem can be used to relay a few simple commands like "land" or "follow".

And a quick little case to make it easier to stick to things.

GPS

I picked up this super cheap Ublox 8 GPS that ReadError recommended on IRC. Conveniently, Sparkfun have also started selling the JST-SH cables needed to connect it. So far the performance looks really quite good, although I haven't flight tested it yet. Our software picked it up right away and autoconfigured it, which was nice.

Unfortunately it didn't have any mounting holes, so I quickly designed a case in OpenSCAD that uses the standard 30.5 spaced holes:

Flight Testing

First test was to put all these pieces together. I'm using a modified UAP-1 frame for this. No particular care taken for wiring or spacing between cables and FC. Let's just see how it does.

I ran through the standard configuration aspects to make sure position hold would work: performed the six point calibration and leveling, got a rough PID tuning dialed in (haven't merged autotune in this branch yet), enabled the appropriate modules, checked the EKF variances, used complementary/INS mode, set one flight mode position switch to leveling and the other to position hold. Took it outside with my phone connected via the modem. 

This is super convenient and practically necessary for any navigation related things - navigation is too complicated and error prone to risk blind. With this I could verify the GPS lock quality, check on the map that things things look sensible, and get audio alerts of any error conditions (e.g. when the GPS loses a lock, which didn't happen today).

Position hold mode performed really well for a first try (and pretty much first flight of this board outside). Loiter also worked well, although sometimes it seems like it moves 10-15 degrees different than what I expect. I need to check into this. Still it was comfortably controllable, and in fact at one point someone was walking near me and I shifted it without thinking.

The graphs showing where the quad is were generated from the android logs and the python parsing code.

Radio Control via RFM22B (TauLink)

I got out my old transmitter designed to hold a tablet that was made by Kendall and JoeCNC to play with transmitting the PPM stream via the RFM22B module (directly from TauLink to Sparky2 without another transmitter). It worked pretty well and android was connected and giving me updates.

I also tested altitude hold for Sparky2. Worked well first try.

Range testing still to do, although of course the first thing I did was verify that turning the transmitter off put the flight controller into failsafe mode. This actually replicates something I did over a year ago with RevoMini and PipXtreme: vimeo.com/51659303 . Of course, it is nice to do it with Open Hardware.

Thanks to Brian Webb specifically for writing the PPM transmission code with OpenPilot.

picoC scripting

A really awesome feature that was implemented for erniefti is the ability to load custom scripts into the flight controller. This makes it possible for people to pass around small pieces of functionality and easily run it without a custom firmware. He also added an awesome UI for loading this code and testing it recently. 

This seemed like a good thing to learn for driving the external LEDs that Sparky2 can run. I decided to write a little code that takes the heading and turns on the LEDs facing northing, giving me an easy external indication of the compass. Here is a video of how easy it is to upload this code and make it run when you start up the flight controller.

Future directions

Now the hardware is working, there are a few things to implement:

1. LED outputs based on flight. This will probably use picoC to make it easy to change on the fly. I'm envisioning something where the color indicates the heading relative to home and brightness the distance.
2. Audio alerts. Again, I envision having some prerecorded sounds stored in flash that it transmits over the VTX audio channel and you can hear on the headphones. Low RSSI, low battery, GPS satellites, distance from home every X m, etc. Also possibly just simple beeps encoding health.

More important is the cool things this will enable. Relaying the accurate GPS position of the pilot via TauLink and then having a chase mode. We already have follow me for the tablet (have for a year or two now), but for higher speed stuff a better control scheme that accounts for the target velocity should help.

If you are interested in testing this board and previously got a Sparky v1 from me, then send me an email.

Vertical control

Vertical control is still not at a state that I'm happy with. We recently took one very important step in the right direction by making the same code and settings be used for standalone altitude control and navigation altitude control. At least with this, there is only one thing that requires tuning. After a fair bit of work, I'm pretty happy with the result. If you just want to see the video here it is

f

Sixteen state INS

One issue I see is that the accelerometer bias is not reproducible enough flight to flight, and because this is integrated into velocity that creates a non-trivial amount of noise. In the review I linked above, the complementary filter that is used for the vertical axis (based on ardupilot, to give credit where it is due) estimates the accelerometer bias. However, when running navigation (and thus the INS algorithm) the bias is not tracked for the accelerometers. The most obvious consequence of this is when you enable position hold mode or something else you initially get a throttle surge or dip. After it drops enough, the outer position control kicks in and the you get about a reasonable behavior.

One solution to this, which has already been merged, is the zero accel button. This will let you correct this error, but because the bias changes flight to flight will only be a short term fix.

This is more of a problem, though, for landing when only the velocity is controlled. If the decent rate is 1 m/s and the velocity axis has a 0.5 m/s error you get quite a large error in landing rate and either a long wait or quite a bump.

We had previously played around with a 16 state INS that added the accelerometer bias but it did not work terribly well. One issue for this was that when the board is sitting still, there is essentially a manifold of valid states - the board could be upside down if the accel bias were set to 16 m/s^2, for example. Unobservable systems are very bad for control. There was a fairly simple way of fixing this: I made the state equation for X and Y accel biases have a drift towards zero. This allows it to get to non-zero values (when flying around and really accelerating there might be good evidence for it) but keeps those values fairly well grounded near zero. This is appropriate, too, because the accel bias does not get that far from zero with a reasonably calibrated system.

The second problem turned out to be some bugs in the automatically generated efficient version of the covariance prediction. Manually finding the five typos in this wall of code (and this is only about a third of it)

was not the most fun I've had in the last few weeks. However, after fixing that and a few other tweaks it seems to be working quite well. Manually miscalibrating the system with an accelerometer bias of even 1 m/s^2 is accounted for perfectly and after thirty seconds the error in the vertical velocity falls to less than 0.1 m/s.

Of course, going to a 16 state INS introduces a higher CPU and memory load. However, on Sparky the INS is still only less than 1/4 of the CPU utilization which seems reasonable if it results in a good state.

INS Testing and tuning

To test this version of the INS, I got some logs with the Overo on Freedom and visualized the output of the INS and the quad movement. It seems to track quite well:

Now one issue I'm running into is that when I tune the baro variance to make sure it does track the altitude well, that couples the noise from the baro into the velocity estimate. This seems to be limiting the tuning settings I can get away with.

Vibrations and Fourteen state INS

Since the main issue bias we have issue with is the vertical, at least for now, I wanted to try and not burn cycles on the horizontal biases (although it is quite possible we might go back on this eventually). With some help from Dale (the original INS author) I jumped in and modified it to a fourteen state estimator, with only the vertical bias term being tracked. This wasn't too hard in the end, and also does a great job of estimating the vertical velocity.

However, no matter what settings I used, the vertical hold was not as good as I wanted. Interestingly, it was much better on my micro-quad. I finally broke down and moved Freedom from my POS HT-FPV frame that shakes like crazy to my QAV500. I hadn't flown this much because the pancake motors on it were not playing nicely with my ESCs and I hadn't had the time to tune them up. However, I switched to some SimonK ESCs:

And now no motor failures in flight. With this frame, suddenly the altitude control was much better. This was true using both the complementary filter and the new 14 state INS.

Tuning

You can do all this tuning in the default StateEstimation (complementary, raw) which runs the simple inertial filter for vertical. Once the updated INS is merged you can also tune in INSIndoor mode, but for now I would avoid that with the 13 state since the velocity bias makes the next step a bad idea.

Next, the altitude control loop should be tuned. Ultimately I found the default values were pretty close to what I wanted but the exercise was interesting. Lux (in IRC) recommended a method from baseflight which is very analogous to how I recommended tuning the main control loop (see this video for a tutorial I did years ago).

Basically first you set the AltiudeHoldSettings.AltitudeKp to zero and fly altitude hold. In this condition it will drift up and down a bit randomly but should behave essentially well. Then you gradually increase the AltiudeHoldSettings.VelocityKp until the motors begin to hunt and sound really twitchy. Once you hit that point bring it back down a bit. I didn't feel the need to tune the Ki component since it seemed to be working fairly well and not overshooting.

Once that is done, then you can dial in the AltitudeHoldSettings.AltitudeKp again. If the altitude is wandering too much then increase that term. If the whole system is jumping up and down and hunting again, then reduce it. I found the default value of 1 or down to 0.5 seems to be a pretty good range.

TL;DR: set Velocity Ki to about 1/5 of Velocity Kp and increase that until you see fast twitches, then back off. Then increase the outer gain until you start seeing oscillations and back off.

Vertical Control

KipK in IRC also asked for more aggressive control of altitude when flying altitude hold mode. This has been on my TODO list for a while, and I kept putting it off.

The current scheme waits for the stick to move out and then into the middle range to enable vario mode, but most people have found this unintuitive. Instead I went ahead and added an exponential to the range outside the dead band. This allows more sensitive control in the middle and aggressive control for further stick movements.

Previously, moving the sticks only shifted the set point. Instead I wanted something more like AxisLock where in the middle range the setpoint stays the same and in the outer range you control rate instead of moving the setpoint. In control parlance this is called full state control where we pass the desired position and rate.

I thought I was pretty clever coming up with this scheme, but looking around it seems like everyone has converged on it for vario mode - which probably means it is a reasonable idea.

Here is a video of OsoGrande testing it out and then myself. I'm pretty happy with the response and he said it felt fairly natural to fly.

There was also a glitch with vertical control when doing position hold and RTH that caused it to have a brief dip when engaged. The integral wasn't being properly preloaded with the hover throttle, but this should be fixed now.

Conclusion

So with all these changes, I think we are set to have some really clean navigation. Some care needs to be taken of vibration if you want to get a really clean hover. This is much easier on smaller quads in my experience.

Magnetometer handling in INS

The Tau Labs INS (written by Dale Schinstock) works really well and has been used for various navigation tasks for the last few years. However, one aspect that I have found problematic is the magnetometer handling. The way it works currently is that the Earth's 3D magnetic vector is rotated by the attitude to predict the 3D measurement we should get.

In principle that is a great idea, and on planes probably fine. However, on multirotors that generate a lot of local field distortions from the motors, it is rather problematic. The reason is that as the Z (down) component is altered, it creates a bias in the roll and pitch components of attitude to deal with this. We already know (from years of experience flying the complementary filter) that the gyros and accels are sufficient to stabilize those components. As a work around we generally have just used fairly high variances for the magnetometer. This isn't optimal though, as I've seen the INS bias estimate for the Z axis sometimes destabilize since it isn't sufficiently constrained by the mag when initially converging if things are fairly miscalibrated.

I have been wanting to alter the EKF for some time to only allow the magnetometer to influence the heading estimate, but the path wasn't obvious. Today I decided to sit down and implement this, and I'm fairly happy with the result.

Derivation

We want to measure just the heading aspect of the mag, in order to get rid of the mag influencing attitude. Currently the predicted measurement of the magnetometer is 

$\bar B_b = R_{be}(q) Be$

So we want a version that only depends on the heading component of $q$. That turns out to be less trivial that it sounds. To achieve this, we essentially want to only work with magnetic information projected into a plane parallel to the earth (called the $h$ plane). The rotation $R_{be}$ can be decomposed into two matricies, $R_{be}=R_{bh} R_{he}$, where $R_{bh}$ applies the pitch and roll transformation and $R_{he}$ applies the yaw transformation. We can rotate the raw mag measurements in order to undo the influence of just the roll and pitch by using the inverse (in this case transpose) of $R_{bh}$, $B_h = R_{bh}^T B$, and then only keep the components in x and y.

To calculate both $R_{bh}$ and $R_{he}$, we can get the equations from the code. I then used this code in matlab and the symbolic toolbox to get the matrix. It took a little bit of massaging afterwards to get an efficient implementation:

syms q1 q2 q3 q4 real
q = [q1 q2 q3 q4];

q0s = q(1) ^ 2;
q1s = q(2) ^ 2;
q2s = q(3) ^ 2;
q3s = q(4) ^ 2;

R13 = 2.0 * (q(:,2) .* q(:,4) - q(:,1) .* q(:,3));
R11 = q0s + q1s - q2s - q3s;
R12 = 2.0 * (q(:,2) .* q(:,3) + q(:,1) .* q(:,4));
R23 = 2.0 * (q(:,3) .* q(:,4) + q(:,1) .* q(:,2));
R33 = q0s - q1s - q2s + q3s;

roll = atan2(R23, R33)
pitch = asin(-R13)
yaw = atan2(R12, R11)

% compute the rotation matrix that is for the attitude component
% this is the matrix that will rotate from the earth frame to the
% current roll and pitch. The inverse will take the magnetometer
% reading back into a horizontal place
sF = sin(roll); cF = cos(roll);
sT = sin(pitch); cT = cos(pitch);

r = sqrt(R23^2 + R33^2)
sF = R23 / r;
cF = R33 / r;
sT = -R13;
cT = sqrt(1 - R13^2);
Rbe_bh = [cT, 0, -sT;
          sF*sT, cF, cT*sF;
   cF*sT, -sF, cT*cF]

Then we need to modify the EKF to predict a measurement in the horizontal plane. That aspect is pretty straighforward, it is the same equation but keeping the yaw component instead of pitch and roll

% direct equation
sP = sin(yaw); cP = cos(yaw);
Rbe_h = [cP, sP;
       -sP, cP];

% another expression that results in code that doesn't use imaginary
% numbers
x = R12;
y = R11;
r = sqrt(R12^2 + R11^2)
Rbe_h = [y/r x/r; -x/r y/r];

% calculate predicted B_h in the horizontal plane
B_h = Rbe_h * Be

% generate c code to compute this
ccode(B_h)

I ended up having to manually expand $cos(atan2(x,y))=\tfrac{y}{\sqrt{x^2+y^2}}$ for matlab because it kept trying to use imaginary numbers to calculate the norm. The last line of this actually generates C code for the two components of $B_h$. It needed a fair bit of massaging after that to factor out common terms and get an efficient implementation.

The other thing needed for the INS is the derivative of the measurements with regard to the state, or in this case $\tfrac{\delta B_h} {\delta \mathbf{q}}$.

% and compute the derivatives wrt to the attitude
ccode([diff(B_h,q0)'; diff(B_h,q1)'; diff(B_h,q2)'; diff(B_h,q3)'])

Again, Matlab symbolic toolbox to the rescue, saving me from a lot of tedious algebra, although again lots of massaging was required.

Testing

So again, Freedom Overo logs saved me tons of time, since I can replay previous flights through the new filter and see how it performs. It performs really nicely. The nice thing about this, is we can tighten up the variances on the mag and make sure the convergence behaves well, and not worry about the magnetic field biasing the attitude as we go.

Here is the raw magnetic heading and the INS output:

, which you can see is nicely tracking together.

I also loaded it up on my Freedom and power cycled it a bunch of times, as well as letting is sit for an hour. Rock solid - none of the oscillations I would sometimes see with the previous code - and now I can keep the gyro bias estimation on reliably. With a good accel calibration and board leveling, you get a perfect PFD:

This is with minimal tuning or tweaking, so that is great. I'll need to play around with a bit in flight but I'm pretty optimistic about this - and hopefully it will reduce the rare times we do see toilet bowling. I also now need to play around again with the mag compensation code, although hopefully it won't be necessary.

It was also a really fun day project. I've played with kalman filters before but never had to go quite as deeply into the extended kalman filter. I also learned some more about playing with quaternions and rotation matricies, so that was productive.