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);
}

Answer 2

I'm not sure why your code isn't working, but I'm almost positive you can't test it to see why, either. You might inject a timer service so you can mock it out and see what's happening:

public interace ITimer 
{
     long GetCurrentTicks()
}

public class Timer : ITimer
{
    public long GetCurrentTicks() 
    {
        return DateTime.Now.Ticks;
    }
}

public class TestTimer : ITimer
{
    private bool firstCall = true;
    private long last;
    private int counter = 1000000000;

    public long GetCurrentTicks()
    {
        if (firstCall)
            last = counter * 10000;
        else
            last += 3500;  //ticks; not sure what a good value is here

        //set up for next call;
        firstCall = !firstCall;
        counter++;

        return last;
    }
}

Then, replace both calls to DateTime.Now.Ticks with GetCurrentTicks(), and you can step through the code and see what the values look like.

Answer 3

The integral term is already accumulated over time, multiplying by deltaTime will make it accumulate at a rate of time-squared. In fact since SteadyError is already erroneously calculated by multiplying error by deltaTime, that is time-cubed!

In SteadyError, if you are trying to compensate for an aperiodic update, it would be better to fix the aperiodicity. However, the calculation is flawed in any case. You have calculated in units of error/time whereas you want just error units. The arithmentiaclly correct way to compensate for timing jitter if really necessary would be:

SteadyError += (error * 50.0f/deltaTime);

if deltaTime remains in milliseconds and the nominal update rate is 20Hz. However deltaTime would be better calculated as a float or not converted to milliseconds at all if it is timing jitter you are trying to detect; you are needlessly discarding precision. Either way what you need is to modify the error value by the ratio of nominal time to actual time.

A good read is PID without a PhD

Answer 4

Are you initializing SteadyError (bizarre name...why not "integrator")? If it contains some random value on start-up it might never return to near zero (1e100 + 1 == 1e100).

You might be suffering from integrator windup, which ordinarily should go away, but not if it takes longer to diminish than it does for your vehicle to complete a full rotation (and windup the integrator again). The trivial solution is to impose limits on the integrator, though there are more advanced solutions (PDF, 879 kB) if your system requires.

Does Ki have the correct sign?

I would strongly discourage the use of floats for PID parameters because of their arbitrary precision. Use integers (maybe fixed point). You will have to impose limit checking, but it will be much more sane than using floats.