Quadcopter PID Controller: Derivative on Measurement / Removing the Derivative Kick

Question

Good day,

I am currently implementing a single loop PID controller using angle setpoints as inputs. I was trying out a different approach for the D part of the PID controller.

What bought this about is that when I was able to reach a 200Hz (0.00419ms) loop rate, when adding a D gain, the quadcopter seems to dampen the movements in a non continous manner. This was not the case when my algorithm was running at around 10Hz. At an angle set point of 0 degrees, I would try to push it to one side by 5 degrees then the quad would try to stay rock solid by resisting the movements but lets go after while enabling me to get it of by 2 degrees (the dampening effect weakens over time) then tries to dampen the motion again.

This is my implementation of the traditional PID:

Derivative on Error:

//Calculate Orientation Error (current - target)
float pitchError = pitchAngleCF - pitchTarget;
pitchErrorSum += (pitchError*deltaTime2);
float pitchErrorDiff = pitchError - pitchPrevError;
pitchPrevError = pitchError;

float rollError = rollAngleCF - rollTarget;
rollErrorSum += (rollError*deltaTime2);
float rollErrorDiff = rollError - rollPrevError;
rollPrevError = rollError;

float yawError = yawAngleCF - yawTarget;
yawErrorSum += (yawError*deltaTime2);
float yawErrorDiff = yawError - yawPrevError;
yawPrevError = yawError;


//PID controller list
float pitchPID = pitchKp*pitchError + pitchKi*pitchErrorSum + pitchKd*pitchErrorDiff/deltaTime2;
float rollPID = rollKp*rollError + rollKi*rollErrorSum + rollKd*rollErrorDiff/deltaTime2;
float yawPID = yawKp*yawError + yawKi*yawErrorSum + yawKd*yawErrorDiff/deltaTime2;

//Motor Control - Mixing    
//Motor Front Left (1)
float motorPwm1 =  -pitchPID + rollPID - yawPID + baseThrottle + baseCompensation;

What I tried to do now is to implement a derivative on measurement method from this article to remove derivative output spikes. However the Derivative part seems to increase the corrective force than dampen it.

Derivative on Measurement:

//Calculate Orientation Error (current - target)
float pitchError = pitchAngleCF - pitchTarget;
pitchErrorSum += (pitchError*deltaTime2);
float pitchErrorDiff = pitchAngleCF - pitchPrevAngleCF; // <----
pitchPrevAngleCF = pitchAngleCF;

float rollError = rollAngleCF - rollTarget;
rollErrorSum += (rollError*deltaTime2);
float rollErrorDiff = rollAngleCF - rollPrevAngleCF; // <----
rollPrevAngleCF = rollAngleCF;

float yawError = yawAngleCF - yawTarget;
yawErrorSum += (yawError*deltaTime2);
float yawErrorDiff = yawAngleCF - yawPrevAngleCF; // <----
yawPrevAngleCF = yawAngleCF;


//PID controller list // <---- The D terms are now negative
float pitchPID = pitchKp*pitchError + pitchKi*pitchErrorSum - pitchKd*pitchErrorDiff/deltaTime2;
float rollPID = rollKp*rollError + rollKi*rollErrorSum - rollKd*rollErrorDiff/deltaTime2;
float yawPID = yawKp*yawError + yawKi*yawErrorSum - yawKd*yawErrorDiff/deltaTime2;


//Motor Control - Mixing    
//Motor Front Left (1)
float motorPwm1 =  -pitchPID + rollPID - yawPID + baseThrottle + baseCompensation;

My question now is:

Is there something wrong with my implementation of the second method?

Source: http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/

The way I've obtained the change in time or DT is by taking the timestamp from the start of the loop then taking the next time stamp at the end of the loop. Their difference is obtained to obtain the DT. getTickCount() is an OpenCV function.

/* Initialize I2c */  
/* Open Files for data logging */ 
while(1){
    deltaTimeInit=(float)getTickCount();

    /* Get IMU data */
    /* Filter using Complementary Filter */
    /* Compute Errors for PID */
    /* Update PWM's */

    //Terminate Program after 40 seconds
    if((((float)getTickCount()-startTime)/(((float)getTickFrequency())))>20){
            float stopTime=((float)getTickCount()-startTime)/((float)getTickFrequency());
    gpioPWM(24,0);  //1
    gpioPWM(17,0);  //2
    gpioPWM(22,0);  //3
    gpioPWM(18,0);  //4
    gpioTerminate(); 
        int i=0;
    for (i=0 ; i < arrPitchCF.size(); i++){
        file8 << arrPitchCF.at(i) << endl;
    }

    for (i=0 ; i < arrYawCF.size(); i++){
        file9 << arrYawCF.at(i) << endl;
    }

    for (i=0 ; i < arrRollCF.size(); i++){
        file10 << arrRollCF.at(i) << endl;
    }

    for (i=0 ; i < arrPitchAccel.size(); i++){
        file2 << arrPitchAccel.at(i) << endl;
    }

    for (i=0 ; i < arrYawAccel.size(); i++){
        file3 << arrYawAccel.at(i) << endl;
    }

    for (i=0 ; i < arrRollAccel.size(); i++){
        file4 << arrRollAccel.at(i) << endl;
    }

    for (i=0 ; i < arrPitchGyro.size(); i++){
        file5 << arrPitchGyro.at(i) << endl;
    }

    for (i=0 ; i < arrYawGyro.size(); i++){
        file6 << arrYawGyro.at(i) << endl;
    }

    for (i=0 ; i < arrRollGyro.size(); i++){
        file7 << arrRollGyro.at(i) << endl;
    }

    for (i=0 ; i < arrPWM1.size(); i++){
        file11 << arrPWM1.at(i) << endl;
    }

    for (i=0 ; i < arrPWM2.size(); i++){
        file12 << arrPWM2.at(i) << endl;
    }

    for (i=0 ; i < arrPWM3.size(); i++){
        file13 << arrPWM3.at(i) << endl;
    }

    for (i=0 ; i < arrPWM4.size(); i++){
        file14 << arrPWM4.at(i) << endl;
    }

    for (i=0 ; i < arrPerr.size(); i++){
        file15 << arrPerr.at(i) << endl;
    }

    for (i=0 ; i < arrDerr.size(); i++){
        file16 << arrDerr.at(i) << endl;
    }


        file2.close();
        file3.close();
        file4.close();
        file5.close();
        file6.close();
        file7.close();
        file8.close();
        file9.close();
        file10.close();
        file11.close();
        file12.close();
        file13.close();
        file14.close();
        file15.close();
        file16.close();
        cout << " Time Elapsed = " << stopTime << endl;
        break;
    }

    while((((float)getTickCount()-deltaTimeInit)/(((float)getTickFrequency())))<=0.00419){ //0.00209715|0.00419
       cout << " DT end = " << deltaTime2 << endl;
       deltaTime2=((float)getTickCount()-deltaTimeInit)/(((float)getTickFrequency()));
    }

    cout << " DT end = " << deltaTime2 << endl;
}

Here's my data:

Answer 1

All PID controllers act on an error signal, so you definitely should stick with your first implementation. The derivative term in the PID controller wants to "see" how the error is changing - if the error is getting bigger, then the derivative term gets larger and "kicks in" a higher contribution. If the error is getting smaller, then the derivative term backs off.

Your problem description is vague; something about "pushing" the quadcopter and damping and feeling something.

Engineers don't have feelings, they have data.

For the moment, with no data, all I can tell you is to reuse your first implementation and double check how you are acquiring your deltaTime2 information. Typically, in industrial controls, the derivative term is left off of the controller and only a PI controller is implemented. This is because any bit of temporal sampling problems (not taking the sample on time or mis-judging the sampling rate) can cause huge problems with the derivative term.

Imagine your sampling rate is supposed to be 0.005 seconds (200Hz). If instead of that sampling rate, you mis-judged the sample rate to be 0.006 seconds for one sample, then the following happens:

1. No change to proportional error because it doesn't interact with the sampling rate.
2. Integral error multiplies proportional error by this sampling rate and then adds it to all of the previous integral error samples, so overall it's a pretty small impact.
3. Derivative error divides proportional error by this sampling rate with no other means of context.

So, conceivably, you could have something like:

prevIntError = 1.8;
prevDerError = 1.1
PropError = 1;
IntError = prevIntError + 1*0.006;
derError = (1 - 1.1)/0.006;

So where the integral error is now 1.806 instead of 1.805 (an error of 0.05%), your derivative error is -16.6 instead of what it should be, (1 - 1.1)/0.005, which is -20. That means that error in the proportional error signal is 0, the error in the integral error signal is 0.05%, and the error in the derivative error signal is 17%!

As your sampling rate increases, your sampling period goes down. Errors in timing can begin to become significant after a point. For example, at 10Hz, you are sampling every 0.1s. If your timing accuracy is +/- 0.001s, then this means that your time sampling error is +/- 1%. If you push to 200Hz (0.005s), and your timing accuracy is still +/- 0.001s, then suddenly your time sampling error is +/- 20%!

So, in summary:

1. Post data, not feelings.
2. Revert to your first implementation.
3. Check how you acquire deltaTime2 and report back.

Please update your question to include the answers to 1 and 3 (don't post it as a comment to this answer please).

:EDIT:

I would move your timestamp evaluation to one location in your code, and I would put that location immediately before you use it. At the moment, you don't update the sampling time until after you've done your derivative/integral calculations. This is even more problematic given the fact that you are performing communications in the same sampling window.

Say your first sweep, you read some data, do your calculations, and that sample time is 5ms. Then, on the second sweep, something happens to the communications (no data, data error, IMU reset, etc.). The time it takes to get back to the derivative/integral calculations won't be 5ms, it'll be 1ms or 10ms or something similar. So you use the wrong sample time for that sample, but then your sample time gets updated after the fact to reflect that sweep took the 10ms or whatever, then that gets used on the following sample, which again may not be correct.

I would revise your code to the following:

/* Initialize I2c */  
/* Open Files for data logging */ 
deltaTimeInit=(float)getTickCount(); //<--- Placeholder value for first pass
while(1){
    /* Get IMU data */
    deltaTime2=((float)getTickCount()-deltaTimeInit)/(((float)getTickFrequency())); //<--- Get the time that elapsed to NOW
    deltaTimeInit=(float)getTickCount(); //<--- Start the timer for the next loop
    /* Filter using Complementary Filter */
    /* Compute Errors for PID */ //<--- Use the actual time elapsed for the calculations
    /* Update PWM's */

  /* (more stuff) */
}

Basically, you care about the time that has elapsed from when you get data to the next time that you get data, and you want to act on current information. Your sample time should be calculated before you use it, on the current sweep.