Tuesday, September 18, 2012

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.

27 comments:

  1. Very Cool! I also prefer to develop my code, hardware, etc from start to finish even though it may be inferior. It is a tremendous learning experience.

    ReplyDelete
  2. I thought I mentioned this before. With your single-axis segway if you turn around on a ramp it will cause some issues due to the coupling of yaw, pitch, and roll. In order for your pitch estimate to remain unchanged, your angle with the surface normal must not change. On a ramp, your angle with the normal is not the same as your angle relative to gravity.

    ReplyDelete
    Replies
    1. That's true. The single-axis self-balancing stuff I've done doesn't assume a 6DOF sensor, so it can work with one accelerometer and one gyro signal. In that case, it's up to the complementary filter to correct for the yaw coupling, albeit slowly.

      Delete
  3. Good Idea. One thing I don't quite understand though is where exactly the INV_RAD term comes from.

    ReplyDelete
    Replies
    1. True, I didn't explain that at all in the post.

      INV_RAD is a constant that is equal to (pi/180). It scales the pitch/roll angle, which is in degrees, back to radians. The cross-coupling term is then scaled by the angle, in radians.

      Delete
    2. Hi Shane,

      Why are you converting to radians when int_pitch or roll seem to be measured in degrees?
      Could you elaborate on that please?

      BTW, Nice work as usual

      Raul

      Delete
    3. Thanks! The amount of yaw rate projected onto the pitch and roll angle estimates is approximately proportional to the pitch and roll angle in radians, for small pitch and roll angles. The units work out as:

      [deg]*[rad/deg]*[deg/s]*[s] = [deg].

      Delete
    4. Sorry can't see why the yaw rate contribution onto pitch and roll is proportional to the reciprocal axis measured in radians.

      [deg]*[rad/deg]*[deg/s]*[s] = [rad][deg]

      Why for small angles this is acceptable?
      [deg]*[rad/deg]*[deg/s]*[s] ≈ [deg]

      Probably I am missing some basic consideration, if you knew any source that would explain that in more depth (don't mind if maths are involved) would appreciate it.

      Like to understand things throughout too :)

      Many thanks,

      Raul

      Delete
    5. As a test case, imagine pitching up by an angle theta and then yawing at a fixed rate omega. The rate of roll (defined with respect to the horizon) is omega*sin(theta). If you were to pitch up 90deg, the yaw rate would be equal to the roll rate, since sin(theta) = 1. For small angles, the approximation to sin(theta) is just theta, in radians.

      Hope that helps!

      Delete
    6. It is all clear now

      Thank you!

      Raul

      Delete
  4. Great Article ....
    I used to develop my own firmware for KK2
    www.HefnyCopter.net ... I had some problems with stabilization and your articles helped me alot.


    Many thanks for your great effort.

    ReplyDelete
  5. could u share ur IMU.ino so that I can run it on my wii board.

    ReplyDelete
  6. I have only implemented the complementary filter on the MultiWii so far, not a full flight controller. (All it does now is estimate the angles.) However, if you want to take a look at the code for just the sensor reading and complementary filter, here it is:

    http://web.mit.edu/scolton/www/smallMWC_INO.ino

    ReplyDelete
  7. It's very great work..
    but I'm curious what the method you do to "yaw decouplling"?

    ReplyDelete
  8. Dear Shane,

    first let me tell that I really enjoy your great work, and your balance filter white paper in particular.

    I have however a serious issue with the claim that it is able to remove the drift of the gyro, as it seems to be demonstrated in the data plots in the white paper. Something important is missing here. To show the point we may assume without loss of generality that the drift is constant over the time scales of interest, i.e., modelled as gyro_rate = real_gyro_rate + drift. If you insert this into your balance filter equation then the drift is NOT high pass filtered:

    filtered_angle = A*[filtered_angle + (real_gyro_rate + drift)*dt] + (1-A)*acc_angle

    Without loss of generality we may assume zero angle and the IMU at rest, you then directly find that the filtered_angle will NOT go to zero but to a constant. The drift is not removed and not highpass filtered! Implementation #2 does in fact show this explicitely. You can also look at it from another perspective. If you consider the balance filter as a highpass + lowpass you would get something like

    filtered_angle = A*[filtered_angle + (gyro_angle - previous_gyro_angle)] + (1-A)*acc_angle

    where the (gyro_angle - previous_gyro_angle) is the highpass filter. Obviously, gyro_angle - previous_gyro_angle = gyro_rate*dt, and you rederive your balance filter. Now, the highpass part, because of its x(n)-x(n-1) structure, would indeed remove a constant bias on the measured angle, exactly as expected. However, we don't have a bias on the angle, but on the rate, which is hence not filtered! Alternatively you may say that difference equations or H(z) functions do not comute as H(s) functions do.

    The balance filter is doing very well in judging which angle-estimate to trust better, but IMHO it doesn't remove or account for gyro drift. I can't see a mistake in the argument, hence I am forced to conclude that something must have went wrong with your data plots, that something done where is not exactly consistent with the balance filter concept as outlined here.

    I would very much appreciate to hear your comments and/or clarifications and/or corrections. :-)

    Thanks a lot,
    Olli


    PS: some few further comments which you might enjoy:
    - of the various implementations of the complementary filter I'd say #3 is best since it involves only one multiply
    - the proof of equivalence of #2 to the others can be done in three lines: expand filtered_angle equation, sort terms with "unwanted" variables to e.g. the left, and the time-back-shifted part is directly recognized.
    - the Kalman filter you presented is essentially identical to Mahony's PI filter
    - with the bias estimation dropped (in the Kalman filter and Mahony's filter) all three filters are identical

    ReplyDelete
    Replies
    1. The complementary filter turns gyro rate bias into a constant angle offset with a value equal to the rate bias multiplied by the time constant of the filter. I mentioned this briefly in the white paper, but it's an important point.


      I'd much rather have a constant angular offset than an accumulating drift which is the rate offset integrated over time. If you want less offset, you can use a shorter time constant on the filter - but then it is more susceptible to horizontal acceleration.

      A more complete solution is to also estimate the gyro bias, which itself might change over time, such as in: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/.

      Delete
  9. Just thought you might like to know... the signal pin for AX2 and AX3 correspond to the TX and RX pins on the 328P chip - No annoying soldering required!

    ReplyDelete
  10. Great work! This is what I need now.

    you use:
    int_pitch += angle_roll * INV_RAD * gyro_yaw * DT;

    For my drone, the posive sign is assigned to roll right, pitch forward and yaw anticlockwise. should I use:
    int_pitch -= angle_roll * INV_RAD * gyro_yaw * DT;

    ReplyDelete
    Replies
    1. Thinking out loud:

      If you are rolled right (+) and yawing anticlockwise (+), pitch will be trending towards backward (-).

      So yes I think you're right!

      Delete
  11. This comment has been removed by the author.

    ReplyDelete
  12. This comment has been removed by the author.

    ReplyDelete
  13. This comment has been removed by the author.

    ReplyDelete
  14. Awsome work, I really like your idea of letting the yaw rate have an impact on roll and pitch by that overcoming the problem you describe.

    But I think there is an even cleaner solution: You have to tranform the angular rates measured by the gyroscope from the quadcopter's body frame into the earth frame with a vector-matrix operation. These transformed rates will recognize roll and pitch rates of an inclined quadcopter even if the quadcopter's gyroscope measures a yaw rate only.

    The needed matrix is described in point 7: http://www.chrobotics.com/library/understanding-euler-angles

    The sensor fusion loop would look like this:

    1) I assume you have a good estimation of RPY (in the earth frame of course).

    2) Get accelerometer data and calculate RPY_accel. This estimation will not be exact unless gravity is the only force.

    3) Get gyroscope rates RPY_body_frame. Transform these rates with RPY from point 1) into new and rates RPY_earth_frame.

    4) sensor fusion: RPY = a*(RPY + RPY_earth_frame*dt) + (1-a)*RPY_accel. RPY is now your updated good estimation of roll, pitch and yaw.

    5) go to point 2) and so on

    I think the transformation of the gyroscope rates from the body frame into the earth frame is crucial. Without it, you don't really use the rates of roll, pitch and yaw because the gyroscope does NOT provide them. It provides rates in it's body frame.

    Therefore, most solutions on the internet are incorrect. Using the raw gyroscope rates for sensor fusion of RPY results in phenomenas as you described it,

    Most quadcopter's do fly though. Rates in body and earth frame are the same if only one angular rate of RPY is nonzero and they are very similiar if the angles are small.

    What do you think about my idea? I wonder why most solutions on the internet ignore this transformation.

    ReplyDelete
    Replies
    1. Yes, you are correct. The above captures a small-angle decoupling of yaw rate into pitch and roll rates. To use the same variables as the CH Robotics matrix:

      r*cos(phi)*tan(theta) ≈ r*theta
      -r*sin(phi) ≈ -r*phi

      But as you point out, it omits the pitch rate coupling terms that show up at non-zero roll angle:

      q*sin(phi)*tan(theta), for roll
      q*sin(phi)/cos(theta), for yaw

      By rights they should be included, even in a small angle approximation, since they are of the same magnitude as the yaw coupling term. They can be approximated in the same fashion as as above, I think:

      int_roll += gyro_pitch * INV_RAD * angle_roll * INV_RAD * angle_pitch;
      int_yaw += gyro_pitch * INV_RAD * angle_roll;

      (To avoid trig, and achieve my goal of writing a full small-angle flight controller in under 300 lines of code. :) )

      I guess I didn't think of it because I was trying to solve yaw coupling specifically, as it most dramatically effects control performance in small angle. (You can yaw integrate into a broken attitude estimate very easily. Pitch integrating can only go so far before you've hit an angle limit. But I can definitely see where the extra decoupling terms would be necessary for stunt flying. At that point I might switch to quaternion representation for the integration though.

      Another interesting question is what variables to use in the control loops and how to integrate the joystick movement into a target Euler angle. If you're in heading lock mode you might say rudder stick slews the target Euler yaw. But if you are in rate mode you would be commanding body rate. Not that that takes away from the desire to have an accurate attitude estimate in Euler angles - just makes it interesting how to integrate the joysticks onto a target in that frame.

      Thanks for the clarification!

      Delete