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: 11484

Comment by Mohamed Abdelkader Zahana on April 20, 2013 at 4:04am

Hey folks,

I wanted to ask what is the process model that you recommend for EKF attitude estimation, and how do you find the optimal covariance matrices the Q and R. Are they time varying or constant ones?


Comment by mark sett on November 6, 2013 at 9:10am
I'm very confused! I have to do aerial mapping e photo..which is stablest and powerful pilot? Apm 2.6, pix hawk, px4 or vrbrain?
Last 3 pilot, I've not understood if are same hw/sw !
Give me light on doubts!

Comment by James Goppert on November 6, 2013 at 9:13am

most stable : apm 2.6

most powerful: pix hawk/ px4 (these are basically the same, think of the pix hawk as px4 2.0)

vrbrain: I don't have much experience with.

Comment by Sid on February 18, 2014 at 9:30pm

The link for the hardware accelerated kalman filter isn't working. Can you provide it again?


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