mirror of https://github.com/rusefi/autopid.git
Version 0.3: double_t + industrial-PID + small fixes
This commit is contained in:
Andrei
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"
|
||||
|
||||
|
||||
//!!!!!!!!!!!!!!
|
||||
#define float_t double_t
|
||||
|
||||
#ifndef CONTROLLERS_GENERATED_ENGINE_CONFIGURATION_GENERATED_STRUCTURES_H
|
||||
struct pid_s {
|
||||
float pFactor;
|
||||
float iFactor;
|
||||
float dFactor;
|
||||
float offset;
|
||||
float periodMs;
|
||||
float_t pFactor;
|
||||
float_t iFactor;
|
||||
float_t dFactor;
|
||||
float_t offset;
|
||||
float_t periodMs;
|
||||
|
||||
float_t antiwindupFreq; // = 1/ResetTime
|
||||
float_t derivativeFilterLoss; // = 1/Gain
|
||||
|
||||
int16_t minValue;
|
||||
int16_t maxValue;
|
||||
|
|
|
|||
|
|
@ -26,19 +26,28 @@ public:
|
|||
// Get the total number of data points
|
||||
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
|
||||
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)))
|
||||
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
|
||||
virtual double calcMerit(double *params) const {
|
||||
double res = 0;
|
||||
virtual double_t calcMerit(double_t *params) const {
|
||||
double_t res = 0;
|
||||
for (int i = 0; i < getNumPoints(); i++) {
|
||||
double r = getResidual(i, params);
|
||||
double_t r = getResidual(i, params);
|
||||
res += r * r;
|
||||
}
|
||||
return res;
|
||||
|
|
@ -46,52 +55,68 @@ public:
|
|||
|
||||
/// 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
|
||||
virtual double getPartialDerivative(int i, const double *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;
|
||||
virtual double_t getPartialDerivative(int i, const double_t *params, int pIndex) const {
|
||||
// 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++)
|
||||
tmpParams[k] = params[k];
|
||||
|
||||
tmpParams[pIndex] = params[pIndex] + delta;
|
||||
double dplusResult = getEstimatedValueAtPoint(i, tmpParams);
|
||||
double_t dplusResult = getEstimatedValueAtPoint(i, tmpParams);
|
||||
|
||||
tmpParams[pIndex] = params[pIndex] - delta;
|
||||
double dminusResult = getEstimatedValueAtPoint(i, tmpParams);
|
||||
double_t dminusResult = getEstimatedValueAtPoint(i, tmpParams);
|
||||
#endif
|
||||
|
||||
return (dplusResult - dminusResult) / (delta * 2.0);
|
||||
}
|
||||
|
||||
void calculateAllPoints(const double_t *params) {
|
||||
#ifdef LM_USE_CACHE
|
||||
void calculateAllPartialDerivatives(const double *params) {
|
||||
for (int i = 0; i < numParams; i++) {
|
||||
pderivs[i].resize(getNumPoints());
|
||||
for (int i = 0; i < numPointGroups; i++) {
|
||||
points[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++) {
|
||||
points[p][i] = calcEstimatedValuesAtPoint(i, tmpParams);
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < getNumPoints(); i++) {
|
||||
pderivs[p][i] = getPartialDerivativeInternal(i, params, p);
|
||||
points[0][i] = calcEstimatedValuesAtPoint(i, params);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
private:
|
||||
#ifdef LM_USE_CACHE
|
||||
std::vector<double> pderivs[numParams];
|
||||
static const int numPointGroups = numParams * 2 + 1;
|
||||
std::vector<double_t> points[numPointGroups];
|
||||
#endif
|
||||
|
||||
protected:
|
||||
// some magic value for the differentiation step to calculate a partial derivative
|
||||
double_t delta = 1.0e-6;
|
||||
};
|
||||
|
||||
template<int numParams>
|
||||
class LevenbergMarquardtSolver {
|
||||
public:
|
||||
// ctor
|
||||
LevenbergMarquardtSolver(LMSFunction<numParams> *func, double *parameters) {
|
||||
LevenbergMarquardtSolver(LMSFunction<numParams> *func, double_t *parameters) {
|
||||
this->func = func;
|
||||
this->parameters = parameters;
|
||||
}
|
||||
|
|
@ -99,22 +124,21 @@ public:
|
|||
// lambda - magic coef.
|
||||
// maxIterations - if too many iterations (loop exit condition #1)
|
||||
// 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_;
|
||||
|
||||
iterationCount = 0;
|
||||
|
||||
double delta = 0;
|
||||
func->calculateAllPoints(parameters);
|
||||
double_t delta = 0;
|
||||
do {
|
||||
double merit = func->calcMerit(parameters);
|
||||
double_t merit = func->calcMerit(parameters);
|
||||
|
||||
#ifdef LM_USE_CACHE
|
||||
func->calculateAllPartialDerivatives(parameters);
|
||||
#endif
|
||||
calcGradient();
|
||||
calcHessian();
|
||||
bool isSolved = calcNewParameters();
|
||||
double newMerit = func->calcMerit(newParameters);
|
||||
func->calculateAllPoints(newParameters);
|
||||
double_t newMerit = func->calcMerit(newParameters);
|
||||
if (!isSolved) {
|
||||
return -iterationCount;
|
||||
}
|
||||
|
|
@ -133,15 +157,15 @@ public:
|
|||
// find out if we progressed enough in this iteration
|
||||
delta = fabs(newMerit - merit);
|
||||
#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,
|
||||
merit, newMerit, newMerit - merit);
|
||||
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],
|
||||
(long double)lambda, (long double)merit, (long double)newMerit, (long double)(newMerit - merit));
|
||||
#endif
|
||||
iterationCount++;
|
||||
} while (delta > minDelta && iterationCount < maxIterations);
|
||||
return iterationCount;
|
||||
}
|
||||
|
||||
double *getParameters() const {
|
||||
double_t *getParameters() const {
|
||||
return parameters;
|
||||
}
|
||||
|
||||
|
|
@ -149,13 +173,13 @@ protected:
|
|||
// Find the parameter increments by solving the Hessian x Gradient equation
|
||||
bool calcNewParameters() {
|
||||
// get H^-1 matrix (inverse Hessian)
|
||||
double hinv[numParams][numParams];
|
||||
bool ret = MatrixHelper<double, numParams>::inverseMatrix(hinv, hessian);
|
||||
double_t hinv[numParams][numParams];
|
||||
bool ret = MatrixHelper<double_t, numParams>::inverseMatrix(hinv, hessian);
|
||||
if (!ret)
|
||||
return false;
|
||||
|
||||
for (int row = 0; row < numParams; row++) {
|
||||
double increment = 0;
|
||||
double_t increment = 0;
|
||||
for (int col = 0; col < numParams; col++) {
|
||||
increment += hinv[row][col] * gradient[col];
|
||||
}
|
||||
|
|
@ -170,7 +194,7 @@ protected:
|
|||
void calcHessian() {
|
||||
for (int row = 0; row < numParams; row++) {
|
||||
for (int col = 0; col < numParams; col++) {
|
||||
double res = 0;
|
||||
double_t res = 0;
|
||||
for (int i = 0; i < func->getNumPoints(); i++) {
|
||||
res += func->getPartialDerivative(i, parameters, row) * func->getPartialDerivative(i, parameters, col);
|
||||
}
|
||||
|
|
@ -182,7 +206,7 @@ protected:
|
|||
// Calculate the 1st derivatives of the residual func
|
||||
void calcGradient() {
|
||||
for (int row = 0; row < numParams; row++) {
|
||||
double res = 0;
|
||||
double_t res = 0;
|
||||
for (int i = 0; i < func->getNumPoints(); i++) {
|
||||
res += func->getResidual(i, parameters) * func->getPartialDerivative(i, parameters, row);
|
||||
}
|
||||
|
|
@ -195,21 +219,21 @@ private:
|
|||
LMSFunction<numParams> *func;
|
||||
|
||||
// Current (accepted) parameters vector
|
||||
double *parameters; // [numParams]
|
||||
double_t *parameters; // [numParams]
|
||||
|
||||
// Incremented (next step) parameters vector
|
||||
double newParameters[numParams];
|
||||
double_t newParameters[numParams];
|
||||
|
||||
// Hessian matrix
|
||||
double hessian[numParams][numParams];
|
||||
double_t hessian[numParams][numParams];
|
||||
// Gradients vector
|
||||
double gradient[numParams];
|
||||
double_t gradient[numParams];
|
||||
|
||||
// some magic number
|
||||
const double lambdaMultiplier = 10.0;
|
||||
const double_t lambdaMultiplier = 10.0;
|
||||
|
||||
// coeff used to adapt the descent step (and speed)
|
||||
double lambda;
|
||||
double_t lambda;
|
||||
|
||||
// Total number of iterations
|
||||
int iterationCount;
|
||||
|
|
|
|||
126
pid_auto.h
126
pid_auto.h
|
|
@ -25,10 +25,10 @@
|
|||
/// These settings should be obtained from the measured data
|
||||
class PidAutoTuneSettings {
|
||||
public:
|
||||
double minValue, maxValue;
|
||||
double stepPoint, maxPoint;
|
||||
double timeScale;
|
||||
double targetValue;
|
||||
double_t minValue, maxValue;
|
||||
double_t stepPoint, maxPoint;
|
||||
double_t timeScale;
|
||||
double_t targetValue;
|
||||
};
|
||||
|
||||
// PID auto-tune method using different tuning rules and FOPDT/SOPDT models
|
||||
|
|
@ -38,7 +38,7 @@ public:
|
|||
measuredData.init();
|
||||
}
|
||||
|
||||
void addData(float v) {
|
||||
void addData(float_t v) {
|
||||
measuredData.addDataPoint(v);
|
||||
}
|
||||
|
||||
|
|
@ -46,7 +46,7 @@ public:
|
|||
// stepPoint - data index where the step was done
|
||||
// maxPoint - data index where the output was saturated
|
||||
// this is not thread-safe (because of internal static vars) but anyway it works...
|
||||
bool findPid(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
|
||||
this->settings = settings;
|
||||
this->simType = simType;
|
||||
|
|
@ -58,7 +58,7 @@ public:
|
|||
pid.maxValue = 100;
|
||||
|
||||
#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
|
||||
StepFunction stepFunc(settings.minValue, settings.maxValue, settings.stepPoint, settings.timeScale);
|
||||
|
||||
|
|
@ -82,7 +82,7 @@ public:
|
|||
}
|
||||
}
|
||||
#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
|
||||
}
|
||||
else {
|
||||
|
|
@ -90,7 +90,7 @@ public:
|
|||
memcpy(params, initialParams, sizeof(params));
|
||||
}
|
||||
|
||||
double merit0, merit;
|
||||
double_t merit0, merit;
|
||||
#ifdef PID_DEBUG
|
||||
printf("* Solving...\r\n");
|
||||
#endif
|
||||
|
|
@ -99,6 +99,7 @@ public:
|
|||
// 1st order
|
||||
FirstOrderPlusDelayLineFunction func(&stepFunc, measuredData.getBuf(), measuredData.getNumDataPoints(), modelBias);
|
||||
func.justifyParams(params);
|
||||
func.calculateAllPoints(params);
|
||||
const int numParams1stOrder = 3;
|
||||
outputFunc<numParams1stOrder>("pid_func01.csv", func, stepFunc, params);
|
||||
LevenbergMarquardtSolver<numParams1stOrder> solver(&func, params);
|
||||
|
|
@ -111,6 +112,7 @@ public:
|
|||
// 2nd order or approximated 2nd->1st order
|
||||
SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, measuredData.getBuf(), measuredData.getNumDataPoints(), modelBias);
|
||||
func.justifyParams(params);
|
||||
func.calculateAllPoints(params);
|
||||
const int numParams2ndOrder = 4;
|
||||
outputFunc<numParams2ndOrder>("pid_func02.csv", func, stepFunc, params);
|
||||
LevenbergMarquardtSolver<numParams2ndOrder> solver(&func, params);
|
||||
|
|
@ -166,8 +168,9 @@ public:
|
|||
pid.pFactor = model->getKp();
|
||||
pid.iFactor = model->getKi();
|
||||
pid.dFactor = model->getKd();
|
||||
pid.offset = getPidOffset(model);
|
||||
pid.periodMs = 1000.0 / settings.timeScale;
|
||||
// round offset and period due to the firmware limitations...
|
||||
pid.offset = round(getPidOffset(model));
|
||||
pid.periodMs = round((float_t)(1000.0 / settings.timeScale));
|
||||
|
||||
pid0 = pid;
|
||||
|
||||
|
|
@ -195,7 +198,7 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
double const *getParams() const {
|
||||
double_t const *getParams() const {
|
||||
return params;
|
||||
}
|
||||
|
||||
|
|
@ -207,20 +210,20 @@ public:
|
|||
return pid0;
|
||||
}
|
||||
|
||||
double getAvgMeasuredMin() const {
|
||||
double_t getAvgMeasuredMin() const {
|
||||
return avgMeasuredMin;
|
||||
}
|
||||
|
||||
double getAvgMeasuredMax() const {
|
||||
double_t getAvgMeasuredMax() const {
|
||||
return avgMeasuredMax;
|
||||
}
|
||||
|
||||
double getModelBias() const {
|
||||
double_t getModelBias() const {
|
||||
return modelBias;
|
||||
}
|
||||
|
||||
// The model output is typically shifted
|
||||
double findModelBias() {
|
||||
double_t findModelBias() {
|
||||
if (settings.stepPoint < 0 || settings.maxPoint <= settings.stepPoint || settings.maxPoint > measuredData.getNumDataPoints())
|
||||
return 0;
|
||||
// find the real 'min value' of the measured output data (before the step function goes up).
|
||||
|
|
@ -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.
|
||||
float getPidOffset(ModelOpenLoopPlant *model) const {
|
||||
if (settings.targetValue == 0.0)
|
||||
float_t getPidOffset(ModelOpenLoopPlant *model) const {
|
||||
if (std::isnan(settings.targetValue))
|
||||
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.
|
||||
// Also see: "Practical PID Control", p. 169
|
||||
bool findFirstOrderInitialParams2Points(double *params) const {
|
||||
bool findFirstOrderInitialParams2Points(double_t *params) const {
|
||||
int i0 = (int)settings.stepPoint;
|
||||
int i1 = (int)settings.maxPoint;
|
||||
|
||||
double dy = avgMeasuredMax - avgMeasuredMin;
|
||||
double_t dy = avgMeasuredMax - avgMeasuredMin;
|
||||
|
||||
double t[2];
|
||||
static const double tCoefs[] = { 0.353, 0.853 };
|
||||
double_t t[2];
|
||||
static const double_t tCoefs[] = { 0.353, 0.853 };
|
||||
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)
|
||||
return false;
|
||||
}
|
||||
|
|
@ -266,37 +269,37 @@ public:
|
|||
|
||||
// See: Rangaiah G.P., Krishnaswamy P.R. Estimating Second-Order plus Dead Time Model Parameters, 1994.
|
||||
// Also see: "Practical PID Control", p. 187
|
||||
bool findSecondOrderInitialParams3Points(double *params) const {
|
||||
bool findSecondOrderInitialParams3Points(double_t *params) const {
|
||||
int i0 = (int)settings.stepPoint;
|
||||
int i1 = (int)settings.maxPoint;
|
||||
|
||||
double dy = avgMeasuredMax - avgMeasuredMin;
|
||||
double_t dy = avgMeasuredMax - avgMeasuredMin;
|
||||
|
||||
double t[3];
|
||||
static const double tCoefs[] = { 0.14, 0.55, 0.91 };
|
||||
double_t t[3];
|
||||
static const double_t tCoefs[] = { 0.14, 0.55, 0.91 };
|
||||
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)
|
||||
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
|
||||
// check if usable range?
|
||||
if (alpha < 1.2323 || alpha > 2.4850) {
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
double 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 beta = log(alpha / (2.485 - alpha));
|
||||
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);
|
||||
double 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 Tcoef = 0.85818 - 0.62907 * xi + 1.2897 * pow(xi, 2) - 0.36859 * pow(xi, 3) + 0.038891 * 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:
|
||||
// T = 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);
|
||||
if (det < 0) {
|
||||
|
|
@ -321,25 +324,25 @@ public:
|
|||
|
||||
// See: Harriott P. Process control (1964). McGraw-Hill. USA.
|
||||
// Also see: "Practical PID Control", p. 182
|
||||
bool findSecondOrderInitialParamsHarriott(double *params) const {
|
||||
bool findSecondOrderInitialParamsHarriott(double_t *params) const {
|
||||
static const HarriotFunction hfunc;
|
||||
|
||||
int i0 = (int)settings.stepPoint;
|
||||
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 tm = i0 + 0.5 * (t73 / 1.3) * settings.timeScale;
|
||||
double ym = measuredData.getValue((float)tm);
|
||||
double_t t73 = getTimeDelta(measuredData.findDataAt((float_t)(avgMeasuredMin + dy * 0.73), i0, i1));
|
||||
double_t tm = i0 + 0.5 * (t73 / 1.3) * settings.timeScale;
|
||||
double_t ym = measuredData.getValue((float_t)tm);
|
||||
// normalize
|
||||
double ymn = (ym - avgMeasuredMin) / dy;
|
||||
double_t ymn = (ym - avgMeasuredMin) / dy;
|
||||
// sanity check?
|
||||
if (ymn < HarriotFunction::minX || ymn > HarriotFunction::maxX)
|
||||
return false;
|
||||
double r = hfunc.getValue(ymn);
|
||||
double_t r = hfunc.getValue(ymn);
|
||||
|
||||
params[PARAM_K] = dy / (settings.maxValue - settings.minValue);
|
||||
params[PARAM_L] = A1 - t73 / 1.3;
|
||||
|
|
@ -349,31 +352,33 @@ public:
|
|||
return true;
|
||||
}
|
||||
|
||||
double getTimeDelta(int i1) const {
|
||||
double_t getTimeDelta(int i1) const {
|
||||
if (i1 < 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.
|
||||
// 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) {
|
||||
double merit0, merit;
|
||||
double_t merit0, merit;
|
||||
|
||||
#ifdef PID_DEBUG
|
||||
printf("* Solving for better coefs:\r\n");
|
||||
#endif
|
||||
// todo: is it correct?
|
||||
double dTime = 1.0 / settings.timeScale;
|
||||
double_t dTime = pid.periodMs / 1000.0;
|
||||
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);
|
||||
func.justifyParams(model.getParams());
|
||||
merit0 = func.calcMerit(model.getParams());
|
||||
|
||||
// now hopefully we'll find even better coefs!
|
||||
LevenbergMarquardtSolver<numParamsForPid> solver((LMSFunction<numParamsForPid> *)&func, model.getParams());
|
||||
int iterationCount = solver.solve();
|
||||
double lambdaForPid = 10.0;
|
||||
double minDeltaForPid = 1.e-7;
|
||||
int iterationCount = solver.solve(lambdaForPid, minDeltaForPid);
|
||||
|
||||
merit = func.calcMerit(model.getParams());
|
||||
#ifdef PID_DEBUG
|
||||
|
|
@ -383,21 +388,22 @@ public:
|
|||
}
|
||||
|
||||
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
|
||||
//func.calculateAllPoints(params);
|
||||
for (int i = 0; i < func.getNumPoints(); i++) {
|
||||
double v = func.getEstimatedValueAtPoint(i, params);
|
||||
double sv = stepFunc.getValue((float)i, 0);
|
||||
output_csv(fname, (double)i, func.getDataPoint(i), v, sv);
|
||||
double_t v = func.getEstimatedValueAtPoint(i, params);
|
||||
double_t sv = stepFunc.getValue((float_t)i, 0);
|
||||
output_csv(fname, (double_t)i, func.getDataPoint(i), v, sv);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void printSolverResult(int iterationCount, double merit0, double merit) {
|
||||
void printSolverResult(int iterationCount, double_t merit0, double_t merit) {
|
||||
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
|
||||
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;
|
||||
PidAutoTuneSettings settings;
|
||||
pid_sim_type_e simType;
|
||||
double params[4] = { 0 };
|
||||
double_t params[4] = { 0 };
|
||||
pid_s pid;
|
||||
pid_s pid0; // not-optimized
|
||||
int iterationCount = 0;
|
||||
double avgMeasuredMin, avgMeasuredMax;
|
||||
double modelBias;
|
||||
double_t avgMeasuredMin, avgMeasuredMax;
|
||||
double_t modelBias;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -25,7 +25,7 @@ public:
|
|||
scaleShift = 0;
|
||||
}
|
||||
|
||||
void addDataPoint(float v) {
|
||||
void addDataPoint(float_t v) {
|
||||
int idx;
|
||||
|
||||
for (;;) {
|
||||
|
|
@ -41,7 +41,7 @@ public:
|
|||
}
|
||||
scaleShift++;
|
||||
}
|
||||
float numInTheCell = (float)(num - ((num >> scaleShift) << scaleShift));
|
||||
float_t numInTheCell = (float_t)(num - ((num >> scaleShift) << scaleShift));
|
||||
buf[idx] *= numInTheCell;
|
||||
buf[idx] += v;
|
||||
buf[idx] /= (numInTheCell + 1.0f);
|
||||
|
|
@ -52,12 +52,12 @@ public:
|
|||
return (num < 1) ? 1 : ((num - 1) >> scaleShift) + 1;
|
||||
}
|
||||
|
||||
float const *getBuf() const {
|
||||
float_t const *getBuf() const {
|
||||
return buf;
|
||||
}
|
||||
|
||||
// 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 :(
|
||||
assert(scaleShift == 0);
|
||||
|
||||
|
|
@ -76,20 +76,20 @@ public:
|
|||
if (I >= num - 1)
|
||||
return buf[num - 1];
|
||||
// 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);
|
||||
}
|
||||
|
||||
float getAveragedData(int from, int to) const {
|
||||
float avg = 0.0f;
|
||||
float_t getAveragedData(int from, int to) const {
|
||||
float_t avg = 0.0f;
|
||||
for (int i = from; i <= to; i++) {
|
||||
avg += buf[i];
|
||||
}
|
||||
avg /= (float)(to - from + 1);
|
||||
avg /= (float_t)(to - from + 1);
|
||||
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++) {
|
||||
if (buf[i] > v)
|
||||
return i;
|
||||
|
|
@ -98,8 +98,8 @@ public:
|
|||
}
|
||||
|
||||
// integrating using simple trapezoidal method
|
||||
float getArea(int from, int to, float v0) const {
|
||||
float sum = 0.0f;
|
||||
float_t getArea(int from, int to, float_t v0) const {
|
||||
float_t sum = 0.0f;
|
||||
for (int i = from; i < to; i++) {
|
||||
sum += buf[i] + buf[i + 1] - 2.0f * v0;
|
||||
}
|
||||
|
|
@ -108,7 +108,7 @@ public:
|
|||
}
|
||||
|
||||
public:
|
||||
float buf[maxPoints];
|
||||
float_t buf[maxPoints];
|
||||
int num = 0;
|
||||
int scaleShift = 0;
|
||||
};
|
||||
|
|
|
|||
124
pid_controller.h
124
pid_controller.h
|
|
@ -1,4 +1,4 @@
|
|||
/*
|
||||
/*
|
||||
* @file pid_controller.h
|
||||
*
|
||||
* PID Controller models needed to verify the parameters.
|
||||
|
|
@ -16,7 +16,7 @@ public:
|
|||
PidController(const pid_s & p_) : p(p_) {
|
||||
}
|
||||
|
||||
double limitOutput(double v) {
|
||||
double_t limitOutput(double_t v) {
|
||||
if (v < p.minValue)
|
||||
v = p.minValue;
|
||||
if (v > p.maxValue)
|
||||
|
|
@ -34,8 +34,8 @@ public:
|
|||
pTerm = iTerm = dTerm = 0.0;
|
||||
}
|
||||
|
||||
double getOutput(double target, double input, double dTime) {
|
||||
double error = target - input;
|
||||
double_t getOutput(double_t target, double_t input, double_t dTime) {
|
||||
double_t error = target - input;
|
||||
pTerm = p.pFactor * error;
|
||||
iTerm += p.iFactor * dTime * error;
|
||||
dTerm = p.dFactor / dTime * (error - previousError);
|
||||
|
|
@ -45,8 +45,58 @@ public:
|
|||
}
|
||||
|
||||
protected:
|
||||
double pTerm, iTerm, dTerm;
|
||||
double previousError = 0;
|
||||
double_t pTerm, iTerm, dTerm;
|
||||
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))
|
||||
|
|
@ -54,24 +104,24 @@ protected:
|
|||
// See: https://www.scilab.org/discrete-time-pid-controller-implementation
|
||||
class PidDerivativeFilterController : public PidController {
|
||||
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 error = target - input;
|
||||
double a0 = (1.0 + N * dTime);
|
||||
double a1 = -(2.0 + N * dTime);
|
||||
double_t getOutput(double_t target, double_t input, double_t dTime) {
|
||||
double_t error = target - input;
|
||||
double_t a0 = (1.0 + N * dTime);
|
||||
double_t a1 = -(2.0 + N * dTime);
|
||||
|
||||
double a2 = 1.0;
|
||||
double 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 b2 = p.pFactor + p.dFactor * N;
|
||||
double_t a2 = 1.0;
|
||||
double_t b0 = p.pFactor * (1.0 + N * dTime) + p.iFactor * dTime * (1.0 + N * dTime) + p.dFactor * N;
|
||||
double_t b1 = -(p.pFactor * (2.0 + N * dTime) + p.iFactor * dTime + 2.0 * p.dFactor * N);
|
||||
double_t b2 = p.pFactor + p.dFactor * N;
|
||||
|
||||
double ku1 = a1 / a0;
|
||||
double ku2 = a2 / a0;
|
||||
double ke0 = b0 / a0;
|
||||
double ke1 = b1 / a0;
|
||||
double ke2 = b2 / a0;
|
||||
double_t ku1 = a1 / a0;
|
||||
double_t ku2 = a2 / a0;
|
||||
double_t ke0 = b0 / a0;
|
||||
double_t ke1 = b1 / a0;
|
||||
double_t ke2 = b2 / a0;
|
||||
|
||||
e2 = e1;
|
||||
e1 = e0;
|
||||
|
|
@ -87,8 +137,8 @@ public:
|
|||
}
|
||||
|
||||
protected:
|
||||
double e2 = 0, e1 = 0, e0 = 0, u2 = 0, u1 = 0, u0 = 0;
|
||||
double N = 1;
|
||||
double_t e2 = 0, e1 = 0, e0 = 0, u2 = 0, u1 = 0, u0 = 0;
|
||||
double_t N = 1;
|
||||
};
|
||||
|
||||
// Calculate ITAE/ISE and Overshoot
|
||||
|
|
@ -101,35 +151,43 @@ public:
|
|||
lastValue = 0;
|
||||
}
|
||||
|
||||
void addPoint(double i, double value, double target) {
|
||||
double e = target - value;
|
||||
void addPoint(double_t i, double_t value, double_t target) {
|
||||
double_t e = target - value;
|
||||
itae += i * fabs(e);
|
||||
ise += e * e;
|
||||
double overshoot = (value - target) / target;
|
||||
double_t overshoot = (value - target) / target;
|
||||
if (overshoot > 0 && overshoot > maxOvershoot)
|
||||
maxOvershoot = overshoot;
|
||||
lastValue = value;
|
||||
}
|
||||
|
||||
double getItae() const {
|
||||
double_t getItae() const {
|
||||
return itae;
|
||||
}
|
||||
|
||||
double getIse() const {
|
||||
double_t getIse() const {
|
||||
return ise;
|
||||
}
|
||||
|
||||
double getMaxOvershoot() const {
|
||||
double_t getMaxOvershoot() const {
|
||||
return maxOvershoot;
|
||||
}
|
||||
|
||||
double getLastValue() const {
|
||||
double_t getLastValue() const {
|
||||
return lastValue;
|
||||
}
|
||||
|
||||
double_t getMerit() const {
|
||||
//return getItae();
|
||||
#if 1
|
||||
double overShootWeight = 10000.0;
|
||||
return getItae() + overShootWeight * getMaxOvershoot() * getMaxOvershoot();
|
||||
#endif
|
||||
}
|
||||
|
||||
private:
|
||||
double itae = 0; // Integral time-weighted absolute error
|
||||
double ise = 0; // Integral square error
|
||||
double maxOvershoot = 0;
|
||||
double lastValue = 0;
|
||||
double_t itae = 0; // Integral time-weighted absolute error
|
||||
double_t ise = 0; // Integral square error
|
||||
double_t maxOvershoot = 0;
|
||||
double_t lastValue = 0;
|
||||
};
|
||||
|
|
@ -14,7 +14,7 @@
|
|||
|
||||
class MslData {
|
||||
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);
|
||||
|
||||
if (!fp)
|
||||
|
|
@ -43,11 +43,11 @@ public:
|
|||
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::string item;
|
||||
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
|
||||
if (j == 0) {
|
||||
if (v < startTime || v > endTime)
|
||||
|
|
@ -67,7 +67,7 @@ public:
|
|||
}
|
||||
curIdx++;
|
||||
} else if (j == outputIdx) {
|
||||
data.push_back((float)v);
|
||||
data.push_back((float_t)v);
|
||||
if (curIdx >= 0 && settings.stepPoint < 0) {
|
||||
// calculate averaged level to determine the acceptable noise level
|
||||
averagedMin = (averagedMin * (curIdx - 1) + v) / curIdx;
|
||||
|
|
@ -79,14 +79,14 @@ public:
|
|||
return true;
|
||||
}
|
||||
|
||||
double getSaturationStartPoint() {
|
||||
double_t getSaturationStartPoint() {
|
||||
int i;
|
||||
double j;
|
||||
double_t j;
|
||||
// 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
|
||||
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;
|
||||
// this is not accurate because 'averagedMax' is continuously changing
|
||||
curNoiseLevel = std::max(curNoiseLevel, abs(v - averagedMax));
|
||||
|
|
@ -102,16 +102,16 @@ public:
|
|||
}
|
||||
|
||||
public:
|
||||
std::vector<float> data;
|
||||
double totalTime = 0, prevTime = 0;
|
||||
std::vector<float_t> data;
|
||||
double_t totalTime = 0, prevTime = 0;
|
||||
PidAutoTuneSettings settings;
|
||||
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';
|
||||
// it's noise level is used to find the saturation limit of the rest of the data (see getSaturationStartPoint())
|
||||
double acceptableNoiseLevel = 0;
|
||||
double averagedMin = 0;
|
||||
double_t acceptableNoiseLevel = 0;
|
||||
double_t averagedMin = 0;
|
||||
};
|
||||
|
||||
const char *getMethodName(pid_tune_method_e method) {
|
||||
|
|
@ -139,28 +139,30 @@ const char *getSimTypeName(pid_sim_type_e simType) {
|
|||
#if 1
|
||||
int main(int argc, char **argv) {
|
||||
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;
|
||||
}
|
||||
|
||||
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]);
|
||||
|
||||
MslData data;
|
||||
if (!data.readMsl(argv[1], atof(argv[2]), atof(argv[3]), atoi(argv[4]), atoi(argv[5]))) {
|
||||
printf("Usage: PID_FROM_MSL <file.msl> <startTime> <endTime> <inColumnIdx> <outColumnIdx> [<targetValue>]\r\n");
|
||||
printf("Error reading from the file!\r\n");
|
||||
return -2;
|
||||
}
|
||||
|
||||
// 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",
|
||||
data.settings.targetValue, data.settings.minValue, data.settings.maxValue,
|
||||
data.settings.stepPoint, data.settings.maxPoint, data.data.size(), data.settings.timeScale);
|
||||
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));
|
||||
printf("Measuring Settings: targetValue=%Lg minValue=%Lg maxValue=%Lg stepPoint=%Lg maxPoint=%Lg numPoints=%d timeScale=%Lg\r\n",
|
||||
(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;
|
||||
|
||||
static const int numPids = 2;
|
||||
PidAutoTune *chr[numPids] = { &chr1, &chr2 };
|
||||
|
||||
|
|
@ -181,38 +183,39 @@ int main(int argc, char **argv) {
|
|||
|
||||
printf("Done!\r\n");
|
||||
|
||||
// todo: is it correct?
|
||||
double dTime = 1.0 / data.settings.timeScale;
|
||||
const int numSimPoints = 1024;
|
||||
|
||||
pid_s bestPid;
|
||||
double smallestItae = DBL_MAX;
|
||||
for (int k = 0; k < 2; k++) {
|
||||
const double *p = chr[k]->getParams();
|
||||
printf("Model-%d Params: K=%g T1=%g T2=%g L=%g\r\n", (k + 1), p[PARAM_K], p[PARAM_T], p[PARAM_T2], p[PARAM_L]);
|
||||
double_t smallestMerit = DBL_MAX;
|
||||
for (int k = 0; k < numPids; k++) {
|
||||
const double_t *p = chr[k]->getParams();
|
||||
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() };
|
||||
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");
|
||||
|
||||
PidSimulator<numSimPoints> sim1(simTypes[k], chr[k]->getMethodOrder(methods[k]),
|
||||
chr[k]->getAvgMeasuredMin(), chr[k]->getAvgMeasuredMax(), dTime, chr[k]->getModelBias(), csvName);
|
||||
PidSimulator<numSimPoints> sim(simTypes[k], chr[k]->getMethodOrder(methods[k]),
|
||||
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,
|
||||
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(" 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,
|
||||
(long double)pid[j].offset, (long double)pid[j].periodMs);
|
||||
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) {
|
||||
smallestItae = metric.getItae();
|
||||
if (metric.getMerit() < smallestMerit) {
|
||||
smallestMerit = metric.getMerit();
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
104
pid_functions.h
104
pid_functions.h
|
|
@ -37,49 +37,49 @@ enum {
|
|||
const int numParamsForPid = 3;
|
||||
|
||||
// the limit used for K and L params
|
||||
static const double minParamK = 0.001;
|
||||
static const double minParamL = 0.1;
|
||||
static const double_t minParamK = 0.001;
|
||||
static const double_t minParamL = 0.1;
|
||||
|
||||
// T (or T2) has a separate limit, because exp(-1/T) is zero for small T values
|
||||
// (and we need to compare between them to find a direction for optimization method).
|
||||
static const double minParamT = 0.01;
|
||||
static const double_t minParamT = 0.01;
|
||||
|
||||
class InputFunction {
|
||||
public:
|
||||
InputFunction(double timeScale_) : timeScale(timeScale_) {
|
||||
InputFunction(double_t timeScale_) : timeScale(timeScale_) {
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
protected:
|
||||
// time scale needed to synchronize between the virtual step function and the real measured data
|
||||
// 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
|
||||
class StepFunction : public InputFunction
|
||||
{
|
||||
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_) {
|
||||
}
|
||||
|
||||
virtual double getValue(double i, double d) const {
|
||||
virtual double_t getValue(double_t i, double_t d) const {
|
||||
// delayed index
|
||||
double id = i - d * timeScale;
|
||||
double_t id = i - d * timeScale;
|
||||
#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)
|
||||
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
|
||||
double vNear = (I < stepPoint) ? minValue : maxValue;
|
||||
double vFar = (I + 1 < stepPoint) ? minValue : maxValue;
|
||||
double_t vNear = (I < stepPoint) ? minValue : maxValue;
|
||||
double_t vFar = (I + 1 < stepPoint) ? minValue : maxValue;
|
||||
// interpolate
|
||||
return vFar * fract + vNear * (1.0f - fract);
|
||||
#else
|
||||
|
|
@ -88,15 +88,15 @@ public:
|
|||
}
|
||||
|
||||
private:
|
||||
double minValue, maxValue;
|
||||
// stepPoint is float because we have AveragingDataBuffer, and the time axis may be scaled
|
||||
double stepPoint;
|
||||
double_t minValue, maxValue;
|
||||
// stepPoint is not integer because we have AveragingDataBuffer, and the time axis may be scaled
|
||||
double_t stepPoint;
|
||||
};
|
||||
|
||||
template <int numPoints>
|
||||
class StoredDataInputFunction : public InputFunction {
|
||||
public:
|
||||
StoredDataInputFunction(double timeScale_) : InputFunction(timeScale_) {
|
||||
StoredDataInputFunction(double_t timeScale_) : InputFunction(timeScale_) {
|
||||
reset();
|
||||
}
|
||||
|
||||
|
|
@ -104,14 +104,14 @@ public:
|
|||
inputData.init();
|
||||
}
|
||||
|
||||
void addDataPoint(float v) {
|
||||
void addDataPoint(float_t v) {
|
||||
// todo: support data scaling
|
||||
assert(inputData.getNumDataPoints() <= numPoints);
|
||||
inputData.addDataPoint(v);
|
||||
}
|
||||
|
||||
virtual double getValue(double i, double d) const {
|
||||
return inputData.getValue((float)(i - d * timeScale));
|
||||
virtual double_t getValue(double_t i, double_t d) const {
|
||||
return inputData.getValue((float_t)(i - d * timeScale));
|
||||
}
|
||||
|
||||
private:
|
||||
|
|
@ -122,34 +122,34 @@ private:
|
|||
template <int numParams>
|
||||
class AbstractDelayLineFunction : public LMSFunction<numParams> {
|
||||
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;
|
||||
inputFunc = input;
|
||||
numPoints = numDataPoints;
|
||||
this->modelBias = modelBias;
|
||||
}
|
||||
|
||||
virtual double getResidual(int i, const double *params) const {
|
||||
return dataPoints[i] - getEstimatedValueAtPoint(i, params);
|
||||
virtual double_t getResidual(int i, const double_t *params) const {
|
||||
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
|
||||
virtual int getNumPoints() const {
|
||||
return numPoints;
|
||||
}
|
||||
|
||||
float getDataPoint(int i) const {
|
||||
float_t getDataPoint(int i) const {
|
||||
return dataPoints[i];
|
||||
}
|
||||
|
||||
protected:
|
||||
const InputFunction *inputFunc;
|
||||
const float *dataPoints;
|
||||
const float_t *dataPoints;
|
||||
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.
|
||||
double modelBias;
|
||||
double_t modelBias;
|
||||
};
|
||||
|
||||
// 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)
|
||||
class FirstOrderPlusDelayLineFunction : public AbstractDelayLineFunction<3> {
|
||||
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) {
|
||||
}
|
||||
|
||||
virtual void justifyParams(double *params) const {
|
||||
virtual void justifyParams(double_t *params) const {
|
||||
params[PARAM_L] = fmax(params[PARAM_L], minParamL);
|
||||
params[PARAM_T] = fmax(params[PARAM_T], minParamT);
|
||||
params[PARAM_K] = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K];
|
||||
}
|
||||
|
||||
// Creating a state-space representation using Rosenbrock system matrix
|
||||
virtual double getEstimatedValueAtPoint(int i, const double *params) const {
|
||||
virtual double_t calcEstimatedValuesAtPoint(int i, const double_t *params) const {
|
||||
// only positive values allowed (todo: choose the limits)
|
||||
double pL = fmax(params[PARAM_L], minParamL);
|
||||
double pT = fmax(params[PARAM_T], minParamT);
|
||||
double pK = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K];
|
||||
double_t pL = fmax(params[PARAM_L], minParamL);
|
||||
double_t pT = fmax(params[PARAM_T], minParamT);
|
||||
double_t pK = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K];
|
||||
|
||||
// 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?
|
||||
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
|
||||
for (int j = 0; j <= i; j++) {
|
||||
// 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)
|
||||
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)
|
||||
class SecondOrderPlusDelayLineOverdampedFunction : public AbstractDelayLineFunction<4> {
|
||||
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) {
|
||||
}
|
||||
|
||||
virtual void justifyParams(double *params) const {
|
||||
virtual void justifyParams(double_t *params) const {
|
||||
params[PARAM_L] = fmax(params[PARAM_L], minParamL);
|
||||
params[PARAM_T] = fmax(params[PARAM_T], minParamT);
|
||||
params[PARAM_T2] = fmax(params[PARAM_T2], minParamT);
|
||||
|
|
@ -212,25 +212,25 @@ public:
|
|||
}
|
||||
|
||||
// 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)
|
||||
double pL = fmax(params[PARAM_L], minParamL);
|
||||
double pT = fmax(params[PARAM_T], minParamT);
|
||||
double pT2 = fmax(params[PARAM_T2], minParamT);
|
||||
double pK = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K];
|
||||
double_t pL = fmax(params[PARAM_L], minParamL);
|
||||
double_t pT = fmax(params[PARAM_T], minParamT);
|
||||
double_t pT2 = fmax(params[PARAM_T2], minParamT);
|
||||
double_t pK = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K];
|
||||
|
||||
// state-space params
|
||||
double lambda = exp(-1.0 / (pT * inputFunc->getTimeScale()));
|
||||
double lambda2 = exp(-1.0 / (pT2 * inputFunc->getTimeScale()));
|
||||
double_t lambda = exp(-1.0 / (pT * inputFunc->getTimeScale()));
|
||||
double_t lambda2 = exp(-1.0 / (pT2 * inputFunc->getTimeScale()));
|
||||
|
||||
// todo: find better initial values?
|
||||
double x = inputFunc->getValue(0, 0) * pK;
|
||||
double y = inputFunc->getValue(0, 0) * pK;
|
||||
double_t x = 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
|
||||
for (int j = 0; j <= i; j++) {
|
||||
// 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)
|
||||
y = lambda2 * y + (1.0 - lambda2) * x;
|
||||
|
|
@ -247,12 +247,12 @@ public:
|
|||
// See: findSecondOrderInitialParamsHarriot() and "Harriot P. Process control (1964). McGraw-Hill. USA."
|
||||
class HarriotFunction {
|
||||
public:
|
||||
double getValue(double x) const {
|
||||
return buf.getValue((float)((x - 2.8 / 719.0 - 0.26) * 719.0 / 2.8));
|
||||
double_t getValue(double_t x) const {
|
||||
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 maxX = 33 / 719.0 * 2.8 + minX;
|
||||
static constexpr double_t minX = 2.8 / 719.0 - 0.26;
|
||||
static constexpr double_t maxX = 33 / 719.0 * 2.8 + minX;
|
||||
|
||||
private:
|
||||
const AveragingDataBuffer<34> buf = { {
|
||||
|
|
|
|||
|
|
@ -39,27 +39,27 @@ class ModelOpenLoopPlant
|
|||
public:
|
||||
ModelOpenLoopPlant() {}
|
||||
|
||||
ModelOpenLoopPlant(const double *params_) : params((double *)params_) {
|
||||
ModelOpenLoopPlant(const double_t *params_) : params((double_t *)params_) {
|
||||
}
|
||||
|
||||
double *getParams() {
|
||||
double_t *getParams() {
|
||||
return params;
|
||||
}
|
||||
|
||||
virtual float getKp() const = 0;
|
||||
virtual float getKi() const = 0;
|
||||
virtual float getKd() const = 0;
|
||||
virtual float_t getKp() const = 0;
|
||||
virtual float_t getKi() const = 0;
|
||||
virtual float_t getKd() const = 0;
|
||||
|
||||
protected:
|
||||
double *params;
|
||||
double_t *params;
|
||||
};
|
||||
|
||||
// Based on FOPDT model approximated from Overdamped-SOPDT model
|
||||
class ModelFopdtApproximatedFromSopdt : public ModelOpenLoopPlant {
|
||||
public:
|
||||
ModelFopdtApproximatedFromSopdt(double *soParams) : ModelOpenLoopPlant(p) {
|
||||
double T2divT1 = soParams[PARAM_T2] / soParams[PARAM_T];
|
||||
double T2mulT1 = soParams[PARAM_T2] * soParams[PARAM_T];
|
||||
ModelFopdtApproximatedFromSopdt(double_t *soParams) : ModelOpenLoopPlant(p) {
|
||||
double_t T2divT1 = soParams[PARAM_T2] / soParams[PARAM_T];
|
||||
double_t T2mulT1 = soParams[PARAM_T2] * soParams[PARAM_T];
|
||||
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_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
|
||||
virtual float getKp() const {
|
||||
virtual float_t getKp() const {
|
||||
return 0.0f;
|
||||
}
|
||||
virtual float getKi() const {
|
||||
virtual float_t getKi() const {
|
||||
return 0.0f;
|
||||
}
|
||||
virtual float getKd() const {
|
||||
virtual float_t getKd() const {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
private:
|
||||
// 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)
|
||||
// This class converts in into our "Parallel" form: Kp + Ki / S + Kd * S
|
||||
class ModelStandard : public ModelOpenLoopPlant {
|
||||
public:
|
||||
ModelStandard(const double *params_) : ModelOpenLoopPlant(params_) {
|
||||
ModelStandard(const double_t *params_) : ModelOpenLoopPlant(params_) {
|
||||
}
|
||||
|
||||
virtual float getKp() const {
|
||||
return (float)Kc;
|
||||
virtual float_t getKp() const {
|
||||
return (float_t)Kc;
|
||||
}
|
||||
virtual float getKi() const {
|
||||
return (float)(Kc / Ti);
|
||||
virtual float_t getKi() const {
|
||||
return (float_t)(Kc / Ti);
|
||||
}
|
||||
virtual float getKd() const {
|
||||
return (float)(Kc * Td);
|
||||
virtual float_t getKd() const {
|
||||
return (float_t)(Kc * Td);
|
||||
}
|
||||
|
||||
protected:
|
||||
// "Standard" PID coefs
|
||||
double Kc, Ti, Td;
|
||||
double_t Kc, Ti, Td;
|
||||
};
|
||||
|
||||
class ModelStandardIMC : public ModelStandard {
|
||||
public:
|
||||
ModelStandardIMC(const double *params_) : ModelStandard(params_) {
|
||||
ModelStandardIMC(const double_t *params_) : ModelStandard(params_) {
|
||||
lambda = fmax(0.25 * params[PARAM_L], 0.2 * Ti);
|
||||
}
|
||||
|
||||
protected:
|
||||
// closed-loop speed of response
|
||||
double lambda;
|
||||
double_t lambda;
|
||||
};
|
||||
|
||||
|
||||
// Chien-Hrones-Reswick PID implementation for the 1st order model (generic model).
|
||||
class ModelChienHronesReswickFirstOrder : public ModelStandardIMC {
|
||||
public:
|
||||
ModelChienHronesReswickFirstOrder(const double *params_) : ModelStandardIMC(params_) {
|
||||
double l2 = params[PARAM_L] / 2.0;
|
||||
ModelChienHronesReswickFirstOrder(const double_t *params_) : ModelStandardIMC(params_) {
|
||||
double_t l2 = params[PARAM_L] / 2.0;
|
||||
Ti = params[PARAM_T] + l2;
|
||||
Td = params[PARAM_T] * params[PARAM_L] / (2 * params[PARAM_T] + params[PARAM_L]);
|
||||
Kc = Ti / (params[PARAM_K] * (lambda + l2));
|
||||
|
|
@ -132,34 +132,34 @@ public:
|
|||
// Chien-Hrones-Reswick PID implementation for the 1st order model (set-point regulation).
|
||||
class ModelChienHronesReswickFirstOrderSetpoint : public ModelOpenLoopPlant {
|
||||
public:
|
||||
ModelChienHronesReswickFirstOrderSetpoint(const double *params_) : ModelOpenLoopPlant(params_) {
|
||||
ModelChienHronesReswickFirstOrderSetpoint(const double_t *params_) : ModelOpenLoopPlant(params_) {
|
||||
}
|
||||
|
||||
virtual float getKp() const {
|
||||
return (float)(0.6f / params[PARAM_K]);
|
||||
virtual float_t getKp() const {
|
||||
return (float_t)(0.6f / params[PARAM_K]);
|
||||
}
|
||||
virtual float getKi() const {
|
||||
return (float)(1.0f / params[PARAM_T]);
|
||||
virtual float_t getKi() const {
|
||||
return (float_t)(1.0f / params[PARAM_T]);
|
||||
}
|
||||
virtual float getKd() const {
|
||||
return (float)(1.0f / (0.5f * params[PARAM_L]));
|
||||
virtual float_t getKd() const {
|
||||
return (float_t)(1.0f / (0.5f * params[PARAM_L]));
|
||||
}
|
||||
};
|
||||
|
||||
// Chien-Hrones-Reswick PID implementation for the 1st order model (disturbance rejection).
|
||||
class ModelChienHronesReswickFirstOrderDisturbance : public ModelOpenLoopPlant {
|
||||
public:
|
||||
ModelChienHronesReswickFirstOrderDisturbance(const double *params_) : ModelOpenLoopPlant(params_) {
|
||||
ModelChienHronesReswickFirstOrderDisturbance(const double_t *params_) : ModelOpenLoopPlant(params_) {
|
||||
}
|
||||
|
||||
virtual float getKp() const {
|
||||
return (float)(0.95f / params[PARAM_K]);
|
||||
virtual float_t getKp() const {
|
||||
return (float_t)(0.95f / params[PARAM_K]);
|
||||
}
|
||||
virtual float getKi() const {
|
||||
return (float)(2.4f / params[PARAM_T]);
|
||||
virtual float_t getKi() const {
|
||||
return (float_t)(2.4f / params[PARAM_T]);
|
||||
}
|
||||
virtual float getKd() const {
|
||||
return (float)(1.0f / (0.42f * params[PARAM_L]));
|
||||
virtual float_t getKd() const {
|
||||
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"
|
||||
class ModelRiveraMorariFirstOrder : public ModelStandardIMC {
|
||||
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]));
|
||||
Ti = params[PARAM_T] + 0.5 * 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."
|
||||
class ModelChienHronesReswickSecondOrder : public ModelStandardIMC {
|
||||
public:
|
||||
ModelChienHronesReswickSecondOrder(const double *params_) : ModelStandardIMC(params_) {
|
||||
ModelChienHronesReswickSecondOrder(const double_t *params_) : ModelStandardIMC(params_) {
|
||||
Ti = params[PARAM_T] + params[PARAM_T2];
|
||||
Td = params[PARAM_T] * params[PARAM_T2] / Ti;
|
||||
Kc = Ti / (params[PARAM_K] * (lambda + params[PARAM_L]));
|
||||
|
|
@ -189,8 +189,8 @@ public:
|
|||
// "Step disturbance".
|
||||
class ModelVanDerGrintenSecondOrder : public ModelStandard {
|
||||
public:
|
||||
ModelVanDerGrintenSecondOrder(const double *params_) : ModelStandard(params_) {
|
||||
double T12 = params[PARAM_T] + params[PARAM_T2];
|
||||
ModelVanDerGrintenSecondOrder(const double_t *params_) : ModelStandard(params_) {
|
||||
double_t T12 = params[PARAM_T] + params[PARAM_T2];
|
||||
Ti = T12 + 0.5 * params[PARAM_L];
|
||||
Td = (T12 * params[PARAM_L] + 2.0 * params[PARAM_T] * params[PARAM_T2]) / (params[PARAM_L] + 2.0 * T12);
|
||||
Kc = (0.5 + T12 / params[PARAM_L]) / params[PARAM_K];
|
||||
|
|
@ -201,9 +201,9 @@ public:
|
|||
// Suited for overdamped response and significant delay.
|
||||
class ModelHaalmanPembertonSecondOrder : public ModelStandard {
|
||||
public:
|
||||
ModelHaalmanPembertonSecondOrder(const double *params_) : ModelStandard(params_) {
|
||||
double T1divT2 = params[PARAM_T] / params[PARAM_T2];
|
||||
double LdivT2 = params[PARAM_L] / params[PARAM_T2];
|
||||
ModelHaalmanPembertonSecondOrder(const double_t *params_) : ModelStandard(params_) {
|
||||
double_t T1divT2 = params[PARAM_T] / params[PARAM_T2];
|
||||
double_t LdivT2 = params[PARAM_L] / params[PARAM_T2];
|
||||
Ti = params[PARAM_T] + params[PARAM_T2];
|
||||
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.
|
||||
class ModelMaclaurinSecondOrder : public ModelStandardIMC {
|
||||
public:
|
||||
ModelMaclaurinSecondOrder(const double *params_) : ModelStandardIMC(params_) {
|
||||
double T1T2 = params[PARAM_T] + params[PARAM_T2];
|
||||
double L = params[PARAM_L];
|
||||
double L2 = L * L;
|
||||
double twolL = 2.0 * lambda + L;
|
||||
ModelMaclaurinSecondOrder(const double_t *params_) : ModelStandardIMC(params_) {
|
||||
double_t T1T2 = params[PARAM_T] + params[PARAM_T2];
|
||||
double_t L = params[PARAM_L];
|
||||
double_t L2 = L * L;
|
||||
double_t twolL = 2.0 * lambda + L;
|
||||
Ti = T1T2 - (2.0 * lambda * lambda - L2) / (2.0 * twolL);
|
||||
Kc = Ti / (params[PARAM_K] * twolL);
|
||||
Td = Ti - T1T2 + (params[PARAM_T] * params[PARAM_T2] - L2 * L / (6.0 * twolL)) / Ti;
|
||||
|
|
@ -239,16 +239,16 @@ public:
|
|||
pidParams[PARAM_Kd] = initial->getKd();
|
||||
}
|
||||
|
||||
virtual float getKp() const {
|
||||
return (float)pidParams[PARAM_Kp];
|
||||
virtual float_t getKp() const {
|
||||
return (float_t)pidParams[PARAM_Kp];
|
||||
}
|
||||
virtual float getKi() const {
|
||||
return (float)pidParams[PARAM_Ki];
|
||||
virtual float_t getKi() const {
|
||||
return (float_t)pidParams[PARAM_Ki];
|
||||
}
|
||||
virtual float getKd() const {
|
||||
return (float)pidParams[PARAM_Kd];
|
||||
virtual float_t getKd() const {
|
||||
return (float_t)pidParams[PARAM_Kd];
|
||||
}
|
||||
|
||||
protected:
|
||||
double pidParams[numParamsForPid];
|
||||
double_t pidParams[numParamsForPid];
|
||||
};
|
||||
|
|
|
|||
94
pid_sim.h
94
pid_sim.h
|
|
@ -21,26 +21,32 @@ typedef enum {
|
|||
template <int maxPoints>
|
||||
class PidSimulator {
|
||||
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_),
|
||||
plantInput(1.0 / dTime_), plant1(&plantInput, nullptr, 0, modelBias), plant2(&plantInput, nullptr, 0, modelBias) {
|
||||
}
|
||||
|
||||
PidAccuracyMetric simulate(int numPoints, const pid_s & pidParams, const double *params) {
|
||||
LMSFunction<4> *plant;
|
||||
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_;
|
||||
|
||||
if (methodOrder == 1)
|
||||
plant = (LMSFunction<4> *)&plant1;
|
||||
else
|
||||
plant = &plant2;
|
||||
}
|
||||
|
||||
PidParallelController pid(pidParams);
|
||||
//PidDerivativeFilterController pid(pidParams, 10);
|
||||
void setModelParams(const double_t *modelParams) {
|
||||
this->modelParams = modelParams;
|
||||
//plant->calculateAllPoints(modelParams);
|
||||
}
|
||||
|
||||
PidAccuracyMetric simulate(int numPoints, const pid_s & pidParams) {
|
||||
//PidParallelController pid(pidParams);
|
||||
PidIndustrialController pid(pidParams);
|
||||
|
||||
plantInput.reset();
|
||||
// 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
|
||||
pid.getOutput(getSetpoint(0), plant->getEstimatedValueAtPoint(0, params), dTime);
|
||||
pid.getOutput(getSetpoint(0), plant->calcEstimatedValuesAtPoint(0, modelParams), dTime);
|
||||
|
||||
stepPoint = maxPoints / 2;
|
||||
|
||||
|
|
@ -49,46 +55,46 @@ public:
|
|||
// simulate over time
|
||||
for (int i = 0; i < numPoints; i++) {
|
||||
// make a step in the middle
|
||||
double target = getSetpoint(i);
|
||||
// "measure" the current value of the plant
|
||||
double pidInput = plant->getEstimatedValueAtPoint(i, params) + getLoadDisturbance(i);
|
||||
double_t target = getSetpoint(i);
|
||||
// "measure" the current value of the plant (we cannot use getEstimatedValueAtPoint() because the function input is changing)
|
||||
double_t pidInput = plant->calcEstimatedValuesAtPoint(i, modelParams) + getLoadDisturbance(i);
|
||||
// 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
|
||||
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!
|
||||
if (i >= stepPoint)
|
||||
metric.addPoint((double)i / (double)numPoints, pidInput, target);
|
||||
metric.addPoint((double_t)i / (double_t)numPoints, pidInput, target);
|
||||
#ifdef PID_DEBUG
|
||||
if (outputFile != nullptr)
|
||||
output_csv(outputFile, (double)i, pidOutput, target, pidInput);
|
||||
output_csv(outputFile, (double_t)i, pidOutput, target, pidInput);
|
||||
#endif
|
||||
}
|
||||
|
||||
return metric;
|
||||
}
|
||||
|
||||
double getSetpoint(int i) const {
|
||||
double_t getSetpoint(int i) const {
|
||||
switch (simType) {
|
||||
case PID_SIM_SERVO:
|
||||
return (i > stepPoint) ? target2 : target1;
|
||||
default:
|
||||
// we want to be in the middle for safety
|
||||
return (target1 + target2) / 2.0;
|
||||
// if we don't know the target, we want to be in the middle for safety
|
||||
return target;
|
||||
}
|
||||
}
|
||||
|
||||
double getLoadDisturbance(int i) const {
|
||||
static const double disturb = 0.10;
|
||||
static const double ampl = 0.25, period = 0.37;
|
||||
double d = 0;
|
||||
double_t getLoadDisturbance(int i) const {
|
||||
static const double_t disturb = 0.10;
|
||||
static const double_t ampl = 0.25, period = 0.37;
|
||||
double_t d = 0;
|
||||
switch (simType) {
|
||||
case PID_SIM_REGULATOR:
|
||||
// add or subtract 10% to imitate the "load"
|
||||
d += target1 * ((i > stepPoint) ? -disturb : disturb);
|
||||
// add periodic noise
|
||||
d += sin(2.0 * 3.14159265 * (double)i * dTime / period) * ampl;
|
||||
d += sin(2.0 * 3.14159265 * (double_t)i * dTime / period) * ampl;
|
||||
return d;
|
||||
default:
|
||||
return 0.0;
|
||||
|
|
@ -100,11 +106,15 @@ protected:
|
|||
int stepPoint;
|
||||
const char *outputFile;
|
||||
StoredDataInputFunction<maxPoints> plantInput;
|
||||
|
||||
LMSFunction<4> *plant;
|
||||
FirstOrderPlusDelayLineFunction plant1;
|
||||
SecondOrderPlusDelayLineOverdampedFunction plant2;
|
||||
|
||||
PidAccuracyMetric metric;
|
||||
double target1, target2, dTime;
|
||||
double_t target, target1, target2, dTime;
|
||||
pid_sim_type_e simType;
|
||||
const double_t *modelParams;
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -114,8 +124,8 @@ protected:
|
|||
template <int numPoints>
|
||||
class PidSimulatorFactory {
|
||||
public:
|
||||
PidSimulatorFactory(pid_sim_type_e simType, int methodOrder_, double target1_, double target2_, double dTime, double modelBias, const pid_s & pid_) :
|
||||
sim(simType, methodOrder_, target1_, target2_, dTime, modelBias, nullptr), pid(pid_) {
|
||||
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_, target_, dTime, modelBias, nullptr), pid(pid_) {
|
||||
}
|
||||
|
||||
public:
|
||||
|
|
@ -129,12 +139,16 @@ public:
|
|||
template <int numPoints>
|
||||
class PidCoefsFinderFunction : public LMSFunction<numParamsForPid> {
|
||||
public:
|
||||
PidCoefsFinderFunction(PidSimulatorFactory<numPoints> *simFactory_, const double *modelParams_) :
|
||||
PidCoefsFinderFunction(PidSimulatorFactory<numPoints> *simFactory_, const double_t *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?
|
||||
//params[PARAM_Ki] = fmax(params[PARAM_Ki], 0.00001);
|
||||
}
|
||||
|
||||
// Get the total number of data points
|
||||
|
|
@ -143,29 +157,29 @@ public:
|
|||
}
|
||||
|
||||
// Calculate the sum of the squares of the residuals
|
||||
virtual double calcMerit(double *params) const {
|
||||
return simulate(numPoints - 1, params).getItae();
|
||||
virtual double_t calcMerit(double_t *params) const {
|
||||
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);
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
PidAccuracyMetric simulate(int i, const double *params) const {
|
||||
PidAccuracyMetric simulate(int i, const double_t *params) const {
|
||||
// update params
|
||||
simFactory->pid.pFactor = (float)params[PARAM_Kp];
|
||||
simFactory->pid.iFactor = (float)params[PARAM_Ki];
|
||||
simFactory->pid.dFactor = (float)params[PARAM_Kd];
|
||||
simFactory->pid.pFactor = (float_t)params[PARAM_Kp];
|
||||
simFactory->pid.iFactor = (float_t)params[PARAM_Ki];
|
||||
simFactory->pid.dFactor = (float_t)params[PARAM_Kd];
|
||||
// simulate PID controller response
|
||||
return simFactory->sim.simulate(i + 1, simFactory->pid, modelParams);
|
||||
return simFactory->sim.simulate(i + 1, simFactory->pid);
|
||||
}
|
||||
|
||||
protected:
|
||||
const double *modelParams;
|
||||
const double_t *modelParams;
|
||||
PidSimulatorFactory<numPoints> *simFactory;
|
||||
};
|
||||
|
||||
|
|
|
|||
108
test_chs_pid.cpp
108
test_chs_pid.cpp
|
|
@ -31,25 +31,27 @@ TEST(pidAutoTune, testMeasuredDataBuffer) {
|
|||
TEST(pidAutoTune, testFOPDT) {
|
||||
StepFunction stepFunc(0, 100, 10, 1.0);
|
||||
FirstOrderPlusDelayLineFunction func(&stepFunc, nullptr, 0, 0.0);
|
||||
double params[3];
|
||||
double_t params[3];
|
||||
params[PARAM_K] = 2.0;
|
||||
params[PARAM_T] = 3.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);
|
||||
}
|
||||
|
||||
TEST(pidAutoTune, testSOPDTOverdamped) {
|
||||
StepFunction stepFunc(0, 100, 10, 1.0);
|
||||
SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, nullptr, 0, 0.0);
|
||||
double params[4];
|
||||
double_t params[4];
|
||||
params[PARAM_K] = 2.0;
|
||||
params[PARAM_T] = 3.0;
|
||||
params[PARAM_T2] = 0.3;
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -60,21 +62,21 @@ const int numData = sizeof(outputData) / sizeof(outputData[0]);
|
|||
void printSOPDT() {
|
||||
StepFunction stepFunc(20.0, 30.0, 178, 1.0);
|
||||
SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, nullptr, 0, 0.0);
|
||||
double params[4];
|
||||
double_t params[4];
|
||||
params[PARAM_K] = 0.251778;
|
||||
params[PARAM_T] = 55.7078;
|
||||
params[PARAM_T2] = 55.7077;
|
||||
params[PARAM_L] = 1.80759;
|
||||
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
TEST(pidAutoTune, chsSopdtPid) {
|
||||
PidAutoTune chr;
|
||||
double params0[4];
|
||||
double_t params0[4];
|
||||
|
||||
// todo: find better initial values?
|
||||
params0[PARAM_K] = 0.1;
|
||||
|
|
@ -95,7 +97,7 @@ TEST(pidAutoTune, chsSopdtPid) {
|
|||
bool ret = chr.findPid(PID_SIM_SERVO, PID_TUNE_CHR2, settings, params0);
|
||||
|
||||
#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]);
|
||||
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);
|
||||
|
|
@ -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%
|
||||
{ 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
|
||||
{ 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_T] = 55.841;
|
||||
params[PARAM_T2] = 55.841;
|
||||
params[PARAM_L] = 1.52685;
|
||||
|
||||
// todo: is it correct?
|
||||
double dTime = 1;
|
||||
double_t dTime = 1;
|
||||
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++) {
|
||||
PidAccuracyMetric metric = sim.simulate(numSimPoints, pidParams[idx], params);
|
||||
PidAccuracyMetric metric = sim.simulate(numSimPoints, pidParams[idx]);
|
||||
#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
|
||||
}
|
||||
|
||||
// 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;
|
||||
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;
|
||||
//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
|
||||
double p[4] = { 0.276267, 0.596307, 0.1, 0.140311 };
|
||||
double modelBias = 7.88121;
|
||||
pid.pFactor = 18.07646561;
|
||||
pid.iFactor = 16.68941116;
|
||||
pid.dFactor = 0.00180748;
|
||||
pid.offset = 21.4;
|
||||
p = { 0.276269, 0.596327, 0.1, 0.140298 };
|
||||
modelBias = 7.88121;
|
||||
pid.pFactor = 15.71419334;
|
||||
pid.iFactor = 12.51472759;
|
||||
pid.dFactor = 1.11415541;
|
||||
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
|
||||
|
||||
#if 0
|
||||
double p[4] = { 0.276267, 0.596307, 0.1, 0.140311 };
|
||||
double modelBias = 7.88121;
|
||||
pid.pFactor = 12.17391205;
|
||||
pid.iFactor = 11.22539520;
|
||||
pid.dFactor = 2.99444199;
|
||||
pid.offset = 28.50101471;
|
||||
//pid.offset = 21.4;
|
||||
p = { 0.276269, 0.596327, 0.1, 0.140298 };
|
||||
modelBias = 7.88121;
|
||||
pid.pFactor = 15.77522947;
|
||||
pid.iFactor = -2.03882839;
|
||||
pid.dFactor = 1.05018034;
|
||||
pid.offset = 21.0f;
|
||||
testPidAndModel(pid, p, modelBias);
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
double p[4] = { 0.276267, 0.596307, 0.1, 0.140311 };
|
||||
double modelBias = 7.88121;
|
||||
p = { 0.276267, 0.596307, 0.1, 0.140311 };
|
||||
modelBias = 7.88121;
|
||||
pid.pFactor = 19.52591324;
|
||||
pid.iFactor = 14.07089329;
|
||||
pid.dFactor = 1.58994818;
|
||||
pid.offset = 21.4;
|
||||
testPidAndModel(pid, p, modelBias);
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -14,10 +14,10 @@
|
|||
class GaussianFunction : public LMSFunction<6> {
|
||||
const int numParams = 6;
|
||||
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
|
||||
|
|
@ -25,24 +25,24 @@ public:
|
|||
return numPoints;
|
||||
}
|
||||
|
||||
virtual double getEstimatedValueAtPoint(int pi, const double *params) const {
|
||||
double val = 0.0;
|
||||
virtual double_t calcEstimatedValuesAtPoint(int pi, const double_t *params) const {
|
||||
double_t val = 0.0;
|
||||
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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
private:
|
||||
int numPoints;
|
||||
double *xValues;
|
||||
double *yValues;
|
||||
double_t *xValues;
|
||||
double_t *yValues;
|
||||
};
|
||||
|
||||
void testGaussianFunction() {
|
||||
|
|
@ -51,14 +51,14 @@ void testGaussianFunction() {
|
|||
const int numParams = 6;
|
||||
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++) {
|
||||
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);
|
||||
|
||||
|
|
@ -67,7 +67,7 @@ void testGaussianFunction() {
|
|||
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);
|
||||
|
||||
int iterationCount = solver.solve(0.001, 1e-15, 100);
|
||||
|
|
@ -82,7 +82,7 @@ void testGaussianFunction() {
|
|||
void testMatrixInverse() {
|
||||
int i, j;
|
||||
const int n = 4;
|
||||
double a[n][n];
|
||||
double_t a[n][n];
|
||||
// fill with some arbitrary values
|
||||
for (i = 0; i < n; i++) {
|
||||
for (j = 0; j < n; j++) {
|
||||
|
|
@ -90,16 +90,16 @@ void testMatrixInverse() {
|
|||
}
|
||||
}
|
||||
|
||||
double ai[n][n];
|
||||
bool ret = MatrixHelper<double, n>::inverseMatrix(ai, a);
|
||||
double_t ai[n][n];
|
||||
bool ret = MatrixHelper<double_t, n>::inverseMatrix(ai, a);
|
||||
ASSERT_EQ(true, ret);
|
||||
|
||||
double mul[n][n];
|
||||
MatrixHelper<double, n>::multiplyMatrix(mul, ai, a);
|
||||
double_t mul[n][n];
|
||||
MatrixHelper<double_t, n>::multiplyMatrix(mul, ai, a);
|
||||
// A^-1 * A = I
|
||||
for (i = 0; i < n; i++) {
|
||||
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]);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue