Having received many positive emails about my Extended Kalman Filter Tutorial, I wanted to see whether I could write my own general-purpose EKF from scratch, suitable for running on a microcontroller like Arduino, Teensy, and the STM32 platform used on today's popular flight controllers (Pixhawk, Naze, CC3D).  I wanted something that could be easily modified as new sensors were added, but that didn't use dynamic memory allocation or other techniques that are impractical in an embedded environment.

The result is TinyEKF, a C/C++ EKF implementation that takes care of most of the EKF algorithm for you.  There is a C++ version for Arduino/Teensy, and a pure C version for STM32.  For both versions, you use #define to specify the number of state values N and the number of sensor measurements M.  You also provide a method to compute the output of the state-transition function f(x), the observation function h(x), and their respective Jacobian matrices F(x) and H(x).  As you'll see from the examples, this involves perhaps ten lines of code for each implementation.  TinyEKF takes care of the rest (prediction, update).  This implementation is naturally not as efficient as the hard-coded EKFs you see in actual flight controllers, but I am hoping that with today's increasingly fast ARM processors, and perhaps some behind-the-scenes optimizations, TinyEKF will become a useful, scaleable way of implementing your own EKF.

I've provided an Arduino example using sensor fusion of a BMP180 barometer / temperature sensor with an LM35 temperature sensor. For those who don't have an Arduino but want to try it out anyway, there's also a GPS example in C, using realistic satellite data stored in a .CSV spreadsheet.  Once I get my STM32 Nucleo board, I plan to  port the Arduino sensor fusion example to that platform as well.

In my previous post I had built and flown this little 250mm quad with the Flip32 flight controller on board.  I was waiting for the arrival of a tiny level-shifter I'd ordered from Sparkfun, to connect the 1.8v UART on the ODROID U-3 to the 5v UART on the Flip32.  (I also took some time off to attend "Dronefest", a wondeful day-long event organized by a couple of really cool guys Colgate University, where my students and I presented some of our work.)

Looking more closely at the small UART header on the U-3, I realized that it would take more engineering skills than I possessed to build a robust connection to the flight controller.  Fortunately, the latest Ubuntu release for the U-3 (14.04) supports USB OTG on the U-3's micro USB port, and of course the Flip32 has its own USB port that you use to configure it through Baseflight or Cleanflight.  So all that remained was to find a little USB OTG cable to connect the two devices.  This nifty little cable from 3DR, designed to connect an Android device to a 3DR radio, was just the ticket. (And it'll even let you connect the ODROID U-3's micro USB OTG to a Pixhawk, if you used one of these ingenious little USB/UART adapters!)

To power the U-3, I soldered some female jumpers onto a DC plug sold specifically for it:

With all the electronic components in hand, it was time to design a plate to support the U-3 on top of the Flip32.  With some help from TinkerCAD, I modified the plate I'd printed in Part I to hold the U-3:

Here's the vehicle with the plate on it, plus some 20mm M3 nylon hex standoffs from McMasterr-Carr, ready to mount the ODROID:

Here's the ODROID mounted on the vehicle:

Another tiny socket-wrench head from Lowes was just right for deez nuts!

Here's a closeup of the power and USB connections:

So now it was time to program the ODROID to show a simple proof-of-concept that it could fly the vehicle autonomously. A network cable plus SSH from my Ubuntu laptop (use PuTTY on Windows) made it easy to interactively adapt an example Python program from our MSPPG examples to do something really simple, like pitching the vehicle forward when the channel 5 switch was down.  Remember, the ODROID is a complete computer on a single board!

Once everything was ready I flew the vehicle and found that it had a weird response in when I flipped the autopilot switch: instead of pitching forward as expected, it pitched and rolled back -- right into my *&^in' arm!

THIS is why I prefer to experiment with the smallest vehicle possible: imagine what the 9" prop on a mid-sized bird would have done!  

After nearly giving up for the evening, I figured I'd try a flight without the ODROID connected to the Flip32. Sure enough, same behavior -- flip the switch, vehicle lurches.  Again, bad things happen when you aren't thorough: in this case, I'd only configured the Flip32 for horizontal (stable) flight in the topmost switch position, the default being rate mode.  So the configuration of the flight controller was masking the RC-demand messages from the ODROID.  Here's the right way to confgiure it in Baseflight:

After configuring the modes properly, it flew as expected: flip the switch, it pitches forward; flip it off, regain ordinary transmitter control. Fortunately, the furniture had been removed from "Dronehenge" for floor waxing, so I had a big space in which to fly. Unfortunately, I didn't have any help this time, so you have to take my word for it: when you see the vehicle lurch forward suddenly in this video, it's because I've hit the autopilot switch.  

So, the good news is, the whole crazy thing worked as expected, and you can have the basis for a sophisticated autonomous flight system, using Python, for under $400 ($281 from previous post, $75 for the ODROID U-3, plus a few bucks for cables and fasteners).  

The bad news is, ODROID has discontinued the amazing little U-3 board -- without a doubt the most advanced combination of mini form factor, super processing power, and low price I've ever seen.  So I've switched to working with the Raspberry Pi 2. Although slower (900 MHz) than the U-3 (1.7GHz), the Pi 2 has (1) a similar ARM Cortex quad-core processor to the U-3, supporting parallelization of crucial algorithms like optical flow;  (2) an inexpensive camera module that connects directly to the Pi using the Camera Serial Interface (CSI) protocol cable, instead of bulky USB; (3) a 3.3v UART that may be able to talk directly to the Flip32 and Pixhawk UARTs without an adapter.  In a future post I will report on my experiments using the Raspberry Pi 2 for this sort of work.

Following on the work described in an earlier post, we wanted to build an inexpensive quadcopter that could be controlled by an onboard ODROID U-3 single-board computer.

We were happy with the Naze32 / Baseflight / Multiwii Serial Protocol, but were delighted to discover the new Flip32 flight controller from Ready To Fly Quads. At $16, the Flip32 is even more absurdly inexpensive than the $25 Naze32! But the real advantage we saw in the Flip32 was its soldering holes for not only UART (like the Naze32), but also I²C (for sensor input) and SWD (for firmware debugging). 

Based on our success with the inexpensive ZMR250 frame from Multirotor Superstore,
we decided to go with the same basic setup, swapping in the Flip32 for the Naze32. (This excellent video has a side-by-side comparison for anyone interested, and this video series helped us get started with the build.)

Two other requirements were: (1) a secure mount for the Flip32 and ODROID U-3 and (2) a cleaner power distribution, to avoid the messiness shown below from the homebrew squid distro we did on our previous build:

For the secure mount, we decided to take the plunge into 3D printing, with the goal of having our first print job be a simple rectangular slab that would support both the Flip32 and the ODROID. For the power distro, we opted for the inexpensive Quadcopter Power Distribution Board from HobbyKing – a little big for this vehicle, perhaps, but featuring pre-soldered 3mm bullet connectors, a vibration-isolating foam pad on the bottom, and widely-spaced holes that proved helpful for mounting the Flip32 and ODROID.

Here are the basic parts and their costs rounded up to whole dollars, excluding the 3D printing, zip-ties,
jumpers, heat-shrink tubing, E6000 adhesive, and other stuff we already had in the shop. Naturally you'll
want to order extra ESCs, motors, and props, but this gives you a basic idea of the cost:

• Multirotor Superstore:
  • ZMR250 Carbon Fiber Mini Quadcopter frame 1 @ $40
  • MS 12amp ESC (SimonK RapidESC) 4 @ $16
  • Airbot Titan 2204 2300kv Brushless Motor 4 @ $20
  • 3S 1300MAH 35C LIPO BATTERY 1 @ $16
  • 3x40 Right Angle Pin Header Set 1 @ $3
  • 2x40 Right Angle Pin Header Set 1 @ $2

• ReadyToFlyQuads:
  • Flip32 Flight Controller 1 @ $16

• HobbyKing:
  • FrSky VD5M Receiver 1 @ $12
  • Quadcopter Power Distribution Board 1 @ $4
  • Micro Server Connectors (5 per bag) 1 @ 2
  • Gemfan 5030 Multirotor ABS Propellers (pair) 2 @ $2

• McMaster-Carr:
  • 10mm M3 male-female nylon hex standoffs 4 @ $2
  • 6mm M3 male-female nylon hex standoffs 4 @ $2
  • M3 nylon hex nuts, 100 pack 1 @ $11
  • M3 nylon pan-head machine screws, 1 pack 1 @ $8

• Lowes:
  • 7/32" six-point socket head 1 @ $3

• TOTAL $281

Here's a quick pictorial run-through of the build:

The ZMR25 frame, minus top plate, FPV plates, legs, and other stuff we didn't use

Adding the Velcro one-wrap for the battery, before sandwiching the plates together

The power distro,with 10mm standoffs

This inexpensive 7/32" socket-wrench head from Lowes was the right size for holding the locknuts in place while we bolted together the frame.  We prefer metric, but none of the metric sizes they had was right for deez nuts!

The power distro, glued to the frame with E6000 (a.k.a. Goop)

Power distro battery cables passed through to the bottom of the frame, then EC-3 connector soldered on

Motors mounted and ESCs Velcroed on the frame and connected to the distro, wires secured by cable tie

Our 3D-printed mount for attaching the flight controller to the power distro. We put our school's logo
(the Trident) on top for fun. STL available here.  Our next version will be longer and have additional holes for mounting the ODROID U-3.

We designed and 3D-printed legs  for extra clearance and stability

The Flip32 with headers soldered on for ESCs, receiver, and UART / I²C

The Flip32 on our 3D-printed mount

Neoprene tubing on power distro standoffs, for additional vibration isolation

All wiring completed, including FrSky VD5M receiver

Closeup side view

When the students return on Monday we'll have the maiden flight with this vehicle, in “Dronehenge”, our tiny aerodrome in the Washington and Lee Science Center. Those standup desks make a nice safety barrier! If we're successful, there will be a Part II to this post, showing how to mount the ODROID.

Dronehenge!

After we demoed a little five-channel "AutoPylot" program on a Naze32 with ODROID companion board, our friends at A2USA, Inc . asked us whether we could do the same for the Pixhawk that they use for flying their big birds.  We started modifying our MSPPG parser generator to output MAVLink messages, but soon stumbled across some awesome gists by Christopher Vo  that showed a better way, using Tridge's pymavlink.mavutils library.  Thanks to their work, we were able to create a little Python class that automates much of the programming.  

As with our ARDroneAutopylot and PyQuadSim packages, the autopilot kicks in when you hit a switch (default = channel5, which I've configured as SC on my Taranis), and automatically calls a Python method that you write, which returns the other four RC channel overrides (pitch, roll, yaw, throttle).  By overriding the getChannelsPos1() and getChannelsPos2() methods, you can create two different automated behaviors based on the two non-default switch positions.  Our code includes a simple example that you can run from an ordinary computer, where getChannelsPos1() cycles the throttle up and down, and getChannelsPos2() cycles the roll.  To help avoid accidents, these methods will not get called until the switch has first been reset to its default position.  Because our goal is to put this code on an ODROID to support missions like the one simulated in this video, we have also included an ODROID-specific copy of this example in the repository.

To run on ODROID, you'll need an inexpensive Pixhawk telemetry cable from 3DR and a level-shifter board  to step up the ODROID 1.8v UART to the 5v required by the Pixhawk's telemetry ports.  (We followed advice from A2USA about not using the simpler USB connection, whose rigid cable can transmit vibrations from the ODROID to the Pixhawk).   Our video shows us using an older level shifter on a breadboard, but as soon as our new level shifter arrives from Sparkfun, we'll make up a little cable to go directly from the ODROID to the Pixhawk through the shifter. The video also shows 5v power from a Castle Creations CC BEC, but on an actual vehicle the 5v would likely come from the BEC output of one of the Electronics Speed Controller (ESC) units. We're going into the Pixhawk through its TELEM 2 port, so TELEM 1 can be used for the 3DR Radio talking to Mission Planner on our Windows box.  

WARNING:  We have not flight-tested this system and will doubtless encounter bugs once we do.  Obviously you should (1) know how to program in Python and (2) thoroughly bench-test your autopilot solution before flying.

This video shows work done this summer by Washington and Lee student Conan Zhao '16. Conan used the PyQuadSim and OpenCV Python packages to create a realistic V-REP simulation of a quadcopter returning to its takeoff point using a combination of GPS and visual information.

This video shows an ODROID U-3 single-board computer running a Python program that talks to a an AcroNaze32 flight controller (FC) over USB. The Python program uses MSPPG, our Multiwii Serial Protocol Parser Generator library, to serialize up/down/up/down MSP_SET_RAW_RC messages to the FC. By polling the incoming R/C receiver channels using an MSP_RC message, we were able to have the Python program switch the autopilot on and off without the need for an additional microcontroller. The ODROID is powered by the clean 5v ESC output on the FC.

You can get the code for this example from https://github.com/simondlevy/MSPPG. Remember to remove all propellers before testing!

As shown in this video, I connected the UART of an Acro Naze32 flight controller to an Arduino Mega, and used the pitch values (pitch, get it?) from the IMU to modify the pitch of a buzzer. 

You can download the code for this example from github.

For a final project in a robotics class, I suggested that a few students try implementing their own version of a parser-generator program like pymavlink.  The students built a simple Python-based system in a few days, which parsed JSON message specifications and generated Python code to parse and serialize a simple variant of MAVLink.  

Since then I have added C++ and Java output to the program, and modified it to work with the popular Multiwii Serial Protocol (MSP) message format.  The program is extremely simple to use (a single Python script; you don't even have to know or care about Python to use it), and the Python output has been tested in a real-time attitude visualization program that one of the students wrote (shown in the image above). Another student is completing an Android program to do the visualization using the Java output, and I am working up a C++ example on Arduino (parsing messages over the Naze's UART pins.)

So if you are using Multiwii-based firmware like Baseflight or Cleanflight and are interested in writing code to interact with your flight controller, you might want to try out MSPPG. We could also look into adding support for other languages like JavaScript, C#, etc. -- just let me know!

My students and I like to write MAV applications in Python, and we were disappointed in that Google does not provide an interactive map-visualization API in that language. So we wrote one ourselves, which we call GooMPy.

GooMPy provides a Python interface to the Google Static Maps API, automatically downloading and stitching together map tiles into a single image that you can zoom and pan dynamically in your Python program. To support using maps when you don't have an internet connection, GooMPY provides a pre-fetching function that stores the tiles in a caching folder. To keep the amount of tile-downloading to a minimum, GooMPy doesn't have functions for adding waypoints or other annotations to the map through Google's API. We plan on implementing these feature ourselves in the future.

To run GooMPy you'll need the Python Image Library (PIL) or equivalent (Pillow for Windows and OS X) installed on your computer. The repository includes an example using Tkinter, though you should be able to use GooMPy with other toolkits like wx and Qt.

Because Google limits the number of tiles that you can download during a given time period, we recommend setting up an API key as described here. Once you have your key, put it in the file goompy/key.py, and GooMPy will use it in fetching map tiles. If you run out of downloads, the tiles will be black with a "capacity exceeded" image in them.

I needed an easy way to access data from the MaxBotix ultrasonic sensors using Python, so I wrote this little package, which you can download from github. It runs on Windows, Linux, and OS X in Python 2 and 3.

In working with autopilot systems like OpenPilot and Pixhawk I have frequently come across references to something called an Extended Kalman Filter (EKF). Googling this term led me to several different web pages and reference papers, most of which I found too difficult to follow. So I decided to create my own tutorial for teaching and learning about the EKF from first principles. This tutorial assumes only high-school-level math and introduces concepts from more advanced areas like linear algebra as needed, rather than assuming you already know them.

The tutorial is currently about 2/3 complete.  I still need to introduce linear algebra concepts for sensor fusion, and then nonlinearity for the EKF.  But in the open-source spirit of "release early, release often", I'm posting this now, in the hope that people will try it out and provide comments.  

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 (http://home.wlu.edu/~levys/software/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.

This shaky little video shows me debugging an app with Android Studio over a wireless connection to my Samsung Galaxy S4.  The app, DroidPlanner, connects to a 3DRobotics Iris over a 915 MHz radio plugged into the Galaxy's micro USB port, making the USB unavailable for debugging.  Thanks to these instructions:

  http://developer.android.com/guide/topics/connectivity/usb/index.html

I was able to connect to the phone over my local wifi network, freeing up the USB port for the 915 Mhz radio.  The end of the video shows the steady green LED on the radio, indicating a connection to the Iris.

This video shows automated Hover-In-Place (HIP) using the PyQuadSim Python Quadrotor Simulator with OpenCV for optical flow.  This setup allows us to prototype HIP and other optical-flow-based algorithms for use with an actual optical-flow sensor like the PX4Flow.

The quadrotor is being flown over a simulated concrete floor using a FrSky R/C transmitter, allowing us to switch position-hold (HIP) on/off.  The small black window shows the pitch/roll/yaw/climb demands from the transmitter. The small gray window shows the optical flow computed by OpenCV from a downward-facing 128x128-pixel vision sensor created in V-REP.  (If you look carefully, you'll see that the side-to-side flow is backwards in the video. I have since corrected this problem in the online code.)

The position-hold signal is computed as the average X,Y pixel flow sampled every 16 pixels, converted to meters-per-second using trigonometry.  The second part of the video shows HIP facing a "breeze" from a blower.  Even without Kalman filtering or other smoothing, the optical-flow signal is sufficient to keep the vehicle more or less stationary.

This project is open source and available at

  http://home.wlu.edu/~levys/software/pyquadsim/

I needed an easy way to access data from the PX4Flow sensor using Python, so I wrote this little package, which you can download from github. It runs on Windows, Linux, and OS X in Python 2 and 3. As with the other Python packages I've written, the API is very simple.  I'm hoping that this package will enable students, hobbyists, and others to use the PX4Flow for cool projects on MAVs and other robots.

One thing I could use some help with is the MAVLink parsing.  I looked over pymavlink and was unable to find a class that I could subclass or include to do what I needed, so I wrote my own little parser.  Among other shortcomings, this meant that I was unable to to checksum validation (again, I looked over the checksum code in pymavlink and was unable to figure out how to use it.)  So if anyone wants to modify the code on github, let me know, and I'll add you as a developer. 

PyQuadSim is a Python program that allows you to fly a simulated quadrotor miniature aerial vehicle (MAV) from a variety of controllers (joystick, R/C transmitter, PS3 controller).

You can modify the source code to add new sensors and other features, as well as configuring the simulated environment for different kinds of missions.

PyQuadSim uses the powerful Virtual Robot Experimentation Platform (V-REP), but does not require programming in Lua or writing a plugin in C++.

I thought some of y'all might find this useful:  http://home.wlu.edu/~levys/software/pyquadsim/

Comments welcome!