Stability Control Loop Improvement

Hey gang,

I just implemented an idea I had a while back to improve the Arducopter stability control. It's basically using the stabilized acro idea and comes from noticing that my acro leveling code was faster at returning the copter to level than the actual stability control and I couldn't up the stability gains to improve it. The jist of it is that instead of running the STB control loop against the error difference between the stick angle and the copter angle, I'm creating an acceleration limited body frame trajectory plot to follow. This to allows much faster stick tracking without error windup. Here's the code:

First, to follow a trajectory, we need to know how far from the trajectory we are. This function is called right before update_yaw_mode()/update_roll_pitch_mode(). error_bf is the current body frame trajectory error, and target_ef is the new projected target position in the earth frame. (Both stored as vectors)

void update_axis_movement()
  float dt = ins.get_delta_time();

  error_bf = error_bf - ((omega * DEGX100) * dt);

  // calculate target rotation so stabilized modes see where we 'will' be
  Matrix3f temp = ahrs.get_dcm_matrix();
  temp.rotate(error_bf * RADX100);
  temp.to_euler(&target_ef.x, &target_ef.y, &target_ef.z);

  target_ef *= DEGX100;
  if(target_ef.z < 0) target_ef.z += 36000;

The earth frame rate calculations are a very simple P controller. Note that they are controlling against where the copter will be not where it is - this is important to avoid asking it to go beyond the desired position. ACRO_P is used to set the max speed and I'm currently using 10, which means a 450deg/s max speed. I'll get to the acceleration limiting below.

static void
get_stabilize_roll(int32_t target_angle)
  // convert constrained angle error to desired Rate:
  int32_t target_rate = constrain(wrap_180(target_angle - target_ef.x), -4500, 4500) * g.acro_p;

  // set targets for rate controller
  set_roll_rate_target(target_rate, EARTH_FRAME);

static void
get_stabilize_pitch(int32_t target_angle)
  // convert constrained angle error to desired Rate:
  int32_t target_rate = constrain(wrap_180(target_angle - target_ef.y), -4500, 4500) * g.acro_p;

  // set targets for rate controller
  set_pitch_rate_target(target_rate, EARTH_FRAME);

static void
get_stabilize_yaw(int32_t target_angle)
  // convert constrained angle error to desired Rate:
  int32_t target_rate = constrain(wrap_180(target_angle - target_ef.z), -4500, 4500) * g.acro_p;

  // set targets for rate controller
  set_yaw_rate_target(target_rate, EARTH_FRAME);

// Yaw with rate input and stabilized in the earth frame
static void

  set_yaw_rate_target(g.rc_4.control_in * g.acro_p, EARTH_FRAME);

Now the actual STB PID stabilization (I'm using PID instead of PI) is happening in the body frame reference. I moved it all into update_rate_contoller_targets(). It should look familiar, as it is the basic stabilized acro code (feed forward + correction) with acceleration limiting added. The idea here is to not ask for something you know you can't have. I'm currently using 5000deg/s/s. 

  if( rate_targets_frame == EARTH_FRAME ) {
    // convert earth frame rates to body frame rates
    roll_rate_target_bf = roll_rate_target_ef - sin_pitch * yaw_rate_target_ef;
    pitch_rate_target_bf = cos_roll_x * pitch_rate_target_ef + sin_roll * cos_pitch_x * yaw_rate_target_ef;
    yaw_rate_target_bf = cos_pitch_x * cos_roll_x * yaw_rate_target_ef - sin_roll * pitch_rate_target_ef;

  // now do body frame trajectory control:

  // limit acceleration to avoid unattainable error accumulation

  prev_roll_rate_bf = constrain(roll_rate_target_bf, prev_roll_rate_bf-g.acro_acclim_roll, prev_roll_rate_bf+g.acro_acclim_roll);

  prev_pitch_rate_bf = constrain(pitch_rate_target_bf, prev_pitch_rate_bf-g.acro_acclim_pitch, prev_pitch_rate_bf+g.acro_acclim_pitch);

  prev_yaw_rate_bf = constrain(yaw_rate_target_bf, prev_yaw_rate_bf-g.acro_acclim_yaw, prev_yaw_rate_bf+g.acro_acclim_yaw);

  // adjust rate target with correction

  roll_rate_target_bf = prev_roll_rate_bf + g.pid_stabilize_roll.get_p(error_bf.x) + g.pid_stabilize_roll.get_i(error_bf.x, G_Dt) + g.pid_stabilize_roll.get_d(error_bf.x, G_Dt);

  pitch_rate_target_bf = prev_pitch_rate_bf + g.pid_stabilize_pitch.get_p(error_bf.y) + g.pid_stabilize_pitch.get_i(error_bf.y, G_Dt) + g.pid_stabilize_pitch.get_d(error_bf.y, G_Dt);

  yaw_rate_target_bf = prev_yaw_rate_bf + g.pid_stabilize_yaw.get_p(error_bf.z) + g.pid_stabilize_yaw.get_i(error_bf.z, G_Dt) + g.pid_stabilize_yaw.get_d(error_bf.z, G_Dt);

  if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
    error_bf.x = 0; error_bf.y = 0; error_bf.z = 0;
    prev_roll_rate_bf = 0; prev_pitch_rate_bf = 0; prev_yaw_rate_bf = 0;
    roll_rate_target_bf = 0; pitch_rate_target_bf = 0; yaw_rate_target_bf = 0;

  // Add requested movement to error for next iteration:

  error_bf.x = constrain(error_bf.x + (prev_roll_rate_bf * G_Dt), -MAX_BF_ROLL_OVERSHOOT, MAX_BF_ROLL_OVERSHOOT);

  error_bf.y = constrain(error_bf.y + (prev_pitch_rate_bf * G_Dt), -MAX_BF_PITCH_OVERSHOOT, MAX_BF_PITCH_OVERSHOOT);

  error_bf.z = constrain(error_bf.z + (prev_yaw_rate_bf * G_Dt), -MAX_BF_YAW_OVERSHOOT, MAX_BF_YAW_OVERSHOOT);


With the original control loop, you really couldn't use I or D terms because the integrator would basically windup the whole time the copter was moving towards the requested angle, and of course would drive it past it before starting to wind down. The D term just wouldn't work properly since it's just one big saturated error. With this coding, the I and D terms can be used to keep the motion on target and avoid overshooting. Any comments?

I'm not flying the official acro code, but as an example, here's how my acro code simplified with this change:

static void
  // Convert the input to the desired roll rate
  int32_t roll_rate = g.rc_1.control_in * g.acro_p;

  // Calculate rate limiter to avoid discontinuity when crossing 180
  float lf = CalcLevelMix() * (g.acro_balance_roll/100.0);
  int32_t limit = CalcLimit(roll_rate, lf * g.acro_p * 4500);

  // Calculate level rate for blending (similar to MultiWiii horizon mode), ignore roll level as we approach pitch +-90 (cos_pitch_x)
  int32_t level_rate = g.acro_p * constrain(-target_ef.x, -4500, 4500) * lf * cos_pitch_x;

  // combine/limit rate
  roll_rate = constrain((roll_rate + level_rate), -limit, limit);

  // Set body frame target for rate controller
  set_roll_rate_target(roll_rate, BODY_FRAME);


You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –


  • This is working well now with a 5hz LPF on the velocity!  It finally smooths it out enough I can use Rate D on a heli!  That's big news.

    roll_angular_rate = constrain_int32(desired_rate, roll_angular_rate - (g.accel_roll * 100 * G_Dt), roll_angular_rate + (g.accel_roll * 100 * G_Dt));
    feedforward_rate = roll_rate_filter.apply(roll_angular_rate);
    controlled_roll_target += feedforward_rate * G_Dt;
    target_angle = controlled_roll_target;


      Hey Rob, I have my earth frame acc limiting implementated now and it appears to be jitter free. Just remember that in my code, the "target_ef" is the location the copter will be when the bodyframe stabilization zeros the error. (i.e. correction occurs in the bodyframe).

      I found that with the implementation we've been discussing, the jitter returns if you're moving the stick. Filtering the output works, but seemed like a bandaid. This keeps the 'desired rate' more continuous :

      #define ACC_SOFTEN 500 // linear region = +/-5deg

      static void get_stabilize_roll(int32_t target_angle)
        float remaining;
        int32_t max_rate;

        // error from here to target, adjusted for rate change delay
        remaining = fwrap_180cd((float)target_angle-target_ef.x) - roll_slew_rate_ef/100.0;

        // determin max rate from here
        max_rate = safe_sqrt(fabs(remaining)*(200.0*g.stb_acc_roll));

        // clip to max rate
        if(max_rate > 100L*g.stb_max_rate) max_rate = 100L*g.stb_max_rate;

        // sign and reduce max rate near the target
        max_rate = (max_rate * (int)constrain(remaining, -ACC_SOFTEN, ACC_SOFTEN)) / ACC_SOFTEN;

        // acceleration limited rate change (assuming perfect 100hz 100*deg/s/s*0.01 -> cd/s/sample)
        roll_slew_rate_ef = constrain(max_rate, roll_slew_rate_ef - g.stb_acc_roll, roll_slew_rate_ef + g.stb_acc_roll);

        // set targets for rate controller
        set_roll_rate_target(roll_slew_rate_ef, EARTH_FRAME);


    • Here's what the feed forward filter does... i'm using 90% old + 10% new to exaggerate the effect a little:


  • The fix is in!  Thanks, Bob.

    Fix to Stabilize deceleration code. Thanks to Bob Doiron. · R-Lefebvre/ardupilot@da51fe0
    Robert Lefebvre's WIP. Contribute to R-Lefebvre/ardupilot development by creating an account on GitHub.
    • I just took a look at the code... it looks like it works, but may not be doing exactly what you intended. I was trying to imagine how the angle error driving it would behave, and decided to do an excel sim - the assumption here is that the roll sensor moves exactly in step the the target rate that was requested in the previous iteration.

      Being angle error driven with no feed forward means it has to fall behind to go anywhere - making the purple/green lines you hope for result in the pink/red lines you get. Also, the exponential growth of the projected stop may need to be considered. Here it only gets up to 160 deg/s and the projected stop is growing at 3 degrees / 100hz sample. 


      • So, I'm trying to get rid of that jitter.  I've got this as a first stab:

        if (projected_stop < (target_angle-100)){
        desired_rate = g.angle_rate_max;
        } else if (projected_stop > (target_angle+100)){
        desired_rate = -g.angle_rate_max;
        } else {
        desired_rate = (target_angle-projected_stop);

        Seems to be working, by linearizing the last degree of movement.  But I know that there's a discontinuity in the acceleration when you go past 1 degree of error.  I just need to try and smooth that out.

        • desired_rate = (target_angle-projected_stop); ?

          Should that maybe be:

          desired_rate = roll_angular_rate; // hold current rate


          I'm not sure "rate = error degrees" is what you intended... roll_angular_rate is likely much larger or smaller than that and will result in the same acceleration step as if one of the rate_max cases passed. 

          If the code snippet in my previous reply, should the feed forward be here?

          set_roll_rate_target(roll_angular_rate + target_rate, EARTH_FRAME);

          • The idea was, when the error is smaller than 100 centidegrees, the desired rate changes to a linear relationship. So at 1 degree, the desired rate is only 1 degree/second. At 1/2 degree error, it's only 1/2 degree per second.

            I want to make the rate slightly U-shaped, rather than a sharp V.  If you have any better ideas, I'm all ears, but this is actually working pretty decent right now.

            Though I think it's the LPF that really is helping smooth it out, and I might even be able to get rid of that cludge, we'll see.  I'd still prefer a U-shape in any case.

            • Here's a a thought - scale max_rate down based on distance remaining to target, and make the desired_rate calculation continuous:

              max_rate = constrain(sqrt(abs(D)*2*A), -g.angle_rate_maxg.angle_rate_max)  

              where D = target angle - controlled roll target and A = g.accel_roll * 100

              desired_rate = constrain( E * max_rate/LINEAR, -max_rate, max_rate )

              where E = target_angle-projected_stop and LINEAR is the number of cdeg you want to smooth out. 

              unfortunately there's a root in the calc, but it looks like it'd work pretty good. Here's 500deg/s with a +/-5deg linear region:


              • This is all on Pixhawk, so the root should hopefully not be a problem...  Famous last words. ;)

                I'll have a good look at this tomorrow.  Does it still need the acceleration rate constrain?  Or that's handled in the motion curve? And would it be hard to make that equation so the "linear" smoothed portion is at both ends?  The LPF I put in really smoothed out the lead-in.  If I can build it into the motion equation, then I can probably just get rid of the filter.

                Really, what you're building here is a Jerk limiter, which is friggen awesome, I didn't know we'd get that far.

                Today went VERY well.  I made a few other improvements to the rate controller for helis. And combined with your 3/2 bug fix and then the smoothened output for the target rate, this thing is feeling completely awesome.  

                Something else to noodle on if you want...  I want to change the Stab correction rate from a V-shape:

                int32_t correction_rate = g.pi_stabilize_roll.kP() *( target_angle - current_angle);

                To something also with a couple degrees of smoothing in the bottom. Right now, if you have any rate error at all (and what copter doesn't?) the Stabilize controller can bang it back and forth pretty hard as is oscillates around the target. I think this prevents us from running at much Rate gain as we could. It also makes for a bit noisier output.

                This Pixhawk is going to be "full" in no time at all! But these helis are going to be flying just amazing.
This reply was deleted.


DIY Robocars via Twitter
How to use the new @donkey_car graphical UI to edit driving data for better training
DIY Robocars via Twitter
RT @SmallpixelCar: Wrote a program to find the light positions at @circuitlaunch. Here is the hypothesis of the light locations updating ba…
Nov 26
DIY Robocars via Twitter
RT @SmallpixelCar: Broke my @HokuyoUsa Lidar today. Luckily the non-cone localization, based on @a1k0n LightSLAM idea, works. It will help…
Nov 25
DIY Robocars via Twitter
@gclue_akira CC @NVIDIAEmbedded
Nov 23
DIY Robocars via Twitter
RT @luxonis: OAK-D PoE Autonomous Vehicle (Courtesy of zonyl in our Discord:
Nov 23
DIY Robocars via Twitter
RT @f1tenth: It is getting dark and rainy on the F1TENTH racetrack in the @LGSVLSimulator. Testing out the new flood lights for the racetra…
Nov 23
DIY Robocars via Twitter
RT @JoeSpeeds: Live Now! Alex of @IndyAChallenge winning @TU_Muenchen team talking about their racing strategy and open source @OpenRobotic…
Nov 20
DIY Robocars via Twitter
RT @DAVGtech: Live NOW! Alexander Wischnewski of Indy Autonomous Challenge winning TUM team talking racing @diyrobocars @Heavy02011 @Ottawa…
Nov 20
DIY Robocars via Twitter
Incredible training performance with Donkeycar
Nov 9
DIY Robocars via Twitter
RT @JoeSpeeds: Sat Nov 6 Virtual DonkeyCar (and other cars, too) Race. So bring any car? @diyrobocars @IndyAChallenge…
Oct 31
DIY Robocars via Twitter
RT @JoeSpeeds: @chr1sa awesomely scary to see in person as our $1M robot almost clipped the walls as it spun at 140mph. But it was also awe…
Oct 29
DIY Robocars via Twitter
RT @chr1sa: Hey, @a1k0n's amazing "localize by the ceiling lights" @diyrobocars made @hackaday! It's consistently been the fastest in our…
Oct 25
DIY Robocars via Twitter
RT @IMS: It’s only fitting that @BostonDynamics Spot is waving the green flag for today’s @IndyAChallenge! Watch LIVE 👉…
Oct 23
DIY Robocars via Twitter
RT @IndyAChallenge: Congratulations to @TU_Muenchen the winners of the historic @IndyAChallenge and $1M. The first autonomous racecar comp…
Oct 23
DIY Robocars via Twitter
RT @JoeSpeeds: 🏎@TU_Muenchen #ROS 2 @EclipseCyclone #DDS #Zenoh 137mph. Saturday 10am EDT @IndyAChallenge @Twitch
Oct 23
DIY Robocars via Twitter
RT @DAVGtech: Another incident:
Oct 23