COMPLEMENTARY COMPLEMENT ARY FILTER
Step 10 A very flattering filter.
To combine the two sensor readings together in a way that produces the best angle estimate, a special technique called a complementary filter is used. (No, not a complimentary complim entary filter, and definitely not a Kalman filter.) The filter is really just a single line of code: angle = A * (angle + rate * DT) + (1 - A) * (float) accel_raw * A_GAIN; A_GAIN; Okay, so it's a long line. You can also do it with an op-amp or two. two. This filter does exactly what is necessary in this scenario: it favors the gyroscope reading for short time durations and the accelerometer a ccelerometer average reading for long time durations. Let's break it down: c utoff time for trusting the gyro and filtering in A is the factor that determines the cutoff the accelerometer. It's always always between 0 and 1, usually close to 1. In this case, A is defined to be 0.962. This means that at every time step, 96.2% of the new angle measurement comes comes from the old angle measurement plus the integrated gyro measurement.. The remaining 3.8% comes from the accelerometer. This slowly measurement averages in the accelerometer over many time steps. DT is the time in seconds between program loops, the time step. Here it is defined to be 0.020 and is set by delay(20) at the end of the loop. The code in the loop itself take much less than 20ms, so the delay dominates the time step.
rate is the gyro reading, converted to degrees per second. accel_raw*A_GAIN is the accelerometer reading, converted to degrees. It is very important that these two be in the same unit base before adding together. (You can't add apples to oranges.)
The time constant of the filter is the duration of time at which the gyroscope reading starts to be filtered out heavily and the accelerometer reading starts to be averaged in heavily. It's actually a continuous process, but the time constant is a single measure of where the balance begins to shift. The time constant is: tau = DT*(A)/(1-A) = 0.5s So, for this filter, the gyro is trusted for about 0.5 seconds and the accelerometer start to average in significantly after that. This value can be tweaked by changing A.
KK2.0 Firmware Mod: Unleash the Complementary Complementary Filter
The KK2.0 is HobbyKing's 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 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), preconverted into compatible 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+gyr (A)*(filtered_angle+gyro_rate*dt); o_rate*dt); filtered_angle += (1-A)*acc_angle;
The previous angle estimate is incremented by the small angle predicted by multiplying 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 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 Implementation #1 above is a little tricky, but it definitely works out. The nice thing about Implementation 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 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 complementary-filtered angle estimate (I call it filtered_angle, the firmware uses GyroAnglePitch GyroAnglePitch and GyroAngleRoll) GyroAngleRoll) is never used. Instead, the self-leveling angle estimate is straight from the accelerometer (what I call acc_angle, and the t he 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 an d see what happens. I uncommented uncommented the "Calculate Tilt Angle" and "Correct tilt angle" sections in imu.asm. Two other lines also needed to be changed: b16sub Error, AccAngleRoll, AccAngleRoll, RxRoll RxRoll ;calculate error becomes b16sub Error, GyroAngleRoll, 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 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 was able to increase the self-level gain a bit and get very nice angle control:
