Yes, yes I do need more flying things.

My fleet of flying things has grown quite a bit over the last few months, and I'm not just referring to Kerbal ships. First off, a little pocket quad!

This is the HobbyKing Pocket Quad, a tiny < 30g single-PCB quadrotor that comes ready to bind to a Spektrum transmitter. (Well, first you had to glue the motors in place with hot glue, and then discover that the entire craft vibrates like crazy unless you glue them much lower in the booms than the product images indicate.) The newer model (v1.1) comes with plastic motor mounts, which is nice. And it really is a pocket quad, no joking:

Well, maybe time for cargo pants...

Having built a single-PCB miniature quadrotor, I know how tough they are to make stable compared to their big counterparts. The propeller slew rates and sensor bandwidth required to keep up with the fast mechanical time constant of a miniature/micro quad are hard to achieve even in a completely rigid system. Throw in nasty mechanical resonance excited by high RPM motors and you get a controls nightmare.

This quad is so small that I feel like quantum mechanics might be coming in to play as well. The heart of the control system is a MultiWii-compatible sensor suite (MPU-6050 flavor) and ATmega32u4. So, you can tune it using the MultiWii GUI which is well known and easy to use. It has directly-controlled brushed motors, so there's no problem of slow brushless ESCs to deal with.

And, oh yes, important point: it does fly.

Out of the box (actually, it came in a static bag), it's not quite as graceful as a Walkera Ladybird. But that could be mostly due to the less-than-ideal motor mounting in the v1.0 frame that I got. Or it could be that it requires a bit of MultiWii fine-tuning. Since I recall one of the very first almost unbelievably stable nano quadrotors was based on MultiWii, I'm not surprised this one is also pretty easy to fly.

Speaking of MultiWii, I still have one laying around that I used to write some dirt-simple attitude estimation code. The ultimate goal is to create a flight controller out of less than 500 lines of code .(It's Arduino C code, so the 500 line limit assumes some libraries to take care of low-level things like I2C reading.) The point isn't to improve on the MultiWii control code, just to strip down quadrotor flight control to its absolute simplest functional form. Something to bridge the gap between control theory and actual, readable C code that people can understand without digging through abstracted libraries.

Anyway, such a project, if it does pan out, will require a new airframe. Actually, that's a complete lie and I just wanted a new airframe.

This is a size I have yet to play with - an F330. (Thanks for the frame, DGonz.) It's a good bit smaller than the Talon, which is probably a good thing for testing a completely custom flight controller. I have a hunch that it's actually a really good size for a GoPro-carrying quad...if you can work out the vibration isolation problem that plagues small GoPro quads. Anyway, even if it proves useless for GoProing, it will make a nice test frame that is essentially free.

The motors are Turnigy Air L2210C-1200's, also from HobbyKing. On one hand, they are well-built, have extremely low cogging, and good Kv/resistance combination for their size. On the other hand, they suffer from the axial play issue that makes small outrunners awful. (The rotor can is not axially preloaded, so it loosens slightly and then makes a ratcheting noise as it moves due to magnetic forces.) 

And on the third hand that you didn't even know you have, the prop adapters are terrible. They are extremely tall, which makes them almost useless for quads for the same reason that gluing the motors in the Pocket Quad as they are pictured in the product image is awful. Having the props so far away from the boom is a recipe for vibration, since any imbalance now has a huge moment arm for twisting the boom. I have attempted to modify them as much as possible to get the props down closer to the booms, but it might be hopeless. Maybe time to try out some different options.

One thing that was very helpful on this build was the 4-way XT60 to 3.5mm bullet splitter.

For ESCs, I used the same FFv1.2s's that have served me well on the Talon for quite a while now. They're a little big for this size frame, but I managed to fit them in sideways in a way that doesn't seem too inefficient.

And some lighting.

And until I create my mythical sub-500 line flight controller, it'll use a KK2 running the latest firmware (v1.6), which is pretty solid. Blah blah Stability blah GPS blah...it's less than $30, it has an LCD on board for tuning, it reliably arms and disarms on command, and I thoroughly understand the control code at work inside of it (because I can go read it myself), which makes me actually trust it. Maybe to people who treat every flight controller as a magic black box the perception of value and reliability is very different...

Oh, I forgot one other important reason why I wanted an F330: It fits in a backpack. This had only hypothetical benefit to me until the day after I completed this quad and went on my very first West-coast hiking trip with high-mountain-and-high-voltage-loving Tyler, as well as some other timezone-shifted MIT people. We went on a "tame" (Tyler's definition, not mine) hike up to Pratt Lake. I asked if there would be some flat, open area around the lake from which to take off and land.

"Yes."

I did find one rock with a flat enough top to take off and land from on the 30º rock slope. The picture above is actually from the quad's GoPro just before returning to my little landing pad rock. Unfortunately the video itself is not very pretty at this point due to the wonderful world of CMOS and vibrations. But the stills were worth the quick test flight.

A view of the lake from about 75-100ft up. Yes, I could have simple climbed back up the rock slope to get virtually the same picture. But dammit this is the future and there must be flying robots involved.

Spying on the rest of wilderness-MITERS (label stolen from Amy) from behind a tree.

So the next step for the F330 project is to travel down the wonderful road of vibration minimization and isolation. Precision propeller balancing, custom prop adapter turning, motor replacement, motor balancing, and silicone/memory foam padding are all stops I might visit along this path. Finding the magic combination to mechanically filter the prop vibrations (~50-100Hz in this case) is an annoying but straightforward task. In addition to making the video more tolerable, it will reduce gyro noise at that frequency, which should simplify the control task a lot.

There's one last new addition to the fleet. This one is also small and from HobbyKing, but it only has one rotor (gasp).

Well, two rotors actually.

It's a Turnigy FBL100, HobbyKing's counterpart to the Blade mCP X. These are nano-scale flybarless helicopters, which are such a giant leap from the counter-rotating-blade mall toys from not that long ago that I had to have one. I've never flown a collective-pitch RC helicopter before, so I figured this would be a good way to start learning. The trick is is that the blade pitch can be negative, meaning you can fly inverted and do other crazy RC helicopter things. And by "you" I mean not me. 

But it was easy enough out of the box for me to simply hover. It has active 3-axis electronic stabilization (in lieu of a flybar) using three very tiny and very cool linear servos controlling a CCPM swash plate, as well as a feedback-controlled tail rotor. That used to be a lot of controls to fit in a tiny package, but not anymore, this is the future. I do wish I could tweak the controls and the pitch/throttle curves. (You can do the latter, if you buy the transmitter module for use with your own radio instead of using the stock included transmitter.) But even with just stock setup it's fun.

And then you add a strobe light and it becomes 10x more fun (and ~3x more difficult to fly):

Fun with the Complementary Filter / MultiWii

I first started using a complementary filter, not even knowing what it was called at the time, on the DIY Segway project six years ago. It's a convenient way to combine measurements from an accelerometer and an angular rate sensor (gyro) into a better angle estimate than either could provide on its own. The single-axis case (like on a self-balancing vehicle or robot) is very simple, especially if the balancing platform stays close to horizontal where the small angle approximation can be used to linearize the accelerometer measurement. This is the case I wrote up in the Balance Filter white paper.

Since then, I've used the 1DOF small-angle complementary filter on a bunch of projects, including Segstick, Mini-Segstick (which was really just Segstick in disguise), and some quadrotors.

Wait, but quadrotors are not constrained to a single axis of rotation. They have the ability to pitch, roll, and yaw, as well as translate in 3D. A full representation of orientation in 3D is itself a difficult concept to grasp. I've mostly avoided the complexity by working only with small pitch and roll angles, using a separate rate-only controller for yaw. On 4pcb, the pitch and roll complementary filters are completely independent, and take up only four lines of code in total.

Recently, I found a similar implementation of independent pitch/roll complementary filters in the firmware for the KK2.0 flight controller. It was written in a different form than the one I've used on my projects, which got me thinking about equivalent forms. There are now at least three different implementations I've seen that are functionally identical. Anyway, the complementary filter-based angle estimate was disabled in the KK2.0 firmware as of v1.2, and I wondered why that was since it seemed to fly well in my testing. My question was answered by KK himself:

It treats the Roll and Pitch axis separately. It keeps track of the absolute angle by integrating the output from the gyro on that axis, and corrects the angle with a small part of the angle from the accelerometer on that axis when it is within a certain range (+- 10 degrees or something like that).

This angle is then fed into the control PI loop resulting in roll/pitch angles controlled by the stick. 

Unfortunately it was not that easy. It works very well as long as no yaw is commanded when the craft is not level, as seen in the video earlier in the thread. If yaw is applied when not level, both roll and pitch angles change, but roll/pitch gyros does not sense this rotation, resulting in wrong angles. Try for your self with a airplane model in your hand.

So, the scenario of concern is when then the normal vector of the quadrotor isn't vertical. Then, rotating about that normal axis (yaw) will cause the pitch and roll angles to change in a way that isn't measurable by the X and Y rate gyros. This animation shows yaw with and without the normal axis vertical. The accelerometers will still pick up the changing pitch and roll angles, but they are low-pass filtered in the complementary filter so they will only update slowly. Rapid yaw while not horizontal could result in bad angle estimates, and consequently a loss of control.

Tangent Alert:

Now is about the time I would normally expect someone to suggest using a Kalman filter. I would argue that the choice between a Kalman filter and a complementary filter lies on a completely different axis (of decision making...) than the choice of 3D angle representation. For example, you could have a complementary filter acting on quaternions, which would solve the above problem by having a complete set of 3D kinematic equations. Conversely, you can have a Kalman filter operating independently on each single axis.

Until recently, I hadn't seen a good, well-explained example of a Kalman filter applied to a single axis. Most sites refer to it as more of a buzzword, hand-waiving the explanation and using copy-pasted code from another project. However, this post lives up to its title by completely developing a simple and easy to implement Kalman filter for a single rotational axis. It uses a method I only recently began to appreciate where the rate sensor signal is treated as an input, rather than a measurement. The state equations are thus purely kinematic; no information is required about the plant dynamics. The states are the angle and the rate sensor bias. The output is the accelerometer-measured angle, either from a linearized approximation or an arctan operation.

One interesting thing I learned about the Kalman filters in 2.KalmanFilters is that in the bone-stock Kalman filter with known, constant process and measurement noise variance (Q and R), it's possible to pre-compute the time-dependent covariance matrix (and hence the Kalman gains) before you even take your first measurement. This is definitely true in the above-mentioned post: P[i][j], S, K[0], and K[1] don't depend on states or measurements. If the filter is stable, the gains should converge after some amount of time. If they do, the remaining state observer looks like this:

rate = newRate - bias;
angle += dt * rate;
y = newAngle - angle;
angle += K[0] * y;
bias += K[1] * y;

This simple algorithm is very similar to Implementation #3 of the complementary filter from my previous post, the method that is in the KK2.0 firmware. It's based on feedback of the error between the angle prediction using the rate sensor alone and the angle measurement using the accelerometer. In this case, K[0] sets the time constant of the complementary filter. This implementation has a nice addition, though: a bias estimator that can track an unknown and variable gyro bias over time. The rate at which the bias estimate is updated based on the output error is set by K[1].

End of tangent.

I finally understand how a single-axis Kalman filter might work, but it won't solve the problem at hand, which is cross-axis coupling between yaw rate and pitch/roll angle on the quadrotor. I'm not quite ready for a full 3D angle representation, though. I'm a fan of inexpensive hardware like the KK2.0 or my recently-acquired HobbyKing MultiWii clone, both of which are under $30 and have a full 6DOF inertial sensor set (plus more, in the case of the MultiWii clone). However, they use 8-bit fixed-point processors that would struggle to do the math required for a full 3D angle estimation algorithm.

Actually...that's not really true. As far as I can tell from reading the source code, MultiWii now implements an angle estimation algorithm that covers any possible orientation and would have no trouble handling the yaw-while-not-horizontal scenario. It's based on a complementary filter and uses an interesting representation that (I think) would not have problems with gimbal lock. (Pitch and roll are always uniquely defined based on the orientation of the gravity vector with respect to the frame. Heading is treated separately.) Interestingly, the problem of yaw while not level is addressed in the second page of this development thread.

I guess the real reason why I want to implement a simple complementary filter is because I will always prefer a piece of code that I developed from start to finish, even if it's inferior to something that already exists. I like understanding how all the pieces work, including their limitations. So, I'd rather make a small improvement to my existing angle estimation code that solves the problem of yaw rate coupling. With that in mind, I erased my MultiWii and started from scratch...

// integrate pitch rate into pitch angle
int_pitch = angle_pitch + gyro_pitch * DT;
// integrate component of yaw rate into pitch angle
int_pitch += angle_roll * INV_RAD * gyro_yaw * DT;
// filter with error feedback from pitch accelerometer
error_pitch = accel_pitch - int_pitch;
angle_pitch = int_pitch + AA * error_pitch;

// integrate roll rate into roll angle
int_roll = angle_roll + gyro_roll * DT;
// integrate component of yaw rate into roll angle
int_roll -= angle_pitch * INV_RAD * gyro_yaw * DT;
// filter with error feedback from roll accelerometer
error_roll = accel_roll - int_roll;
angle_roll = int_roll + AA * error_roll;

This Implementation #3 again, but with an extra term to include a component of yaw rate in the integration from the previous angle to the gyro-only new angle estimate. The component of yaw rate integrated into pitch is proportional to the roll angle. The component of yaw rate integrated into roll is proportional to the (negative) pitch angle. This linear approximation should hold for small angles (less than 30º) to within 5%. It should provide reasonable performance at the cost of two extra lines of code with some multiplies (still no trig).

To test it, I made a MultiWii-on-a-stick that I could spin around in a cordless drill...

Since it obviously couldn't be USB-tethered, this required an on-board battery and an XBee radio connected to the TX pin of the ATmega328. (It's broken out for use with an external serial LCD module.) This way, I could power it, tilt to some angle, and yaw like crazy while collecting data wirelessly.

First, I did a test with the yaw terms turned off, making the two complementary filters independent again. This is what I have running on 4pcb, but 4pcb is so twitchy anyway I wouldn't have noticed any quirks with yaw-while-not-horizontal. As expected, at a constant angle of about 30º to horizontal, yawing causes the pitch and roll angles to do strange things:

The complementary filter was set to a time constant of 0.99s, so even a modest yaw rate of 65º/s was enough to cause major offset problems due to the accelerometer lag. At higher rates (570º/s), it's hopeless. The filter barely has time to react before a complete rotation is finished, so it barely registers a change in orientation at all. Here's what it looks like in phase space:

If all were working as it should, the initial -30º roll angle would trace out a circle of 30º radius (yes, the radius is now measured in degrees) as it transforms into pitch angle, then back to roll, etc. Clearly, all is not working as it should. At the higher yaw rate, the "circle" becomes a blob off in one corner.

With the extra integration terms from the yaw rate turned on , the result is much better:

Now it seems to have no trouble with yawing at any rate right up to the gyro limit of 2000º/s. The initial angle traces out a nice circle in phase space as the yaw rate shifts it back and forth between pitch and roll. There's a little bit of skew, but it could very likely be from my hack spinny board mount.

So that fixes the yaw problem. I can already think of other minor tweaks to make that wouldn't cost any trig or matrix math. But I think this was the biggest of them. Now that I've taken care of reading in sensors and estimating angles, I think I should have no trouble finishing up my ultra-simple MultiWii control software. My goal is to have a working flight controller in less than 500 lines of (Arduino) code. (The sensor reading and angle filtering code is only 150 lines, 4pcb's entire flight controller is 495 lines.)  

It will only work for "small angles", but with +/-30º of pitch and roll range (45º in a pinch), it should handle the non-acrobatic flying that I do. Maybe when I get myself an STM32F3Discovery board I will do a full 3D flight controller with real math. In the mean time, I'll also attempt to implement the yaw decoupling terms on the KK2.0, but that will be a bit harder since it's all written in assembly.