PID controller integral term causing extreme instability

Question

I have a PID controller running on a robot that is designed to make the robot steer onto a compass heading. The PID correction is recalculated/applied at a rate of 20Hz.

Answer 1

It looks like you are applying your time base to the integral three times. Error is already the accumulated error since the last sample so yo don't need to multiply deltaTime times it. So I would change the code to the following.

SteadyError += error ;

SteadyError is the integral or sum of error.

So the integral should just be SteadyError * Ki

float integral = Ki * SteadyError;

Edit:

I have gone through your code again and there are several other items that I would fix in addition to the above fix.

1) You don't want delta time in milliseconds. In a normal sampled system the delta term would be one but you are putting in a value like 50 for the 20Hz rate this has the effect of increasing Ki by this factor and decreasing Kd by a factor of 50 as well. If you are worried about jitter then you need to convert delta time to a relative sample time. I would use the formula instead.

float deltaTime = (LastCorrectionTime - DateTime.Now.Ticks) / 500000.0

the 500000.0 is the number of expected ticks per sample which for 20Hz is 50ms.

2) Keep the integral term within a range.

if ( SteadyError > MaxSteadyError ) SteadyError = MaxSteadyError;
if ( SteadyError < MinSteadyError ) SteadyError = MinSteadyError;

3) Change the following code so that when error is around -180 you do not get a step in error with a small change.

if (error < -270) error += 360;
if (error >  270) error -= 360;

4) Verify Steering.Degree is receiving the correct resolution and sign.

5) Lastly yo can probably just drop deltaTime all together and calculate the differential term the following way.

float derivative = Kd * (error - PrevError);

With all of that your code becomes.

private static void DoPID(object o)
{
    // Bring the LED up to signify frame start
    BoardLED.Write(true);

    // Get IMU heading
    float currentHeading = (float)RazorIMU.Yaw;


    // Calculate error
    // (let's just assume CurrentHeading really is the current GPS heading, OK?)
    float error = (TargetHeading - currentHeading);

    LCD.Lines[0].Text = "Heading: "+ currentHeading.ToString("F2");

    // We calculated the error, but we need to make sure the error is set 
    // so that we will be correcting in the 
    // direction of least work. For example, if we are flying a heading 
    // of 2 degrees and the error is a few degrees
    // to the left of that ( IE, somewhere around 360) there will be a 
    // large error and the rover will try to turn all
    // the way around to correct, when it could just turn to the right 
    // a few degrees.
    // In short, we are adjusting for the fact that a compass heading wraps 
    // around in a circle instead of continuing infinity on a line
    if (error < -270) error += 360;
    if (error >  270) error -= 360;

    // Add the error calculated in this frame to the running total
    SteadyError += error;

    if ( SteadyError > MaxSteadyError ) SteadyError = MaxSteadyError;
    if ( SteadyError < MinSteadyError ) SteadyError = MinSteadyError;

    LCD.Lines[2].Text = "Error:   " + error.ToString("F2");

    // Calculate proportional term
    float proportional = Kp * error;

    // Calculate integral term
    float integral = Ki * SteadyError ;

    // Calculate derivative term
    float derivative = Kd * (error - PrevError) ;

    // Add them all together to get the correction delta
    // Set the steering servo to the correction
    Steering.Degree = 90 + proportional + integral + derivative;

    // At this point, the current PID frame is finished
    // ------------------------------------------------------------
    // Now, we need to setup for the next PID frame and close out

    // The "current" error is now the previous error
    // (Remember, we are done with the current frame, so in
    // relative terms, the previous frame IS the "current" frame)
    PrevError = error;

    // Done
    BoardLED.Write(false);
}