The art of reduced expectations

After poring through this code for a while, I ended up with more questions than answers. Are they indeed using a Kalman filter, or an "Extended" Kalman filter, or a "Kalman-Bucy" filter? The use of Jacobian determinants implies that it's an EKF, but where's the nonlinearity that would require an EKF? (This is especially puzzling in this simplified version that a lot of people like to crib from, which also uses a Jacobian even though it's a constant.) The use of the "P_dot = A*P + P*A_transpose + Q" form of the covariance update equation seems to imply the continuous-time ("Kalman-Bucy") formulation, but why would they do that for something that's explicitly using a series of discrete measurements?

What nobody really talks about is that most uses of the Kalman filter for accelerometer/gyro fusion are a little bit of a hack, treating the gyro as a control input, treating the accelerometer as a noisy orientation reference (even though vehicle acceleration does not behave anything like the Gaussian white noise the Kalman filter assumes), and various other questionable things.

This poor fellow had some very similar questions and got some really not very useful answers. Several people declared it was impossible because his system was "not fully determined", other people said things like "You just have to get a book on Kalman filters and work through the examples until you understand" without actually answering the questions. The grand winner of the "insanely awful reply" contest was the guy who said

And remember that for stability, the "stable platform" must have an
undamped resonance with a period of 88 minutes. (88 minutes is the
orbital time at zero altitude assuming no air, and also the period of a
pendulum whose length is the earth's radius, assuming uniform gravity.)

Yes, yes, the Earth's rotation is super important to worry about for our little MEMS accelerometers and gyros. Idiot.

It may well be that the "go read some books" advice is good, but I'm pretty sure that would take another month or six.

Anyway, on top of all of that, the way rotations are represented by quaternions in things like the Paparazzi code are also complicated. I mean, quaternions are simple enough, but mixing them with all this Kalman filter linear algebra gets hairy. And porting their carefully unrolled matrix multiplications to my Propeller processor -- which would almost certainly need to be done in assembly -- seemed more than I wanted to undertake just now. So eventually I decided, okay, screw that crap, I'm going to treat this as independent 1-D tilt estimation problems for pitch, roll, and yaw. They're not actually independent, but they are mostly so when the platform is mostly level, and I'm sort of hoping to keep it mostly level most of the time, and this way I can actually have a fighting chance of getting my head around the equations without exploding.

So, I managed to implement the basic tilt sensor Kalman filter thing for pitch and roll in Python code on my computer, and it's simple enough I bet I can port it to the Propeller pretty readily. Here it is in action:


The Kalman-filtered roll indicator is the needle near the lower-right of the grey data display window. You can see that it's perpindicular to the propeller.


See, it's still perpindicular to the propeller, in this new orientation. Of course, to get the real effect I'd have to post a video or something, but I'm just not that elite, so take my word for it that it seems to work OK and updates pretty fast and doesn't go too haywire if I shake the unit (that is, non-gravitational acceleration doesn't confuse it too badly).


Detail of the IMU test app's data display. I had just set it down on the table when this was taken, which is why pitch and roll are both zero. Check out that wacky, wacky magnetometer.

So, the next step is to port the pitch/roll computation to the Propeller, and make sure it's working okay, and then... set up some kind of simple PID controller to try to take off and keep the platform level. Yaw is going to be an issue unless I can get some kind of absolute heading out of that crazy magnetometer data, but maybe I can sort of fudge that for now with manual control.

What's the collective noun for "annoyance"?

I have learned several things the hard way over the last two weeks.

Sensors, such as the accelerometers, magnetometers, and gyroscopes that I'm using, have a significant amount of fixed bias (in addition to a nontrivial amount of variable bias). That is, the zero point of any particular sensor is really nowhere in particular. As far as I can tell from reading about other people's autopilot projects, they just take a lot of calibration measurements. I'm about to write some scripts to do the same.

The magnetometers are extremely fiddly and sensitive to noise. I guess I knew that in theory, but now I know it in practice, too. One thing I wouldn't have guessed is that the radio is hugely disruptive to the magnetometer signal (easily dwarfing the Earth's field -- and it's not a purely AC disruption, either, it adds DC bias as well). After an evening or two of cursing, I ended up relocating the radio to the end of one of the prop arms (yes, the propeller clears the tip of the antenna):

Note that the radio is strapped to the motor speed controller. The first time I did this, I strapped it directly to the heat sink of the speed controller whose auxiliary voltage regulator (aka "Battery Elimination Circuit") I'm using to power the electronics. Turns out that heat sink gets hot and that's a bad idea. Now it's strapped to the opposite side of a different speed controller.

I also wrapped the sensor board in four layers of aluminum foil (sandwiched between paper to avoid accidental shorts):

It looks ridiculous, and I could be imagining it, but it does seem to help the magnetometer noise. The idea is that the nonferrous aluminum shouldn't interfere with the Earth's magnetic field, but should help shield RF noise from the radio.

On the bright side, in testing so far running the motors doesn't seem to bother the magnetometer at all. I would have guessed quite the opposite -- that the 2.4GHz 100mW radio wouldn't bother the magnetometer, but 200W+ of low frequency electromagnetic grunting coming out of the motors would drive it crazy. But other people report the same thing, and apparently modern brushless motors really don't leak much in the way of AC fields. (It probably also helps that they are perched on the ends of arms several inches away from the magnetometer.)

I set up a test rig to calibrate motor speed and thrust:

Features of the test rig include:

• An elevated stand (EVERT by IKEA) to avoid ground effect
• A kitchen scale (0.5g resolution) to measure collective thrust
• A stroboscope to measure propeller RPM
• Foam sheets (from House of Foam) to soften the impact of crashes
  
• A cat, for general assistance and interference purposes

Originally I had each of the two batteries powering two motors. I had been hoping that the speed controllers would regulate motor speed independent of battery voltage, but it turns out that's not the case -- at a given power setting, motor speed varies depending on how charged the battery is. (This was one of the first things I discovered on the test stand.) That meant that two of the motors run slower than the other two, especially since one of the batteries is powering the electronics and the other isn't.

So I spent some time and rewired the power harness so the batteries are in parallel, so everything gets a uniform voltage. Then I measured thrust (in grams, all 4 propellers spinning) and RPM at a variety of power levels (expressed as % of maximum power):

Measurements stop above 55% power because that's where it started to take off. (I could have extended the measurements by weighting down the craft, but I didn't.)

After taking all the measurements (which of course drained the batteries some) I went through the whole set again, which are the "low battery" measurements. The bad news, as noted, is that as the battery drains the speed changes. The good news is that it doesn't change all that much. The other good news is that power level, RPM, and thrust are all (above the zero level) more or less linearly related. I'm certainly hopeful that within the range of 40%-60% power, where I expect to be operating, that I can assume output is basically proportional to input.

Also, the tomato plants are doing ridiculously well:

The real bummer is that I only have a week of leave left, and plenty of puzzle hunt related work in that time, so it's looking increasingly less likely that I'll actually get to the point of stable flight -- witness how the last two weeks were spent fussing with one annoyance or another. Still, I'll see how far I can get.

First crash damage

A variety of boards and connectors and things came in, so I pretty much finished putting everything together, and verified that the CPU can talk to the GPS and IMU and radio.




Why yes, that is a lovely rat's nest of wires! I will have to do something about the motor's power harnesses in particular, lest they dangle all about and get caught in the propellers.

So, having assembled a mostly complete (but unprogrammed) flying device, of course I had to take it for a spin (so to speak). Somewhere around 30% or 40% power, it lifted off the carpet, rapidly tipped to one side, and fell back down when I cut the power to the rotors. During one of these hops, some damage was sustained:


Yes, yes, it's kind of minor. It was a tab I had glued on earlier (it broke during some assembly mishaps). As designed, the interlocking undercarriage and zip ties held everything together just fine. Still, next time I'm at TechShop I'll make a new undercarriage with thicker tabs. (And then I'll have to cut and re-attach all the zip-ties to replace it.)

Meanwhile, since everything's hooked up and mounted, I can route IMU data through the XBee radio so at least I don't have to fuss around with that godawful Bluetooth interface. Or at least I will be able to once I reconfigure the XBee from 57600 to 115220 baud. Also, I can wave the IMU around for testing without its power coming loose every few seconds (because it's all wired up properly now, instead of using a jury-rigged connector).

Then, back to my schooling in sensor fusion and control theory.

Inscrutable Measurement Unit

When SparkFun told me the battery connector on their IMU was a "standard 2-pin JST connector", I didn't realize that JST is a global manufacturer of dozens of connectors, and that what SparkFun considers standard is not at all compatible with the "JST connector" widely used in the R/C hobby world. Oh well, I have something jury-rigged for now, and the proper connector on order.


Once powered, you can talk to the SparkFun IMU with Bluetooth. I'll use a hardwired connection when flying, but this is handy for experimenting, or at least it would be if this weren't the most irritating radio link ever! It constantly gets hung up if you breathe on it long (requiring a reboot), it doesn't reconnect cleanly after disconnection (requiring a reboot), it has some bizarre control protocol that doesn't give feedback, it tends to drop characters (or something) if you send them too quickly. This really makes me appreciate the XBee modules a lot more, which just (by default) give you a transparent serial link with no fuss and no muss.

(Speaking of which, I got my new XBee Pro XSC modules. Their range is indeed far greater than my old XBee Pro's; in a range test, the XSC has good signal several blocks away, right through suburban houses and trees and stuff. Line-of-sight they're supposed to get 15 miles or something ridiculous like that.)

I also discovered that I need to program my ESCs for the proper throttle range (which in this case is 0-100 because I'm synthesizing the PWM pulses directly). That's sort of a pain the way things are set up now, I should see if I can find a Turnigy ESC programmer somewhere (they're all out of stock everywhere?).

Anyway, back to the IMU. So I got it powered up and wrote a Python script to read out and plot the data it's reporting:


After playing with it for a while, my conclusion is, wow, that's a lot of data (9 channels * 350 Hz), and it's noisy and drifty data, not super well calibrated or zeroed, and not trivial to interpret. So I've been doing a bunch of reading about IMUs and Kalman filters and so on. Here are some links I found handy, for my reference as much as anything:

• The Paparazzi Kalman filter code is well documented
  
• Tom Pycke has some great tutorial blog posts about IMU data
• UNC has a good collection of general Kalman filter links
• The Quaternions and Spatial Rotation wikipedia page
• Terence Tong's Kalman Filters Made Easy is surprisingly helpful
• OpenUAV has a nice collection of amusing documents
  

There are also a lot of bad resources, confused people, and clumsy papers out there, but generally speaking this is the deal: Accelerometers give absolute pitch and roll (not yaw) for stationary objects. However, your object is not stationary (you want to fly, not just sit on a desk). Gyros give delta pitch, roll, and yaw (but not the absolute value). However, inexpensive (MEMS) gyros have a drifting zero point. Using a Kalman filter can combine the two for decent overall pitch/roll reading. The Kalman state variables to use are the current attitude and the gyro zero point.

For yaw, you need to bring in the magnetic compass. Apparently the magnetic compass is finicky and annoying to work with, and gets screwed up by nearby motors and things. (Great.) You need to compensate the compass for the current pitch, roll, and magnetic declination.

If you can get all that put together, then theoretically you can get a decent model of attitude. That still leaves positioning, where presumably you use accelerometer data (subtracting out the expected component from the attitude) combined with the GPS. This is all kind of cool in theory, cross-checking your way between 12 data streams (compass X/Y/Z, accelerometer X/Y/Z, gyro pitch/roll/yaw, GPS latitude/longitude/elevation) to get a decent model of what's going. And then you feed all *that* back into a control system and hope for the best? In practice, I'm somewhat surprised this ever works at all.

Maybe instead of an autopilot I can learn to fly my unit manually by sending keystrokes over the XBee. Ha ha.

On a completely unrelated note, my hydroponic tomato plants seem to be sprouting nicely:

Motors, ESCs, batteries...

Today was all about physically assembling stuff. I strapped in both batteries and mounted all four motors and their speed controllers:


With all this new weight, one propeller at 90% power is no longer enough to lift it off the table. Fortunately, there are four, not that I've tried running them all at once yet...

Anyway, assembling this was actually more annoying and time consuming than I thought. First I had to fuss around to find a layout for the batteries and ESCs that would leave enough room for the CPU, GPS, and IMU. Then I had to solder the ESCs to the motors and to power connectors, and I'm really not very good at soldering and so that took a while. I still need to make (or otherwise acquire?) power harnesses to run two ESCs from one battery. You don't see people in the forums talking much about routing their power harnesses. I guess this is a downside of my whole "design as you go" approach.

The battery pack I had ordered for the IMU came in. Unfortunately, the connector doesn't actually fit. (I sent SparkFun a little notice about that.) I'll have to go to Halted or something tomorrow to see if I can find the connector myself -- I'm hopeful it's just an ordinary small two-pin Molex connector. (Update: it's a JST connector. AeroMicro has them.) Then I can power up the IMU and start playing with it, which is something I desperately need to get on top of; it's by far the most mysterious (and expensive!) component in this project, and it will be the primary source of data for the controller.

I also need to find an appropriate sized perfboard (or something) for hosting the CPU. I've mostly got everything set up so I mostly just need to wire it to header pins which will go directly to the various peripherals, but that still needs doing. Then I can strap it down along with the GPS and IMU and we'll have ourselves an unlikely flying object.

Radio, GPS, attempted liftoff

Turns out I goofed with the airframe; the thickened undercarriage nearly covers the motor mount screw-holes, not leaving enough room for the screw heads. When I was trying to assemble it anyway, I ended up breaking off the tab on one of the undercarraige struts.

For some reason the motors ship with two long screws with smaller heads and two shorter screws with large heads (and have a total of four identical screw holes). By using only two screws with smaller heads I was able to mount the motor and assemble the airframe. (I used superglue to reattach the broken off tab.) I wonder if TAP carries 3/16" acrylic...

So anyway, I mounted a motor, and strapped down a battery for good measure, and hooked up the XBee to the Propeller chip, and made a little test program that turns the numbers 0-9 (when received over the XBee radio) into 0%-90% motor power. Around 60% or 70% the platform wants to pull into the air (totally off-kilter of course, since there's only one motor mounted). At 90%, it's really pulling hard. This is with one of the batteries strapped on. With four rotors and two batteries -- especially two smaller batteries than what I'm using now -- I'm pretty sure it will have no trouble getting airborne.

(You can't really tell, but the unit is lifting up against my hand.)

I wonder how ESC input corresponds to actual propeller RPM. Seems like it more corresponds to power output, and it adjusts RPM as necessary to hit the target power level? That could be annoying, especially for things like yaw control where I really want to adjust the RPM directly. But the next level would be to use something like the mikrokopter BrushlessCtrl ESC instead of my off-the-shelf ESC, and that's complicated and fiddly. Other people manage to fly with off-the-shelf ESC using servo inputs, so I should be able to as well.

Also, I got the GPS module hooked up and wrote a test program to echo GPS data over the radio. (I had to change the radio config from 9600 baud to 57600 baud to accommodate the data!) The GPS says I'm at
37° 25' 57.41" N, 122° 8' 43.18" W which is by far the most accurate fix I've ever gotten from my living room. In fact it seems to know exactly *where* in my living room the sensor is located, which is pretty impressive given that it's indoors. I'll need to turn down the update rate to 1Hz (or less?) for every message except $GPGLL -- getting the entire NMEA dump 5 times a second is pretty overwhelming.

Frickin' lasers

I suspect many people who use a laser cutter go through these phases:

1. Hmm, a laser cutter could be useful for this project. I should learn how to use one.
2. Wow! I can make all kinds of stuff! This is so much fun!
3. I AM TOTALLY BUILDING MY ENTIRE HOUSE OUT OF QUARTER INCH ACRYLIC
4. Crappity crap crap, nothing works, my stuff won't separate, everything is charred, grr

Presumably there's some phase afterwards that involves a mature understanding and respect for the strengths and limitations of this tool as well as an expanding repertoire of tricks and techniques. I wouldn't know, I'm not there yet. The world of matter continues to disappoint.


Actually, today wasn't so bad. I learned the following things:

1. 1/4" acrylic is actually pushing it on the 45W laser. You have to go really slow.
2. The laser has a nontrivial kerf, which is bigger for thicker substances (due to defocusing).
3. Cutting depth varies (nonflat bed?), so use a little more power than you think you need.
4. Small holes in thick materials don't work very well. The center melts and sticks.
5. Acrylic is kind of brittle, and tends to crack at the thinnest load bearing point.
   
6. Hardwood tends to get smoky and sticky, and doesn't etch very well.
7. Acrylic etches perfectly well, but the result is somewhat subtle and not clearly visible.

Nonetheless, I have a second revision (I'm sure not the last) of the base airframe.



(You can see remnants of previous versions in the background of the second photo.) Notable features include a plethora of slots for zip-tie tie-downs, a newly beefed up "undercarriage" (the old one kept breaking), and an expanded "deck". I haven't taken the protective film off the acrylic yet, is why it's blue. And yes, the leads to the ESC are way too long, I'll shorten them eventually so the ESC is tied to the strut leading to the motor.

At Fry's today, I discovered these:
These are part of the "schmart board" system, which is designed to make it easy to prototype with surface mount parts, which I don't care about -- but ooh, a bundle of 100 wires attached to single-pin slide-on header connectors? This solves a whole bunch of connectivity issues all at once! I just have to make sure everything is exposed as some kind of pin header, and then I can wire them all up individually. I'm sure this will all end in tears somehow like all my hare-brained schemes for avoiding PCB manufacture do, but for now I'm looking forward to a new era of zip-tie-and-plugboard construction.

In other news, I got the XBee radio modules going using the Adafruit adapter board, connected to a laptop. The next step there will be to connect it to the Propeller (using those exciting new jumper cables!) and see if I can drive it from there. Because then in theory I can start talking to the Propeller over the air (look, ma, no USB) and telling it to do things like run the motors at different speeds (which I also got working in the last couple days -- turns out Parallax has a lovely servo control object in the standard library, and ESCs take servo inputs).