mirror of https://github.com/rusefi/autopid.git
266 lines
8.9 KiB
C++
266 lines
8.9 KiB
C++
/*
|
|
* @file pid_functions.h
|
|
*
|
|
* Functions used by PID tuner
|
|
*
|
|
* @date Sep 27, 2019
|
|
* @author andreika, (c) 2019
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "global.h"
|
|
|
|
#include "levenberg_marquardt_solver.h"
|
|
#include "pid_avg_buf.h"
|
|
|
|
// This helps to differentiate with respect to 'delay' axis (while finding the Hessian matrix)
|
|
#define INTERPOLATED_STEP_FUNCTION
|
|
|
|
// Generic Step model params:
|
|
enum {
|
|
PARAM_K = 0, // K = Gain
|
|
PARAM_T, // T = Time constant
|
|
PARAM_L, // L = Delay (dead time)
|
|
|
|
// 2nd order params
|
|
PARAM_T2, // T2 = Time constant
|
|
|
|
};
|
|
|
|
// PID model params:
|
|
enum {
|
|
PARAM_Kp = 0,
|
|
PARAM_Ki,
|
|
PARAM_Kd,
|
|
};
|
|
const int numParamsForPid = 3;
|
|
|
|
// the limit used for K and L params
|
|
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_t minParamT = 0.01;
|
|
|
|
class InputFunction {
|
|
public:
|
|
InputFunction(double_t timeScale_) : timeScale(timeScale_) {
|
|
}
|
|
|
|
// i = index, d = delay time (in seconds?)
|
|
virtual double_t getValue(double_t i, double_t d) const = 0;
|
|
|
|
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_t timeScale;
|
|
};
|
|
|
|
// Heaviside step function interpolated between 'min' and 'max' values with 'stepPoint' time offset
|
|
class StepFunction : public InputFunction
|
|
{
|
|
public:
|
|
StepFunction(double_t minValue_, double_t maxValue_, double_t stepPoint_, double_t timeScale_) :
|
|
minValue(minValue_), maxValue(maxValue_), stepPoint(stepPoint_), InputFunction(timeScale_) {
|
|
}
|
|
|
|
virtual double_t getValue(double_t i, double_t d) const {
|
|
// delayed index
|
|
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_t fract = id - I; // 0 = choose near value, 1 = choose far value
|
|
// find two closest input values for the given delay
|
|
double_t vNear = (I < stepPoint) ? minValue : maxValue;
|
|
double_t vFar = (I + 1 < stepPoint) ? minValue : maxValue;
|
|
// interpolate
|
|
return vFar * fract + vNear * (1.0f - fract);
|
|
#else
|
|
return ((id < stepPoint) ? minValue : maxValue);
|
|
#endif
|
|
}
|
|
|
|
private:
|
|
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_t timeScale_) : InputFunction(timeScale_) {
|
|
reset();
|
|
}
|
|
|
|
void reset() {
|
|
inputData.init();
|
|
}
|
|
|
|
void addDataPoint(float_t v) {
|
|
// todo: support data scaling
|
|
assert(inputData.getNumDataPoints() <= numPoints);
|
|
inputData.addDataPoint(v);
|
|
}
|
|
|
|
virtual double_t getValue(double_t i, double_t d) const {
|
|
return inputData.getValue((float_t)(i - d * timeScale));
|
|
}
|
|
|
|
private:
|
|
AveragingDataBuffer<numPoints> inputData;
|
|
};
|
|
|
|
// Abstract indirect transfer function used for step response analytic simulation
|
|
template <int numParams>
|
|
class AbstractDelayLineFunction : public LMSFunction<numParams> {
|
|
public:
|
|
AbstractDelayLineFunction(const InputFunction *input, const float_t *measuredOutput, int numDataPoints, double_t modelBias) {
|
|
dataPoints = measuredOutput;
|
|
inputFunc = input;
|
|
numPoints = numDataPoints;
|
|
this->modelBias = modelBias;
|
|
}
|
|
|
|
virtual double_t getResidual(int i, const double_t *params) const {
|
|
return dataPoints[i] - this->getEstimatedValueAtPoint(i, params);
|
|
}
|
|
|
|
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_t getDataPoint(int i) const {
|
|
return dataPoints[i];
|
|
}
|
|
|
|
protected:
|
|
const InputFunction *inputFunc;
|
|
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_t modelBias;
|
|
};
|
|
|
|
// FODPT indirect transfer function used for step response analytic simulation.
|
|
// Used mostly as an approximate model for chemical or thermal processes
|
|
// The Laplace representation is: K * exp(-L*s) / (T*s + 1)
|
|
class FirstOrderPlusDelayLineFunction : public AbstractDelayLineFunction<3> {
|
|
public:
|
|
FirstOrderPlusDelayLineFunction(const InputFunction *input, const float_t *measuredOutput, int numDataPoints, double_t modelBias) :
|
|
AbstractDelayLineFunction(input, measuredOutput, numDataPoints, modelBias) {
|
|
}
|
|
|
|
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_t calcEstimatedValuesAtPoint(int i, const double_t *params) const {
|
|
// only positive values allowed (todo: choose the limits)
|
|
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_t lambda = exp(-1.0 / (pT * inputFunc->getTimeScale()));
|
|
|
|
// todo: find better initial value?
|
|
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_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;
|
|
}
|
|
|
|
// the output can be biased
|
|
return y + modelBias;
|
|
}
|
|
};
|
|
|
|
|
|
// "Overdamped" SODPT indirect transfer function used for step response analytic simulation (xi > 1)
|
|
// Used mostly as an approximate model for electro-mechanical processes (e.g. manometer
|
|
// 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_t *measuredOutput, int numDataPoints, double_t modelBias) :
|
|
AbstractDelayLineFunction(input, measuredOutput, numDataPoints, modelBias) {
|
|
}
|
|
|
|
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);
|
|
params[PARAM_K] = (fabs(params[PARAM_K]) < minParamK) ? minParamK : params[PARAM_K];
|
|
}
|
|
|
|
// Creating a state-space representation using Rosenbrock system matrix
|
|
virtual double_t calcEstimatedValuesAtPoint(int i, const double_t *params) const {
|
|
// only positive values allowed (todo: choose the limits)
|
|
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_t lambda = exp(-1.0 / (pT * inputFunc->getTimeScale()));
|
|
double_t lambda2 = exp(-1.0 / (pT2 * inputFunc->getTimeScale()));
|
|
|
|
// todo: find better initial values?
|
|
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_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;
|
|
x = lambda * x + pK * (1.0 - lambda) * inp;
|
|
}
|
|
|
|
// the output can be biased
|
|
return y + modelBias;
|
|
}
|
|
};
|
|
|
|
// Harriot's relation function (based on the graph)
|
|
// Used to approximate initial parameters for SOPDT model
|
|
// See: findSecondOrderInitialParamsHarriot() and "Harriot P. Process control (1964). McGraw-Hill. USA."
|
|
class HarriotFunction {
|
|
public:
|
|
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_t minX = 2.8 / 719.0 - 0.26;
|
|
static constexpr double_t maxX = 33 / 719.0 * 2.8 + minX;
|
|
|
|
private:
|
|
const AveragingDataBuffer<34> buf = { {
|
|
0.500000000f, 0.589560440f, 0.624725275f, 0.652747253f, 0.675274725f, 0.694505495f, 0.712637363f, 0.729120879f,
|
|
0.743406593f, 0.757142857f, 0.769780220f, 0.781318681f, 0.793956044f, 0.804395604f, 0.814285714f, 0.824725275f,
|
|
0.834065934f, 0.844505495f, 0.853296703f, 0.862637363f, 0.870879121f, 0.880219780f, 0.889010989f, 0.897802198f,
|
|
0.906593407f, 0.915384615f, 0.924175824f, 0.933516484f, 0.942857143f, 0.953296703f, 0.963736264f, 0.975274725f,
|
|
0.986813187f, 1.000000000f
|
|
}, 34, 0 };
|
|
};
|