PX4 Autopilot: New Software! Hardware Accelerated Extended Kalman Filter/ Sensor Level HIL/ OO Control Library

The PX4 autopilot is an amazing open source platform for research. It is one of the first open source autopilots capable of running an on-board extended kalman filter and other advanced control and navigation algorithms. It is also mass produced by 3D Robotics and very affordable.

Hardware Accelerated Extended Kalman Filter

Recently I have completed a C++ matrix library wrapper around the CMSIS digital signal processing library. This means we can now type matrix math like this:

// continuous covariance prediction
P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;

// attitude correction
Vector y = zAtt - zAttHat; // residual
Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
Matrix K = P * HAtt.transpose() * S.inverse();
Vector xCorrect = K * y;
P = P - K * HAtt * P;

// attitude fault detection
float beta = y.dot(S.inverse() * y);

// position correction
Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
Matrix K = P * HPos.transpose() * S.inverse();
Vector xCorrect = K * y;
P = P - K * HPos * P;

// position fault detection
float beta = y.dot(S.inverse() * y);

Not only is this very easy to read (similar to Matlab/ScicosLab), it is also hardware accelerated! Thanks to the CMSIS library, multiple floating point operations are executed in one CPU cycle. The result is running a 9 state, 9 measurement discrete time extended kalman filter only consumes 20% of the ARM cortex M4 processor.

You can see the entire example here.

Sensor Level HIL

In order to develop and test the EKF I also developed the capability for full sensor-level hardware-in-the-loop testing. Simulated sensor data is sent from the flight simulator directly to the autopilot. This means you can run a very high fidelity UAV simulation while sitting at your desk.

You can see the python script to run PX4 hardware in the loop here.

Object Oriented Control Library

For those that have followed my development, you know that I am a big fan of object-oriented code. While developing the fixed-wing autopilot, I also created a new control library that has a similar feel to what I wrote for ArduPilotOne. This makes it easy for control engineers and the like , who are familiar with block diagram control systems, to easily translate their ideas to code.

You can see an example here for fixed wing.

The features above are just some that I have recently contributed. There are many developers working on PX4 from around the world and many new developments happening every day! Soon a very powerful optical flow board will be released! I hope that you will join this great community!

Views: 11485

Comment by William Premerlani on January 22, 2013 at 8:34pm


I took a closer look at equation 4 of my paper on the subject of acceleration compensation. There are some differences in our two methods, I do not think they are the same. Your method is basically based on velocity, my method is basically based on acceleration. Two different concepts.

The core of my method is equation 2, which says that applying the estimated rotation matrix to the body frame accelerometer measurement vector should produce a vector that aligns with gravity minus acceleration in the earth frame. This is an identity, no matter what the acceleration is. Any misalignment between the two vectors is due to attitude error in the estimated rotation matrix.

To improve signal to noise, I integrate equation 2 between GPS readings. This is a very short time interval, so its not the same as integrating to get velocity. Its really just a filtered acceleration measure.

Another way to look at it is to consider my equation 3, for the case where t1 = 0, V(t1) = 0.

Equation 3 is an identity. If you rearrange it, you will see that the true velocity vector in the earth (GPS) frame is equal to the integral of the true rotation matrix times the true accelerometer vector in body frame, minus gravity vector times time.

The point is, the integral smears out the effects of time varying attitude errors. In effect, only the average of attitude error for the entire flight can be estimated, not the most recent value.

So, I do not think that comparing GPS velocity with IMU velocity is a substitute for acceleration compensation. I think the choices still are:

1. for fixed wing, use omega cross velocity.

2. for quads, use my method, or use nothing at all.

Best regards,


Comment by James Goppert on January 22, 2013 at 9:02pm

Here is why I think the EKF is doing something similar to your algorithm but in a more robust way. Granted your way is a good way to do it without a combined position/velocity/attitude EKF.

If we start with the EKF navigation velocity propagation:
V1 = V0 + int(C_nb * a_b, t, t1, t2)
The can be rearranged to Eq 3:
int(C_nb * A_b, t, t1, t2) = -(t2-t1)*g - (V2 - V1)

But the V here is the predicted velocity not the GPS velocity.

Now for the correction, I think the kalman filter is doing something different. If we use Vg for GPS velocity:

(new V2 estimate)   = (kalman gain mapping error to state correction) * (Vg2 - V2)

(corrected attitude)

And we know for the linear case that it is "optimal". So it might not be doing the exact same as what you are suggesting but I think that it should be more robust. If the GPS velocity suddenly gives an erroneous measurement, the EKF will do a better job of rejecting it.

And as you suggest, I should not use accel vector correction at all for the quad if this velocity correction is helping it anyways. We will have to test it out and see at some point I guess :-).

Maybe I'm still completely missing something on your error correction that the EKF doesn't account for, if so, forgive me for being stubborn. I really want to make sure we are doing this right.

Comment by Dan Wilson on January 23, 2013 at 12:28am

Hey guys,

You can avoid calculating/compensating for centripetal acceleration etc but not updating with the accelerometers. The attitude is corrected naturally within the GPS/barometer/magnetometer update when the attitude is used to form the rotation matrix to transform between the IMU body frame and the global GPS frame. James, this is basically what you were saying in your comment about correcting with GPS velocity. For this to work with hovering multicopters you would need a good magnetometer heading measurement.

I would also suggest including the gyro and accelerometer bias' (at least the gyro bias) in the state vector, modelled as a random walk. It's fairly simple to implement and works really well, converging in a few seconds. Doing so eliminates the need for IMU zeroing during initialisation.

Hope this helps.

Comment by William Premerlani on January 23, 2013 at 6:52am


Regarding acceleration, I agree that my equation 3 is consistent with EKF velocity propagation. In other words, we are agreed on what an accelerometer actually measures. Where we diverge is how we use velocity for tilt drift compensation. You are using velocity directly, I am taking its time derivative to work in terms of acceleration. In my opinion, that is the better way to go, because we are using gravity as the vertical reference vector, and gravity is an acceleration, not a velocity. But then, I might be missing something in your EKF, and maybe I am being stubborn as well. A fair test of your method would be to see if it automatically compensates for centrifugal effects during a sustained turn, with a small but time varying gyro offset. I will keep an open mind until you do some simulation testing.

Tridge has done simulations of my method, and reports that it works, as long as there is not much error in the yaw estimate.

Best regards,


Comment by William Premerlani on January 23, 2013 at 6:53am


Another issue to consider is sensor gain errors versus offsets. Gyros and accelerometers have both sorts of errors. For example, gyros typically have about 3% gain error. This means that if the aircraft does a quick roll of 360 degrees, there will be an error of 10.8 degrees in the attitude estimate. You want to make sure that your implementation of EKF does not misinterpret this as an offset. Ideally, you should account for gains and offsets separately.

I dug into this question in one of the postings that I mentioned previously in this thread.

Best regards,


Comment by William Premerlani on January 23, 2013 at 7:09am


Regarding acceleration compensation, I think it is best not to rely on GPS unless you have to. So, for fixed wing, I think it is better to address acceleration by computing acceleration in the body frame as rotation rate vector crossed with air speed vector. This way, GPS does not enter the picture at all, provided you have a pitot.

Quads are another matter.

Best regards,


Comment by James Goppert on January 23, 2013 at 7:41am

I just heard from the guys at ETHZ that the first flight of this EKF went well for the fixed wing! Our control gains still need some tuning though.

 @Dan Thanks for the input. Really respect the work you guys do at ACFR. I can see how a simultaneous GPS/mag/baro correction would be helpful and more robust. Currently I have a GPS (baro added soon) and mag/accel correction (accel soon to be removed) :-). Hopefully the combined measurement won't slow me down considerably, will be going from inverting 2 6x6 matrices to inverting a 10x10 matrix. Also, if I don't get a GPS measurement, but want to still update the filter with baro/mag, how best to deal with that? I've seen some implementations where they have a different H for each possible sensor data assortment you might have at that time. For me, I probably only need an H for GPS/mag/baro, and mag/baro. Do you think this would be a good setup?  Also, for the gyro bias's w/ random walk model, that is something we are planning to add soon, wanted to make sure this simpler version was working well first.

@Bill Yeah, looks like we will have to do some testing. With all this input we should get something really robust in the end. From what I have heard about low grade gyros is that they are more likely to have a random walk in the bias than in the scale factor. Is it possible to calibrate the scale factor, or will it also change considerably during flight/ based on temp etc.? I have a little centrifuge that I setup for calibration/ testing purposes if needed. I can see the benefit of using a non gps based attitude ref for fixed wing, will have to see if I can include that somehow.

Comment by William Premerlani on January 23, 2013 at 8:33am


I have done a lot of testing with the gyros that are used by MatrixPilot and by Ardu, and found that random walk is less than 1 degree in a minute, provided you do oversampling. Gain is a much larger problem. For example for a 500 degree/second gyro, with typically a 3% gain error, that is an error of 15 degrees/second at 500 degrees/second, or 900 times as large as the random walk error.

The gain does not change much, so you can compute it once and for all for a particular gyro.

What we do in MatrixPilot is auto-calibrate the gyros in flight. I described the method in that link I sent you. Basically, at high rotation rate we treat gyro error as gain error and adjust the gains, and at low rotation rate, we treat gyro error as offset and adjust the offsets.

Best regards,


Comment by Rob_Lefebvre on January 23, 2013 at 9:15am

Really enjoying trying to follow along here.  It's all pretty black-box to me, but just reading this makes me realize what went into it all.

For multicopters, the above technique does not work. There are a couple of other options. One method is to ignore acceleration. There is another method that I have developed, described here and here, that is rigorously correct. However, it does require that you have accurate yaw information. It is my understanding that Tridge and Jonathan implemented it. Initially they had some problems with it, but I think they extended the idea, and now have something working.

Bill, I think that Tridge and Jonathan did get this working quite well.  I've been flying with the "AHRS_GPS_USE" parameter turned on since ~September.  I think I can actually "feel" it working.  Maybe it's selection bias, but I doubt it, as when I initially tried it I was actually expecting it to crash the heli. It was not well tested, I think only Jonathan had flown it on a copter, and I didn't want to try it until I had a heli setup with which I could control it fully manual (flybarred heli with a direct control-passthrough mode). When I tried it, it felt more accurate in high-G turns.  It's hard to describe, but one of those "don't know what you're missing until you try it" things.  Actually, the reason I tried turning it on, is that I was experiencing a nose-down pitch error when trying to turn long sweeping corners in waypoint flight with a heli at speed.  Jonathan suggested I try it, and it helped with that problem a lot.

Since then, I've been using Arducopter in high performance helis with fairly dynamics flights, and have never felt that the AHRS had an error.  Well, at least not since we started using the internal filter in the MPU6000 to cure vibration aliasing, but that is not the AHRS's fault.

Here's an example of some dynamic flight with a helicopter, I've flown much more actively than this as well, I have 3g 180° turns and 4g pitch-ups.  Sustained turns is not something I'd normally do. (just skip ahead to the mid-point).

I've also done a bit of testing to see how it handles acrobatics, and I can detect no problem there either.

At the end you can see where I intentionally tried to get the AHRS confused by rolling spinning and pitching simultaneously, and it pulled out cleanly when released the sticks. I did another test that isn't on video which did get quite ugly and it seemed to come out with a 30-40° error, but it re-converged in a few seconds.  But this was because of a really violent shaking in flight.  I've also suffered a mechanical rudder failure, which resulted in the helicopter spinning uncontrollably.  I was able to land it with only broken landing gear, because it was able to stay upright.

So it's all working REALLY well, and you, Andrew and Jonathan should be commended.  Amazing piece of work, and all on a little 8-bit processor!  Without the accurate AHRS system, the flight control guys wouldn't be able to do what they do.

Comment by James Goppert on January 23, 2013 at 1:27pm

That is some impressive flying/ attitude estimation :-). I agree it is amazing all that they manged to do with the 8-bit processor.

On the PX4 right now we are playing around with some of the suggestions on this post and doing some other work to compare the effects of combined vs uncombined EKF and accel correction/ no accel correction in the EKF, also we uncommented all of the cross terms in the EKF position/attitude updates. Running tests with Sensor-HIL now. It only took 30 min to make each of these branches, which really demonstrates how easy the px4 code/ mathlib is to work with. It should be easy for new developers/ researchers to jump in and test their own algorithms.




You need to be a member of DIY Drones to add comments!

Join DIY Drones

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service