px4fmuv1.6_top.png?w=400The 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);

// 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);

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!

E-mail me when people leave their comments –

You need to be a member of diydrones to add comments!

Join diydrones


  • T3


    I think there are some similarities between what you and James are doing and some of the extensions I have made to DCM. There is possibly an equivalence relation for some of it. For example, it looks like the way that I account for acceleration is the derivative of the way James does it. But then the error vector that I compute gets fed into an integrator, so it may be that the overall result is approximately the same.

    A major difference between EKF and complementary filters is the inverse of the covariance matrix used by EKF.

    Best regards,


  • @Bill,

    It sounds like 1. and 2. are an indirect method of my approach. By finding the cross product of the horizontal component of the derotated mag vector and the declination vector (which I assume does not include the inclination and is thus also the horizontal component) the result is always a vector in the earth frame vertical axis, whose magnitude is the cosine of the heading error (multiplied by the magnitude of the mag and declination vectors). This sounds similar to directly calculating the heading error. 

    I don't know how you do the drift compensation so I'll have to take the time to have a detailed look at MatrixPilot particularly the navigation algorithms!

  • T3


    I agree with your comments about gimbal lock, and also your comments about updating with magnetometer data. That implies that mag data should not be used to update the psi Euler angle, particularly if you plan to fly an aircraft both horizontally and vertically. If you use mag data to estimate the error in psi, the magnetometer becomes useless when the aircraft is vertical. Here is what we do in MatrixPilot instead:

    1. Transform magnetometer vector from body frame to earth frame.

    2. Take the cross product of horizontal component of magnetometer vector in the earth frame, with the declination vector, producing a yaw error vector in the earth frame.

    3. Transform the earth frame yaw error vector back to the body frame, and use it to provide drift compensation for all three rotation axes.

    In other words, we do not treat the magnetometer as a tilt-compensated compass.

    We do not use Euler angles in MatrixPilot. These days we have a mix of quaternion based computations, and rotation matrix based computations.

    Best regards,


  • @James Cheers, I've also been following your work on JSBSim - awesome stuff! I currently have a H for both GPS/mag/baro and mag/baro and use the GPS version when new GPS data is available. Alternatively you can do a sequential update with a separate H for GPS and mag/baro i.e. when all sensor data is available in a single timestep update with GPS first and then mag/baro directly afterwards. I have implemented both and accuracy differences are negligible. I'll be interested to see your results when updating with accel vs not. I have tried both and gained better results without the accel, however you will probably get a better attitude estimate in the static case when you use the accel update. Something else worth mentioning - updating with the heading as calculated by the mag (+ declination) is better than updating with the raw magnetometer 3D vector because the full 3D vector corrects all the attitude states. This means that any magnetic disturbances will perturb phi, theta and psi rather than just psi. Other things to think about for the future:

    • Estimating attitude using quaternions rather than euler angles to avoid gimbal lock (if you're not already)
    • Better prediction integration (Runge-Kutta etc) although this is more computationally expensive
    • GPS latency - there is a nice way to deal with this when using a UKF formulation
    • Implementing a UKF - I haven't done this yet but,
      • Pros:
        • Can approximate 2nd order nonlinearities (rather than first order for EKF)
        • No need to derive Jacobians
        • More robust to initialisation errors
        • Elegant way to handle sensor latency
      • Cons:
        • More computationally expensive
        • Bit more complicated

    @Bill I agree that small inaccuracies in IMU scale factors can have large consequences, particularly for highly dynamic vehicles. Thankfully I have never had to deal with this since my vehicles are rather benign. In addition to a good offline calibration you could add the scale factors to the EKF state vector and estimate them online, much in the same way that you do with the IMU bias. The downside, of course, is extra computation.

  • Developer

    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.

    PX4 Firmware for PX4FMU autopilot and PX4IO servo / failsafe board - jgoppert/Firmware
  • 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.

  • T3


    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,


  • Developer

    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.

  • T3


    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,


  • T3


    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,


This reply was deleted.