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

Comment by John on January 22, 2013 at 11:58am
Great write up! Thank you for the update and info. Is this recommended over the APM 2.5? A list or side by side comparison with the APM would be very helpful. Thanks!

Comment by James Goppert on January 22, 2013 at 12:06pm

At this time, I would recommend PX4 for researchers and developers and would recommend APM 2.5 for users. It is not as well tested as APM 2.5. There is still a lot of development going on. For instance, this EKF code will probably first fly within a few days. Sensor-level HIL is great, but is not a substitute for a real test flight! :-) I agree that a side by side comparison of features would be useful. I know that currently some developers are working on porting the existing APM software for the PX4. The PX4 definitely has more memory and a faster processor etc. The current sensors are similar, but the PX4 can just do more with the data it is receiving.

Comment by John on January 22, 2013 at 12:49pm

Thank you! Will be eagerly watching the development as it progresses.

Comment by James Goppert on January 22, 2013 at 1:12pm

The processor isn't fast enough. The PX4 is using the 168 MHz ARM Cortex M4F CPU. APM 2.5 is using the 16 MHz ATmega 2560. The EKF without using the CMSIS hardware acceleration uses almost all of the PX4 processor, and the APM 2.5 does not have a hardware accelerated matrix library to help.

Comment by Rob_Lefebvre on January 22, 2013 at 1:26pm

James, this is very interesting.  Now, when you say that you have the EKF running and using up to 20% of the processor, what rate are you running it at?  I'm hoping we could run our EKF or DCM attitude system, and the rate control loops at 400Hz, which is about as fast as well can the ESC or servos can operate anyway.  This would mean that every update to the actuators would have "fresh" data.  Currently Arducopter is only running the control loop at 100Hz, so the ESC's being updated at 400Hz are getting 4 sets of identical data.

Comment by James Goppert on January 22, 2013 at 1:37pm

Currently we have optimized it for fixed wing.

Sensor sampling: 200 Hz (accel/gyro/mag)

EKF state/ covariance propagation (using accel/gyro): 200 Hz

Attitude Correction (using mag/accel): 20 Hz

GPS correction: 10 Hz

Control output: 50 Hz

The EKF propagation is not the bottle neck however. The correction steps for attitude/ gps are. So running the propagation at 400 Hz and using every accel/gyro packet would be possible without increasing the cpu usage linearly. My guess, mabye 30%, but I haven't tried. This will be an area we will look at once we apply the new math library to the quad estimator.

Comment by Jesse on January 22, 2013 at 1:45pm

A source you site in your code, "Titterton pg. 291", etc. .. what publication is this?

Comment by James Goppert on January 22, 2013 at 1:49pm

If you lose a few GPS packets, you will probably be fine. However, for any prolonged period (a few minutes) due to the low cost sensors employed, you will be in trouble. The attitude will still be stable due to the accel/mag correction, but the lat/lon/alt will get ugly. If you have another sensor for altitude (ultrasonic/ baro) it will help. But eventually your lat/lon will go screwy and you won't be flying to the right spot.

Comment by James Goppert on January 22, 2013 at 1:53pm

The books I reference in the code are:

Strapdown Inertial Navigation Technology, Second Edition, Titterton/ Weston, 2004.

Aircraft Control and Simulation, Stevens/Lewis 2003.

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

I don't have those number currently. It is something we could estimate with a few runs of Sensor-HIL. Currently, we also don't have the baro correction included. Luckily, adding the baro correction won't take much effort /CPU.


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

Join DIY Drones

© 2020   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service