GPS Kalman Filtering in PyQuadSim

This video shows recent work by Washington and Lee undergraduate student Shannon Nollet: a simple Kalman filter being applied to the a simulated noisy GPS signal in PyQuadSim (, a Python quadrotor simulator.  The yellow plot is the original, noisy simulated GPS signal (latitude / longitude), and the green plot is the GPS signal filtered using the Kalman filter in OpenCV.  Despite our use of a linear filter (as opposed to the Extended Kalman Filter used in actual vehicles), and despite the filter having only two state variables (latitude, longitude), the filtered GPS signal is good enough to support hover-in-place in the absence of stick input.  I am hoping that students, researchers, and hobbyists can use these kinds of simulations to help  learn about the Kalman Filtering that is essential to the firmware of Autopilot systems like Pixhawk, OpenPilot, and others.

E-mail me when people leave their comments –

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

Join diydrones


  • Sorry about the broken link, Filip!  I took that code down because OpenCV changed the API for its Kalman Filter.  If you want to try the code I wrote for the old version, you can grab a zipfile here, but I can no longer support or help with it.  

    In a week or too I plan to add Python support toTinyEKF.  If I can get it working with PyQuadSim, I'll try and re-post the branch of PyQuadSim that I took down. 

  • @ Thomas I believe that the noise in most inexpensive GPS units comes from the quality of the electronics, rather than from deliberate tampering.  A geologist friend has a unit that is accurate to within a centimeter, but of course it costs in the tens of thousands of dollars.  I appreciate the link to the C++ Kalman code, which should help in putting together the online tutorial.

    @Phillip : you're exactly write: this an illustration of the principle of Kalman filtering, to allow students and others to learn and experiment, without crashing an actual MAV.   By learning about how it works, I hope to be able to make a contribution to the sensor-fusion code on actual firmware someday.

  •   I believe that currently the Pixhawk uses an enhanced kalman filter (EKF) for navigation, which would include handling the GPS data.  As a side note, the default is to handle attitude estimation with the DCM algorithm while comparing that result to the EKF estimate to detect excessive error.  Optionally, the operator can select the EKF as the main attitude estimator.

     I don't believe that the APM has the power to do a full EKF for attitude estimation, but it may be using a "simplified" kalman filter for estimating GPS position, since that would require significantly less processing power.  It uses DCM for attitude estimation.

      I hope this helps.  Regards,


  • Thomas,

      The significance of this post is not so much that they are using a kalman filter for gps data (as that has been done a million times before), but that it has been integrated into pyQuadSim using openCV.

  • I'm working on an online, interactive tutorial for the Extended Kalman Filter.  It's a huge, important technological accomplishment, but as usual the engineers have made it look a lot more exotic and difficult than it is. ;^)

  • Cool beans.  I was expecting something more exotic given pretty much everything I've read about Kalman filters.

  • Thanks for the support, guys!  Code is now available at

    Be sure to follow setup instructions:

  • Great job! I would love to see your code.

  • As we East Coasters might say, "That's like buddah."
    Have you updated the Github project yet to include this filter?

  • We need to make the change to FPGA and do all this and video processing on the autopilot making a change to an optical IMU which the ardueye is the start of. That way we can do optical obstacle avoidance and optical flow as well as face recognition and target tracking.

This reply was deleted.