KK2.0 Firmware Mod: Unleash the Complementary Filter

The KK2.0 is HobbyKing's ultra-cheap ($30) multirotor flight controller, which I bought a few of for testing some time ago. Unlike its predecessor, the KK2.0 has a full 6DOF IMU with an InvenSense 3-axis gyro and an Analog Devices three-axis accelerometer.

That means it should be fully-capable of doing angle control (a.k.a. attitude control, self-leveling), where the joystick axes correspond to a commanded pitch and roll angle, and center-stick returns the multirotor to level. This is in contrast to rate control, where the joystick axes correspond to a commanded pitch and roll rate, and center-stick holds the current angle (but does not return the quadrotor to level).

Rate control is good for doing tricks and flips, but angle control is better in most other cases when you want to stay within say +/-30º of horizontal. If you get disoriented, you can go to center-stick and know that the quadrotor will return to level and not go rocketing off into the distance. It's also more well-suited to camera platforms, since it helps maintain a level horizon.

The KK2.0 has a "self-leveling" feature that I was very eager to try, but I was a little disappointed in the performance. It was sluggish, and increasing the gains to make it return to level faster would lead to pendulum-like behavior, especially in the wind. It would eventually return to level, but not before overshooting back and forth a couple times.

Skimming through the 200+ page RCGroups thread for the KK2.0, which is frequently updated by the designer of the KK board, it seems that in the initial firmwares (v1.0, v1.1, and v1.2), self-leveling is implemented using only the accelerometer angle estimate. From the tuning guide,

Q: Why is Autolevel so slow?

A: The autolevel is based only on the ACC. It takes some time before the ACC registers tilt due to the horizontal acceleration of the craft when tilting. It needs some speed to get air resistance and a force on the ACC. Therefore the autolevel works best on draggy crafts. Later I will try to implement a AHRS algorithm, which uses both ACC and gyros to get a immediate tilt angle.

This all sounds very familiar. I have explored the specific problem of creating an angle estimate using both accelerometers and gyros at length on many projects, using a technique known as a complementary filter. I used it on all the self-balancing things I've built or helped build, as well as two quadrotors: 4pcb and the original Edgerton Center copter.

Tangent: Two complementary filter implementations in simple code.

I won't go into the theoretical details here, since they are well-explained in several of the links above, but I will put forward the typical code snippet I use to implement the complementary filter on a single axis of the accelerometer/gyro data. It takes as its input the gyro rate (gyro_rate) and the accelerometer angle (acc_angle), pre-converted into compatible units (deg and deg/s, or rad and rad/s, for example). The accelerometer angle can be calculated with an arctan function or by linearization for small angles.

// Complementary Filter Implementation #1:
filtered_angle = (A)*(filtered_angle+gyro_rate*dt); 
filtered_angle += (1-A)*acc_angle;

The previous angle estimate is incremented by the small angle predicted by multiplying the rate by the loop time (dt). This gyro-only angle estimate is high-pass filtered. The accelerometer-only estimate is low-pass filtered and the two estimates are added to created the new filtered angle estimate (filtered_angle). The time constant of the high-pass and low-pass filters (they are the same) is:

tau = A / (1-A) * dt;

and it depends on the loop time. Larger time constant trusts the gyro for longer and filters the accelerometer more. Making this time constant long enough to overcome the time required for buildup of aerodynamic drag mentioned in tuning FAQ is the key. However, if the gyro rate has a lot of bias, a time constant that is too large can lead to offset in the angle estimate.

Interestingly, the complementary filter can also be formulated as follows:

// Complementary Filter Implementation #2:
filtered_gyro = A*filtered_gyro + (1-A)*gyro_rate;
filtered_acc = A*filtered_acc + (1-A)*acc_angle;
filtered_angle = tau*filtered_gyro + filtered_acc;

Unwrapping the math to show that this is equivalent to Implementation #1 above is a little tricky, but it definitely works out. The nice thing about Implementation #2 is that it's just two low-pass filters, a multiply, and an add. It could be done entirely in analog hardware if one so desired.

Ok but what does this have to do with the KK2.0?

Well, while browsing through the open-source KK2.0 firmware v1.2 (the latest at the time of writing) I stumbled onto a curious section of code. Now, it's entirely written in assembly, and it's been a long time since I programmed in assembly, so it took me some time to interpret it. It's in imu.asm in a section labeled "Calculate Tilt Angle" and another section labeled "Correct tilt angle". If I'm reading it correctly, it behaves roughly like this:

// Complementary Filter Implementation #3:
filtered_angle = filtered_angle + gyro_rate*dt;
error_angle = acc_angle - filtered_angle;
filtered_angle = filtered_angle + (1-A)*error_angle;

And I label it as Implementation #3 because in fact, if you rearrange the math, you get exactly the same thing as the other two. This version is formulated more as an error feedback approach, which is intuitive.

The strange thing about this complementary filter isn't that it's written in a different way...it's that it's entirely commented out! The complementary-filtered angle estimate (I call it filtered_angle, the firmware uses GyroAnglePitch and GyroAngleRoll) is never used. Instead, the self-leveling angle estimate is straight from the accelerometer (what I call acc_angle, and the firmware calls AccAnglePitch and AccAngleRoll).

Maybe it's a planned future upgrade and needs more testing. In any case I decided to turn it on and see what happens. I uncommented the "Calculate Tilt Angle" and "Correct tilt angle" sections in imu.asm. Two other lines also needed to be changed:

b16sub Error, AccAngleRoll, RxRoll ;calculate error

becomes

b16sub Error, GyroAngleRoll, RxRoll ;calculate error

and

b16sub Error, AccAnglePitch, RxPitch ;calculate error

becomes

b16sub Error, GyroAnglePitch, RxPitch ;calculate error

And that's it. Here is the modified firmware I used. No other changes were made to the project. If you want to stare at the code or rebuild the project yourself, you'll need AVR Studio. Otherwise, KK2_1V2_modS.hex is the file to be flashed. Note: Use at your own risk! Most likely, this is a future upgrade that may not be fully ready yet!

I wasn't really expecting this to work so easily, but on the fist test flight after loading the new firmware, I was able to increase the self-level gain  a bit and get very nice angle control:

The stick response is very fast. Going back to center gives very fast return to level, with no overshoot and no pendulum effect. It now flies as well or better than other (way more expensive) flight controllers I've tried. I'm sure that by tuning the complementary filter time constant, the self-level P gain and the inner rate loop PI gains, I could get it to be even better.

So now my $30 flight controller looks like an incredibly good deal.