From 2453e92bb3b84ffd6e6bbbbe814d49eee2ef03f2 Mon Sep 17 00:00:00 2001 From: Andrei Date: Sat, 19 Oct 2019 15:56:17 +0300 Subject: [PATCH] Version 0.2: modelBias, pid.offset --- Makefile | 2 + global.h | 2 + pid_auto.h | 136 ++++++++++++++++++-------------- pid_from_msl.cpp | 92 +++++++++++++++------- pid_functions.h | 54 +++++++------ pid_open_loop_models.h | 174 +++++++++++++++++++++-------------------- pid_sim.h | 65 +++++++++++---- test_chs_pid.cpp | 84 ++++++++++++++------ 8 files changed, 373 insertions(+), 236 deletions(-) diff --git a/Makefile b/Makefile index 32d048f..da13b51 100644 --- a/Makefile +++ b/Makefile @@ -1,2 +1,4 @@ pid_from_msl: pid_from_msl.cpp + @if [ ! -d "googletest" ]; then echo "Error! googletest not found! Put it into this folder!"; exit 1; fi g++ -std=c++17 -fpermissive -O3 -static -o pid_from_msl.exe pid_from_msl.cpp googletest/gtest-all.cpp googletest/gmock-all.cpp -Igoogletest/googletest -Igoogletest/googletest/include -Igoogletest/googlemock/include + diff --git a/global.h b/global.h index da2a267..cca12ad 100644 --- a/global.h +++ b/global.h @@ -4,6 +4,7 @@ #include +#include #include #include #include @@ -19,6 +20,7 @@ struct pid_s { float iFactor; float dFactor; float offset; + float periodMs; int16_t minValue; int16_t maxValue; diff --git a/pid_auto.h b/pid_auto.h index 505db72..8c9dc44 100644 --- a/pid_auto.h +++ b/pid_auto.h @@ -28,6 +28,7 @@ public: double minValue, maxValue; double stepPoint, maxPoint; double timeScale; + double targetValue; }; // PID auto-tune method using different tuning rules and FOPDT/SOPDT models @@ -45,21 +46,21 @@ public: // stepPoint - data index where the step was done // maxPoint - data index where the output was saturated // this is not thread-safe (because of internal static vars) but anyway it works... - bool findPid(PidTuneMethod method, const PidAutoTuneSettings & settings, const double *initialParams) { - // save current settings + bool findPid(pid_sim_type_e simType, pid_tune_method_e method, const PidAutoTuneSettings & settings, const double *initialParams) { + // save current settings & simType this->settings = settings; - // without offset we cannot fit the analytic curve - double offset = findOffset(); + this->simType = simType; + // without bias we cannot fit the analytic curve + modelBias = findModelBias(); - pid.offset = (float)offset; // todo: where do we get them? pid.minValue = 0; pid.maxValue = 100; #ifdef PID_DEBUG - printf("* offset=%g avgMin=%g avgMax=%g\r\n", offset, avgMeasuredMin, avgMeasuredMax); + printf("* modelBias=%g avgMin=%g avgMax=%g\r\n", modelBias, avgMeasuredMin, avgMeasuredMax); #endif - StepFunction stepFunc(settings.minValue, settings.maxValue, offset, settings.stepPoint, settings.timeScale); + StepFunction stepFunc(settings.minValue, settings.maxValue, settings.stepPoint, settings.timeScale); int methodOrder = getMethodOrder(method); @@ -89,47 +90,39 @@ public: memcpy(params, initialParams, sizeof(params)); } - // try to solve several times unless we see some progress (iterationCount > 1) - for (double minParamT = minParamT0; minParamT <= minParamT1; minParamT *= 10.0) { - double merit0, merit; + double merit0, merit; #ifdef PID_DEBUG - printf("* Solving with minParamT=%g...\r\n", minParamT); + printf("* Solving...\r\n"); #endif - // create and start solver - if (methodOrder == 1) { - // 1st order - FirstOrderPlusDelayLineFunction func(&stepFunc, measuredData.getBuf(), measuredData.getNumDataPoints(), minParamT); - func.justifyParams(params); - const int numParams1stOrder = 3; - outputFunc("pid_func0.csv", func, stepFunc, params); - LevenbergMarquardtSolver solver(&func, params); - merit0 = func.calcMerit(params); - iterationCount = solver.solve(minParamT); - outputFunc("pid_func.csv", func, stepFunc, params); - merit = func.calcMerit(params); - } - else { - // 2nd order or approximated 2nd->1st order - SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, measuredData.getBuf(), measuredData.getNumDataPoints(), minParamT); - func.justifyParams(params); - const int numParams2ndOrder = 4; - outputFunc("pid_func0.csv", func, stepFunc, params); - LevenbergMarquardtSolver solver(&func, params); - merit0 = func.calcMerit(params); - iterationCount = solver.solve(minParamT); - outputFunc("pid_func.csv", func, stepFunc, params); - merit = func.calcMerit(params); - } + // create and start solver + if (methodOrder == 1) { + // 1st order + FirstOrderPlusDelayLineFunction func(&stepFunc, measuredData.getBuf(), measuredData.getNumDataPoints(), modelBias); + func.justifyParams(params); + const int numParams1stOrder = 3; + outputFunc("pid_func01.csv", func, stepFunc, params); + LevenbergMarquardtSolver solver(&func, params); + merit0 = func.calcMerit(params); + iterationCount = solver.solve(minParamT); + outputFunc("pid_func1.csv", func, stepFunc, params); + merit = func.calcMerit(params); + } + else { + // 2nd order or approximated 2nd->1st order + SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, measuredData.getBuf(), measuredData.getNumDataPoints(), modelBias); + func.justifyParams(params); + const int numParams2ndOrder = 4; + outputFunc("pid_func02.csv", func, stepFunc, params); + LevenbergMarquardtSolver solver(&func, params); + merit0 = func.calcMerit(params); + iterationCount = solver.solve(minParamT); + outputFunc("pid_func2.csv", func, stepFunc, params); + merit = func.calcMerit(params); + } #ifdef PID_DEBUG - if (iterationCount > 0) - printf("* The solver finished in %d iterations! Merit (%g -> %g)\r\n", iterationCount, merit0, merit); - else - printf("* The solver aborted after %d iterations! Merit (%g -> %g)\r\n", -iterationCount, merit0, merit); + printSolverResult(iterationCount, merit0, merit); #endif - if (abs(iterationCount) > 1) - break; - } ModelOpenLoopPlant *model = nullptr; switch (method) { @@ -141,7 +134,7 @@ public: } case PID_TUNE_IMC2_1: { ModelFopdtApproximatedFromSopdt fo(params); - static ModelImcPidFirstOrder imc(fo.getParams()); + static ModelRiveraMorariFirstOrder imc(fo.getParams()); model = &imc; break; } @@ -173,14 +166,13 @@ public: pid.pFactor = model->getKp(); pid.iFactor = model->getKi(); pid.dFactor = model->getKd(); + pid.offset = getPidOffset(model); + pid.periodMs = 1000.0 / settings.timeScale; pid0 = pid; - // for "automatic" methods, we try to make coefs even better! + // for "automatic" methods, we try to make the coeffs even better! if (method == PID_TUNE_AUTO1 || method == PID_TUNE_AUTO2) { -#ifdef PID_DEBUG - printf("* Solving for better coefs:\r\n"); -#endif ModelAutoSolver autoSolver(model); solveModel(method, autoSolver); pid.pFactor = autoSolver.getKp(); @@ -191,7 +183,7 @@ public: return true; } - int getMethodOrder(PidTuneMethod method) { + int getMethodOrder(pid_tune_method_e method) { switch (method) { // 1st order case PID_TUNE_CHR1: @@ -223,7 +215,12 @@ public: return avgMeasuredMax; } - double findOffset() { + double getModelBias() const { + return modelBias; + } + + // The model output is typically shifted + double findModelBias() { if (settings.stepPoint < 0 || settings.maxPoint <= settings.stepPoint || settings.maxPoint > measuredData.getNumDataPoints()) return 0; // find the real 'min value' of the measured output data (before the step function goes up). @@ -233,8 +230,15 @@ public: if (avgMeasuredMax == avgMeasuredMin) return 0; - // solve the system of equations and find the offset - return (settings.maxValue * avgMeasuredMin - settings.minValue * avgMeasuredMax) / (avgMeasuredMax - avgMeasuredMin); + // solve the system of equations and find the bias + return (settings.maxValue * avgMeasuredMin - settings.minValue * avgMeasuredMax) / (settings.maxValue - settings.minValue); + } + + // If the target value is known, we can estimate the PID offset value based on the model gain and bias. + float getPidOffset(ModelOpenLoopPlant *model) const { + if (settings.targetValue == 0.0) + return 0; + return (settings.targetValue - modelBias) / model->getParams()[PARAM_K]; } // See: Rangaiah G.P., Krishnaswamy P.R. Estimating Second-Order plus Dead Time Model Parameters, 1994. @@ -353,21 +357,27 @@ public: // Use automatic LM-solver to find the best PID coefs which satisfy the minimal PID metric. // The initial PID coefs are already calculated using the well-known CHR method (1st or 2nd order). - bool solveModel(PidTuneMethod method, ModelAutoSolver & model) { + bool solveModel(pid_tune_method_e method, ModelAutoSolver & model) { + double merit0, merit; + +#ifdef PID_DEBUG + printf("* Solving for better coefs:\r\n"); +#endif // todo: is it correct? double dTime = 1.0 / settings.timeScale; const int numSimPoints = 1024; - PidSimulatorFactory simFactory(getMethodOrder(method), getAvgMeasuredMin(), getAvgMeasuredMax(), dTime, pid); + PidSimulatorFactory simFactory(simType, getMethodOrder(method), getAvgMeasuredMin(), getAvgMeasuredMax(), dTime, modelBias, pid); PidCoefsFinderFunction func(&simFactory, params); + func.justifyParams(model.getParams()); + merit0 = func.calcMerit(model.getParams()); // now hopefully we'll find even better coefs! LevenbergMarquardtSolver solver((LMSFunction *)&func, model.getParams()); int iterationCount = solver.solve(); + + merit = func.calcMerit(model.getParams()); #ifdef PID_DEBUG - if (iterationCount > 0) - printf("* The solver finished in %d iterations!\r\n", iterationCount); - else - printf("* The solver aborted after %d iterations!\r\n", -iterationCount); + printSolverResult(iterationCount, merit0, merit); #endif return true; } @@ -383,13 +393,23 @@ public: #endif } + void printSolverResult(int iterationCount, double merit0, double merit) { + if (iterationCount > 0) + printf("* The solver finished in %d iterations! (Merit: %g -> %g)\r\n", iterationCount, merit0, merit); + else + printf("* The solver aborted after %d iterations! (Merit: %g -> %g)\r\n", -iterationCount, merit0, merit); + } + + protected: AveragingDataBuffer measuredData; PidAutoTuneSettings settings; + pid_sim_type_e simType; double params[4] = { 0 }; pid_s pid; pid_s pid0; // not-optimized int iterationCount = 0; double avgMeasuredMin, avgMeasuredMax; + double modelBias; }; diff --git a/pid_from_msl.cpp b/pid_from_msl.cpp index b43cf5e..d0fe682 100644 --- a/pid_from_msl.cpp +++ b/pid_from_msl.cpp @@ -28,7 +28,7 @@ public: std::string str; for (int i = 0; std::getline(fp, str); i++) { - // data starts at 4th line + // data starts on the 4th line if (i < 4) continue; parseLine(str, startTime, endTime, inputIdx, outputIdx); @@ -67,10 +67,6 @@ public: } curIdx++; } else if (j == outputIdx) { - /*const float alpha = 0.5f; - float fv = alpha * prevV + (1.0f - alpha) * (float)v; - prevV = v; - data.push_back(fv);*/ data.push_back((float)v); if (curIdx >= 0 && settings.stepPoint < 0) { // calculate averaged level to determine the acceptable noise level @@ -118,6 +114,28 @@ public: double averagedMin = 0; }; +const char *getMethodName(pid_tune_method_e method) { + switch (method) { + case PID_TUNE_CHR1: return "CHR1"; + case PID_TUNE_AUTO1: return "AUTO1"; + case PID_TUNE_IMC2_1: return "IMC2_1"; + case PID_TUNE_CHR2_1: return "CHR2_1"; + case PID_TUNE_CHR2: return "CHR2"; + case PID_TUNE_VDG2: return "VDG2"; + case PID_TUNE_HP2: return "HP2"; + case PID_TUNE_AUTO2: return "AUTO2"; + } + return "(N/A)"; +} + +const char *getSimTypeName(pid_sim_type_e simType) { + switch (simType) { + case PID_SIM_REGULATOR: return "Regulator"; + case PID_SIM_SERVO: return "Servo"; + } + return "(N/A)"; +} + #if 1 int main(int argc, char **argv) { if (argc < 6) { @@ -126,32 +144,40 @@ int main(int argc, char **argv) { } printf("PID_FROM_MSL - find PID controller coefficients based on a measured step response in a rusEFI log file.\r\n"); - printf("Version 0.1 (c) andreika, 2019\r\n\r\n"); + printf("Version 0.2 (c) andreika, 2019\r\n\r\n"); printf("Reading file %s...\r\n", argv[1]); MslData data; if (!data.readMsl(argv[1], atof(argv[2]), atof(argv[3]), atoi(argv[4]), atoi(argv[5]))) { + printf("Usage: PID_FROM_MSL []\r\n"); return -2; } - printf("Measuring Settings: minValue=%g maxValue=%g stepPoint=%g maxPoint=%g numPoints=%d timeScale=%g\r\n", - data.settings.minValue, data.settings.maxValue, data.settings.stepPoint, data.settings.maxPoint, data.data.size(), data.settings.timeScale); + // Target value is optional, for PID_SIM_REGULATOR only + data.settings.targetValue = (argc > 6) ? atof(argv[6]) : 0.0; + + printf("Measuring Settings: targetValue=%g minValue=%g maxValue=%g stepPoint=%g maxPoint=%g numPoints=%d timeScale=%g\r\n", + data.settings.targetValue, data.settings.minValue, data.settings.maxValue, + data.settings.stepPoint, data.settings.maxPoint, data.data.size(), data.settings.timeScale); PidAutoTune chr1, chr2; - + static const int numPids = 2; + PidAutoTune *chr[numPids] = { &chr1, &chr2 }; + for (size_t i = 0; i < data.data.size(); i++) { - chr1.addData(data.data[i]); - chr2.addData(data.data[i]); + for (int j = 0; j < numPids; j++) { + chr[j]->addData(data.data[i]); + } } // todo: more flexible method chooser - PidTuneMethod method = PID_TUNE_AUTO1; - printf("\r\nTrying method Auto1:\r\n"); - chr1.findPid(method, data.settings, nullptr); + pid_sim_type_e simTypes[numPids] = { PID_SIM_REGULATOR, PID_SIM_REGULATOR }; + pid_tune_method_e methods[numPids] = { PID_TUNE_AUTO1, PID_TUNE_AUTO2 }; - method = PID_TUNE_AUTO2; - printf("\r\nTrying method Auto2:\r\n"); - chr2.findPid(method, data.settings, nullptr); + for (int k = 0; k < numPids; k++) { + printf("\r\n%d) Trying method %s on \"%s\" PID model:\r\n", k + 1, getMethodName(methods[k]), getSimTypeName(simTypes[k])); + chr[k]->findPid(simTypes[k], methods[k], data.settings, nullptr); + } printf("Done!\r\n"); @@ -159,23 +185,35 @@ int main(int argc, char **argv) { double dTime = 1.0 / data.settings.timeScale; const int numSimPoints = 1024; - PidAutoTune *chr[2] = { &chr1, &chr2 }; + pid_s bestPid; + double smallestItae = DBL_MAX; for (int k = 0; k < 2; k++) { const double *p = chr[k]->getParams(); printf("Model-%d Params: K=%g T1=%g T2=%g L=%g\r\n", (k + 1), p[PARAM_K], p[PARAM_T], p[PARAM_T2], p[PARAM_L]); - const pid_s pid0 = chr[k]->getPid0(); - const pid_s pid = chr[k]->getPid(); - printf(" PID0: P=%.8f I=%.8f D=%.8f offset=%.8f\r\n", pid0.pFactor, pid0.iFactor, pid0.dFactor, pid0.offset); - PidSimulator sim(chr[k]->getMethodOrder(method), chr[k]->getAvgMeasuredMin(), chr[k]->getAvgMeasuredMax(), dTime, true); - PidAccuracyMetric metric0 = sim.simulate(numSimPoints, pid0, p); - printf(" Metric0 result: ITAE=%g ISE=%g Overshoot=%g%%\r\n", metric0.getItae(), metric0.getIse(), metric0.getMaxOvershoot() * 100.0); + pid_s pid[2] = { chr[k]->getPid0(), chr[k]->getPid() }; + for (int j = 0; j < 2; j++) { - printf(" PID: P=%.8f I=%.8f D=%.8f offset=%.8f\r\n", pid.pFactor, pid.iFactor, pid.dFactor, pid.offset); - PidAccuracyMetric metric = sim.simulate(numSimPoints, pid, p); - printf(" Metric result: ITAE=%g ISE=%g Overshoot=%g%%\r\n", metric.getItae(), metric.getIse(), metric.getMaxOvershoot() * 100.0); + const char *csvName = (j == 0) ? ((k == 0) ? "pid_test01.csv" : "pid_test02.csv") : ((k == 0) ? "pid_test1.csv" : "pid_test2.csv"); + + PidSimulator sim1(simTypes[k], chr[k]->getMethodOrder(methods[k]), + chr[k]->getAvgMeasuredMin(), chr[k]->getAvgMeasuredMax(), dTime, chr[k]->getModelBias(), csvName); + + PidAccuracyMetric metric = sim1.simulate(numSimPoints, pid[j], p); + + printf(" PID%d: P=%.8f I=%.8f D=%.8f offset=%.8f period=%.8fms\r\n", j, pid[j].pFactor, pid[j].iFactor, pid[j].dFactor, pid[j].offset, + pid[j].periodMs); + printf(" Metric%d result: ITAE=%g ISE=%g Overshoot=%g%%\r\n", j, metric.getItae(), metric.getIse(), metric.getMaxOvershoot() * 100.0); + + if (metric.getItae() < smallestItae) { + smallestItae = metric.getItae(); + bestPid = pid[j]; + } + } } + printf("The best PID: P=%.8f I=%.8f D=%.8f offset=%.1f period=%.1fms\r\n", bestPid.pFactor, bestPid.iFactor, bestPid.dFactor, bestPid.offset, bestPid.periodMs); + return 0; } #endif diff --git a/pid_functions.h b/pid_functions.h index cdbdbf3..67a4a1d 100644 --- a/pid_functions.h +++ b/pid_functions.h @@ -37,11 +37,12 @@ enum { const int numParamsForPid = 3; // the limit used for K and L params -static const double minParamKL = 0.00001; +static const double minParamK = 0.001; +static const double minParamL = 0.1; // T (or T2) has a separate limit, because exp(-1/T) is zero for small T values // (and we need to compare between them to find a direction for optimization method). -static const double minParamT0 = 0.0001, minParamT1 = 0.01; +static const double minParamT = 0.01; class InputFunction { public: @@ -65,8 +66,8 @@ protected: class StepFunction : public InputFunction { public: - StepFunction(double minValue_, double maxValue_, double offset_, double stepPoint_, double timeScale_) : - minValue(minValue_), maxValue(maxValue_), offset(offset_), stepPoint(stepPoint_), InputFunction(timeScale_) { + StepFunction(double minValue_, double maxValue_, double stepPoint_, double timeScale_) : + minValue(minValue_), maxValue(maxValue_), stepPoint(stepPoint_), InputFunction(timeScale_) { } virtual double getValue(double i, double d) const { @@ -80,9 +81,9 @@ public: double vNear = (I < stepPoint) ? minValue : maxValue; double vFar = (I + 1 < stepPoint) ? minValue : maxValue; // interpolate - return offset + vFar * fract + vNear * (1.0f - fract); + return vFar * fract + vNear * (1.0f - fract); #else - return offset + ((id < stepPoint) ? minValue : maxValue); + return ((id < stepPoint) ? minValue : maxValue); #endif } @@ -90,8 +91,6 @@ private: double minValue, maxValue; // stepPoint is float because we have AveragingDataBuffer, and the time axis may be scaled double stepPoint; - // needed to use PARAM_K coefficient properly; also offset is used by PID - double offset; }; template @@ -123,11 +122,11 @@ private: template class AbstractDelayLineFunction : public LMSFunction { public: - AbstractDelayLineFunction(const InputFunction *input, const float *measuredOutput, int numDataPoints, double minParamT) { + AbstractDelayLineFunction(const InputFunction *input, const float *measuredOutput, int numDataPoints, double modelBias) { dataPoints = measuredOutput; inputFunc = input; numPoints = numDataPoints; - this->minParamT = minParamT; + this->modelBias = modelBias; } virtual double getResidual(int i, const double *params) const { @@ -149,7 +148,8 @@ protected: const InputFunction *inputFunc; const float *dataPoints; int numPoints; - double minParamT; + // needed to match the "ideal" curve and the real plant data; it doesn't affect the params but helps to fit the curve. + double modelBias; }; // FODPT indirect transfer function used for step response analytic simulation. @@ -157,22 +157,22 @@ protected: // The Laplace representation is: K * exp(-L*s) / (T*s + 1) class FirstOrderPlusDelayLineFunction : public AbstractDelayLineFunction<3> { public: - FirstOrderPlusDelayLineFunction(const InputFunction *input, const float *measuredOutput, int numDataPoints, double minParamT) : - AbstractDelayLineFunction(input, measuredOutput, numDataPoints, minParamT) { + FirstOrderPlusDelayLineFunction(const InputFunction *input, const float *measuredOutput, int numDataPoints, double modelBias) : + AbstractDelayLineFunction(input, measuredOutput, numDataPoints, modelBias) { } virtual void justifyParams(double *params) const { - params[PARAM_L] = fmax(params[PARAM_L], minParamKL); + params[PARAM_L] = fmax(params[PARAM_L], minParamL); params[PARAM_T] = fmax(params[PARAM_T], minParamT); - params[PARAM_K] = (fabs(params[PARAM_K]) < minParamKL) ? minParamKL : params[PARAM_K]; + params[PARAM_K] = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K]; } // Creating a state-space representation using Rosenbrock system matrix virtual double getEstimatedValueAtPoint(int i, const double *params) const { // only positive values allowed (todo: choose the limits) - double pL = fmax(params[PARAM_L], minParamKL); + double pL = fmax(params[PARAM_L], minParamL); double pT = fmax(params[PARAM_T], minParamT); - double pK = (fabs(params[PARAM_K]) < minParamKL) ? minParamKL : params[PARAM_K]; + double pK = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K]; // state-space params double lambda = exp(-1.0 / (pT * inputFunc->getTimeScale())); @@ -188,7 +188,9 @@ public: // indirect model response in Controllable Canonical Form (1st order CCF) y = lambda * y + pK * (1.0 - lambda) * inp; } - return y; + + // the output can be biased + return y + modelBias; } }; @@ -198,24 +200,24 @@ public: // The Laplace representation is: K * exp(-L * s) / ((T1*T2)*s^2 + (T1+T2)*s + 1) class SecondOrderPlusDelayLineOverdampedFunction : public AbstractDelayLineFunction<4> { public: - SecondOrderPlusDelayLineOverdampedFunction(const InputFunction *input, const float *measuredOutput, int numDataPoints, double minParamT) : - AbstractDelayLineFunction(input, measuredOutput, numDataPoints, minParamT) { + SecondOrderPlusDelayLineOverdampedFunction(const InputFunction *input, const float *measuredOutput, int numDataPoints, double modelBias) : + AbstractDelayLineFunction(input, measuredOutput, numDataPoints, modelBias) { } virtual void justifyParams(double *params) const { - params[PARAM_L] = fmax(params[PARAM_L], minParamKL); + params[PARAM_L] = fmax(params[PARAM_L], minParamL); params[PARAM_T] = fmax(params[PARAM_T], minParamT); params[PARAM_T2] = fmax(params[PARAM_T2], minParamT); - params[PARAM_K] = (fabs(params[PARAM_K]) < minParamKL) ? minParamKL : params[PARAM_K]; + params[PARAM_K] = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K]; } // Creating a state-space representation using Rosenbrock system matrix virtual double getEstimatedValueAtPoint(int i, const double *params) const { // only positive values allowed (todo: choose the limits) - double pL = fmax(params[PARAM_L], minParamKL); + double pL = fmax(params[PARAM_L], minParamL); double pT = fmax(params[PARAM_T], minParamT); double pT2 = fmax(params[PARAM_T2], minParamT); - double pK = (fabs(params[PARAM_K]) < minParamKL) ? minParamKL : params[PARAM_K]; + double pK = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K]; // state-space params double lambda = exp(-1.0 / (pT * inputFunc->getTimeScale())); @@ -234,7 +236,9 @@ public: y = lambda2 * y + (1.0 - lambda2) * x; x = lambda * x + pK * (1.0 - lambda) * inp; } - return y; + + // the output can be biased + return y + modelBias; } }; diff --git a/pid_open_loop_models.h b/pid_open_loop_models.h index 85c3659..11dbc10 100644 --- a/pid_open_loop_models.h +++ b/pid_open_loop_models.h @@ -19,7 +19,7 @@ // should be multiple of 2 #define MAX_DATA_POINTS 700 -enum PidTuneMethod { +typedef enum { // 1st order PID_TUNE_CHR1, PID_TUNE_AUTO1, @@ -31,7 +31,7 @@ enum PidTuneMethod { PID_TUNE_VDG2, PID_TUNE_HP2, PID_TUNE_AUTO2, -}; +} pid_tune_method_e; // Used as an open-loop plant model for the "manual bump test" and as an input of a transfer function class ModelOpenLoopPlant @@ -84,10 +84,55 @@ private: double p[3]; }; -// Chien-Hrones-Reswick PID implementation for the 1st order model -class ModelChienHronesReswickFirstOrder : public ModelOpenLoopPlant { +// Standard PID model: Kc * (1 + 1/(Ti*S) + Td * S) +// This class converts in into our "Parallel" form: Kp + Ki / S + Kd * S +class ModelStandard : public ModelOpenLoopPlant { +public: + ModelStandard(const double *params_) : ModelOpenLoopPlant(params_) { + } + + virtual float getKp() const { + return (float)Kc; + } + virtual float getKi() const { + return (float)(Kc / Ti); + } + virtual float getKd() const { + return (float)(Kc * Td); + } + +protected: + // "Standard" PID coefs + double Kc, Ti, Td; +}; + +class ModelStandardIMC : public ModelStandard { +public: + ModelStandardIMC(const double *params_) : ModelStandard(params_) { + lambda = fmax(0.25 * params[PARAM_L], 0.2 * Ti); + } + +protected: + // closed-loop speed of response + double lambda; +}; + + +// Chien-Hrones-Reswick PID implementation for the 1st order model (generic model). +class ModelChienHronesReswickFirstOrder : public ModelStandardIMC { +public: + ModelChienHronesReswickFirstOrder(const double *params_) : ModelStandardIMC(params_) { + double l2 = params[PARAM_L] / 2.0; + Ti = params[PARAM_T] + l2; + Td = params[PARAM_T] * params[PARAM_L] / (2 * params[PARAM_T] + params[PARAM_L]); + Kc = Ti / (params[PARAM_K] * (lambda + l2)); + } +}; + +// Chien-Hrones-Reswick PID implementation for the 1st order model (set-point regulation). +class ModelChienHronesReswickFirstOrderSetpoint : public ModelOpenLoopPlant { public: - ModelChienHronesReswickFirstOrder(const double *params_) : ModelOpenLoopPlant(params_) { + ModelChienHronesReswickFirstOrderSetpoint(const double *params_) : ModelOpenLoopPlant(params_) { } virtual float getKp() const { @@ -101,91 +146,62 @@ public: } }; +// Chien-Hrones-Reswick PID implementation for the 1st order model (disturbance rejection). +class ModelChienHronesReswickFirstOrderDisturbance : public ModelOpenLoopPlant { +public: + ModelChienHronesReswickFirstOrderDisturbance(const double *params_) : ModelOpenLoopPlant(params_) { + } + + virtual float getKp() const { + return (float)(0.95f / params[PARAM_K]); + } + virtual float getKi() const { + return (float)(2.4f / params[PARAM_T]); + } + virtual float getKd() const { + return (float)(1.0f / (0.42f * params[PARAM_L])); + } +}; + // "IMC-PID" Rivera-Morari-Zafiriou implementation for the 1st order model // See "Panda R.C., Yu C.C., Huang H.P. PID tuning rules for SOPDT systems: Review and some new results" -class ModelImcPidFirstOrder : public ModelOpenLoopPlant { +class ModelRiveraMorariFirstOrder : public ModelStandardIMC { public: - ModelImcPidFirstOrder(const double *params_) : ModelOpenLoopPlant(params_) { - lambda = fmax(0.25 * params[PARAM_L], 0.2 * params[PARAM_T]); + ModelRiveraMorariFirstOrder(const double *params_) : ModelStandardIMC(params_) { Kc = (2 * params[PARAM_T] + params[PARAM_L]) / (2 * params[PARAM_K] * (lambda + params[PARAM_L])); Ti = params[PARAM_T] + 0.5 * params[PARAM_L]; Td = params[PARAM_T] * params[PARAM_L] / (2.0 * params[PARAM_T] + params[PARAM_L]); } - - virtual float getKp() const { - return (float)Kc; - } - virtual float getKi() const { - return (float)(Kc / Ti); - } - virtual float getKd() const { - return (float)(Kc * Td); - } - -private: - double lambda; - double Kc, Ti, Td; }; -// Based on "IMC-Chien" model: "Chien, I.L., IMC-PID controller design - An extension." +// Based on "IMC-Chien" (aka Rivera/Smith) model: "Chien, I.L., IMC-PID controller design - An extension." // "Proceedings of the IFAC adaptive control of chemical processes conference, Copenhagen, Denmark, 1988, pp. 147-152." -class ModelChienHronesReswickSecondOrder : public ModelOpenLoopPlant { +class ModelChienHronesReswickSecondOrder : public ModelStandardIMC { public: - ModelChienHronesReswickSecondOrder(const double *params_) : ModelOpenLoopPlant(params_) { + ModelChienHronesReswickSecondOrder(const double *params_) : ModelStandardIMC(params_) { Ti = params[PARAM_T] + params[PARAM_T2]; Td = params[PARAM_T] * params[PARAM_T2] / Ti; - lamda = fmax(0.25 * params[PARAM_L], 0.2 * Ti); - Kc = Ti / (params[PARAM_K] * (lamda + params[PARAM_L])); + Kc = Ti / (params[PARAM_K] * (lambda + params[PARAM_L])); } - - virtual float getKp() const { - return (float)Kc; - } - virtual float getKi() const { - return (float)(Kc / Ti); - } - virtual float getKd() const { - return (float)(Kc * Td); - } - -protected: - // closed-loop speed of response - double lamda; - // "Standard" PID coefs - double Kc, Ti, Td; }; // Basen on Van der Grinten Model (1963) // "Step disturbance". -class ModelVanDerGrintenSecondOrder : public ModelOpenLoopPlant { +class ModelVanDerGrintenSecondOrder : public ModelStandard { public: - ModelVanDerGrintenSecondOrder(const double *params_) : ModelOpenLoopPlant(params_) { + ModelVanDerGrintenSecondOrder(const double *params_) : ModelStandard(params_) { double T12 = params[PARAM_T] + params[PARAM_T2]; Ti = T12 + 0.5 * params[PARAM_L]; Td = (T12 * params[PARAM_L] + 2.0 * params[PARAM_T] * params[PARAM_T2]) / (params[PARAM_L] + 2.0 * T12); Kc = (0.5 + T12 / params[PARAM_L]) / params[PARAM_K]; } - - virtual float getKp() const { - return (float)Kc; - } - virtual float getKi() const { - return (float)(Kc / Ti); - } - virtual float getKd() const { - return (float)(Kc * Td); - } - -protected: - double lamda; - double Kc, Ti, Td; }; // Based on Haalman-Pemberton model: "Haalman, A.: Adjusting controllers for a deadtime process. Control Eng. 65, 71–73 (1965)" // Suited for overdamped response and significant delay. -class ModelHaalmanPembertonSecondOrder : public ModelChienHronesReswickSecondOrder { +class ModelHaalmanPembertonSecondOrder : public ModelStandard { public: - ModelHaalmanPembertonSecondOrder(const double *params_) : ModelChienHronesReswickSecondOrder(params_) { + ModelHaalmanPembertonSecondOrder(const double *params_) : ModelStandard(params_) { double T1divT2 = params[PARAM_T] / params[PARAM_T2]; double LdivT2 = params[PARAM_L] / params[PARAM_T2]; Ti = params[PARAM_T] + params[PARAM_T2]; @@ -200,34 +216,20 @@ public: } }; -#if 0 -// Based on "IMC-Rivera/Smith" model: -// "Proceedings of the IFAC adaptive control of chemical processes conference, Copenhagen, Denmark, 1988, pp. 147-152." -class ModelRiveraSmithSecondOrder : public ModelOpenLoopPlant { +// Based on IMC-Maclaurin model: +// Lee, Y., Park, S., Lee, M., and Brosilow, C., PID controller tuning for desired closed - loop responses for SI / SO systems. AIChE J. 44, 106–115 1998. +class ModelMaclaurinSecondOrder : public ModelStandardIMC { public: - ModelRiveraSmithSecondOrder(const double *params_) : ModelOpenLoopPlant(params_) { - Ti = params[PARAM_T] + params[PARAM_T2]; - Td = params[PARAM_T] * params[PARAM_T2] / Ti; - lamda = fmax(0.25 * params[PARAM_L], 0.2 * Ti); - Kc = (params[PARAM_T] + params[PARAM_T2]) / (params[PARAM_K] * (lamda + params[PARAM_L])); + ModelMaclaurinSecondOrder(const double *params_) : ModelStandardIMC(params_) { + double T1T2 = params[PARAM_T] + params[PARAM_T2]; + double L = params[PARAM_L]; + double L2 = L * L; + double twolL = 2.0 * lambda + L; + Ti = T1T2 - (2.0 * lambda * lambda - L2) / (2.0 * twolL); + Kc = Ti / (params[PARAM_K] * twolL); + Td = Ti - T1T2 + (params[PARAM_T] * params[PARAM_T2] - L2 * L / (6.0 * twolL)) / Ti; } - - virtual float getKp() const { - //return (float)(Kc * (1.0 + Td / Ti)); - return (float)Kc; - } - virtual float getKi() const { - return (float)(Kc / Ti); - } - virtual float getKd() const { - return (float)(Kc * Td); - } - -private: - double lamda; - double Kc, Ti, Td; }; -#endif class ModelAutoSolver : public ModelOpenLoopPlant { public: diff --git a/pid_sim.h b/pid_sim.h index 49b4bc8..ee0ba73 100644 --- a/pid_sim.h +++ b/pid_sim.h @@ -13,13 +13,17 @@ #include "global.h" +typedef enum { + PID_SIM_REGULATOR = 0, // imitate load disturbance + PID_SIM_SERVO, // imitate setpoint change +} pid_sim_type_e; template class PidSimulator { public: - PidSimulator(int order, double target1_, double target2_, double dTime_, bool showOutput_) : - target1(target1_), target2(target2_), dTime(dTime_), methodOrder(order), showOutput(showOutput_), - plantInput(1.0 / dTime_), plant1(&plantInput, nullptr, 0, minParamT0), plant2(&plantInput, nullptr, 0, minParamT0) { + PidSimulator(pid_sim_type_e simType_, int order, double target1_, double target2_, double dTime_, double modelBias, const char *outputFile_) : + simType(simType_), target1(target1_), target2(target2_), dTime(dTime_), methodOrder(order), outputFile(outputFile_), + plantInput(1.0 / dTime_), plant1(&plantInput, nullptr, 0, modelBias), plant2(&plantInput, nullptr, 0, modelBias) { } PidAccuracyMetric simulate(int numPoints, const pid_s & pidParams, const double *params) { @@ -34,44 +38,73 @@ public: plantInput.reset(); // guess a previous state to minimize the system "shock" - plantInput.addDataPoint((float)(getTarget(0) / params[PARAM_K]) - pidParams.offset); + plantInput.addDataPoint((float)(getSetpoint(0) / params[PARAM_K]) - pidParams.offset); // "calm down" the PID controller to avoid huge spikes at the beginning - pid.getOutput(getTarget(0), plant->getEstimatedValueAtPoint(0, params), dTime); + pid.getOutput(getSetpoint(0), plant->getEstimatedValueAtPoint(0, params), dTime); + + stepPoint = maxPoints / 2; metric.reset(); // simulate over time for (int i = 0; i < numPoints; i++) { // make a step in the middle - double target = getTarget(i); + double target = getSetpoint(i); // "measure" the current value of the plant - double pidInput = plant->getEstimatedValueAtPoint(i, params); + double pidInput = plant->getEstimatedValueAtPoint(i, params) + getLoadDisturbance(i); // wait for the controller reaction double pidOutput = pid.getOutput(target, pidInput, dTime); + // apply the reaction to the plant's pidInput plantInput.addDataPoint((float)pidOutput); - metric.addPoint((double)i / (double)numPoints, pidInput, target); + // don't take into account any start-up transients, we're interested only in our step response! + if (i >= stepPoint) + metric.addPoint((double)i / (double)numPoints, pidInput, target); #ifdef PID_DEBUG - if (showOutput) - output_csv((methodOrder == 1) ? "pid_test.csv" : "pid_test2.csv", (double)i, pidInput, pidOutput, target); + if (outputFile != nullptr) + output_csv(outputFile, (double)i, pidOutput, target, pidInput); #endif } return metric; } - double getTarget(int i) const { - return (i > maxPoints / 2) ? target2 : target1; + double getSetpoint(int i) const { + switch (simType) { + case PID_SIM_SERVO: + return (i > stepPoint) ? target2 : target1; + default: + // we want to be in the middle for safety + return (target1 + target2) / 2.0; + } + } + + double getLoadDisturbance(int i) const { + static const double disturb = 0.10; + static const double ampl = 0.25, period = 0.37; + double d = 0; + switch (simType) { + case PID_SIM_REGULATOR: + // add or subtract 10% to imitate the "load" + d += target1 * ((i > stepPoint) ? -disturb : disturb); + // add periodic noise + d += sin(2.0 * 3.14159265 * (double)i * dTime / period) * ampl; + return d; + default: + return 0.0; + } } protected: int methodOrder; - bool showOutput; + int stepPoint; + const char *outputFile; StoredDataInputFunction plantInput; FirstOrderPlusDelayLineFunction plant1; SecondOrderPlusDelayLineOverdampedFunction plant2; PidAccuracyMetric metric; double target1, target2, dTime; + pid_sim_type_e simType; }; @@ -81,8 +114,8 @@ protected: template class PidSimulatorFactory { public: - PidSimulatorFactory(int methodOrder_, double target1_, double target2_, double dTime, const pid_s & pid_) : - sim(methodOrder_, target1_, target2_, dTime, false), pid(pid_) { + PidSimulatorFactory(pid_sim_type_e simType, int methodOrder_, double target1_, double target2_, double dTime, double modelBias, const pid_s & pid_) : + sim(simType, methodOrder_, target1_, target2_, dTime, modelBias, nullptr), pid(pid_) { } public: @@ -115,7 +148,7 @@ public: } virtual double getResidual(int i, const double *params) const { - return simFactory->sim.getTarget(i) - getEstimatedValueAtPoint(i, params); + return simFactory->sim.getSetpoint(i) - getEstimatedValueAtPoint(i, params); } virtual double getEstimatedValueAtPoint(int i, const double *params) const { diff --git a/test_chs_pid.cpp b/test_chs_pid.cpp index 3eda2e7..73feab3 100644 --- a/test_chs_pid.cpp +++ b/test_chs_pid.cpp @@ -29,8 +29,8 @@ TEST(pidAutoTune, testMeasuredDataBuffer) { } TEST(pidAutoTune, testFOPDT) { - StepFunction stepFunc(0, 100, 0, 10, 1.0); - FirstOrderPlusDelayLineFunction func(&stepFunc, nullptr, 0, minParamT0); + StepFunction stepFunc(0, 100, 10, 1.0); + FirstOrderPlusDelayLineFunction func(&stepFunc, nullptr, 0, 0.0); double params[3]; params[PARAM_K] = 2.0; params[PARAM_T] = 3.0; @@ -41,8 +41,8 @@ TEST(pidAutoTune, testFOPDT) { } TEST(pidAutoTune, testSOPDTOverdamped) { - StepFunction stepFunc(0, 100, 0, 10, 1.0); - SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, nullptr, 0, minParamT0); + StepFunction stepFunc(0, 100, 10, 1.0); + SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, nullptr, 0, 0.0); double params[4]; params[PARAM_K] = 2.0; params[PARAM_T] = 3.0; @@ -58,8 +58,8 @@ static const float outputData[] = { 13.29, 13.29, 13.33, 13.33, 13.33, 13.33, 13 const int numData = sizeof(outputData) / sizeof(outputData[0]); void printSOPDT() { - StepFunction stepFunc(20.0, 30.0, 32.823277, 178, 1.0); - SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, nullptr, 0, minParamT0); + StepFunction stepFunc(20.0, 30.0, 178, 1.0); + SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, nullptr, 0, 0.0); double params[4]; params[PARAM_K] = 0.251778; params[PARAM_T] = 55.7078; @@ -82,24 +82,18 @@ TEST(pidAutoTune, chsSopdtPid) { params0[PARAM_T2] = 1; params0[PARAM_L] = 1; - for (int tries = 0; tries < 10; tries ++) { - for (int i = 0; i < numData; i++) { - chr.addData(outputData[i]); - } - - PidAutoTuneSettings settings; - settings.minValue = 20.0; - settings.maxValue = 30.0; - settings.stepPoint = 178; // todo: adjust for the buffer scale - settings.maxPoint = 460; - settings.timeScale = 1.0; - if (chr.findPid(PID_TUNE_CHR2, settings, params0)) - break; - // todo: the solver has failed. Choose other initial params? - // params0[0] = ; params0[1] = ; params0[2] = ; - break; + for (int i = 0; i < numData; i++) { + chr.addData(outputData[i]); } + PidAutoTuneSettings settings; + settings.minValue = 20.0; + settings.maxValue = 30.0; + settings.stepPoint = 178; // todo: adjust for the buffer scale + settings.maxPoint = 460; + settings.timeScale = 1.0; + bool ret = chr.findPid(PID_SIM_SERVO, PID_TUNE_CHR2, settings, params0); + #ifdef PID_DEBUG const double *p = chr.getParams(); printf("Params: K=%g T1=%g T2=%g L=%g\r\n", p[PARAM_K], p[PARAM_T], p[PARAM_T2], p[PARAM_L]); @@ -128,7 +122,7 @@ TEST(pidAutoTune, testPidCoefs) { // todo: is it correct? double dTime = 1; const int numSimPoints = 2048; - PidSimulator sim(2, 13.0, 14.0, dTime, true); + PidSimulator sim(PID_SIM_SERVO, 2, 13.0, 14.0, dTime, 0.0, nullptr); for (int idx = 0; idx <= 4; idx++) { PidAccuracyMetric metric = sim.simulate(numSimPoints, pidParams[idx], params); #ifdef PID_DEBUG @@ -139,6 +133,48 @@ TEST(pidAutoTune, testPidCoefs) { // todo: check results } +TEST(pidAutoTune, testPidSim) { + const int numSimPoints = 1024; + + pid_s pid; + //double p[4] = { 0.27659, 1.08301, 0.3, 1.08 }; +#if 1 + double p[4] = { 0.276267, 0.596307, 0.1, 0.140311 }; + double modelBias = 7.88121; + pid.pFactor = 18.07646561; + pid.iFactor = 16.68941116; + pid.dFactor = 0.00180748; + pid.offset = 21.4; +#endif + +#if 0 + double p[4] = { 0.276267, 0.596307, 0.1, 0.140311 }; + double modelBias = 7.88121; + pid.pFactor = 12.17391205; + pid.iFactor = 11.22539520; + pid.dFactor = 2.99444199; + pid.offset = 28.50101471; + //pid.offset = 21.4; +#endif + +#if 0 + double p[4] = { 0.276267, 0.596307, 0.1, 0.140311 }; + double modelBias = 7.88121; + pid.pFactor = 19.52591324; + pid.iFactor = 14.07089329; + pid.dFactor = 1.58994818; + pid.offset = 21.4; +#endif + pid.periodMs = 11; + pid.minValue = 10; + pid.maxValue = 90; + + PidSimulator sim(PID_SIM_REGULATOR, 2, 13.4117, 16.1769, pid.periodMs / 1000.0, modelBias, "pid_test.csv"); + printf(" PID: P=%.8f I=%.8f D=%.8f offset=%.8f period=%.8fms\r\n", pid.pFactor, pid.iFactor, pid.dFactor, pid.offset, pid.periodMs); + PidAccuracyMetric metric = sim.simulate(numSimPoints, pid, p); + printf(" Metric result: ITAE=%g ISE=%g Overshoot=%g%%\r\n", metric.getItae(), metric.getIse(), metric.getMaxOvershoot() * 100.0); +} + #if 0 GTEST_API_ int main(int argc, char **argv) { //testPidCoefs(); @@ -154,7 +190,7 @@ GTEST_API_ int main(int argc, char **argv) { testSOPDTOverdamped(); testChsSopdtPid(); #endif - ::testing::GTEST_FLAG(filter) = "*testPidCoefs*"; + ::testing::GTEST_FLAG(filter) = "*testPidSim*"; int result = RUN_ALL_TESTS(); // windows ERRORLEVEL in Jenkins batch file seems to want negative value to detect failure return result == 0 ? 0 : -1;