mirror of https://github.com/rusefi/autopid.git
Version 0.3: double_t + industrial-PID + small fixes
This commit is contained in:
parent
2453e92bb3
commit
224193312d
|
@ -0,0 +1,10 @@
|
||||||
|
rem Alternator PID for 2003 Mazda Miata:
|
||||||
|
|
||||||
|
@rem 54=debugFloatField1=altpid.output
|
||||||
|
@rem 10=VBatt
|
||||||
|
@rem 1=rpm
|
||||||
|
@rem 22=IAC_position
|
||||||
|
@rem 12=advance angle
|
||||||
|
|
||||||
|
@rem Usage: PID_FROM_MSL <file.msl> <startTime> <endTime> <inputColumnIdx> <outputColumnIdx> [<targetValue>]
|
||||||
|
..\..\pid_from_msl.exe "miata_alt_log.msl" 66.737 71.662 54 10 13.8
|
File diff suppressed because it is too large
Load Diff
16
global.h
16
global.h
|
@ -14,13 +14,19 @@
|
||||||
#include "gmock/gmock.h"
|
#include "gmock/gmock.h"
|
||||||
|
|
||||||
|
|
||||||
|
//!!!!!!!!!!!!!!
|
||||||
|
#define float_t double_t
|
||||||
|
|
||||||
#ifndef CONTROLLERS_GENERATED_ENGINE_CONFIGURATION_GENERATED_STRUCTURES_H
|
#ifndef CONTROLLERS_GENERATED_ENGINE_CONFIGURATION_GENERATED_STRUCTURES_H
|
||||||
struct pid_s {
|
struct pid_s {
|
||||||
float pFactor;
|
float_t pFactor;
|
||||||
float iFactor;
|
float_t iFactor;
|
||||||
float dFactor;
|
float_t dFactor;
|
||||||
float offset;
|
float_t offset;
|
||||||
float periodMs;
|
float_t periodMs;
|
||||||
|
|
||||||
|
float_t antiwindupFreq; // = 1/ResetTime
|
||||||
|
float_t derivativeFilterLoss; // = 1/Gain
|
||||||
|
|
||||||
int16_t minValue;
|
int16_t minValue;
|
||||||
int16_t maxValue;
|
int16_t maxValue;
|
||||||
|
|
|
@ -26,19 +26,28 @@ public:
|
||||||
// Get the total number of data points
|
// Get the total number of data points
|
||||||
virtual int getNumPoints() const = 0;
|
virtual int getNumPoints() const = 0;
|
||||||
|
|
||||||
virtual void justifyParams(double *params) const = 0;
|
virtual void justifyParams(double_t *params) const = 0;
|
||||||
|
|
||||||
/// Returns the y value of the function for the given x and vector of parameters
|
/// Returns the y value of the function for the given x and vector of parameters
|
||||||
virtual double getEstimatedValueAtPoint(int i, const double *params) const = 0;
|
virtual double_t getEstimatedValueAtPoint(int i, const double_t *params) const {
|
||||||
|
#ifdef LM_USE_CACHE
|
||||||
|
return points[0][i];
|
||||||
|
#else
|
||||||
|
return calcEstimatedValuesAtPoint(i, params);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Returns the y value of the function for the given x and vector of parameters
|
||||||
|
virtual double_t calcEstimatedValuesAtPoint(int i, const double_t *params) const = 0;
|
||||||
|
|
||||||
/// Returns the residual (error delta) of the function (return (dataPoints[i] - estimatedPoint(i)))
|
/// Returns the residual (error delta) of the function (return (dataPoints[i] - estimatedPoint(i)))
|
||||||
virtual double getResidual(int i, const double *params) const = 0;
|
virtual double_t getResidual(int i, const double_t *params) const = 0;
|
||||||
|
|
||||||
// Calculate the sum of the squares of the residuals
|
// Calculate the sum of the squares of the residuals
|
||||||
virtual double calcMerit(double *params) const {
|
virtual double_t calcMerit(double_t *params) const {
|
||||||
double res = 0;
|
double_t res = 0;
|
||||||
for (int i = 0; i < getNumPoints(); i++) {
|
for (int i = 0; i < getNumPoints(); i++) {
|
||||||
double r = getResidual(i, params);
|
double_t r = getResidual(i, params);
|
||||||
res += r * r;
|
res += r * r;
|
||||||
}
|
}
|
||||||
return res;
|
return res;
|
||||||
|
@ -46,52 +55,68 @@ public:
|
||||||
|
|
||||||
/// Return the partial derivate of the function with respect to parameter pIndex at point [i].
|
/// Return the partial derivate of the function with respect to parameter pIndex at point [i].
|
||||||
/// Can be overridden if analytical gradient function's representation is available
|
/// Can be overridden if analytical gradient function's representation is available
|
||||||
virtual double getPartialDerivative(int i, const double *params, int pIndex) const {
|
virtual double_t getPartialDerivative(int i, const double_t *params, int pIndex) const {
|
||||||
#ifdef LM_USE_CACHE
|
|
||||||
return pderivs[pIndex][i];
|
|
||||||
}
|
|
||||||
virtual double getPartialDerivativeInternal(int i, const double *params, int pIndex) const {
|
|
||||||
#endif
|
|
||||||
// some magic value
|
|
||||||
const double delta = 1.0e-6;
|
|
||||||
// we need to alter parameters around the neighborhood of 'pIndex', so we make a working copy
|
// we need to alter parameters around the neighborhood of 'pIndex', so we make a working copy
|
||||||
double tmpParams[numParams];
|
#ifdef LM_USE_CACHE
|
||||||
|
int p = 1 + pIndex * 2;
|
||||||
|
double_t dplusResult = points[p][i];
|
||||||
|
double_t dminusResult = points[p + 1][i];
|
||||||
|
#else
|
||||||
|
double_t tmpParams[numParams];
|
||||||
for (int k = 0; k < numParams; k++)
|
for (int k = 0; k < numParams; k++)
|
||||||
tmpParams[k] = params[k];
|
tmpParams[k] = params[k];
|
||||||
|
|
||||||
tmpParams[pIndex] = params[pIndex] + delta;
|
tmpParams[pIndex] = params[pIndex] + delta;
|
||||||
double dplusResult = getEstimatedValueAtPoint(i, tmpParams);
|
double_t dplusResult = getEstimatedValueAtPoint(i, tmpParams);
|
||||||
|
|
||||||
tmpParams[pIndex] = params[pIndex] - delta;
|
tmpParams[pIndex] = params[pIndex] - delta;
|
||||||
double dminusResult = getEstimatedValueAtPoint(i, tmpParams);
|
double_t dminusResult = getEstimatedValueAtPoint(i, tmpParams);
|
||||||
|
#endif
|
||||||
|
|
||||||
return (dplusResult - dminusResult) / (delta * 2.0);
|
return (dplusResult - dminusResult) / (delta * 2.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void calculateAllPoints(const double_t *params) {
|
||||||
#ifdef LM_USE_CACHE
|
#ifdef LM_USE_CACHE
|
||||||
void calculateAllPartialDerivatives(const double *params) {
|
for (int i = 0; i < numPointGroups; i++) {
|
||||||
for (int i = 0; i < numParams; i++) {
|
points[i].resize(getNumPoints());
|
||||||
pderivs[i].resize(getNumPoints());
|
|
||||||
}
|
}
|
||||||
for (int p = 0; p < numParams; p++) {
|
// calculate displaced points for partial derivatives
|
||||||
|
for (int pIndex = 0; pIndex < numParams; pIndex++) {
|
||||||
|
double_t tmpParams[numParams];
|
||||||
|
for (int k = 0; k < numParams; k++)
|
||||||
|
tmpParams[k] = params[k];
|
||||||
|
// 2 points for each derivative: +delta and -delta
|
||||||
|
for (int plusMinus = 0; plusMinus < 2; plusMinus++) {
|
||||||
|
tmpParams[pIndex] = params[pIndex] + (plusMinus == 0 ? delta : -delta);
|
||||||
|
int p = 1 + pIndex * 2 + plusMinus;
|
||||||
for (int i = 0; i < getNumPoints(); i++) {
|
for (int i = 0; i < getNumPoints(); i++) {
|
||||||
pderivs[p][i] = getPartialDerivativeInternal(i, params, p);
|
points[p][i] = calcEstimatedValuesAtPoint(i, tmpParams);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
for (int i = 0; i < getNumPoints(); i++) {
|
||||||
|
points[0][i] = calcEstimatedValuesAtPoint(i, params);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
#ifdef LM_USE_CACHE
|
#ifdef LM_USE_CACHE
|
||||||
std::vector<double> pderivs[numParams];
|
static const int numPointGroups = numParams * 2 + 1;
|
||||||
|
std::vector<double_t> points[numPointGroups];
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
protected:
|
||||||
|
// some magic value for the differentiation step to calculate a partial derivative
|
||||||
|
double_t delta = 1.0e-6;
|
||||||
};
|
};
|
||||||
|
|
||||||
template<int numParams>
|
template<int numParams>
|
||||||
class LevenbergMarquardtSolver {
|
class LevenbergMarquardtSolver {
|
||||||
public:
|
public:
|
||||||
// ctor
|
// ctor
|
||||||
LevenbergMarquardtSolver(LMSFunction<numParams> *func, double *parameters) {
|
LevenbergMarquardtSolver(LMSFunction<numParams> *func, double_t *parameters) {
|
||||||
this->func = func;
|
this->func = func;
|
||||||
this->parameters = parameters;
|
this->parameters = parameters;
|
||||||
}
|
}
|
||||||
|
@ -99,22 +124,21 @@ public:
|
||||||
// lambda - magic coef.
|
// lambda - magic coef.
|
||||||
// maxIterations - if too many iterations (loop exit condition #1)
|
// maxIterations - if too many iterations (loop exit condition #1)
|
||||||
// minDelta - if the progress on iteration is too small (loop exit condition #2)
|
// minDelta - if the progress on iteration is too small (loop exit condition #2)
|
||||||
int solve(double lambda_ = 0.001, double minDelta = 1e-15, int maxIterations = 100) {
|
int solve(double_t lambda_ = 0.001, double_t minDelta = 1e-15, int maxIterations = 100) {
|
||||||
this->lambda = lambda_;
|
this->lambda = lambda_;
|
||||||
|
|
||||||
iterationCount = 0;
|
iterationCount = 0;
|
||||||
|
|
||||||
double delta = 0;
|
func->calculateAllPoints(parameters);
|
||||||
|
double_t delta = 0;
|
||||||
do {
|
do {
|
||||||
double merit = func->calcMerit(parameters);
|
double_t merit = func->calcMerit(parameters);
|
||||||
|
|
||||||
#ifdef LM_USE_CACHE
|
|
||||||
func->calculateAllPartialDerivatives(parameters);
|
|
||||||
#endif
|
|
||||||
calcGradient();
|
calcGradient();
|
||||||
calcHessian();
|
calcHessian();
|
||||||
bool isSolved = calcNewParameters();
|
bool isSolved = calcNewParameters();
|
||||||
double newMerit = func->calcMerit(newParameters);
|
func->calculateAllPoints(newParameters);
|
||||||
|
double_t newMerit = func->calcMerit(newParameters);
|
||||||
if (!isSolved) {
|
if (!isSolved) {
|
||||||
return -iterationCount;
|
return -iterationCount;
|
||||||
}
|
}
|
||||||
|
@ -133,15 +157,15 @@ public:
|
||||||
// find out if we progressed enough in this iteration
|
// find out if we progressed enough in this iteration
|
||||||
delta = fabs(newMerit - merit);
|
delta = fabs(newMerit - merit);
|
||||||
#ifdef LMS_DEBUG
|
#ifdef LMS_DEBUG
|
||||||
printf("[%d] (%g,%g,%g,%g) l=%g merit %g->%g, dm=%g\r\n", iterationCount, parameters[0], parameters[1], parameters[2], parameters[3], lambda,
|
printf("[%d] (%Lg,%Lg,%Lg,%Lg) l=%Lg merit %Lg->%Lg, dm=%Lg\r\n", iterationCount, (long double)parameters[0], (long double)parameters[1], (long double)parameters[2], (long double)parameters[3],
|
||||||
merit, newMerit, newMerit - merit);
|
(long double)lambda, (long double)merit, (long double)newMerit, (long double)(newMerit - merit));
|
||||||
#endif
|
#endif
|
||||||
iterationCount++;
|
iterationCount++;
|
||||||
} while (delta > minDelta && iterationCount < maxIterations);
|
} while (delta > minDelta && iterationCount < maxIterations);
|
||||||
return iterationCount;
|
return iterationCount;
|
||||||
}
|
}
|
||||||
|
|
||||||
double *getParameters() const {
|
double_t *getParameters() const {
|
||||||
return parameters;
|
return parameters;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -149,13 +173,13 @@ protected:
|
||||||
// Find the parameter increments by solving the Hessian x Gradient equation
|
// Find the parameter increments by solving the Hessian x Gradient equation
|
||||||
bool calcNewParameters() {
|
bool calcNewParameters() {
|
||||||
// get H^-1 matrix (inverse Hessian)
|
// get H^-1 matrix (inverse Hessian)
|
||||||
double hinv[numParams][numParams];
|
double_t hinv[numParams][numParams];
|
||||||
bool ret = MatrixHelper<double, numParams>::inverseMatrix(hinv, hessian);
|
bool ret = MatrixHelper<double_t, numParams>::inverseMatrix(hinv, hessian);
|
||||||
if (!ret)
|
if (!ret)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
for (int row = 0; row < numParams; row++) {
|
for (int row = 0; row < numParams; row++) {
|
||||||
double increment = 0;
|
double_t increment = 0;
|
||||||
for (int col = 0; col < numParams; col++) {
|
for (int col = 0; col < numParams; col++) {
|
||||||
increment += hinv[row][col] * gradient[col];
|
increment += hinv[row][col] * gradient[col];
|
||||||
}
|
}
|
||||||
|
@ -170,7 +194,7 @@ protected:
|
||||||
void calcHessian() {
|
void calcHessian() {
|
||||||
for (int row = 0; row < numParams; row++) {
|
for (int row = 0; row < numParams; row++) {
|
||||||
for (int col = 0; col < numParams; col++) {
|
for (int col = 0; col < numParams; col++) {
|
||||||
double res = 0;
|
double_t res = 0;
|
||||||
for (int i = 0; i < func->getNumPoints(); i++) {
|
for (int i = 0; i < func->getNumPoints(); i++) {
|
||||||
res += func->getPartialDerivative(i, parameters, row) * func->getPartialDerivative(i, parameters, col);
|
res += func->getPartialDerivative(i, parameters, row) * func->getPartialDerivative(i, parameters, col);
|
||||||
}
|
}
|
||||||
|
@ -182,7 +206,7 @@ protected:
|
||||||
// Calculate the 1st derivatives of the residual func
|
// Calculate the 1st derivatives of the residual func
|
||||||
void calcGradient() {
|
void calcGradient() {
|
||||||
for (int row = 0; row < numParams; row++) {
|
for (int row = 0; row < numParams; row++) {
|
||||||
double res = 0;
|
double_t res = 0;
|
||||||
for (int i = 0; i < func->getNumPoints(); i++) {
|
for (int i = 0; i < func->getNumPoints(); i++) {
|
||||||
res += func->getResidual(i, parameters) * func->getPartialDerivative(i, parameters, row);
|
res += func->getResidual(i, parameters) * func->getPartialDerivative(i, parameters, row);
|
||||||
}
|
}
|
||||||
|
@ -195,21 +219,21 @@ private:
|
||||||
LMSFunction<numParams> *func;
|
LMSFunction<numParams> *func;
|
||||||
|
|
||||||
// Current (accepted) parameters vector
|
// Current (accepted) parameters vector
|
||||||
double *parameters; // [numParams]
|
double_t *parameters; // [numParams]
|
||||||
|
|
||||||
// Incremented (next step) parameters vector
|
// Incremented (next step) parameters vector
|
||||||
double newParameters[numParams];
|
double_t newParameters[numParams];
|
||||||
|
|
||||||
// Hessian matrix
|
// Hessian matrix
|
||||||
double hessian[numParams][numParams];
|
double_t hessian[numParams][numParams];
|
||||||
// Gradients vector
|
// Gradients vector
|
||||||
double gradient[numParams];
|
double_t gradient[numParams];
|
||||||
|
|
||||||
// some magic number
|
// some magic number
|
||||||
const double lambdaMultiplier = 10.0;
|
const double_t lambdaMultiplier = 10.0;
|
||||||
|
|
||||||
// coeff used to adapt the descent step (and speed)
|
// coeff used to adapt the descent step (and speed)
|
||||||
double lambda;
|
double_t lambda;
|
||||||
|
|
||||||
// Total number of iterations
|
// Total number of iterations
|
||||||
int iterationCount;
|
int iterationCount;
|
||||||
|
|
126
pid_auto.h
126
pid_auto.h
|
@ -25,10 +25,10 @@
|
||||||
/// These settings should be obtained from the measured data
|
/// These settings should be obtained from the measured data
|
||||||
class PidAutoTuneSettings {
|
class PidAutoTuneSettings {
|
||||||
public:
|
public:
|
||||||
double minValue, maxValue;
|
double_t minValue, maxValue;
|
||||||
double stepPoint, maxPoint;
|
double_t stepPoint, maxPoint;
|
||||||
double timeScale;
|
double_t timeScale;
|
||||||
double targetValue;
|
double_t targetValue;
|
||||||
};
|
};
|
||||||
|
|
||||||
// PID auto-tune method using different tuning rules and FOPDT/SOPDT models
|
// PID auto-tune method using different tuning rules and FOPDT/SOPDT models
|
||||||
|
@ -38,7 +38,7 @@ public:
|
||||||
measuredData.init();
|
measuredData.init();
|
||||||
}
|
}
|
||||||
|
|
||||||
void addData(float v) {
|
void addData(float_t v) {
|
||||||
measuredData.addDataPoint(v);
|
measuredData.addDataPoint(v);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -46,7 +46,7 @@ public:
|
||||||
// stepPoint - data index where the step was done
|
// stepPoint - data index where the step was done
|
||||||
// maxPoint - data index where the output was saturated
|
// maxPoint - data index where the output was saturated
|
||||||
// this is not thread-safe (because of internal static vars) but anyway it works...
|
// this is not thread-safe (because of internal static vars) but anyway it works...
|
||||||
bool findPid(pid_sim_type_e simType, pid_tune_method_e method, const PidAutoTuneSettings & settings, const double *initialParams) {
|
bool findPid(pid_sim_type_e simType, pid_tune_method_e method, const PidAutoTuneSettings & settings, const double_t *initialParams) {
|
||||||
// save current settings & simType
|
// save current settings & simType
|
||||||
this->settings = settings;
|
this->settings = settings;
|
||||||
this->simType = simType;
|
this->simType = simType;
|
||||||
|
@ -58,7 +58,7 @@ public:
|
||||||
pid.maxValue = 100;
|
pid.maxValue = 100;
|
||||||
|
|
||||||
#ifdef PID_DEBUG
|
#ifdef PID_DEBUG
|
||||||
printf("* modelBias=%g avgMin=%g avgMax=%g\r\n", modelBias, avgMeasuredMin, avgMeasuredMax);
|
printf("* modelBias=%Lg avgMin=%Lg avgMax=%Lg\r\n", (long double)modelBias, (long double)avgMeasuredMin, (long double)avgMeasuredMax);
|
||||||
#endif
|
#endif
|
||||||
StepFunction stepFunc(settings.minValue, settings.maxValue, settings.stepPoint, settings.timeScale);
|
StepFunction stepFunc(settings.minValue, settings.maxValue, settings.stepPoint, settings.timeScale);
|
||||||
|
|
||||||
|
@ -82,7 +82,7 @@ public:
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#ifdef PID_DEBUG
|
#ifdef PID_DEBUG
|
||||||
printf("* Params0: K=%g T1=%g T2=%g L=%g\r\n", params[PARAM_K], params[PARAM_T], params[PARAM_T2], params[PARAM_L]);
|
printf("* Params0: K=%Lg T1=%Lg T2=%Lg L=%Lg\r\n", (long double)params[PARAM_K], (long double)params[PARAM_T], (long double)params[PARAM_T2], (long double)params[PARAM_L]);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
|
@ -90,7 +90,7 @@ public:
|
||||||
memcpy(params, initialParams, sizeof(params));
|
memcpy(params, initialParams, sizeof(params));
|
||||||
}
|
}
|
||||||
|
|
||||||
double merit0, merit;
|
double_t merit0, merit;
|
||||||
#ifdef PID_DEBUG
|
#ifdef PID_DEBUG
|
||||||
printf("* Solving...\r\n");
|
printf("* Solving...\r\n");
|
||||||
#endif
|
#endif
|
||||||
|
@ -99,6 +99,7 @@ public:
|
||||||
// 1st order
|
// 1st order
|
||||||
FirstOrderPlusDelayLineFunction func(&stepFunc, measuredData.getBuf(), measuredData.getNumDataPoints(), modelBias);
|
FirstOrderPlusDelayLineFunction func(&stepFunc, measuredData.getBuf(), measuredData.getNumDataPoints(), modelBias);
|
||||||
func.justifyParams(params);
|
func.justifyParams(params);
|
||||||
|
func.calculateAllPoints(params);
|
||||||
const int numParams1stOrder = 3;
|
const int numParams1stOrder = 3;
|
||||||
outputFunc<numParams1stOrder>("pid_func01.csv", func, stepFunc, params);
|
outputFunc<numParams1stOrder>("pid_func01.csv", func, stepFunc, params);
|
||||||
LevenbergMarquardtSolver<numParams1stOrder> solver(&func, params);
|
LevenbergMarquardtSolver<numParams1stOrder> solver(&func, params);
|
||||||
|
@ -111,6 +112,7 @@ public:
|
||||||
// 2nd order or approximated 2nd->1st order
|
// 2nd order or approximated 2nd->1st order
|
||||||
SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, measuredData.getBuf(), measuredData.getNumDataPoints(), modelBias);
|
SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, measuredData.getBuf(), measuredData.getNumDataPoints(), modelBias);
|
||||||
func.justifyParams(params);
|
func.justifyParams(params);
|
||||||
|
func.calculateAllPoints(params);
|
||||||
const int numParams2ndOrder = 4;
|
const int numParams2ndOrder = 4;
|
||||||
outputFunc<numParams2ndOrder>("pid_func02.csv", func, stepFunc, params);
|
outputFunc<numParams2ndOrder>("pid_func02.csv", func, stepFunc, params);
|
||||||
LevenbergMarquardtSolver<numParams2ndOrder> solver(&func, params);
|
LevenbergMarquardtSolver<numParams2ndOrder> solver(&func, params);
|
||||||
|
@ -166,8 +168,9 @@ public:
|
||||||
pid.pFactor = model->getKp();
|
pid.pFactor = model->getKp();
|
||||||
pid.iFactor = model->getKi();
|
pid.iFactor = model->getKi();
|
||||||
pid.dFactor = model->getKd();
|
pid.dFactor = model->getKd();
|
||||||
pid.offset = getPidOffset(model);
|
// round offset and period due to the firmware limitations...
|
||||||
pid.periodMs = 1000.0 / settings.timeScale;
|
pid.offset = round(getPidOffset(model));
|
||||||
|
pid.periodMs = round((float_t)(1000.0 / settings.timeScale));
|
||||||
|
|
||||||
pid0 = pid;
|
pid0 = pid;
|
||||||
|
|
||||||
|
@ -195,7 +198,7 @@ public:
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
double const *getParams() const {
|
double_t const *getParams() const {
|
||||||
return params;
|
return params;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -207,20 +210,20 @@ public:
|
||||||
return pid0;
|
return pid0;
|
||||||
}
|
}
|
||||||
|
|
||||||
double getAvgMeasuredMin() const {
|
double_t getAvgMeasuredMin() const {
|
||||||
return avgMeasuredMin;
|
return avgMeasuredMin;
|
||||||
}
|
}
|
||||||
|
|
||||||
double getAvgMeasuredMax() const {
|
double_t getAvgMeasuredMax() const {
|
||||||
return avgMeasuredMax;
|
return avgMeasuredMax;
|
||||||
}
|
}
|
||||||
|
|
||||||
double getModelBias() const {
|
double_t getModelBias() const {
|
||||||
return modelBias;
|
return modelBias;
|
||||||
}
|
}
|
||||||
|
|
||||||
// The model output is typically shifted
|
// The model output is typically shifted
|
||||||
double findModelBias() {
|
double_t findModelBias() {
|
||||||
if (settings.stepPoint < 0 || settings.maxPoint <= settings.stepPoint || settings.maxPoint > measuredData.getNumDataPoints())
|
if (settings.stepPoint < 0 || settings.maxPoint <= settings.stepPoint || settings.maxPoint > measuredData.getNumDataPoints())
|
||||||
return 0;
|
return 0;
|
||||||
// find the real 'min value' of the measured output data (before the step function goes up).
|
// find the real 'min value' of the measured output data (before the step function goes up).
|
||||||
|
@ -235,24 +238,24 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
// If the target value is known, we can estimate the PID offset value based on the model gain and bias.
|
// 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 {
|
float_t getPidOffset(ModelOpenLoopPlant *model) const {
|
||||||
if (settings.targetValue == 0.0)
|
if (std::isnan(settings.targetValue))
|
||||||
return 0;
|
return 0;
|
||||||
return (settings.targetValue - modelBias) / model->getParams()[PARAM_K];
|
return (float_t)((settings.targetValue - modelBias) / model->getParams()[PARAM_K]);
|
||||||
}
|
}
|
||||||
|
|
||||||
// See: Rangaiah G.P., Krishnaswamy P.R. Estimating Second-Order plus Dead Time Model Parameters, 1994.
|
// See: Rangaiah G.P., Krishnaswamy P.R. Estimating Second-Order plus Dead Time Model Parameters, 1994.
|
||||||
// Also see: "Practical PID Control", p. 169
|
// Also see: "Practical PID Control", p. 169
|
||||||
bool findFirstOrderInitialParams2Points(double *params) const {
|
bool findFirstOrderInitialParams2Points(double_t *params) const {
|
||||||
int i0 = (int)settings.stepPoint;
|
int i0 = (int)settings.stepPoint;
|
||||||
int i1 = (int)settings.maxPoint;
|
int i1 = (int)settings.maxPoint;
|
||||||
|
|
||||||
double dy = avgMeasuredMax - avgMeasuredMin;
|
double_t dy = avgMeasuredMax - avgMeasuredMin;
|
||||||
|
|
||||||
double t[2];
|
double_t t[2];
|
||||||
static const double tCoefs[] = { 0.353, 0.853 };
|
static const double_t tCoefs[] = { 0.353, 0.853 };
|
||||||
for (int i = 0; i < 2; i++) {
|
for (int i = 0; i < 2; i++) {
|
||||||
t[i] = getTimeDelta(measuredData.findDataAt((float)(avgMeasuredMin + dy * tCoefs[i]), i0, i1));
|
t[i] = getTimeDelta(measuredData.findDataAt((float_t)(avgMeasuredMin + dy * tCoefs[i]), i0, i1));
|
||||||
if (t[i] < 0.0)
|
if (t[i] < 0.0)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -266,37 +269,37 @@ public:
|
||||||
|
|
||||||
// See: Rangaiah G.P., Krishnaswamy P.R. Estimating Second-Order plus Dead Time Model Parameters, 1994.
|
// See: Rangaiah G.P., Krishnaswamy P.R. Estimating Second-Order plus Dead Time Model Parameters, 1994.
|
||||||
// Also see: "Practical PID Control", p. 187
|
// Also see: "Practical PID Control", p. 187
|
||||||
bool findSecondOrderInitialParams3Points(double *params) const {
|
bool findSecondOrderInitialParams3Points(double_t *params) const {
|
||||||
int i0 = (int)settings.stepPoint;
|
int i0 = (int)settings.stepPoint;
|
||||||
int i1 = (int)settings.maxPoint;
|
int i1 = (int)settings.maxPoint;
|
||||||
|
|
||||||
double dy = avgMeasuredMax - avgMeasuredMin;
|
double_t dy = avgMeasuredMax - avgMeasuredMin;
|
||||||
|
|
||||||
double t[3];
|
double_t t[3];
|
||||||
static const double tCoefs[] = { 0.14, 0.55, 0.91 };
|
static const double_t tCoefs[] = { 0.14, 0.55, 0.91 };
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
t[i] = getTimeDelta(measuredData.findDataAt((float)(avgMeasuredMin + dy * tCoefs[i]), i0, i1));
|
t[i] = getTimeDelta(measuredData.findDataAt((float_t)(avgMeasuredMin + dy * tCoefs[i]), i0, i1));
|
||||||
if (t[i] == 0.0)
|
if (t[i] == 0.0)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
double alpha = (t[2] - t[1]) / (t[1] - t[0]);
|
double_t alpha = (t[2] - t[1]) / (t[1] - t[0]);
|
||||||
#if 0
|
#if 0
|
||||||
// check if usable range?
|
// check if usable range?
|
||||||
if (alpha < 1.2323 || alpha > 2.4850) {
|
if (alpha < 1.2323 || alpha > 2.4850) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
double beta = log(alpha / (2.485 - alpha));
|
double_t beta = log(alpha / (2.485 - alpha));
|
||||||
double xi = 0.50906 + 0.51743 * beta - 0.076284 * pow(beta, 2) + 0.041363 * pow(beta, 3)
|
double_t xi = 0.50906 + 0.51743 * beta - 0.076284 * pow(beta, 2) + 0.041363 * pow(beta, 3)
|
||||||
- 0.0049224 * pow(beta, 4) + 0.00021234 * pow(beta, 5);
|
- 0.0049224 * pow(beta, 4) + 0.00021234 * pow(beta, 5);
|
||||||
double Tcoef = 0.85818 - 0.62907 * xi + 1.2897 * pow(xi, 2) - 0.36859 * pow(xi, 3) + 0.038891 * pow(xi, 4);
|
double_t Tcoef = 0.85818 - 0.62907 * xi + 1.2897 * pow(xi, 2) - 0.36859 * pow(xi, 3) + 0.038891 * pow(xi, 4);
|
||||||
double Lcoef = 1.39200 - 0.52536 * xi + 1.2991 * pow(xi, 2) - 0.36859 * pow(xi, 3) + 0.037605 * pow(xi, 4);
|
double_t Lcoef = 1.39200 - 0.52536 * xi + 1.2991 * pow(xi, 2) - 0.36859 * pow(xi, 3) + 0.037605 * pow(xi, 4);
|
||||||
|
|
||||||
double T = Tcoef / (t[1] - t[0]);
|
double_t T = Tcoef / (t[1] - t[0]);
|
||||||
// we've got T and xi, and we have to solve quadratic equation to get T1 and T2:
|
// we've got T and xi, and we have to solve quadratic equation to get T1 and T2:
|
||||||
// T = T1*T2
|
// T = T1*T2
|
||||||
// Xi = (T1 + T2) / (T1*T2)
|
// Xi = (T1 + T2) / (T1*T2)
|
||||||
double det = (T * xi) * (T * xi) - 4.0 * T;
|
double_t det = (T * xi) * (T * xi) - 4.0 * T;
|
||||||
|
|
||||||
params[PARAM_K] = dy / (settings.maxValue - settings.minValue);
|
params[PARAM_K] = dy / (settings.maxValue - settings.minValue);
|
||||||
if (det < 0) {
|
if (det < 0) {
|
||||||
|
@ -321,25 +324,25 @@ public:
|
||||||
|
|
||||||
// See: Harriott P. Process control (1964). McGraw-Hill. USA.
|
// See: Harriott P. Process control (1964). McGraw-Hill. USA.
|
||||||
// Also see: "Practical PID Control", p. 182
|
// Also see: "Practical PID Control", p. 182
|
||||||
bool findSecondOrderInitialParamsHarriott(double *params) const {
|
bool findSecondOrderInitialParamsHarriott(double_t *params) const {
|
||||||
static const HarriotFunction hfunc;
|
static const HarriotFunction hfunc;
|
||||||
|
|
||||||
int i0 = (int)settings.stepPoint;
|
int i0 = (int)settings.stepPoint;
|
||||||
int i1 = (int)settings.maxPoint;
|
int i1 = (int)settings.maxPoint;
|
||||||
|
|
||||||
double dy = avgMeasuredMax - avgMeasuredMin;
|
double_t dy = avgMeasuredMax - avgMeasuredMin;
|
||||||
|
|
||||||
double A1 = -measuredData.getArea(i0, i1, (float)avgMeasuredMax) / settings.timeScale;
|
double_t A1 = -measuredData.getArea(i0, i1, (float_t)avgMeasuredMax) / settings.timeScale;
|
||||||
|
|
||||||
double t73 = getTimeDelta(measuredData.findDataAt((float)(avgMeasuredMin + dy * 0.73), i0, i1));
|
double_t t73 = getTimeDelta(measuredData.findDataAt((float_t)(avgMeasuredMin + dy * 0.73), i0, i1));
|
||||||
double tm = i0 + 0.5 * (t73 / 1.3) * settings.timeScale;
|
double_t tm = i0 + 0.5 * (t73 / 1.3) * settings.timeScale;
|
||||||
double ym = measuredData.getValue((float)tm);
|
double_t ym = measuredData.getValue((float_t)tm);
|
||||||
// normalize
|
// normalize
|
||||||
double ymn = (ym - avgMeasuredMin) / dy;
|
double_t ymn = (ym - avgMeasuredMin) / dy;
|
||||||
// sanity check?
|
// sanity check?
|
||||||
if (ymn < HarriotFunction::minX || ymn > HarriotFunction::maxX)
|
if (ymn < HarriotFunction::minX || ymn > HarriotFunction::maxX)
|
||||||
return false;
|
return false;
|
||||||
double r = hfunc.getValue(ymn);
|
double_t r = hfunc.getValue(ymn);
|
||||||
|
|
||||||
params[PARAM_K] = dy / (settings.maxValue - settings.minValue);
|
params[PARAM_K] = dy / (settings.maxValue - settings.minValue);
|
||||||
params[PARAM_L] = A1 - t73 / 1.3;
|
params[PARAM_L] = A1 - t73 / 1.3;
|
||||||
|
@ -349,31 +352,33 @@ public:
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
double getTimeDelta(int i1) const {
|
double_t getTimeDelta(int i1) const {
|
||||||
if (i1 < 0)
|
if (i1 < 0)
|
||||||
return -1.0;
|
return -1.0;
|
||||||
return (double)(i1 - settings.stepPoint) / settings.timeScale;
|
return (double_t)(i1 - settings.stepPoint) / settings.timeScale;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Use automatic LM-solver to find the best PID coefs which satisfy the minimal PID metric.
|
// 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).
|
// The initial PID coefs are already calculated using the well-known CHR method (1st or 2nd order).
|
||||||
bool solveModel(pid_tune_method_e method, ModelAutoSolver & model) {
|
bool solveModel(pid_tune_method_e method, ModelAutoSolver & model) {
|
||||||
double merit0, merit;
|
double_t merit0, merit;
|
||||||
|
|
||||||
#ifdef PID_DEBUG
|
#ifdef PID_DEBUG
|
||||||
printf("* Solving for better coefs:\r\n");
|
printf("* Solving for better coefs:\r\n");
|
||||||
#endif
|
#endif
|
||||||
// todo: is it correct?
|
// todo: is it correct?
|
||||||
double dTime = 1.0 / settings.timeScale;
|
double_t dTime = pid.periodMs / 1000.0;
|
||||||
const int numSimPoints = 1024;
|
const int numSimPoints = 1024;
|
||||||
PidSimulatorFactory<numSimPoints> simFactory(simType, getMethodOrder(method), getAvgMeasuredMin(), getAvgMeasuredMax(), dTime, modelBias, pid);
|
PidSimulatorFactory<numSimPoints> simFactory(simType, getMethodOrder(method), getAvgMeasuredMin(), getAvgMeasuredMax(), settings.targetValue, dTime, modelBias, pid);
|
||||||
PidCoefsFinderFunction<numSimPoints> func(&simFactory, params);
|
PidCoefsFinderFunction<numSimPoints> func(&simFactory, params);
|
||||||
func.justifyParams(model.getParams());
|
func.justifyParams(model.getParams());
|
||||||
merit0 = func.calcMerit(model.getParams());
|
merit0 = func.calcMerit(model.getParams());
|
||||||
|
|
||||||
// now hopefully we'll find even better coefs!
|
// now hopefully we'll find even better coefs!
|
||||||
LevenbergMarquardtSolver<numParamsForPid> solver((LMSFunction<numParamsForPid> *)&func, model.getParams());
|
LevenbergMarquardtSolver<numParamsForPid> solver((LMSFunction<numParamsForPid> *)&func, model.getParams());
|
||||||
int iterationCount = solver.solve();
|
double lambdaForPid = 10.0;
|
||||||
|
double minDeltaForPid = 1.e-7;
|
||||||
|
int iterationCount = solver.solve(lambdaForPid, minDeltaForPid);
|
||||||
|
|
||||||
merit = func.calcMerit(model.getParams());
|
merit = func.calcMerit(model.getParams());
|
||||||
#ifdef PID_DEBUG
|
#ifdef PID_DEBUG
|
||||||
|
@ -383,21 +388,22 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
template <int numParams>
|
template <int numParams>
|
||||||
void outputFunc(const char *fname, const AbstractDelayLineFunction<numParams> & func, const StepFunction & stepFunc, double *params) {
|
void outputFunc(const char *fname, const AbstractDelayLineFunction<numParams> & func, const StepFunction & stepFunc, double_t *params) {
|
||||||
#ifdef PID_DEBUG
|
#ifdef PID_DEBUG
|
||||||
|
//func.calculateAllPoints(params);
|
||||||
for (int i = 0; i < func.getNumPoints(); i++) {
|
for (int i = 0; i < func.getNumPoints(); i++) {
|
||||||
double v = func.getEstimatedValueAtPoint(i, params);
|
double_t v = func.getEstimatedValueAtPoint(i, params);
|
||||||
double sv = stepFunc.getValue((float)i, 0);
|
double_t sv = stepFunc.getValue((float_t)i, 0);
|
||||||
output_csv(fname, (double)i, func.getDataPoint(i), v, sv);
|
output_csv(fname, (double_t)i, func.getDataPoint(i), v, sv);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void printSolverResult(int iterationCount, double merit0, double merit) {
|
void printSolverResult(int iterationCount, double_t merit0, double_t merit) {
|
||||||
if (iterationCount > 0)
|
if (iterationCount > 0)
|
||||||
printf("* The solver finished in %d iterations! (Merit: %g -> %g)\r\n", iterationCount, merit0, merit);
|
printf("* The solver finished in %d iterations! (Merit: %Lg -> %Lg)\r\n", iterationCount, (long double)merit0, (long double)merit);
|
||||||
else
|
else
|
||||||
printf("* The solver aborted after %d iterations! (Merit: %g -> %g)\r\n", -iterationCount, merit0, merit);
|
printf("* The solver aborted after %d iterations! (Merit: %Lg -> %Lg)\r\n", -iterationCount, (long double)merit0, (long double)merit);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -405,11 +411,11 @@ protected:
|
||||||
AveragingDataBuffer<MAX_DATA_POINTS> measuredData;
|
AveragingDataBuffer<MAX_DATA_POINTS> measuredData;
|
||||||
PidAutoTuneSettings settings;
|
PidAutoTuneSettings settings;
|
||||||
pid_sim_type_e simType;
|
pid_sim_type_e simType;
|
||||||
double params[4] = { 0 };
|
double_t params[4] = { 0 };
|
||||||
pid_s pid;
|
pid_s pid;
|
||||||
pid_s pid0; // not-optimized
|
pid_s pid0; // not-optimized
|
||||||
int iterationCount = 0;
|
int iterationCount = 0;
|
||||||
double avgMeasuredMin, avgMeasuredMax;
|
double_t avgMeasuredMin, avgMeasuredMax;
|
||||||
double modelBias;
|
double_t modelBias;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -25,7 +25,7 @@ public:
|
||||||
scaleShift = 0;
|
scaleShift = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void addDataPoint(float v) {
|
void addDataPoint(float_t v) {
|
||||||
int idx;
|
int idx;
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
|
@ -41,7 +41,7 @@ public:
|
||||||
}
|
}
|
||||||
scaleShift++;
|
scaleShift++;
|
||||||
}
|
}
|
||||||
float numInTheCell = (float)(num - ((num >> scaleShift) << scaleShift));
|
float_t numInTheCell = (float_t)(num - ((num >> scaleShift) << scaleShift));
|
||||||
buf[idx] *= numInTheCell;
|
buf[idx] *= numInTheCell;
|
||||||
buf[idx] += v;
|
buf[idx] += v;
|
||||||
buf[idx] /= (numInTheCell + 1.0f);
|
buf[idx] /= (numInTheCell + 1.0f);
|
||||||
|
@ -52,12 +52,12 @@ public:
|
||||||
return (num < 1) ? 1 : ((num - 1) >> scaleShift) + 1;
|
return (num < 1) ? 1 : ((num - 1) >> scaleShift) + 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
float const *getBuf() const {
|
float_t const *getBuf() const {
|
||||||
return buf;
|
return buf;
|
||||||
}
|
}
|
||||||
|
|
||||||
// This is a robust method for all rational indices
|
// This is a robust method for all rational indices
|
||||||
float getValue(float i) const {
|
float_t getValue(float_t i) const {
|
||||||
// todo: this works only for scale=1 :(
|
// todo: this works only for scale=1 :(
|
||||||
assert(scaleShift == 0);
|
assert(scaleShift == 0);
|
||||||
|
|
||||||
|
@ -76,20 +76,20 @@ public:
|
||||||
if (I >= num - 1)
|
if (I >= num - 1)
|
||||||
return buf[num - 1];
|
return buf[num - 1];
|
||||||
// get 2 closest values and interpolate
|
// get 2 closest values and interpolate
|
||||||
float fract = i - I;
|
float_t fract = i - I;
|
||||||
return buf[I + 1] * fract + buf[I] * (1.0f - fract);
|
return buf[I + 1] * fract + buf[I] * (1.0f - fract);
|
||||||
}
|
}
|
||||||
|
|
||||||
float getAveragedData(int from, int to) const {
|
float_t getAveragedData(int from, int to) const {
|
||||||
float avg = 0.0f;
|
float_t avg = 0.0f;
|
||||||
for (int i = from; i <= to; i++) {
|
for (int i = from; i <= to; i++) {
|
||||||
avg += buf[i];
|
avg += buf[i];
|
||||||
}
|
}
|
||||||
avg /= (float)(to - from + 1);
|
avg /= (float_t)(to - from + 1);
|
||||||
return avg;
|
return avg;
|
||||||
}
|
}
|
||||||
|
|
||||||
int findDataAt(float v, int from, int to) const {
|
int findDataAt(float_t v, int from, int to) const {
|
||||||
for (int i = from; i <= to; i++) {
|
for (int i = from; i <= to; i++) {
|
||||||
if (buf[i] > v)
|
if (buf[i] > v)
|
||||||
return i;
|
return i;
|
||||||
|
@ -98,8 +98,8 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
// integrating using simple trapezoidal method
|
// integrating using simple trapezoidal method
|
||||||
float getArea(int from, int to, float v0) const {
|
float_t getArea(int from, int to, float_t v0) const {
|
||||||
float sum = 0.0f;
|
float_t sum = 0.0f;
|
||||||
for (int i = from; i < to; i++) {
|
for (int i = from; i < to; i++) {
|
||||||
sum += buf[i] + buf[i + 1] - 2.0f * v0;
|
sum += buf[i] + buf[i + 1] - 2.0f * v0;
|
||||||
}
|
}
|
||||||
|
@ -108,7 +108,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
float buf[maxPoints];
|
float_t buf[maxPoints];
|
||||||
int num = 0;
|
int num = 0;
|
||||||
int scaleShift = 0;
|
int scaleShift = 0;
|
||||||
};
|
};
|
||||||
|
|
124
pid_controller.h
124
pid_controller.h
|
@ -1,4 +1,4 @@
|
||||||
/*
|
/*
|
||||||
* @file pid_controller.h
|
* @file pid_controller.h
|
||||||
*
|
*
|
||||||
* PID Controller models needed to verify the parameters.
|
* PID Controller models needed to verify the parameters.
|
||||||
|
@ -16,7 +16,7 @@ public:
|
||||||
PidController(const pid_s & p_) : p(p_) {
|
PidController(const pid_s & p_) : p(p_) {
|
||||||
}
|
}
|
||||||
|
|
||||||
double limitOutput(double v) {
|
double_t limitOutput(double_t v) {
|
||||||
if (v < p.minValue)
|
if (v < p.minValue)
|
||||||
v = p.minValue;
|
v = p.minValue;
|
||||||
if (v > p.maxValue)
|
if (v > p.maxValue)
|
||||||
|
@ -34,8 +34,8 @@ public:
|
||||||
pTerm = iTerm = dTerm = 0.0;
|
pTerm = iTerm = dTerm = 0.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
double getOutput(double target, double input, double dTime) {
|
double_t getOutput(double_t target, double_t input, double_t dTime) {
|
||||||
double error = target - input;
|
double_t error = target - input;
|
||||||
pTerm = p.pFactor * error;
|
pTerm = p.pFactor * error;
|
||||||
iTerm += p.iFactor * dTime * error;
|
iTerm += p.iFactor * dTime * error;
|
||||||
dTerm = p.dFactor / dTime * (error - previousError);
|
dTerm = p.dFactor / dTime * (error - previousError);
|
||||||
|
@ -45,8 +45,58 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
double pTerm, iTerm, dTerm;
|
double_t pTerm, iTerm, dTerm;
|
||||||
double previousError = 0;
|
double_t previousError = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
// PID with derivative filtering (backward differences) and integrator anti-windup.
|
||||||
|
// See: Wittenmark B., Åström K., Årzén K. IFAC Professional Brief. Computer Control: An Overview.
|
||||||
|
// Two additional parameters used: derivativeFilterLoss and antiwindupFreq
|
||||||
|
// (If both are 0, then this controller is identical to PidParallelController)
|
||||||
|
class PidIndustrialController : public PidParallelController {
|
||||||
|
public:
|
||||||
|
PidIndustrialController(const pid_s & p_) : PidParallelController(p_) {
|
||||||
|
}
|
||||||
|
|
||||||
|
double_t getOutput(double_t target, double_t input, double_t dTime) {
|
||||||
|
double_t ad, bd;
|
||||||
|
double_t error = target - input;
|
||||||
|
pTerm = p.pFactor * error;
|
||||||
|
|
||||||
|
// update the I-term
|
||||||
|
iTerm += p.iFactor * dTime * error;
|
||||||
|
|
||||||
|
// calculate dTerm coefficients
|
||||||
|
if (fabs(p.derivativeFilterLoss) > DBL_EPSILON) {
|
||||||
|
// restore Td in the Standard form from the Parallel form: Td = Kd / Kc
|
||||||
|
double_t Td = p.dFactor / p.pFactor;
|
||||||
|
// calculate the backward differences approximation of the derivative term
|
||||||
|
ad = Td / (Td + dTime / p.derivativeFilterLoss);
|
||||||
|
bd = p.pFactor * ad / p.derivativeFilterLoss;
|
||||||
|
} else {
|
||||||
|
// According to the Theory of limits, if p.derivativeFilterLoss -> 0, then
|
||||||
|
// lim(ad) = 0; lim(bd) = p.pFactor * Td / dTime = p.dFactor / dTime
|
||||||
|
// i.e. dTerm becomes equal to PidParallelController's
|
||||||
|
ad = 0.0;
|
||||||
|
bd = p.dFactor / dTime;
|
||||||
|
}
|
||||||
|
|
||||||
|
// (error - previousError) = (target-input) - (target-prevousInput) = -(input - prevousInput)
|
||||||
|
dTerm = dTerm * ad + (error - previousError) * bd;
|
||||||
|
|
||||||
|
// calculate output and apply the limits
|
||||||
|
double_t output = pTerm + iTerm + dTerm + p.offset;
|
||||||
|
double_t limitedOutput = limitOutput(output);
|
||||||
|
|
||||||
|
// apply the integrator anti-windup
|
||||||
|
// If p.antiwindupFreq = 0, then iTerm is equal to PidParallelController's
|
||||||
|
iTerm += dTime * p.antiwindupFreq * (limitedOutput - output);
|
||||||
|
|
||||||
|
// update the state
|
||||||
|
previousError = error;
|
||||||
|
|
||||||
|
return limitedOutput;
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
// C(s) = Kp + (Ki / s) + (N * Kd * s / (1 + N / s))
|
// C(s) = Kp + (Ki / s) + (N * Kd * s / (1 + N / s))
|
||||||
|
@ -54,24 +104,24 @@ protected:
|
||||||
// See: https://www.scilab.org/discrete-time-pid-controller-implementation
|
// See: https://www.scilab.org/discrete-time-pid-controller-implementation
|
||||||
class PidDerivativeFilterController : public PidController {
|
class PidDerivativeFilterController : public PidController {
|
||||||
public:
|
public:
|
||||||
PidDerivativeFilterController(const pid_s & p_, double n_) : PidController(p_), N(n_) {
|
PidDerivativeFilterController(const pid_s & p_, double_t n_) : PidController(p_), N(n_) {
|
||||||
}
|
}
|
||||||
|
|
||||||
double getOutput(double target, double input, double dTime) {
|
double_t getOutput(double_t target, double_t input, double_t dTime) {
|
||||||
double error = target - input;
|
double_t error = target - input;
|
||||||
double a0 = (1.0 + N * dTime);
|
double_t a0 = (1.0 + N * dTime);
|
||||||
double a1 = -(2.0 + N * dTime);
|
double_t a1 = -(2.0 + N * dTime);
|
||||||
|
|
||||||
double a2 = 1.0;
|
double_t a2 = 1.0;
|
||||||
double b0 = p.pFactor * (1.0 + N * dTime) + p.iFactor * dTime * (1.0 + N * dTime) + p.dFactor * N;
|
double_t b0 = p.pFactor * (1.0 + N * dTime) + p.iFactor * dTime * (1.0 + N * dTime) + p.dFactor * N;
|
||||||
double b1 = -(p.pFactor * (2.0 + N * dTime) + p.iFactor * dTime + 2.0 * p.dFactor * N);
|
double_t b1 = -(p.pFactor * (2.0 + N * dTime) + p.iFactor * dTime + 2.0 * p.dFactor * N);
|
||||||
double b2 = p.pFactor + p.dFactor * N;
|
double_t b2 = p.pFactor + p.dFactor * N;
|
||||||
|
|
||||||
double ku1 = a1 / a0;
|
double_t ku1 = a1 / a0;
|
||||||
double ku2 = a2 / a0;
|
double_t ku2 = a2 / a0;
|
||||||
double ke0 = b0 / a0;
|
double_t ke0 = b0 / a0;
|
||||||
double ke1 = b1 / a0;
|
double_t ke1 = b1 / a0;
|
||||||
double ke2 = b2 / a0;
|
double_t ke2 = b2 / a0;
|
||||||
|
|
||||||
e2 = e1;
|
e2 = e1;
|
||||||
e1 = e0;
|
e1 = e0;
|
||||||
|
@ -87,8 +137,8 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
double e2 = 0, e1 = 0, e0 = 0, u2 = 0, u1 = 0, u0 = 0;
|
double_t e2 = 0, e1 = 0, e0 = 0, u2 = 0, u1 = 0, u0 = 0;
|
||||||
double N = 1;
|
double_t N = 1;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Calculate ITAE/ISE and Overshoot
|
// Calculate ITAE/ISE and Overshoot
|
||||||
|
@ -101,35 +151,43 @@ public:
|
||||||
lastValue = 0;
|
lastValue = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void addPoint(double i, double value, double target) {
|
void addPoint(double_t i, double_t value, double_t target) {
|
||||||
double e = target - value;
|
double_t e = target - value;
|
||||||
itae += i * fabs(e);
|
itae += i * fabs(e);
|
||||||
ise += e * e;
|
ise += e * e;
|
||||||
double overshoot = (value - target) / target;
|
double_t overshoot = (value - target) / target;
|
||||||
if (overshoot > 0 && overshoot > maxOvershoot)
|
if (overshoot > 0 && overshoot > maxOvershoot)
|
||||||
maxOvershoot = overshoot;
|
maxOvershoot = overshoot;
|
||||||
lastValue = value;
|
lastValue = value;
|
||||||
}
|
}
|
||||||
|
|
||||||
double getItae() const {
|
double_t getItae() const {
|
||||||
return itae;
|
return itae;
|
||||||
}
|
}
|
||||||
|
|
||||||
double getIse() const {
|
double_t getIse() const {
|
||||||
return ise;
|
return ise;
|
||||||
}
|
}
|
||||||
|
|
||||||
double getMaxOvershoot() const {
|
double_t getMaxOvershoot() const {
|
||||||
return maxOvershoot;
|
return maxOvershoot;
|
||||||
}
|
}
|
||||||
|
|
||||||
double getLastValue() const {
|
double_t getLastValue() const {
|
||||||
return lastValue;
|
return lastValue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
double_t getMerit() const {
|
||||||
|
//return getItae();
|
||||||
|
#if 1
|
||||||
|
double overShootWeight = 10000.0;
|
||||||
|
return getItae() + overShootWeight * getMaxOvershoot() * getMaxOvershoot();
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
double itae = 0; // Integral time-weighted absolute error
|
double_t itae = 0; // Integral time-weighted absolute error
|
||||||
double ise = 0; // Integral square error
|
double_t ise = 0; // Integral square error
|
||||||
double maxOvershoot = 0;
|
double_t maxOvershoot = 0;
|
||||||
double lastValue = 0;
|
double_t lastValue = 0;
|
||||||
};
|
};
|
|
@ -14,7 +14,7 @@
|
||||||
|
|
||||||
class MslData {
|
class MslData {
|
||||||
public:
|
public:
|
||||||
bool readMsl(const char *fname, double startTime, double endTime, int inputIdx, int outputIdx) {
|
bool readMsl(const char *fname, double_t startTime, double_t endTime, int inputIdx, int outputIdx) {
|
||||||
std::ifstream fp(fname);
|
std::ifstream fp(fname);
|
||||||
|
|
||||||
if (!fp)
|
if (!fp)
|
||||||
|
@ -43,11 +43,11 @@ public:
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool parseLine(const std::string & str, double startTime, double endTime, int inputIdx, int outputIdx) {
|
bool parseLine(const std::string & str, double_t startTime, double_t endTime, int inputIdx, int outputIdx) {
|
||||||
std::stringstream sstr(str);
|
std::stringstream sstr(str);
|
||||||
std::string item;
|
std::string item;
|
||||||
for (int j = 0; getline(sstr, item, '\t'); j++) {
|
for (int j = 0; getline(sstr, item, '\t'); j++) {
|
||||||
double v = atof(item.c_str());
|
double_t v = atof(item.c_str());
|
||||||
// the first column is timestamp
|
// the first column is timestamp
|
||||||
if (j == 0) {
|
if (j == 0) {
|
||||||
if (v < startTime || v > endTime)
|
if (v < startTime || v > endTime)
|
||||||
|
@ -67,7 +67,7 @@ public:
|
||||||
}
|
}
|
||||||
curIdx++;
|
curIdx++;
|
||||||
} else if (j == outputIdx) {
|
} else if (j == outputIdx) {
|
||||||
data.push_back((float)v);
|
data.push_back((float_t)v);
|
||||||
if (curIdx >= 0 && settings.stepPoint < 0) {
|
if (curIdx >= 0 && settings.stepPoint < 0) {
|
||||||
// calculate averaged level to determine the acceptable noise level
|
// calculate averaged level to determine the acceptable noise level
|
||||||
averagedMin = (averagedMin * (curIdx - 1) + v) / curIdx;
|
averagedMin = (averagedMin * (curIdx - 1) + v) / curIdx;
|
||||||
|
@ -79,14 +79,14 @@ public:
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
double getSaturationStartPoint() {
|
double_t getSaturationStartPoint() {
|
||||||
int i;
|
int i;
|
||||||
double j;
|
double_t j;
|
||||||
// max noise level is used to get the saturation limit of the signal
|
// max noise level is used to get the saturation limit of the signal
|
||||||
double curNoiseLevel = 0, averagedMax = 0;
|
double_t curNoiseLevel = 0, averagedMax = 0;
|
||||||
// we step back some points from the last one and find the saturation start
|
// we step back some points from the last one and find the saturation start
|
||||||
for (i = curIdx - 1, j = 1.0; i > settings.stepPoint; i--, j += 1.0) {
|
for (i = curIdx - 1, j = 1.0; i > settings.stepPoint; i--, j += 1.0) {
|
||||||
double v = data[i];
|
double_t v = data[i];
|
||||||
averagedMax = (averagedMax * (j - 1) + v) / j;
|
averagedMax = (averagedMax * (j - 1) + v) / j;
|
||||||
// this is not accurate because 'averagedMax' is continuously changing
|
// this is not accurate because 'averagedMax' is continuously changing
|
||||||
curNoiseLevel = std::max(curNoiseLevel, abs(v - averagedMax));
|
curNoiseLevel = std::max(curNoiseLevel, abs(v - averagedMax));
|
||||||
|
@ -102,16 +102,16 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
std::vector<float> data;
|
std::vector<float_t> data;
|
||||||
double totalTime = 0, prevTime = 0;
|
double_t totalTime = 0, prevTime = 0;
|
||||||
PidAutoTuneSettings settings;
|
PidAutoTuneSettings settings;
|
||||||
int curIdx = -1;
|
int curIdx = -1;
|
||||||
float prevV = 0;
|
float_t prevV = 0;
|
||||||
|
|
||||||
// we assume that the signal is quasi-stable (asymptotic) from the start point until the 'stepPoint';
|
// we assume that the signal is quasi-stable (asymptotic) from the start point until the 'stepPoint';
|
||||||
// it's noise level is used to find the saturation limit of the rest of the data (see getSaturationStartPoint())
|
// it's noise level is used to find the saturation limit of the rest of the data (see getSaturationStartPoint())
|
||||||
double acceptableNoiseLevel = 0;
|
double_t acceptableNoiseLevel = 0;
|
||||||
double averagedMin = 0;
|
double_t averagedMin = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
const char *getMethodName(pid_tune_method_e method) {
|
const char *getMethodName(pid_tune_method_e method) {
|
||||||
|
@ -139,28 +139,30 @@ const char *getSimTypeName(pid_sim_type_e simType) {
|
||||||
#if 1
|
#if 1
|
||||||
int main(int argc, char **argv) {
|
int main(int argc, char **argv) {
|
||||||
if (argc < 6) {
|
if (argc < 6) {
|
||||||
printf("Usage: PID_FROM_MSL file.msl start_time end_time input_column output_column...\r\n");
|
printf("Usage: PID_FROM_MSL <file.msl> <startTime> <endTime> <inputColumnIdx> <outputColumnIdx> [<targetValue>]\r\n");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
printf("PID_FROM_MSL - find PID controller coefficients based on a measured step response in a rusEFI log file.\r\n");
|
printf("PID_FROM_MSL - find PID controller coefficients based on a measured step response in a rusEFI log file.\r\n");
|
||||||
printf("Version 0.2 (c) andreika, 2019\r\n\r\n");
|
printf("Version 0.3 (c) andreika, 2019\r\n\r\n");
|
||||||
printf("Reading file %s...\r\n", argv[1]);
|
printf("Reading file %s...\r\n", argv[1]);
|
||||||
|
|
||||||
MslData data;
|
MslData data;
|
||||||
if (!data.readMsl(argv[1], atof(argv[2]), atof(argv[3]), atoi(argv[4]), atoi(argv[5]))) {
|
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");
|
printf("Error reading from the file!\r\n");
|
||||||
return -2;
|
return -2;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Target value is optional, for PID_SIM_REGULATOR only
|
// Target value is optional, for PID_SIM_REGULATOR only
|
||||||
data.settings.targetValue = (argc > 6) ? atof(argv[6]) : 0.0;
|
data.settings.targetValue = (argc > 6) ? atof(argv[6]) : NAN;
|
||||||
|
|
||||||
printf("Measuring Settings: targetValue=%g minValue=%g maxValue=%g stepPoint=%g maxPoint=%g numPoints=%d timeScale=%g\r\n",
|
printf("Project Settings: FLT_EVAL_METHOD=%d sizeof(float_t)=%d sizeof(double_t)=%d\r\n", FLT_EVAL_METHOD, sizeof(float_t), sizeof(double_t));
|
||||||
data.settings.targetValue, data.settings.minValue, data.settings.maxValue,
|
printf("Measuring Settings: targetValue=%Lg minValue=%Lg maxValue=%Lg stepPoint=%Lg maxPoint=%Lg numPoints=%d timeScale=%Lg\r\n",
|
||||||
data.settings.stepPoint, data.settings.maxPoint, data.data.size(), data.settings.timeScale);
|
(long double)data.settings.targetValue, (long double)data.settings.minValue, (long double)data.settings.maxValue,
|
||||||
|
(long double)data.settings.stepPoint, (long double)data.settings.maxPoint, data.data.size(), (long double)data.settings.timeScale);
|
||||||
|
|
||||||
PidAutoTune chr1, chr2;
|
PidAutoTune chr1, chr2;
|
||||||
|
|
||||||
static const int numPids = 2;
|
static const int numPids = 2;
|
||||||
PidAutoTune *chr[numPids] = { &chr1, &chr2 };
|
PidAutoTune *chr[numPids] = { &chr1, &chr2 };
|
||||||
|
|
||||||
|
@ -181,38 +183,39 @@ int main(int argc, char **argv) {
|
||||||
|
|
||||||
printf("Done!\r\n");
|
printf("Done!\r\n");
|
||||||
|
|
||||||
// todo: is it correct?
|
|
||||||
double dTime = 1.0 / data.settings.timeScale;
|
|
||||||
const int numSimPoints = 1024;
|
const int numSimPoints = 1024;
|
||||||
|
|
||||||
pid_s bestPid;
|
pid_s bestPid;
|
||||||
double smallestItae = DBL_MAX;
|
double_t smallestMerit = DBL_MAX;
|
||||||
for (int k = 0; k < 2; k++) {
|
for (int k = 0; k < numPids; k++) {
|
||||||
const double *p = chr[k]->getParams();
|
const double_t *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]);
|
printf("Model-%d Params: K=%Lg T1=%Lg T2=%Lg L=%Lg\r\n", (k + 1), (long double)p[PARAM_K], (long double)p[PARAM_T], (long double)p[PARAM_T2], (long double)p[PARAM_L]);
|
||||||
|
|
||||||
pid_s pid[2] = { chr[k]->getPid0(), chr[k]->getPid() };
|
pid_s pid[2] = { chr[k]->getPid0(), chr[k]->getPid() };
|
||||||
for (int j = 0; j < 2; j++) {
|
for (int j = 0; j < 2; j++) {
|
||||||
|
double_t dTime = pid[j].periodMs / 1000.0;
|
||||||
const char *csvName = (j == 0) ? ((k == 0) ? "pid_test01.csv" : "pid_test02.csv") : ((k == 0) ? "pid_test1.csv" : "pid_test2.csv");
|
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]),
|
PidSimulator<numSimPoints> sim(simTypes[k], chr[k]->getMethodOrder(methods[k]),
|
||||||
chr[k]->getAvgMeasuredMin(), chr[k]->getAvgMeasuredMax(), dTime, chr[k]->getModelBias(), csvName);
|
chr[k]->getAvgMeasuredMin(), chr[k]->getAvgMeasuredMax(), data.settings.targetValue, dTime, chr[k]->getModelBias(), csvName);
|
||||||
|
|
||||||
PidAccuracyMetric metric = sim1.simulate(numSimPoints, pid[j], p);
|
sim.setModelParams(p);
|
||||||
|
PidAccuracyMetric metric = sim.simulate(numSimPoints, pid[j]);
|
||||||
|
|
||||||
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,
|
printf(" PID%d: P=%.8Lf I=%.8Lf D=%.8Lf offset=%.8Lf period=%.8Lfms\r\n", j, (long double)pid[j].pFactor, (long double)pid[j].iFactor, (long double)pid[j].dFactor,
|
||||||
pid[j].periodMs);
|
(long double)pid[j].offset, (long double)pid[j].periodMs);
|
||||||
printf(" Metric%d result: ITAE=%g ISE=%g Overshoot=%g%%\r\n", j, metric.getItae(), metric.getIse(), metric.getMaxOvershoot() * 100.0);
|
printf(" Metric%d result: %Lg ITAE=%Lg ISE=%Lg Overshoot=%Lg%%\r\n", j, (long double)metric.getMerit(), (long double)metric.getItae(), (long double)metric.getIse(),
|
||||||
|
(long double)(metric.getMaxOvershoot() * 100.0));
|
||||||
|
|
||||||
if (metric.getItae() < smallestItae) {
|
if (metric.getMerit() < smallestMerit) {
|
||||||
smallestItae = metric.getItae();
|
smallestMerit = metric.getMerit();
|
||||||
bestPid = pid[j];
|
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);
|
printf("The best PID: P=%.8Lf I=%.8Lf D=%.8Lf offset=%.1Lf period=%.1Lfms\r\n", (long double)bestPid.pFactor, (long double)bestPid.iFactor, (long double)bestPid.dFactor,
|
||||||
|
(long double)bestPid.offset, (long double)bestPid.periodMs);
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
104
pid_functions.h
104
pid_functions.h
|
@ -37,49 +37,49 @@ enum {
|
||||||
const int numParamsForPid = 3;
|
const int numParamsForPid = 3;
|
||||||
|
|
||||||
// the limit used for K and L params
|
// the limit used for K and L params
|
||||||
static const double minParamK = 0.001;
|
static const double_t minParamK = 0.001;
|
||||||
static const double minParamL = 0.1;
|
static const double_t minParamL = 0.1;
|
||||||
|
|
||||||
// T (or T2) has a separate limit, because exp(-1/T) is zero for small T values
|
// 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).
|
// (and we need to compare between them to find a direction for optimization method).
|
||||||
static const double minParamT = 0.01;
|
static const double_t minParamT = 0.01;
|
||||||
|
|
||||||
class InputFunction {
|
class InputFunction {
|
||||||
public:
|
public:
|
||||||
InputFunction(double timeScale_) : timeScale(timeScale_) {
|
InputFunction(double_t timeScale_) : timeScale(timeScale_) {
|
||||||
}
|
}
|
||||||
|
|
||||||
// i = index, d = delay time (in seconds?)
|
// i = index, d = delay time (in seconds?)
|
||||||
virtual double getValue(double i, double d) const = 0;
|
virtual double_t getValue(double_t i, double_t d) const = 0;
|
||||||
|
|
||||||
virtual double getTimeScale() const {
|
virtual double_t getTimeScale() const {
|
||||||
return timeScale;
|
return timeScale;
|
||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// time scale needed to synchronize between the virtual step function and the real measured data
|
// time scale needed to synchronize between the virtual step function and the real measured data
|
||||||
// timeScale=100 means 100 points per second.
|
// timeScale=100 means 100 points per second.
|
||||||
double timeScale;
|
double_t timeScale;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Heaviside step function interpolated between 'min' and 'max' values with 'stepPoint' time offset
|
// Heaviside step function interpolated between 'min' and 'max' values with 'stepPoint' time offset
|
||||||
class StepFunction : public InputFunction
|
class StepFunction : public InputFunction
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
StepFunction(double minValue_, double maxValue_, double stepPoint_, double timeScale_) :
|
StepFunction(double_t minValue_, double_t maxValue_, double_t stepPoint_, double_t timeScale_) :
|
||||||
minValue(minValue_), maxValue(maxValue_), stepPoint(stepPoint_), InputFunction(timeScale_) {
|
minValue(minValue_), maxValue(maxValue_), stepPoint(stepPoint_), InputFunction(timeScale_) {
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual double getValue(double i, double d) const {
|
virtual double_t getValue(double_t i, double_t d) const {
|
||||||
// delayed index
|
// delayed index
|
||||||
double id = i - d * timeScale;
|
double_t id = i - d * timeScale;
|
||||||
#ifdef INTERPOLATED_STEP_FUNCTION
|
#ifdef INTERPOLATED_STEP_FUNCTION
|
||||||
// the delay parameter L may not be integer, so we have to interpolate between the closest input values (near and far in the past)
|
// the delay parameter L may not be integer, so we have to interpolate between the closest input values (near and far in the past)
|
||||||
int I = (int)id;
|
int I = (int)id;
|
||||||
double fract = id - I; // 0 = choose near value, 1 = choose far value
|
double_t fract = id - I; // 0 = choose near value, 1 = choose far value
|
||||||
// find two closest input values for the given delay
|
// find two closest input values for the given delay
|
||||||
double vNear = (I < stepPoint) ? minValue : maxValue;
|
double_t vNear = (I < stepPoint) ? minValue : maxValue;
|
||||||
double vFar = (I + 1 < stepPoint) ? minValue : maxValue;
|
double_t vFar = (I + 1 < stepPoint) ? minValue : maxValue;
|
||||||
// interpolate
|
// interpolate
|
||||||
return vFar * fract + vNear * (1.0f - fract);
|
return vFar * fract + vNear * (1.0f - fract);
|
||||||
#else
|
#else
|
||||||
|
@ -88,15 +88,15 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
double minValue, maxValue;
|
double_t minValue, maxValue;
|
||||||
// stepPoint is float because we have AveragingDataBuffer, and the time axis may be scaled
|
// stepPoint is not integer because we have AveragingDataBuffer, and the time axis may be scaled
|
||||||
double stepPoint;
|
double_t stepPoint;
|
||||||
};
|
};
|
||||||
|
|
||||||
template <int numPoints>
|
template <int numPoints>
|
||||||
class StoredDataInputFunction : public InputFunction {
|
class StoredDataInputFunction : public InputFunction {
|
||||||
public:
|
public:
|
||||||
StoredDataInputFunction(double timeScale_) : InputFunction(timeScale_) {
|
StoredDataInputFunction(double_t timeScale_) : InputFunction(timeScale_) {
|
||||||
reset();
|
reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -104,14 +104,14 @@ public:
|
||||||
inputData.init();
|
inputData.init();
|
||||||
}
|
}
|
||||||
|
|
||||||
void addDataPoint(float v) {
|
void addDataPoint(float_t v) {
|
||||||
// todo: support data scaling
|
// todo: support data scaling
|
||||||
assert(inputData.getNumDataPoints() <= numPoints);
|
assert(inputData.getNumDataPoints() <= numPoints);
|
||||||
inputData.addDataPoint(v);
|
inputData.addDataPoint(v);
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual double getValue(double i, double d) const {
|
virtual double_t getValue(double_t i, double_t d) const {
|
||||||
return inputData.getValue((float)(i - d * timeScale));
|
return inputData.getValue((float_t)(i - d * timeScale));
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
@ -122,34 +122,34 @@ private:
|
||||||
template <int numParams>
|
template <int numParams>
|
||||||
class AbstractDelayLineFunction : public LMSFunction<numParams> {
|
class AbstractDelayLineFunction : public LMSFunction<numParams> {
|
||||||
public:
|
public:
|
||||||
AbstractDelayLineFunction(const InputFunction *input, const float *measuredOutput, int numDataPoints, double modelBias) {
|
AbstractDelayLineFunction(const InputFunction *input, const float_t *measuredOutput, int numDataPoints, double_t modelBias) {
|
||||||
dataPoints = measuredOutput;
|
dataPoints = measuredOutput;
|
||||||
inputFunc = input;
|
inputFunc = input;
|
||||||
numPoints = numDataPoints;
|
numPoints = numDataPoints;
|
||||||
this->modelBias = modelBias;
|
this->modelBias = modelBias;
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual double getResidual(int i, const double *params) const {
|
virtual double_t getResidual(int i, const double_t *params) const {
|
||||||
return dataPoints[i] - getEstimatedValueAtPoint(i, params);
|
return dataPoints[i] - this->getEstimatedValueAtPoint(i, params);
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual double getEstimatedValueAtPoint(int i, const double *params) const = 0;
|
virtual double_t calcEstimatedValuesAtPoint(int i, const double_t *params) const = 0;
|
||||||
|
|
||||||
// Get the total number of data points
|
// Get the total number of data points
|
||||||
virtual int getNumPoints() const {
|
virtual int getNumPoints() const {
|
||||||
return numPoints;
|
return numPoints;
|
||||||
}
|
}
|
||||||
|
|
||||||
float getDataPoint(int i) const {
|
float_t getDataPoint(int i) const {
|
||||||
return dataPoints[i];
|
return dataPoints[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
const InputFunction *inputFunc;
|
const InputFunction *inputFunc;
|
||||||
const float *dataPoints;
|
const float_t *dataPoints;
|
||||||
int numPoints;
|
int numPoints;
|
||||||
// needed to match the "ideal" curve and the real plant data; it doesn't affect the params but helps to fit the curve.
|
// 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;
|
double_t modelBias;
|
||||||
};
|
};
|
||||||
|
|
||||||
// FODPT indirect transfer function used for step response analytic simulation.
|
// FODPT indirect transfer function used for step response analytic simulation.
|
||||||
|
@ -157,33 +157,33 @@ protected:
|
||||||
// The Laplace representation is: K * exp(-L*s) / (T*s + 1)
|
// The Laplace representation is: K * exp(-L*s) / (T*s + 1)
|
||||||
class FirstOrderPlusDelayLineFunction : public AbstractDelayLineFunction<3> {
|
class FirstOrderPlusDelayLineFunction : public AbstractDelayLineFunction<3> {
|
||||||
public:
|
public:
|
||||||
FirstOrderPlusDelayLineFunction(const InputFunction *input, const float *measuredOutput, int numDataPoints, double modelBias) :
|
FirstOrderPlusDelayLineFunction(const InputFunction *input, const float_t *measuredOutput, int numDataPoints, double_t modelBias) :
|
||||||
AbstractDelayLineFunction(input, measuredOutput, numDataPoints, modelBias) {
|
AbstractDelayLineFunction(input, measuredOutput, numDataPoints, modelBias) {
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual void justifyParams(double *params) const {
|
virtual void justifyParams(double_t *params) const {
|
||||||
params[PARAM_L] = fmax(params[PARAM_L], minParamL);
|
params[PARAM_L] = fmax(params[PARAM_L], minParamL);
|
||||||
params[PARAM_T] = fmax(params[PARAM_T], minParamT);
|
params[PARAM_T] = fmax(params[PARAM_T], minParamT);
|
||||||
params[PARAM_K] = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K];
|
params[PARAM_K] = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K];
|
||||||
}
|
}
|
||||||
|
|
||||||
// Creating a state-space representation using Rosenbrock system matrix
|
// Creating a state-space representation using Rosenbrock system matrix
|
||||||
virtual double getEstimatedValueAtPoint(int i, const double *params) const {
|
virtual double_t calcEstimatedValuesAtPoint(int i, const double_t *params) const {
|
||||||
// only positive values allowed (todo: choose the limits)
|
// only positive values allowed (todo: choose the limits)
|
||||||
double pL = fmax(params[PARAM_L], minParamL);
|
double_t pL = fmax(params[PARAM_L], minParamL);
|
||||||
double pT = fmax(params[PARAM_T], minParamT);
|
double_t pT = fmax(params[PARAM_T], minParamT);
|
||||||
double pK = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K];
|
double_t pK = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K];
|
||||||
|
|
||||||
// state-space params
|
// state-space params
|
||||||
double lambda = exp(-1.0 / (pT * inputFunc->getTimeScale()));
|
double_t lambda = exp(-1.0 / (pT * inputFunc->getTimeScale()));
|
||||||
|
|
||||||
// todo: find better initial value?
|
// todo: find better initial value?
|
||||||
double y = inputFunc->getValue(0, 0) * pK;
|
double_t y = inputFunc->getValue(0, 0) * pK;
|
||||||
|
|
||||||
// The FO response function is indirect, so we need to iterate all previous values to find the current one
|
// The FO response function is indirect, so we need to iterate all previous values to find the current one
|
||||||
for (int j = 0; j <= i; j++) {
|
for (int j = 0; j <= i; j++) {
|
||||||
// delayed input
|
// delayed input
|
||||||
double inp = inputFunc->getValue((double)j, pL);
|
double_t inp = inputFunc->getValue((double_t)j, pL);
|
||||||
|
|
||||||
// indirect model response in Controllable Canonical Form (1st order CCF)
|
// indirect model response in Controllable Canonical Form (1st order CCF)
|
||||||
y = lambda * y + pK * (1.0 - lambda) * inp;
|
y = lambda * y + pK * (1.0 - lambda) * inp;
|
||||||
|
@ -200,11 +200,11 @@ public:
|
||||||
// The Laplace representation is: K * exp(-L * s) / ((T1*T2)*s^2 + (T1+T2)*s + 1)
|
// The Laplace representation is: K * exp(-L * s) / ((T1*T2)*s^2 + (T1+T2)*s + 1)
|
||||||
class SecondOrderPlusDelayLineOverdampedFunction : public AbstractDelayLineFunction<4> {
|
class SecondOrderPlusDelayLineOverdampedFunction : public AbstractDelayLineFunction<4> {
|
||||||
public:
|
public:
|
||||||
SecondOrderPlusDelayLineOverdampedFunction(const InputFunction *input, const float *measuredOutput, int numDataPoints, double modelBias) :
|
SecondOrderPlusDelayLineOverdampedFunction(const InputFunction *input, const float_t *measuredOutput, int numDataPoints, double_t modelBias) :
|
||||||
AbstractDelayLineFunction(input, measuredOutput, numDataPoints, modelBias) {
|
AbstractDelayLineFunction(input, measuredOutput, numDataPoints, modelBias) {
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual void justifyParams(double *params) const {
|
virtual void justifyParams(double_t *params) const {
|
||||||
params[PARAM_L] = fmax(params[PARAM_L], minParamL);
|
params[PARAM_L] = fmax(params[PARAM_L], minParamL);
|
||||||
params[PARAM_T] = fmax(params[PARAM_T], minParamT);
|
params[PARAM_T] = fmax(params[PARAM_T], minParamT);
|
||||||
params[PARAM_T2] = fmax(params[PARAM_T2], minParamT);
|
params[PARAM_T2] = fmax(params[PARAM_T2], minParamT);
|
||||||
|
@ -212,25 +212,25 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
// Creating a state-space representation using Rosenbrock system matrix
|
// Creating a state-space representation using Rosenbrock system matrix
|
||||||
virtual double getEstimatedValueAtPoint(int i, const double *params) const {
|
virtual double_t calcEstimatedValuesAtPoint(int i, const double_t *params) const {
|
||||||
// only positive values allowed (todo: choose the limits)
|
// only positive values allowed (todo: choose the limits)
|
||||||
double pL = fmax(params[PARAM_L], minParamL);
|
double_t pL = fmax(params[PARAM_L], minParamL);
|
||||||
double pT = fmax(params[PARAM_T], minParamT);
|
double_t pT = fmax(params[PARAM_T], minParamT);
|
||||||
double pT2 = fmax(params[PARAM_T2], minParamT);
|
double_t pT2 = fmax(params[PARAM_T2], minParamT);
|
||||||
double pK = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K];
|
double_t pK = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K];
|
||||||
|
|
||||||
// state-space params
|
// state-space params
|
||||||
double lambda = exp(-1.0 / (pT * inputFunc->getTimeScale()));
|
double_t lambda = exp(-1.0 / (pT * inputFunc->getTimeScale()));
|
||||||
double lambda2 = exp(-1.0 / (pT2 * inputFunc->getTimeScale()));
|
double_t lambda2 = exp(-1.0 / (pT2 * inputFunc->getTimeScale()));
|
||||||
|
|
||||||
// todo: find better initial values?
|
// todo: find better initial values?
|
||||||
double x = inputFunc->getValue(0, 0) * pK;
|
double_t x = inputFunc->getValue(0, 0) * pK;
|
||||||
double y = inputFunc->getValue(0, 0) * pK;
|
double_t y = inputFunc->getValue(0, 0) * pK;
|
||||||
|
|
||||||
// The SO response function is indirect, so we need to iterate all previous values to find the current one
|
// The SO response function is indirect, so we need to iterate all previous values to find the current one
|
||||||
for (int j = 0; j <= i; j++) {
|
for (int j = 0; j <= i; j++) {
|
||||||
// delayed input
|
// delayed input
|
||||||
double inp = inputFunc->getValue((double)j, pL);
|
double_t inp = inputFunc->getValue((double_t)j, pL);
|
||||||
|
|
||||||
// indirect model response in Controllable Canonical Form (2nd order CCF)
|
// indirect model response in Controllable Canonical Form (2nd order CCF)
|
||||||
y = lambda2 * y + (1.0 - lambda2) * x;
|
y = lambda2 * y + (1.0 - lambda2) * x;
|
||||||
|
@ -247,12 +247,12 @@ public:
|
||||||
// See: findSecondOrderInitialParamsHarriot() and "Harriot P. Process control (1964). McGraw-Hill. USA."
|
// See: findSecondOrderInitialParamsHarriot() and "Harriot P. Process control (1964). McGraw-Hill. USA."
|
||||||
class HarriotFunction {
|
class HarriotFunction {
|
||||||
public:
|
public:
|
||||||
double getValue(double x) const {
|
double_t getValue(double_t x) const {
|
||||||
return buf.getValue((float)((x - 2.8 / 719.0 - 0.26) * 719.0 / 2.8));
|
return buf.getValue((float_t)((x - 2.8 / 719.0 - 0.26) * 719.0 / 2.8));
|
||||||
}
|
}
|
||||||
|
|
||||||
static constexpr double minX = 2.8 / 719.0 - 0.26;
|
static constexpr double_t minX = 2.8 / 719.0 - 0.26;
|
||||||
static constexpr double maxX = 33 / 719.0 * 2.8 + minX;
|
static constexpr double_t maxX = 33 / 719.0 * 2.8 + minX;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const AveragingDataBuffer<34> buf = { {
|
const AveragingDataBuffer<34> buf = { {
|
||||||
|
|
|
@ -39,27 +39,27 @@ class ModelOpenLoopPlant
|
||||||
public:
|
public:
|
||||||
ModelOpenLoopPlant() {}
|
ModelOpenLoopPlant() {}
|
||||||
|
|
||||||
ModelOpenLoopPlant(const double *params_) : params((double *)params_) {
|
ModelOpenLoopPlant(const double_t *params_) : params((double_t *)params_) {
|
||||||
}
|
}
|
||||||
|
|
||||||
double *getParams() {
|
double_t *getParams() {
|
||||||
return params;
|
return params;
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual float getKp() const = 0;
|
virtual float_t getKp() const = 0;
|
||||||
virtual float getKi() const = 0;
|
virtual float_t getKi() const = 0;
|
||||||
virtual float getKd() const = 0;
|
virtual float_t getKd() const = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
double *params;
|
double_t *params;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Based on FOPDT model approximated from Overdamped-SOPDT model
|
// Based on FOPDT model approximated from Overdamped-SOPDT model
|
||||||
class ModelFopdtApproximatedFromSopdt : public ModelOpenLoopPlant {
|
class ModelFopdtApproximatedFromSopdt : public ModelOpenLoopPlant {
|
||||||
public:
|
public:
|
||||||
ModelFopdtApproximatedFromSopdt(double *soParams) : ModelOpenLoopPlant(p) {
|
ModelFopdtApproximatedFromSopdt(double_t *soParams) : ModelOpenLoopPlant(p) {
|
||||||
double T2divT1 = soParams[PARAM_T2] / soParams[PARAM_T];
|
double_t T2divT1 = soParams[PARAM_T2] / soParams[PARAM_T];
|
||||||
double T2mulT1 = soParams[PARAM_T2] * soParams[PARAM_T];
|
double_t T2mulT1 = soParams[PARAM_T2] * soParams[PARAM_T];
|
||||||
params[PARAM_K] = soParams[PARAM_K];
|
params[PARAM_K] = soParams[PARAM_K];
|
||||||
params[PARAM_T] = 0.828 + 0.812 * T2divT1 + 0.172 * exp(-6.9 * T2divT1) * soParams[PARAM_T];
|
params[PARAM_T] = 0.828 + 0.812 * T2divT1 + 0.172 * exp(-6.9 * T2divT1) * soParams[PARAM_T];
|
||||||
params[PARAM_L] = 1.116 * T2mulT1 / (soParams[PARAM_T] + 1.208 * soParams[PARAM_T2]) + soParams[PARAM_L];
|
params[PARAM_L] = 1.116 * T2mulT1 / (soParams[PARAM_T] + 1.208 * soParams[PARAM_T2]) + soParams[PARAM_L];
|
||||||
|
@ -69,60 +69,60 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
// we don't really need them, because this model is just an intermediate
|
// we don't really need them, because this model is just an intermediate
|
||||||
virtual float getKp() const {
|
virtual float_t getKp() const {
|
||||||
return 0.0f;
|
return 0.0f;
|
||||||
}
|
}
|
||||||
virtual float getKi() const {
|
virtual float_t getKi() const {
|
||||||
return 0.0f;
|
return 0.0f;
|
||||||
}
|
}
|
||||||
virtual float getKd() const {
|
virtual float_t getKd() const {
|
||||||
return 0.0f;
|
return 0.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// A storage for the new 1st order params
|
// A storage for the new 1st order params
|
||||||
double p[3];
|
double_t p[3];
|
||||||
};
|
};
|
||||||
|
|
||||||
// Standard PID model: Kc * (1 + 1/(Ti*S) + Td * S)
|
// Standard PID model: Kc * (1 + 1/(Ti*S) + Td * S)
|
||||||
// This class converts in into our "Parallel" form: Kp + Ki / S + Kd * S
|
// This class converts in into our "Parallel" form: Kp + Ki / S + Kd * S
|
||||||
class ModelStandard : public ModelOpenLoopPlant {
|
class ModelStandard : public ModelOpenLoopPlant {
|
||||||
public:
|
public:
|
||||||
ModelStandard(const double *params_) : ModelOpenLoopPlant(params_) {
|
ModelStandard(const double_t *params_) : ModelOpenLoopPlant(params_) {
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual float getKp() const {
|
virtual float_t getKp() const {
|
||||||
return (float)Kc;
|
return (float_t)Kc;
|
||||||
}
|
}
|
||||||
virtual float getKi() const {
|
virtual float_t getKi() const {
|
||||||
return (float)(Kc / Ti);
|
return (float_t)(Kc / Ti);
|
||||||
}
|
}
|
||||||
virtual float getKd() const {
|
virtual float_t getKd() const {
|
||||||
return (float)(Kc * Td);
|
return (float_t)(Kc * Td);
|
||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// "Standard" PID coefs
|
// "Standard" PID coefs
|
||||||
double Kc, Ti, Td;
|
double_t Kc, Ti, Td;
|
||||||
};
|
};
|
||||||
|
|
||||||
class ModelStandardIMC : public ModelStandard {
|
class ModelStandardIMC : public ModelStandard {
|
||||||
public:
|
public:
|
||||||
ModelStandardIMC(const double *params_) : ModelStandard(params_) {
|
ModelStandardIMC(const double_t *params_) : ModelStandard(params_) {
|
||||||
lambda = fmax(0.25 * params[PARAM_L], 0.2 * Ti);
|
lambda = fmax(0.25 * params[PARAM_L], 0.2 * Ti);
|
||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// closed-loop speed of response
|
// closed-loop speed of response
|
||||||
double lambda;
|
double_t lambda;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
// Chien-Hrones-Reswick PID implementation for the 1st order model (generic model).
|
// Chien-Hrones-Reswick PID implementation for the 1st order model (generic model).
|
||||||
class ModelChienHronesReswickFirstOrder : public ModelStandardIMC {
|
class ModelChienHronesReswickFirstOrder : public ModelStandardIMC {
|
||||||
public:
|
public:
|
||||||
ModelChienHronesReswickFirstOrder(const double *params_) : ModelStandardIMC(params_) {
|
ModelChienHronesReswickFirstOrder(const double_t *params_) : ModelStandardIMC(params_) {
|
||||||
double l2 = params[PARAM_L] / 2.0;
|
double_t l2 = params[PARAM_L] / 2.0;
|
||||||
Ti = params[PARAM_T] + l2;
|
Ti = params[PARAM_T] + l2;
|
||||||
Td = params[PARAM_T] * params[PARAM_L] / (2 * params[PARAM_T] + params[PARAM_L]);
|
Td = params[PARAM_T] * params[PARAM_L] / (2 * params[PARAM_T] + params[PARAM_L]);
|
||||||
Kc = Ti / (params[PARAM_K] * (lambda + l2));
|
Kc = Ti / (params[PARAM_K] * (lambda + l2));
|
||||||
|
@ -132,34 +132,34 @@ public:
|
||||||
// Chien-Hrones-Reswick PID implementation for the 1st order model (set-point regulation).
|
// Chien-Hrones-Reswick PID implementation for the 1st order model (set-point regulation).
|
||||||
class ModelChienHronesReswickFirstOrderSetpoint : public ModelOpenLoopPlant {
|
class ModelChienHronesReswickFirstOrderSetpoint : public ModelOpenLoopPlant {
|
||||||
public:
|
public:
|
||||||
ModelChienHronesReswickFirstOrderSetpoint(const double *params_) : ModelOpenLoopPlant(params_) {
|
ModelChienHronesReswickFirstOrderSetpoint(const double_t *params_) : ModelOpenLoopPlant(params_) {
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual float getKp() const {
|
virtual float_t getKp() const {
|
||||||
return (float)(0.6f / params[PARAM_K]);
|
return (float_t)(0.6f / params[PARAM_K]);
|
||||||
}
|
}
|
||||||
virtual float getKi() const {
|
virtual float_t getKi() const {
|
||||||
return (float)(1.0f / params[PARAM_T]);
|
return (float_t)(1.0f / params[PARAM_T]);
|
||||||
}
|
}
|
||||||
virtual float getKd() const {
|
virtual float_t getKd() const {
|
||||||
return (float)(1.0f / (0.5f * params[PARAM_L]));
|
return (float_t)(1.0f / (0.5f * params[PARAM_L]));
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
// Chien-Hrones-Reswick PID implementation for the 1st order model (disturbance rejection).
|
// Chien-Hrones-Reswick PID implementation for the 1st order model (disturbance rejection).
|
||||||
class ModelChienHronesReswickFirstOrderDisturbance : public ModelOpenLoopPlant {
|
class ModelChienHronesReswickFirstOrderDisturbance : public ModelOpenLoopPlant {
|
||||||
public:
|
public:
|
||||||
ModelChienHronesReswickFirstOrderDisturbance(const double *params_) : ModelOpenLoopPlant(params_) {
|
ModelChienHronesReswickFirstOrderDisturbance(const double_t *params_) : ModelOpenLoopPlant(params_) {
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual float getKp() const {
|
virtual float_t getKp() const {
|
||||||
return (float)(0.95f / params[PARAM_K]);
|
return (float_t)(0.95f / params[PARAM_K]);
|
||||||
}
|
}
|
||||||
virtual float getKi() const {
|
virtual float_t getKi() const {
|
||||||
return (float)(2.4f / params[PARAM_T]);
|
return (float_t)(2.4f / params[PARAM_T]);
|
||||||
}
|
}
|
||||||
virtual float getKd() const {
|
virtual float_t getKd() const {
|
||||||
return (float)(1.0f / (0.42f * params[PARAM_L]));
|
return (float_t)(1.0f / (0.42f * params[PARAM_L]));
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -167,7 +167,7 @@ public:
|
||||||
// See "Panda R.C., Yu C.C., Huang H.P. PID tuning rules for SOPDT systems: Review and some new results"
|
// See "Panda R.C., Yu C.C., Huang H.P. PID tuning rules for SOPDT systems: Review and some new results"
|
||||||
class ModelRiveraMorariFirstOrder : public ModelStandardIMC {
|
class ModelRiveraMorariFirstOrder : public ModelStandardIMC {
|
||||||
public:
|
public:
|
||||||
ModelRiveraMorariFirstOrder(const double *params_) : ModelStandardIMC(params_) {
|
ModelRiveraMorariFirstOrder(const double_t *params_) : ModelStandardIMC(params_) {
|
||||||
Kc = (2 * params[PARAM_T] + params[PARAM_L]) / (2 * params[PARAM_K] * (lambda + params[PARAM_L]));
|
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];
|
Ti = params[PARAM_T] + 0.5 * params[PARAM_L];
|
||||||
Td = params[PARAM_T] * params[PARAM_L] / (2.0 * params[PARAM_T] + params[PARAM_L]);
|
Td = params[PARAM_T] * params[PARAM_L] / (2.0 * params[PARAM_T] + params[PARAM_L]);
|
||||||
|
@ -178,7 +178,7 @@ public:
|
||||||
// "Proceedings of the IFAC adaptive control of chemical processes conference, Copenhagen, Denmark, 1988, pp. 147-152."
|
// "Proceedings of the IFAC adaptive control of chemical processes conference, Copenhagen, Denmark, 1988, pp. 147-152."
|
||||||
class ModelChienHronesReswickSecondOrder : public ModelStandardIMC {
|
class ModelChienHronesReswickSecondOrder : public ModelStandardIMC {
|
||||||
public:
|
public:
|
||||||
ModelChienHronesReswickSecondOrder(const double *params_) : ModelStandardIMC(params_) {
|
ModelChienHronesReswickSecondOrder(const double_t *params_) : ModelStandardIMC(params_) {
|
||||||
Ti = params[PARAM_T] + params[PARAM_T2];
|
Ti = params[PARAM_T] + params[PARAM_T2];
|
||||||
Td = params[PARAM_T] * params[PARAM_T2] / Ti;
|
Td = params[PARAM_T] * params[PARAM_T2] / Ti;
|
||||||
Kc = Ti / (params[PARAM_K] * (lambda + params[PARAM_L]));
|
Kc = Ti / (params[PARAM_K] * (lambda + params[PARAM_L]));
|
||||||
|
@ -189,8 +189,8 @@ public:
|
||||||
// "Step disturbance".
|
// "Step disturbance".
|
||||||
class ModelVanDerGrintenSecondOrder : public ModelStandard {
|
class ModelVanDerGrintenSecondOrder : public ModelStandard {
|
||||||
public:
|
public:
|
||||||
ModelVanDerGrintenSecondOrder(const double *params_) : ModelStandard(params_) {
|
ModelVanDerGrintenSecondOrder(const double_t *params_) : ModelStandard(params_) {
|
||||||
double T12 = params[PARAM_T] + params[PARAM_T2];
|
double_t T12 = params[PARAM_T] + params[PARAM_T2];
|
||||||
Ti = T12 + 0.5 * params[PARAM_L];
|
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);
|
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];
|
Kc = (0.5 + T12 / params[PARAM_L]) / params[PARAM_K];
|
||||||
|
@ -201,9 +201,9 @@ public:
|
||||||
// Suited for overdamped response and significant delay.
|
// Suited for overdamped response and significant delay.
|
||||||
class ModelHaalmanPembertonSecondOrder : public ModelStandard {
|
class ModelHaalmanPembertonSecondOrder : public ModelStandard {
|
||||||
public:
|
public:
|
||||||
ModelHaalmanPembertonSecondOrder(const double *params_) : ModelStandard(params_) {
|
ModelHaalmanPembertonSecondOrder(const double_t *params_) : ModelStandard(params_) {
|
||||||
double T1divT2 = params[PARAM_T] / params[PARAM_T2];
|
double_t T1divT2 = params[PARAM_T] / params[PARAM_T2];
|
||||||
double LdivT2 = params[PARAM_L] / params[PARAM_T2];
|
double_t LdivT2 = params[PARAM_L] / params[PARAM_T2];
|
||||||
Ti = params[PARAM_T] + params[PARAM_T2];
|
Ti = params[PARAM_T] + params[PARAM_T2];
|
||||||
Kc = 2.0 * Ti / (3 * params[PARAM_K] * params[PARAM_L]);
|
Kc = 2.0 * Ti / (3 * params[PARAM_K] * params[PARAM_L]);
|
||||||
|
|
||||||
|
@ -220,11 +220,11 @@ public:
|
||||||
// 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.
|
// 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 {
|
class ModelMaclaurinSecondOrder : public ModelStandardIMC {
|
||||||
public:
|
public:
|
||||||
ModelMaclaurinSecondOrder(const double *params_) : ModelStandardIMC(params_) {
|
ModelMaclaurinSecondOrder(const double_t *params_) : ModelStandardIMC(params_) {
|
||||||
double T1T2 = params[PARAM_T] + params[PARAM_T2];
|
double_t T1T2 = params[PARAM_T] + params[PARAM_T2];
|
||||||
double L = params[PARAM_L];
|
double_t L = params[PARAM_L];
|
||||||
double L2 = L * L;
|
double_t L2 = L * L;
|
||||||
double twolL = 2.0 * lambda + L;
|
double_t twolL = 2.0 * lambda + L;
|
||||||
Ti = T1T2 - (2.0 * lambda * lambda - L2) / (2.0 * twolL);
|
Ti = T1T2 - (2.0 * lambda * lambda - L2) / (2.0 * twolL);
|
||||||
Kc = Ti / (params[PARAM_K] * twolL);
|
Kc = Ti / (params[PARAM_K] * twolL);
|
||||||
Td = Ti - T1T2 + (params[PARAM_T] * params[PARAM_T2] - L2 * L / (6.0 * twolL)) / Ti;
|
Td = Ti - T1T2 + (params[PARAM_T] * params[PARAM_T2] - L2 * L / (6.0 * twolL)) / Ti;
|
||||||
|
@ -239,16 +239,16 @@ public:
|
||||||
pidParams[PARAM_Kd] = initial->getKd();
|
pidParams[PARAM_Kd] = initial->getKd();
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual float getKp() const {
|
virtual float_t getKp() const {
|
||||||
return (float)pidParams[PARAM_Kp];
|
return (float_t)pidParams[PARAM_Kp];
|
||||||
}
|
}
|
||||||
virtual float getKi() const {
|
virtual float_t getKi() const {
|
||||||
return (float)pidParams[PARAM_Ki];
|
return (float_t)pidParams[PARAM_Ki];
|
||||||
}
|
}
|
||||||
virtual float getKd() const {
|
virtual float_t getKd() const {
|
||||||
return (float)pidParams[PARAM_Kd];
|
return (float_t)pidParams[PARAM_Kd];
|
||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
double pidParams[numParamsForPid];
|
double_t pidParams[numParamsForPid];
|
||||||
};
|
};
|
||||||
|
|
92
pid_sim.h
92
pid_sim.h
|
@ -21,26 +21,32 @@ typedef enum {
|
||||||
template <int maxPoints>
|
template <int maxPoints>
|
||||||
class PidSimulator {
|
class PidSimulator {
|
||||||
public:
|
public:
|
||||||
PidSimulator(pid_sim_type_e simType_, int order, double target1_, double target2_, double dTime_, double modelBias, const char *outputFile_) :
|
PidSimulator(pid_sim_type_e simType_, int order, double_t target1_, double_t target2_, double_t target_, double_t dTime_, double_t modelBias, const char *outputFile_) :
|
||||||
simType(simType_), target1(target1_), target2(target2_), dTime(dTime_), methodOrder(order), outputFile(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) {
|
plantInput(1.0 / dTime_), plant1(&plantInput, nullptr, maxPoints, modelBias), plant2(&plantInput, nullptr, maxPoints, modelBias) {
|
||||||
}
|
// if we don't know the target, we want to be in the middle for safety
|
||||||
|
target = std::isnan(target_) ? ((target1 + target2) / 2.0) : target_;
|
||||||
|
|
||||||
PidAccuracyMetric simulate(int numPoints, const pid_s & pidParams, const double *params) {
|
|
||||||
LMSFunction<4> *plant;
|
|
||||||
if (methodOrder == 1)
|
if (methodOrder == 1)
|
||||||
plant = (LMSFunction<4> *)&plant1;
|
plant = (LMSFunction<4> *)&plant1;
|
||||||
else
|
else
|
||||||
plant = &plant2;
|
plant = &plant2;
|
||||||
|
}
|
||||||
|
|
||||||
PidParallelController pid(pidParams);
|
void setModelParams(const double_t *modelParams) {
|
||||||
//PidDerivativeFilterController pid(pidParams, 10);
|
this->modelParams = modelParams;
|
||||||
|
//plant->calculateAllPoints(modelParams);
|
||||||
|
}
|
||||||
|
|
||||||
|
PidAccuracyMetric simulate(int numPoints, const pid_s & pidParams) {
|
||||||
|
//PidParallelController pid(pidParams);
|
||||||
|
PidIndustrialController pid(pidParams);
|
||||||
|
|
||||||
plantInput.reset();
|
plantInput.reset();
|
||||||
// guess a previous state to minimize the system "shock"
|
// guess a previous state to minimize the system "shock"
|
||||||
plantInput.addDataPoint((float)(getSetpoint(0) / params[PARAM_K]) - pidParams.offset);
|
plantInput.addDataPoint((float_t)(getSetpoint(0) / modelParams[PARAM_K]) - pidParams.offset);
|
||||||
// "calm down" the PID controller to avoid huge spikes at the beginning
|
// "calm down" the PID controller to avoid huge spikes at the beginning
|
||||||
pid.getOutput(getSetpoint(0), plant->getEstimatedValueAtPoint(0, params), dTime);
|
pid.getOutput(getSetpoint(0), plant->calcEstimatedValuesAtPoint(0, modelParams), dTime);
|
||||||
|
|
||||||
stepPoint = maxPoints / 2;
|
stepPoint = maxPoints / 2;
|
||||||
|
|
||||||
|
@ -49,46 +55,46 @@ public:
|
||||||
// simulate over time
|
// simulate over time
|
||||||
for (int i = 0; i < numPoints; i++) {
|
for (int i = 0; i < numPoints; i++) {
|
||||||
// make a step in the middle
|
// make a step in the middle
|
||||||
double target = getSetpoint(i);
|
double_t target = getSetpoint(i);
|
||||||
// "measure" the current value of the plant
|
// "measure" the current value of the plant (we cannot use getEstimatedValueAtPoint() because the function input is changing)
|
||||||
double pidInput = plant->getEstimatedValueAtPoint(i, params) + getLoadDisturbance(i);
|
double_t pidInput = plant->calcEstimatedValuesAtPoint(i, modelParams) + getLoadDisturbance(i);
|
||||||
// wait for the controller reaction
|
// wait for the controller reaction
|
||||||
double pidOutput = pid.getOutput(target, pidInput, dTime);
|
double_t pidOutput = pid.getOutput(target, pidInput, dTime);
|
||||||
|
|
||||||
// apply the reaction to the plant's pidInput
|
// apply the reaction to the plant's pidInput
|
||||||
plantInput.addDataPoint((float)pidOutput);
|
plantInput.addDataPoint((float_t)pidOutput);
|
||||||
// don't take into account any start-up transients, we're interested only in our step response!
|
// don't take into account any start-up transients, we're interested only in our step response!
|
||||||
if (i >= stepPoint)
|
if (i >= stepPoint)
|
||||||
metric.addPoint((double)i / (double)numPoints, pidInput, target);
|
metric.addPoint((double_t)i / (double_t)numPoints, pidInput, target);
|
||||||
#ifdef PID_DEBUG
|
#ifdef PID_DEBUG
|
||||||
if (outputFile != nullptr)
|
if (outputFile != nullptr)
|
||||||
output_csv(outputFile, (double)i, pidOutput, target, pidInput);
|
output_csv(outputFile, (double_t)i, pidOutput, target, pidInput);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
return metric;
|
return metric;
|
||||||
}
|
}
|
||||||
|
|
||||||
double getSetpoint(int i) const {
|
double_t getSetpoint(int i) const {
|
||||||
switch (simType) {
|
switch (simType) {
|
||||||
case PID_SIM_SERVO:
|
case PID_SIM_SERVO:
|
||||||
return (i > stepPoint) ? target2 : target1;
|
return (i > stepPoint) ? target2 : target1;
|
||||||
default:
|
default:
|
||||||
// we want to be in the middle for safety
|
// if we don't know the target, we want to be in the middle for safety
|
||||||
return (target1 + target2) / 2.0;
|
return target;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
double getLoadDisturbance(int i) const {
|
double_t getLoadDisturbance(int i) const {
|
||||||
static const double disturb = 0.10;
|
static const double_t disturb = 0.10;
|
||||||
static const double ampl = 0.25, period = 0.37;
|
static const double_t ampl = 0.25, period = 0.37;
|
||||||
double d = 0;
|
double_t d = 0;
|
||||||
switch (simType) {
|
switch (simType) {
|
||||||
case PID_SIM_REGULATOR:
|
case PID_SIM_REGULATOR:
|
||||||
// add or subtract 10% to imitate the "load"
|
// add or subtract 10% to imitate the "load"
|
||||||
d += target1 * ((i > stepPoint) ? -disturb : disturb);
|
d += target1 * ((i > stepPoint) ? -disturb : disturb);
|
||||||
// add periodic noise
|
// add periodic noise
|
||||||
d += sin(2.0 * 3.14159265 * (double)i * dTime / period) * ampl;
|
d += sin(2.0 * 3.14159265 * (double_t)i * dTime / period) * ampl;
|
||||||
return d;
|
return d;
|
||||||
default:
|
default:
|
||||||
return 0.0;
|
return 0.0;
|
||||||
|
@ -100,11 +106,15 @@ protected:
|
||||||
int stepPoint;
|
int stepPoint;
|
||||||
const char *outputFile;
|
const char *outputFile;
|
||||||
StoredDataInputFunction<maxPoints> plantInput;
|
StoredDataInputFunction<maxPoints> plantInput;
|
||||||
|
|
||||||
|
LMSFunction<4> *plant;
|
||||||
FirstOrderPlusDelayLineFunction plant1;
|
FirstOrderPlusDelayLineFunction plant1;
|
||||||
SecondOrderPlusDelayLineOverdampedFunction plant2;
|
SecondOrderPlusDelayLineOverdampedFunction plant2;
|
||||||
|
|
||||||
PidAccuracyMetric metric;
|
PidAccuracyMetric metric;
|
||||||
double target1, target2, dTime;
|
double_t target, target1, target2, dTime;
|
||||||
pid_sim_type_e simType;
|
pid_sim_type_e simType;
|
||||||
|
const double_t *modelParams;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
@ -114,8 +124,8 @@ protected:
|
||||||
template <int numPoints>
|
template <int numPoints>
|
||||||
class PidSimulatorFactory {
|
class PidSimulatorFactory {
|
||||||
public:
|
public:
|
||||||
PidSimulatorFactory(pid_sim_type_e simType, int methodOrder_, double target1_, double target2_, double dTime, double modelBias, const pid_s & pid_) :
|
PidSimulatorFactory(pid_sim_type_e simType, int methodOrder_, double_t target1_, double_t target2_, double_t target_, double_t dTime, double_t modelBias, const pid_s & pid_) :
|
||||||
sim(simType, methodOrder_, target1_, target2_, dTime, modelBias, nullptr), pid(pid_) {
|
sim(simType, methodOrder_, target1_, target2_, target_, dTime, modelBias, nullptr), pid(pid_) {
|
||||||
}
|
}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
@ -129,12 +139,16 @@ public:
|
||||||
template <int numPoints>
|
template <int numPoints>
|
||||||
class PidCoefsFinderFunction : public LMSFunction<numParamsForPid> {
|
class PidCoefsFinderFunction : public LMSFunction<numParamsForPid> {
|
||||||
public:
|
public:
|
||||||
PidCoefsFinderFunction(PidSimulatorFactory<numPoints> *simFactory_, const double *modelParams_) :
|
PidCoefsFinderFunction(PidSimulatorFactory<numPoints> *simFactory_, const double_t *modelParams_) :
|
||||||
simFactory(simFactory_), modelParams(modelParams_) {
|
simFactory(simFactory_), modelParams(modelParams_) {
|
||||||
|
// try different value because this function is too complicated and not smooth
|
||||||
|
delta = 1.0e-5;
|
||||||
|
simFactory->sim.setModelParams(modelParams_);
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual void justifyParams(double *params) const {
|
virtual void justifyParams(double_t *params) const {
|
||||||
// todo: limit PID coefs somehow?
|
// todo: limit PID coefs somehow?
|
||||||
|
//params[PARAM_Ki] = fmax(params[PARAM_Ki], 0.00001);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Get the total number of data points
|
// Get the total number of data points
|
||||||
|
@ -143,29 +157,29 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculate the sum of the squares of the residuals
|
// Calculate the sum of the squares of the residuals
|
||||||
virtual double calcMerit(double *params) const {
|
virtual double_t calcMerit(double_t *params) const {
|
||||||
return simulate(numPoints - 1, params).getItae();
|
return simulate(numPoints - 1, params).getMerit();
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual double getResidual(int i, const double *params) const {
|
virtual double_t getResidual(int i, const double_t *params) const {
|
||||||
return simFactory->sim.getSetpoint(i) - getEstimatedValueAtPoint(i, params);
|
return simFactory->sim.getSetpoint(i) - getEstimatedValueAtPoint(i, params);
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual double getEstimatedValueAtPoint(int i, const double *params) const {
|
virtual double_t calcEstimatedValuesAtPoint(int i, const double_t *params) const {
|
||||||
return simulate(i, params).getLastValue();
|
return simulate(i, params).getLastValue();
|
||||||
}
|
}
|
||||||
|
|
||||||
PidAccuracyMetric simulate(int i, const double *params) const {
|
PidAccuracyMetric simulate(int i, const double_t *params) const {
|
||||||
// update params
|
// update params
|
||||||
simFactory->pid.pFactor = (float)params[PARAM_Kp];
|
simFactory->pid.pFactor = (float_t)params[PARAM_Kp];
|
||||||
simFactory->pid.iFactor = (float)params[PARAM_Ki];
|
simFactory->pid.iFactor = (float_t)params[PARAM_Ki];
|
||||||
simFactory->pid.dFactor = (float)params[PARAM_Kd];
|
simFactory->pid.dFactor = (float_t)params[PARAM_Kd];
|
||||||
// simulate PID controller response
|
// simulate PID controller response
|
||||||
return simFactory->sim.simulate(i + 1, simFactory->pid, modelParams);
|
return simFactory->sim.simulate(i + 1, simFactory->pid);
|
||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
const double *modelParams;
|
const double_t *modelParams;
|
||||||
PidSimulatorFactory<numPoints> *simFactory;
|
PidSimulatorFactory<numPoints> *simFactory;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
108
test_chs_pid.cpp
108
test_chs_pid.cpp
|
@ -31,25 +31,27 @@ TEST(pidAutoTune, testMeasuredDataBuffer) {
|
||||||
TEST(pidAutoTune, testFOPDT) {
|
TEST(pidAutoTune, testFOPDT) {
|
||||||
StepFunction stepFunc(0, 100, 10, 1.0);
|
StepFunction stepFunc(0, 100, 10, 1.0);
|
||||||
FirstOrderPlusDelayLineFunction func(&stepFunc, nullptr, 0, 0.0);
|
FirstOrderPlusDelayLineFunction func(&stepFunc, nullptr, 0, 0.0);
|
||||||
double params[3];
|
double_t params[3];
|
||||||
params[PARAM_K] = 2.0;
|
params[PARAM_K] = 2.0;
|
||||||
params[PARAM_T] = 3.0;
|
params[PARAM_T] = 3.0;
|
||||||
params[PARAM_L] = 4.0;
|
params[PARAM_L] = 4.0;
|
||||||
|
func.calculateAllPoints(params);
|
||||||
|
|
||||||
double v = func.getEstimatedValueAtPoint(24, params);
|
double_t v = func.getEstimatedValueAtPoint(24, params);
|
||||||
ASSERT_DOUBLE_EQ(25.200031003972988, v);
|
ASSERT_DOUBLE_EQ(25.200031003972988, v);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(pidAutoTune, testSOPDTOverdamped) {
|
TEST(pidAutoTune, testSOPDTOverdamped) {
|
||||||
StepFunction stepFunc(0, 100, 10, 1.0);
|
StepFunction stepFunc(0, 100, 10, 1.0);
|
||||||
SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, nullptr, 0, 0.0);
|
SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, nullptr, 0, 0.0);
|
||||||
double params[4];
|
double_t params[4];
|
||||||
params[PARAM_K] = 2.0;
|
params[PARAM_K] = 2.0;
|
||||||
params[PARAM_T] = 3.0;
|
params[PARAM_T] = 3.0;
|
||||||
params[PARAM_T2] = 0.3;
|
params[PARAM_T2] = 0.3;
|
||||||
params[PARAM_L] = 4.0;
|
params[PARAM_L] = 4.0;
|
||||||
|
func.calculateAllPoints(params);
|
||||||
|
|
||||||
double v = func.getEstimatedValueAtPoint(24, params);
|
double_t v = func.getEstimatedValueAtPoint(24, params);
|
||||||
ASSERT_DOUBLE_EQ(25.200031003972988, v);
|
ASSERT_DOUBLE_EQ(25.200031003972988, v);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -60,21 +62,21 @@ const int numData = sizeof(outputData) / sizeof(outputData[0]);
|
||||||
void printSOPDT() {
|
void printSOPDT() {
|
||||||
StepFunction stepFunc(20.0, 30.0, 178, 1.0);
|
StepFunction stepFunc(20.0, 30.0, 178, 1.0);
|
||||||
SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, nullptr, 0, 0.0);
|
SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, nullptr, 0, 0.0);
|
||||||
double params[4];
|
double_t params[4];
|
||||||
params[PARAM_K] = 0.251778;
|
params[PARAM_K] = 0.251778;
|
||||||
params[PARAM_T] = 55.7078;
|
params[PARAM_T] = 55.7078;
|
||||||
params[PARAM_T2] = 55.7077;
|
params[PARAM_T2] = 55.7077;
|
||||||
params[PARAM_L] = 1.80759;
|
params[PARAM_L] = 1.80759;
|
||||||
|
|
||||||
for (int i = 0; i < numData; i++) {
|
for (int i = 0; i < numData; i++) {
|
||||||
double v = func.getEstimatedValueAtPoint(i, params);
|
double_t v = func.getEstimatedValueAtPoint(i, params);
|
||||||
printf("%d,%f,%f,%f\r\n", i, outputData[i], v, stepFunc.getValue((float)i, 0));
|
printf("%d,%f,%f,%f\r\n", i, outputData[i], v, stepFunc.getValue((float)i, 0));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(pidAutoTune, chsSopdtPid) {
|
TEST(pidAutoTune, chsSopdtPid) {
|
||||||
PidAutoTune chr;
|
PidAutoTune chr;
|
||||||
double params0[4];
|
double_t params0[4];
|
||||||
|
|
||||||
// todo: find better initial values?
|
// todo: find better initial values?
|
||||||
params0[PARAM_K] = 0.1;
|
params0[PARAM_K] = 0.1;
|
||||||
|
@ -95,7 +97,7 @@ TEST(pidAutoTune, chsSopdtPid) {
|
||||||
bool ret = chr.findPid(PID_SIM_SERVO, PID_TUNE_CHR2, settings, params0);
|
bool ret = chr.findPid(PID_SIM_SERVO, PID_TUNE_CHR2, settings, params0);
|
||||||
|
|
||||||
#ifdef PID_DEBUG
|
#ifdef PID_DEBUG
|
||||||
const double *p = chr.getParams();
|
const double_t *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]);
|
printf("Params: K=%g T1=%g T2=%g L=%g\r\n", p[PARAM_K], p[PARAM_T], p[PARAM_T2], p[PARAM_L]);
|
||||||
const pid_s & pid = chr.getPid();
|
const pid_s & pid = chr.getPid();
|
||||||
printf("PID: P=%f I=%f D=%f offset=%f\r\n", pid.pFactor, pid.iFactor, pid.dFactor, pid.offset);
|
printf("PID: P=%f I=%f D=%f offset=%f\r\n", pid.pFactor, pid.iFactor, pid.dFactor, pid.offset);
|
||||||
|
@ -110,69 +112,99 @@ TEST(pidAutoTune, testPidCoefs) {
|
||||||
{ 18.588152f, 0.166438f, 518.990417f, 32.823277f, 0, 100 }, // CHR2 ITAE=28.1444 ISE=186.409 Overshoot=7.86222%
|
{ 18.588152f, 0.166438f, 518.990417f, 32.823277f, 0, 100 }, // CHR2 ITAE=28.1444 ISE=186.409 Overshoot=7.86222%
|
||||||
{ 2.383054f, 0.606178f, 0.067225f, 32.823277f, 0, 100 }, // CHR21 ITAE=215.15 Overshoot=18.5046
|
{ 2.383054f, 0.606178f, 0.067225f, 32.823277f, 0, 100 }, // CHR21 ITAE=215.15 Overshoot=18.5046
|
||||||
{ 1.764889f, 0.106801f, 2.620852f, 32.823277f, 0, 100 }, // IMC21 ITAE=112.804 Overshoot=17.7643
|
{ 1.764889f, 0.106801f, 2.620852f, 32.823277f, 0, 100 }, // IMC21 ITAE=112.804 Overshoot=17.7643
|
||||||
{ 292.501831f, 2.601279f, 8333.136719, 32.823277f, 0, 100 },// VDG2 ITAE=130.496 ISE=891.474 Overshoot=13.4626%
|
{ 292.501831f, 2.601279f, 8333.136719f, 32.823277f, 0, 100 },// VDG2 ITAE=130.496 ISE=891.474 Overshoot=13.4626%
|
||||||
};
|
};
|
||||||
|
|
||||||
double params[4];
|
double_t params[4];
|
||||||
params[PARAM_K] = 0.251778;
|
params[PARAM_K] = 0.251778;
|
||||||
params[PARAM_T] = 55.841;
|
params[PARAM_T] = 55.841;
|
||||||
params[PARAM_T2] = 55.841;
|
params[PARAM_T2] = 55.841;
|
||||||
params[PARAM_L] = 1.52685;
|
params[PARAM_L] = 1.52685;
|
||||||
|
|
||||||
// todo: is it correct?
|
// todo: is it correct?
|
||||||
double dTime = 1;
|
double_t dTime = 1;
|
||||||
const int numSimPoints = 2048;
|
const int numSimPoints = 2048;
|
||||||
PidSimulator<numSimPoints> sim(PID_SIM_SERVO, 2, 13.0, 14.0, dTime, 0.0, nullptr);
|
PidSimulator<numSimPoints> sim(PID_SIM_SERVO, 2, 13.0, 14.0, NAN, dTime, 0.0, nullptr);
|
||||||
|
sim.setModelParams(params);
|
||||||
for (int idx = 0; idx <= 4; idx++) {
|
for (int idx = 0; idx <= 4; idx++) {
|
||||||
PidAccuracyMetric metric = sim.simulate(numSimPoints, pidParams[idx], params);
|
PidAccuracyMetric metric = sim.simulate(numSimPoints, pidParams[idx]);
|
||||||
#ifdef PID_DEBUG
|
#ifdef PID_DEBUG
|
||||||
printf("Metric result: ITAE=%g ISE=%g Overshoot=%g%%\r\n", metric.getItae(), metric.getIse(), metric.getMaxOvershoot() * 100.0);
|
printf("Metric result: ITAE=%g ISE=%g Overshoot=%g%%\r\n", (long double)metric.getItae(), (long double)metric.getIse(), (long double)(metric.getMaxOvershoot() * 100.0));
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
// todo: check results
|
// todo: check results
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(pidAutoTune, testPidSim) {
|
void testPidAndModel(const pid_s & pid, const std::vector<double_t> & p, double_t modelBias) {
|
||||||
const int numSimPoints = 1024;
|
const int numSimPoints = 1024;
|
||||||
|
PidSimulator<numSimPoints> sim(PID_SIM_REGULATOR, 2, 13.4117, 16.1769, 13.8, pid.periodMs / 1000.0, modelBias, "pid_test.csv");
|
||||||
|
printf(" PID: P=%.8f I=%.8f D=%.8f offset=%.8f period=%.8fms\r\n", (long double)pid.pFactor, (long double)pid.iFactor, (long double)pid.dFactor, (long double)pid.offset, (long double)pid.periodMs);
|
||||||
|
sim.setModelParams(p.data());
|
||||||
|
PidAccuracyMetric metric = sim.simulate(numSimPoints, pid);
|
||||||
|
printf(" Metric result: %.8g ITAE=%.8g ISE=%.8g Overshoot=%.8g%%\r\n", (long double)metric.getMerit(), (long double)metric.getItae(), (long double)metric.getIse(), (long double)(metric.getMaxOvershoot() * 100.0));
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(pidAutoTune, testPidSim) {
|
||||||
|
|
||||||
pid_s pid;
|
pid_s pid;
|
||||||
//double p[4] = { 0.27659, 1.08301, 0.3, 1.08 };
|
std::vector<double_t> p;
|
||||||
|
double_t modelBias;
|
||||||
|
|
||||||
|
pid.periodMs = 10.9;
|
||||||
|
pid.minValue = 10;
|
||||||
|
pid.maxValue = 90;
|
||||||
|
|
||||||
#if 1
|
#if 1
|
||||||
double p[4] = { 0.276267, 0.596307, 0.1, 0.140311 };
|
p = { 0.276269, 0.596327, 0.1, 0.140298 };
|
||||||
double modelBias = 7.88121;
|
modelBias = 7.88121;
|
||||||
pid.pFactor = 18.07646561;
|
pid.pFactor = 15.71419334;
|
||||||
pid.iFactor = 16.68941116;
|
pid.iFactor = 12.51472759;
|
||||||
pid.dFactor = 0.00180748;
|
pid.dFactor = 1.11415541;
|
||||||
pid.offset = 21.4;
|
pid.offset = 21.0f;
|
||||||
|
testPidAndModel(pid, p, modelBias);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if 1
|
||||||
|
p = { 0.276269, 0.596327, 0.1, 0.140298 };
|
||||||
|
modelBias = 7.88121;
|
||||||
|
pid.pFactor = 13.93473148;
|
||||||
|
pid.iFactor = 13.82630730;
|
||||||
|
pid.dFactor = 1.03833103;
|
||||||
|
pid.offset = 21.0f;
|
||||||
|
testPidAndModel(pid, p, modelBias);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if 1
|
||||||
|
p = { 0.276269, 0.596327, 0.1, 0.140298 };
|
||||||
|
modelBias = 7.88121;
|
||||||
|
pid.pFactor = 16.82204052;
|
||||||
|
pid.iFactor = 14.79944581;
|
||||||
|
pid.dFactor = 1.38008648;
|
||||||
|
pid.offset = 21.0f;
|
||||||
|
testPidAndModel(pid, p, modelBias);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if 0
|
#if 0
|
||||||
double p[4] = { 0.276267, 0.596307, 0.1, 0.140311 };
|
p = { 0.276269, 0.596327, 0.1, 0.140298 };
|
||||||
double modelBias = 7.88121;
|
modelBias = 7.88121;
|
||||||
pid.pFactor = 12.17391205;
|
pid.pFactor = 15.77522947;
|
||||||
pid.iFactor = 11.22539520;
|
pid.iFactor = -2.03882839;
|
||||||
pid.dFactor = 2.99444199;
|
pid.dFactor = 1.05018034;
|
||||||
pid.offset = 28.50101471;
|
pid.offset = 21.0f;
|
||||||
//pid.offset = 21.4;
|
testPidAndModel(pid, p, modelBias);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if 0
|
#if 0
|
||||||
double p[4] = { 0.276267, 0.596307, 0.1, 0.140311 };
|
p = { 0.276267, 0.596307, 0.1, 0.140311 };
|
||||||
double modelBias = 7.88121;
|
modelBias = 7.88121;
|
||||||
pid.pFactor = 19.52591324;
|
pid.pFactor = 19.52591324;
|
||||||
pid.iFactor = 14.07089329;
|
pid.iFactor = 14.07089329;
|
||||||
pid.dFactor = 1.58994818;
|
pid.dFactor = 1.58994818;
|
||||||
pid.offset = 21.4;
|
pid.offset = 21.4;
|
||||||
|
testPidAndModel(pid, p, modelBias);
|
||||||
#endif
|
#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
|
#if 0
|
||||||
|
|
|
@ -14,10 +14,10 @@
|
||||||
class GaussianFunction : public LMSFunction<6> {
|
class GaussianFunction : public LMSFunction<6> {
|
||||||
const int numParams = 6;
|
const int numParams = 6;
|
||||||
public:
|
public:
|
||||||
GaussianFunction(int numPoints_, double *xValues_, double *yValues_) : numPoints(numPoints_), xValues(xValues_), yValues(yValues_) {
|
GaussianFunction(int numPoints_, double_t *xValues_, double_t *yValues_) : numPoints(numPoints_), xValues(xValues_), yValues(yValues_) {
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual void justifyParams(double *params) const {
|
virtual void justifyParams(double_t *params) const {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Get the total number of data points
|
// Get the total number of data points
|
||||||
|
@ -25,24 +25,24 @@ public:
|
||||||
return numPoints;
|
return numPoints;
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual double getEstimatedValueAtPoint(int pi, const double *params) const {
|
virtual double_t calcEstimatedValuesAtPoint(int pi, const double_t *params) const {
|
||||||
double val = 0.0;
|
double_t val = 0.0;
|
||||||
for (int j = 0, i = 0; j < (numParams / 3); j++, i += 3)
|
for (int j = 0, i = 0; j < (numParams / 3); j++, i += 3)
|
||||||
{
|
{
|
||||||
double arg = (xValues[pi] - params[i + 1]) / params[i + 2];
|
double_t arg = (xValues[pi] - params[i + 1]) / params[i + 2];
|
||||||
val += params[i] * exp(-arg * arg);
|
val += params[i] * exp(-arg * arg);
|
||||||
}
|
}
|
||||||
return val;
|
return val;
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual double getResidual(int i, const double *params) const {
|
virtual double_t getResidual(int i, const double_t *params) const {
|
||||||
return yValues[i] - getEstimatedValueAtPoint(i, params);
|
return yValues[i] - getEstimatedValueAtPoint(i, params);
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
int numPoints;
|
int numPoints;
|
||||||
double *xValues;
|
double_t *xValues;
|
||||||
double *yValues;
|
double_t *yValues;
|
||||||
};
|
};
|
||||||
|
|
||||||
void testGaussianFunction() {
|
void testGaussianFunction() {
|
||||||
|
@ -51,14 +51,14 @@ void testGaussianFunction() {
|
||||||
const int numParams = 6;
|
const int numParams = 6;
|
||||||
const int numPoints = 100;
|
const int numPoints = 100;
|
||||||
|
|
||||||
const double goodParams[numParams] = { 5, 2, 3, 2, 5, 3 };
|
const double_t goodParams[numParams] = { 5, 2, 3, 2, 5, 3 };
|
||||||
|
|
||||||
double xValues[numPoints];
|
double_t xValues[numPoints];
|
||||||
for (i = 0; i < numPoints; i++) {
|
for (i = 0; i < numPoints; i++) {
|
||||||
xValues[i] = 0.1 * (double)(i + 1);
|
xValues[i] = 0.1 * (double_t)(i + 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
double yValues[numPoints];
|
double_t yValues[numPoints];
|
||||||
|
|
||||||
GaussianFunction func(numPoints, xValues, yValues);
|
GaussianFunction func(numPoints, xValues, yValues);
|
||||||
|
|
||||||
|
@ -67,7 +67,7 @@ void testGaussianFunction() {
|
||||||
yValues[i] = func.getEstimatedValueAtPoint(i, goodParams);
|
yValues[i] = func.getEstimatedValueAtPoint(i, goodParams);
|
||||||
}
|
}
|
||||||
|
|
||||||
double params[numParams] = { 4, 2, 2, 2, 5, 2 };
|
double_t params[numParams] = { 4, 2, 2, 2, 5, 2 };
|
||||||
LevenbergMarquardtSolver<numParams> solver(&func, params);
|
LevenbergMarquardtSolver<numParams> solver(&func, params);
|
||||||
|
|
||||||
int iterationCount = solver.solve(0.001, 1e-15, 100);
|
int iterationCount = solver.solve(0.001, 1e-15, 100);
|
||||||
|
@ -82,7 +82,7 @@ void testGaussianFunction() {
|
||||||
void testMatrixInverse() {
|
void testMatrixInverse() {
|
||||||
int i, j;
|
int i, j;
|
||||||
const int n = 4;
|
const int n = 4;
|
||||||
double a[n][n];
|
double_t a[n][n];
|
||||||
// fill with some arbitrary values
|
// fill with some arbitrary values
|
||||||
for (i = 0; i < n; i++) {
|
for (i = 0; i < n; i++) {
|
||||||
for (j = 0; j < n; j++) {
|
for (j = 0; j < n; j++) {
|
||||||
|
@ -90,16 +90,16 @@ void testMatrixInverse() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
double ai[n][n];
|
double_t ai[n][n];
|
||||||
bool ret = MatrixHelper<double, n>::inverseMatrix(ai, a);
|
bool ret = MatrixHelper<double_t, n>::inverseMatrix(ai, a);
|
||||||
ASSERT_EQ(true, ret);
|
ASSERT_EQ(true, ret);
|
||||||
|
|
||||||
double mul[n][n];
|
double_t mul[n][n];
|
||||||
MatrixHelper<double, n>::multiplyMatrix(mul, ai, a);
|
MatrixHelper<double_t, n>::multiplyMatrix(mul, ai, a);
|
||||||
// A^-1 * A = I
|
// A^-1 * A = I
|
||||||
for (i = 0; i < n; i++) {
|
for (i = 0; i < n; i++) {
|
||||||
for (j = 0; j < n; j++) {
|
for (j = 0; j < n; j++) {
|
||||||
double va = (i == j) ? 1.0 : 0; // identity matrix element
|
double_t va = (i == j) ? 1.0 : 0; // identity matrix element
|
||||||
ASSERT_DOUBLE_EQ(va, mul[i][j]);
|
ASSERT_DOUBLE_EQ(va, mul[i][j]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue