mirror of https://github.com/rusefi/autopid.git
Version 0.2: modelBias, pid.offset
This commit is contained in:
Andrei
parent
be4b50d847
commit
2453e92bb3
2
Makefile
2
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
|
||||
|
||||
|
|
|
|||
2
global.h
2
global.h
|
|
@ -4,6 +4,7 @@
|
|||
|
||||
|
||||
#include <assert.h>
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
|
@ -19,6 +20,7 @@ struct pid_s {
|
|||
float iFactor;
|
||||
float dFactor;
|
||||
float offset;
|
||||
float periodMs;
|
||||
|
||||
int16_t minValue;
|
||||
int16_t maxValue;
|
||||
|
|
|
|||
136
pid_auto.h
136
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<numParams1stOrder>("pid_func0.csv", func, stepFunc, params);
|
||||
LevenbergMarquardtSolver<numParams1stOrder> solver(&func, params);
|
||||
merit0 = func.calcMerit(params);
|
||||
iterationCount = solver.solve(minParamT);
|
||||
outputFunc<numParams1stOrder>("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<numParams2ndOrder>("pid_func0.csv", func, stepFunc, params);
|
||||
LevenbergMarquardtSolver<numParams2ndOrder> solver(&func, params);
|
||||
merit0 = func.calcMerit(params);
|
||||
iterationCount = solver.solve(minParamT);
|
||||
outputFunc<numParams2ndOrder>("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<numParams1stOrder>("pid_func01.csv", func, stepFunc, params);
|
||||
LevenbergMarquardtSolver<numParams1stOrder> solver(&func, params);
|
||||
merit0 = func.calcMerit(params);
|
||||
iterationCount = solver.solve(minParamT);
|
||||
outputFunc<numParams1stOrder>("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<numParams2ndOrder>("pid_func02.csv", func, stepFunc, params);
|
||||
LevenbergMarquardtSolver<numParams2ndOrder> solver(&func, params);
|
||||
merit0 = func.calcMerit(params);
|
||||
iterationCount = solver.solve(minParamT);
|
||||
outputFunc<numParams2ndOrder>("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<numSimPoints> simFactory(getMethodOrder(method), getAvgMeasuredMin(), getAvgMeasuredMax(), dTime, pid);
|
||||
PidSimulatorFactory<numSimPoints> simFactory(simType, getMethodOrder(method), getAvgMeasuredMin(), getAvgMeasuredMax(), dTime, modelBias, pid);
|
||||
PidCoefsFinderFunction<numSimPoints> func(&simFactory, params);
|
||||
func.justifyParams(model.getParams());
|
||||
merit0 = func.calcMerit(model.getParams());
|
||||
|
||||
// now hopefully we'll find even better coefs!
|
||||
LevenbergMarquardtSolver<numParamsForPid> solver((LMSFunction<numParamsForPid> *)&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<MAX_DATA_POINTS> 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;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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 <file.msl> <startTime> <endTime> <inColumnIdx> <outColumnIdx> [<targetValue>]\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<numSimPoints> 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<numSimPoints> 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
|
||||
|
|
|
|||
|
|
@ -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 <int numPoints>
|
||||
|
|
@ -123,11 +122,11 @@ private:
|
|||
template <int numParams>
|
||||
class AbstractDelayLineFunction : public LMSFunction<numParams> {
|
||||
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;
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
65
pid_sim.h
65
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 <int maxPoints>
|
||||
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<maxPoints> plantInput;
|
||||
FirstOrderPlusDelayLineFunction plant1;
|
||||
SecondOrderPlusDelayLineOverdampedFunction plant2;
|
||||
PidAccuracyMetric metric;
|
||||
double target1, target2, dTime;
|
||||
pid_sim_type_e simType;
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -81,8 +114,8 @@ protected:
|
|||
template <int numPoints>
|
||||
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 {
|
||||
|
|
|
|||
|
|
@ -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<numSimPoints> sim(2, 13.0, 14.0, dTime, true);
|
||||
PidSimulator<numSimPoints> 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<numSimPoints> 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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue