Saturday, January 24, 2015

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


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.


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)

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


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.


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:

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


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:


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


And having a biased accelerometer is nicely tracked and corrected for


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


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.


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


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


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.


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.

Wednesday, December 31, 2014

More log analysis

I've written previously about using python and log parsing, which this writeup uses heavily.

I was testing position hold with the new 14 state INS the other day with Sparky2 on Seeing Spark. The new filter is working great. I ran autotune and 6 point calibration and engaged position hold and it held beautifully still. The new estimation of the z-axis accel bias worked as it should so there was no glitch in the altitude. The new magnetometer handling meant that the attitude was wonderfully locked in and not biased while tracking the heading really well. Spinning around while holding and it didn't budge a bit.

I wrote some code to calibrate the magnetometer while spinning which I think will be quite useful. It fits the data to a sphere as well as making sure the horizontal component has the appropriate magnitude (this prevents fitting to an edge condition).

You can see that the blue data (x versus y axis) quite nicely fits a circle. This is both an easier calibration procedure than 6 point and more useful since you can see the deviation from correct (the plot on the right shows the magnitude of the mag data) and the is performed with the motors running at hover. This will also work with the built in logging to flash.

Analyzing glitch

However, at one point it was just hovering and then started going to the side. I've occasionally seen things like this in the past and really wanted to get to the bottom of this. Whenever I started digging into navigation logs I typically end up writing the same lines in python over and over again. I finally decided to sit down and write a log analyzer to facilitate this.

./python/ -v TauLabs-2014-12-31_16-11-30.tll

This shows a snippet of the log file. The upper left panel shows the position and the upper right the velocity. You can see there is substantially less than a meter movement while holding and very low velocities. The bottom left shows the attitude and there are only a few degrees perturbation. The bottom right shows the gyro and shows that the quad was spinning around at the time. The interface also has a few options to toggle extra plots.

I zoomed in on when the hold deviation occurred:

What was extremely informative about this is that you can see the raw GPS position and velocity jump by a few meters at 188 seconds. Critically this occurs before the attitude deviates and not as a result of flying. Here is that time point zoomed in:

Again showing that there are clearly sample with the attitude nearly horizontal right up to the point where the bad position sample comes. In addition, you can see the INS racing to catch up (which wouldn't happen if there was a real change first since the accelerometers would sense it).

The end result of this bad position sample was the UAV flies the opposite direction to fix the perceived error. This is a tough problem since we have to trust the GPS generally to have any hope of a good position hold. It is also exactly what ArduCopter had to implement GPS glitch protection to solve. It has been extremely rare in my experience and within 2 seconds the GPS had corrected the error. However, it is definitely something where I'd like to get better logs.

Saturday, December 20, 2014

BrainFPV - cool new board using Tau Labs

I was lucky enough to get a BrainFPV from HeliShredder last week, which is a new flight controller that uses Tau Labs.

It is an impressive board with a lot of features in it's 36x36 mm size. Beyond having a full sensor suite (3-axis gyro, 3-axis accel, mag, and baro), its biggest unique feature is of course an integrated OSD.

This kind of integration also gives a lot of power - like the ability to switch OSD display modes on the fly.

I'll just paste from Brain's page for htFPV Specific Features:

FPV features:

  • Full-graphic OSD (360x266 for PAL):
    • Software adjustable black and white levels
    • PAL/NTSC autodetect
    • 4 fully user configurable OSD pages, selectable using switch on transmitter
  • Audio output (not yet supported by software)
  • 3 analog inputs for voltage, current, RSSI measurement
  • RSSI measurement using PWM, PPM, or analog input
  • Other Features:

Other features:

  • CPU: STM32F405RG (32bit, 168MHz, 192kB RAM, 1MB Flash)
  • 64Mbit flash for settings, way points, logging
  • InvenSense MPU-9250 latest generation 3-axis gyro/accel/mag
  • Barometer: MeasSpec MS5611
  • Receiver compatibility: PWM, PPM, S.Bus, DSM2, DSMX, HoTT SUMD/SUMH
  • Up to 10 PWM outputs (up to 400Hz update rate) 
  • Up to 3 serial ports for telemetry, GPS, RC receiver, etc.
  • External I2C port, can e.g. be used with an external HMC5883 compass
  • Micro USB port for configuration via PC 

Installing into MHQ Quadcopter

To test it out, I decided to use my foldable MHQ quadcopter from Steve (thingiverse link). First I had to splice a JST connector into the video line so it could plug into the BrainFPV controller.

This is the Pico camera from GetFPV with a mount that I designed to hold it in the MHQ. Then I plugged it into the BrainFPV and immediately had a nice little OSD showing.

Unfortunately my video recorder crops the edges so you can't see most of the OSD field. In the GCS you can also modify the layout, white black balance, and even switch the layout with a toggle of the switch.

HeliShredder did a nice job of fitting the OSD configuration into the configuration gadget. With it all tested, then I just had to finish reassembling my quadcopter.

It definitely makes the wiring really nice and simple having just that one board. I really need to get add a battery current/voltage sensor to this frame now, since the OSD can show that to me. I just hate soldering on the wiring harness :(

And all closed up and ready to fly. Time to practice my FPV flying ... indoors ...

You can see a bit of line noise from the motors. It is recommended to have a filter on the power line, but I don't have one at the moment. I'd recommend checking out HeliShredder's videos to better see the performance.

OSD History

I'm really stoked to see this board out there and running Tau Labs. Hopefully the hardware designs will be open sourced in the not-too-distant-future and it can become included as an official target.

Sambas (now with the LinuxDrone project) started the OSD project in 2011 back with OpenPilot. The original design had some issues and I ended up cutting up one of the prototypes to design a new way of syncing the two SPI channels together. 

The end result was we got it going, although with some issues that limited drawing to the edge of the screen and required running the microcontroller at a strange rate.

I believe some more work was done on the code subsequently, but ultimately the OSD with OpenPilot stagnated and nothing came of it. Luckily HeliShredder came along at picked up the code and ran with it. I believe he'd previously also had some experience developing on the Super-OSD project in the past.

Interestingly, he independently came up with a better way to sync the SPI channels that resolved those issues that Sambas and I had discussed way back when and it works great. There is also some nice tricks he used to also adjust the white / black balance to make sure it always looks nice that ended up an issue for us. He also did a lot of work to optimize the code so it can run with the flight controller jitter-free as well as the configuration interface to make it adjustable. 

Ultimately, this is a great example of open source where something would have otherwise died in a pile of unused code (sadly like my ESC has become) is now improved and used and I think will make a lot of people happy. Consistent with OSS principles, all his changes are available on github.

Final Thoughts

This board is going to be really popular, I suspect. Of course since it runs Tau Labs (and from my limited testing) it flies wonderful ;-) and I was able to simply import my configuration on this frame from when it was running Sparky and immediately it was tuned and flew well.

The OSD looks super crisp and sharp with good black and white levels and responds nice and quickly. I haven't even tried the modes where it shows waypoints and such, but apparently it does that. I can't wait to throw the battery monitor and GPS on there so I can see my speed, direction to home and battery status.

Between this and Gemini, I really need to spend more time FPVing.

Sunday, November 2, 2014

Seeing Spark - Sparky2 and SparkyBGC quad

One of my longer term goals for a while is a system that can chase me around and film my friends and myself having fun. This seems to be a trendy topic this year with piles of kickstarter projects about this, so I won't pretend this is a unique idea. However I finally have something that is getting close. You can find this on Thingiverse.


Some of my recent boards have been coming together towards this aim (see this post for a longer discussion of boards and some history). Sparky BGC is a cleaned up revision on my previous daughter board for Sparky (providing BGC outputs to a normal Sparky) and adding an external sensor board for light weight gimbals. It also drops the whole thing into the standard 36x36 mm board size which makes it very convenient to stack up with a Sparky.

Tau Link is a miniature radio board that works with the Tau Labs Android GCS for telemetry. I ended up doing another revision that shrinks it down and adds a male USB connector which is more convenient when quickly plugging into an OTG cable or laptop.

Sparky 2 is a pretty major overhaul of Sparky 1, adding lots more IO and processing power, as well as radio capabilities which is convenient for communicating with Tau Link and ground stations.

One of the nice things about Sparky2 and SparkyBGC is that they both support CAN which provides a robust high-speed bidirectional communication bus between both boards. This means that Sparky2 (or Sparky1) can tell the gimbal about the desired angle to a position of interest for the camera.

Through discussion on IRC recently, I also realized that means it is possible to pass the heading measurements from the magnetometer on the gimbal (which is often fairly far from the motors) back to the flight controller to provide an external magnetometer for free. I'm pretty excited to try this.

All this means that the android application can easily pass the tablet or phone location up to the UAV, which can then point the camera at that location and acquire nice stabilized video. I have a few multirotors with gimbals that could be used for testing this, but they are all big and less conveinent for testing, so it was design time!

Power distro

I also really wanted to try out KISS ESC18A, which are tiny little things and also do not provide a BEC outputs. That seemed like a great motivator to make a power distro board that would route the PWM signals to these little ESCs and provide 5V to the flight controller. This board also provides current and voltage monitoring back to Sparky2 and comes in at just a hair below 30mm square.

You can see the wires for current voltage monitoring coming off the top, and for the ESC outputs below. The extra two outputs at the bottom at 5V and 0V. The footprints align to the KISS ESCs but can also be connected to any other ESCs. The connections for the PWM line are routed quite carefully around the edge to not obscure the power planes.

Frame Design

The goal was something about as small as I could achieve that could carry a mobius camera and stabilize it with the above hardware. I started with something similar in my mind as Flying Spark. I wanted a closed canopy over most of the hardware with the battery on the inside.  I also wanted the gimbal integrated into the main body to keep it compact.

 It wasn't possible to design a solid body that could hold all the components that could be assembled as a single piece. One thing I ended up doing to get it quite compact was to make a recessed pocket in the main body for the ESCs and PDB and hand the flight controller and gimbal controller from the top of the canopy, and then printing it upside down.

The arms have a little protective hub around the motors and a channel for the wires to run into the body.

The cables then go into the central body to the ESCs.

Here are the ESCs and power distribution board installed into the body of the frame.

And then Sparky2 and SparkyBGC installed into the bottom of the canopy and connected to the PDB ESCs and battery/voltage measurements.

And then covers placed over the boards and PDB to keep the battery away from them.

And here is the integrated gimbal on the front with the SparkyBGC sensor mounted to the top.

Camera Control

The camera is stabilized via the SparkyBGC board. This communicates via CAN (which is a nice high speed bidirectional bus) to the Sparky2 board. This allows information from Sparky2 to control the camera angle. For example, you can set it up to only correct part of the flight controller roll which creates a smooth video that still shows some of the action. You can also set up the pitch to be controlled via a slider or switch from the transmitter that is relayed via the flight controller.

Finally, you can also use the Tau Labs Android GCS to talk to the flight controller and pass the location of the phone or tablet. The FC then calculates the heading and camera pitch to focus on that point of interest and tracks it. This doesn't work perfectly because the phone GPS is sometimes noisy or slow, but generally works quite well. You can also work around this by directly dragging and moving the POI location rather than tracking the tablet.

Final Assembly and Flights

Here you can see it all put together.

And with an updated canopy that has smoother lines:

Here it is being used to do some simple point of interest tracking to my phone, being held by my lovely assistant.

Crashing :D

This quad got plenty of crashing :). You can see in the long video at the end where I plowed it into the ground upside down. The result was two broken arms:

I also had a good time taking it out this weekend. My friends dog also absolutely loved chasing it (he loves running). I'm still not the best at flying backwards at high speeds while leading a dog, though. One of the earlier flights I caught a tree enough to unscrew a prop nut. It landed flat enough and unharmed. Unfortunately despite ten minutes of a number of us looking, we couldn't find the nut and I foolishly forgot spares.

I turns out you can use a spacer screwed on with camping tongs to get airborne again :D

Unfortunately, that only lasted too long until I backed into another tree, this time much higher...

So back to the 3D printer. I think I might try and smooth some of the curves in the canopy anyway, and possibly reinforce the walls some more.