rusefi
/
autopid
mirror of https://github.com/rusefi/autopid.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

pid coefs auto-solver

This commit is contained in:
Andrei 2019-10-11 21:14:56 +03:00
parent 4246b9ca16
commit 557a2131f1
9 changed files with 295 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
levenberg_marquardt_solver.h
View File

@ -1,4 +1,4 @@
/*
/*
* @file levenberg_marquardt_solver.h
*
* Levenberg-Marquardt Algorithm, and efficient non-linear optimization solver used for regression analysis ("least squares problem").
@ -15,11 +15,16 @@
#include "matrix_helper.h"
#ifndef LM_USE_CACHE
#define LM_USE_CACHE
#endif /* LM_USE_CACHE */
// Used to get all the values for the Levenberg-Marquardt solver.
template <int numParams>
class LMSFunction {
public:
// Get the total number of data points
virtual double getNumPoints() const = 0;
virtual int getNumPoints() const = 0;
virtual void justifyParams(double *params) const = 0;
@ -29,9 +34,24 @@ public:
/// Returns the residual (error delta) of the function (return (dataPoints[i] - estimatedPoint(i)))
virtual double getResidual(int i, const double *params) const = 0;
// Calculate the sum of the squares of the residuals
virtual double calcMerit(double *params) const {
double res = 0;
for (int i = 0; i < getNumPoints(); i++) {
double r = getResidual(i, params);
res += r * r;
}
return res;
}
/// 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;
// we need to alter parameters around the neighborhood of 'pIndex', so we make a working copy
@ -47,13 +67,31 @@ public:
return (dplusResult - dminusResult) / (delta * 2.0);
}
#ifdef LM_USE_CACHE
void calculateAllPartialDerivatives(const double *params) {
for (int i = 0; i < numParams; i++) {
pderivs[i].resize(getNumPoints());
}
for (int p = 0; p < numParams; p++) {
for (int i = 0; i < getNumPoints(); i++) {
pderivs[p][i] = getPartialDerivativeInternal(i, params, p);
}
}
}
#endif
private:
#ifdef LM_USE_CACHE
std::vector<double> pderivs[numParams];
#endif
};
template<int numParams>
class LevenbergMarquardtSolver {
public:
// ctor
LevenbergMarquardtSolver(LMSFunction<numParams> *func, double parameters[numParams]) {
LevenbergMarquardtSolver(LMSFunction<numParams> *func, double *parameters) {
this->func = func;
this->parameters = parameters;
}
@ -68,11 +106,15 @@ public:
double delta = 0;
do {
double merit = calcMerit(parameters);
double merit = func->calcMerit(parameters);
#ifdef LM_USE_CACHE
func->calculateAllPartialDerivatives(parameters);
#endif
calcGradient();
calcHessian();
bool isSolved = calcNewParameters();
double newMerit = calcMerit(newParameters);
double newMerit = func->calcMerit(newParameters);
if (!isSolved) {
return -iterationCount;
}
@ -91,8 +133,8 @@ 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 m=%g (%g-%g = %g)\r\n", iterationCount, parameters[0], parameters[1], parameters[2], parameters[3], lambda, merit,
newMerit, merit, newMerit - merit);
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);
#endif
iterationCount++;
} while (delta > minDelta && iterationCount < maxIterations);
@ -103,16 +145,6 @@ public:
return parameters;
}
// Calculate the sum of the squares of the residuals
double calcMerit(double *params) {
double res = 0;
for (int i = 0; i < func->getNumPoints(); i++) {
double r = func->getResidual(i, params);
res += r * r;
}
return res;
}
protected:
// Find the parameter increments by solving the Hessian x Gradient equation
bool calcNewParameters() {

109
pid_auto.h
View File

@ -18,6 +18,7 @@
#include "pid_avg_buf.h"
#include "pid_controller.h"
#include "pid_open_loop_models.h"
#include "pid_sim.h"
#include "output_csv.h"
@ -29,10 +30,10 @@ public:
double timeScale;
};
// PID auto-tune method using Chien-Hrones-Reswick rules and SOPDT model
class PidAutoTuneChrSopdt {
// PID auto-tune method using different tuning rules and FOPDT/SOPDT models
class PidAutoTune {
public:
PidAutoTuneChrSopdt() {
PidAutoTune() {
measuredData.init();
}
@ -49,6 +50,12 @@ public:
this->settings = settings;
// without offset we cannot fit the analytic curve
double offset = findOffset();
pid.offset = (float)offset;
// todo: where do we get them?
pid.minValue = 0;
pid.maxValue = 100;
#ifdef PID_DEBUG
printf("* offset=%g avgMin=%g avgMax=%g\r\n", offset, avgMeasuredMin, avgMeasuredMax);
#endif
@ -96,10 +103,10 @@ public:
const int numParams1stOrder = 3;
outputFunc<numParams1stOrder>("pid_func0.csv", func, stepFunc, params);
LevenbergMarquardtSolver<numParams1stOrder> solver(&func, params);
merit0 = solver.calcMerit(params);
merit0 = func.calcMerit(params);
iterationCount = solver.solve(minParamT);
outputFunc<numParams1stOrder>("pid_func.csv", func, stepFunc, params);
merit = solver.calcMerit(params);
merit = func.calcMerit(params);
}
else {
// 2nd order or approximated 2nd->1st order
@ -108,10 +115,10 @@ public:
const int numParams2ndOrder = 4;
outputFunc<numParams2ndOrder>("pid_func0.csv", func, stepFunc, params);
LevenbergMarquardtSolver<numParams2ndOrder> solver(&func, params);
merit0 = solver.calcMerit(params);
merit0 = func.calcMerit(params);
iterationCount = solver.solve(minParamT);
outputFunc<numParams2ndOrder>("pid_func.csv", func, stepFunc, params);
merit = solver.calcMerit(params);
merit = func.calcMerit(params);
}
#ifdef PID_DEBUG
@ -126,7 +133,8 @@ public:
ModelOpenLoopPlant *model = nullptr;
switch (method) {
case PID_TUNE_CHR1: {
case PID_TUNE_CHR1:
case PID_TUNE_AUTO1: {
static ModelChienHronesReswickFirstOrder chr(params);
model = &chr;
break;
@ -143,7 +151,8 @@ public:
model = &chr;
break;
}
case PID_TUNE_CHR2: {
case PID_TUNE_CHR2:
case PID_TUNE_AUTO2: {
static ModelChienHronesReswickSecondOrder chr(params);
model = &chr;
break;
@ -165,10 +174,19 @@ public:
pid.iFactor = model->getKi();
pid.dFactor = model->getKd();
pid.offset = (float)offset;
// todo: where do we get them?
pid.minValue = 0;
pid.maxValue = 100;
pid0 = pid;
// for "automatic" methods, we try to make coefs even better!
if (method == PID_TUNE_AUTO1 || method == PID_TUNE_AUTO2) {
#ifdef PID_DEBUG
printf("* Solving for better coefs:\r\n");
#endif
ModelAutoSolver autoSolver(model);
solveModel(method, autoSolver);
pid.pFactor = autoSolver.getKp();
pid.iFactor = autoSolver.getKi();
pid.dFactor = autoSolver.getKd();
}
return true;
}
@ -177,6 +195,7 @@ public:
switch (method) {
// 1st order
case PID_TUNE_CHR1:
case PID_TUNE_AUTO1:
return 1;
// 2nd order
default:
@ -184,45 +203,6 @@ public:
}
}
template <int numPoints>
static PidAccuracyMetric simulatePid(int order, double target1, double target2, double dTime, const pid_s & pidParams, const double *params) {
StoredDataInputFunction<numPoints> plantInput(1.0 / dTime);
FirstOrderPlusDelayLineFunction plant1(&plantInput, nullptr, 0, minParamT0);
SecondOrderPlusDelayLineOverdampedFunction plant2(&plantInput, nullptr, 0, minParamT0);
LMSFunction<4> *plant;
if (order == 1)
plant = (LMSFunction<4> *)&plant1;
else
plant = &plant2;
PidParallelController pid(pidParams);
//PidDerivativeFilterController pid(pidParams, 10);
double target = target1;
PidAccuracyMetric metric;
// guess a previous state to minimize the system "shock"
plantInput.addDataPoint((float)(target / params[PARAM_K]) - pidParams.offset);
// simulate over time
for (int i = 1; i < numPoints; i++) {
// make a step in the middle
if (i > numPoints / 2)
target = target2;
// "measure" the current value of the plant
double pidInput = plant->getEstimatedValueAtPoint(i, params);
// wait for the controller reaction
double pidOutput = pid.getOutput(target, pidInput, dTime);
// apply the reaction to the plant's pidInput
plantInput.addDataPoint((float)pidOutput);
metric.addPoint((double)i / (double)numPoints, pidInput, target);
#ifdef PID_DEBUG
output_csv((order == 1) ? "pid_test.csv" : "pid_test2.csv", (double)i, pidInput, pidOutput, target);
#endif
}
return metric;
}
double const *getParams() const {
return params;
}
@ -231,6 +211,10 @@ public:
return pid;
}
pid_s const &getPid0() const {
return pid0;
}
double getAvgMeasuredMin() const {
return avgMeasuredMin;
}
@ -367,6 +351,26 @@ public:
return (double)(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(PidTuneMethod method, ModelAutoSolver & model) {
// todo: is it correct?
double dTime = 1.0 / settings.timeScale;
const int numSimPoints = 1024;
PidSimulatorFactory<numSimPoints> simFactory(getMethodOrder(method), getAvgMeasuredMin(), getAvgMeasuredMax(), dTime, pid);
PidCoefsFinderFunction<numSimPoints> func(&simFactory, params);
// now hopefully we'll find even better coefs!
LevenbergMarquardtSolver<numParamsForPid> solver((LMSFunction<numParamsForPid> *)&func, model.getParams());
int iterationCount = solver.solve();
#ifdef PID_DEBUG
if (iterationCount > 0)
printf("* The solver finished in %d iterations!\r\n", iterationCount);
else
printf("* The solver aborted after %d iterations!\r\n", -iterationCount);
#endif
return true;
}
template <int numParams>
void outputFunc(const char *fname, const AbstractDelayLineFunction<numParams> & func, const StepFunction & stepFunc, double *params) {
@ -384,6 +388,7 @@ protected:
PidAutoTuneSettings settings;
double params[4] = { 0 };
pid_s pid;
pid_s pid0; // not-optimized
int iterationCount = 0;
double avgMeasuredMin, avgMeasuredMax;
};

13
pid_controller.h
View File

@ -94,6 +94,13 @@ protected:
// Calculate ITAE/ISE and Overshoot
class PidAccuracyMetric {
public:
void reset() {
itae = 0;
ise = 0;
maxOvershoot = 0;
lastValue = 0;
}
void addPoint(double i, double value, double target) {
double e = target - value;
itae += i * fabs(e);
@ -101,6 +108,7 @@ public:
double overshoot = (value - target) / target;
if (overshoot > 0 && overshoot > maxOvershoot)
maxOvershoot = overshoot;
lastValue = value;
}
double getItae() const {
@ -115,8 +123,13 @@ public:
return maxOvershoot;
}
double getLastValue() const {
return lastValue;
}
private:
double itae = 0; // Integral time-weighted absolute error
double ise = 0; // Integral square error
double maxOvershoot = 0;
double lastValue = 0;
};

33
pid_from_msl.cpp
View File

@ -137,7 +137,7 @@ int main(int argc, char **argv) {
printf("Measuring Settings: minValue=%g maxValue=%g stepPoint=%g maxPoint=%g numPoints=%d timeScale=%g\r\n",
data.settings.minValue, data.settings.maxValue, data.settings.stepPoint, data.settings.maxPoint, data.data.size(), data.settings.timeScale);
PidAutoTuneChrSopdt chr1, chr2;
PidAutoTune chr1, chr2;
for (size_t i = 0; i < data.data.size(); i++) {
chr1.addData(data.data[i]);
@ -145,28 +145,35 @@ int main(int argc, char **argv) {
}
// todo: more flexible method chooser
PidTuneMethod method = PID_TUNE_CHR1;
printf("\r\nTrying method CHR1:\r\n");
PidTuneMethod method = PID_TUNE_AUTO1;
printf("\r\nTrying method Auto1:\r\n");
chr1.findPid(method, data.settings, nullptr);
method = PID_TUNE_CHR2;
printf("\r\nTrying method CHR2:\r\n");
method = PID_TUNE_AUTO2;
printf("\r\nTrying method Auto2:\r\n");
chr2.findPid(method, data.settings, nullptr);
printf("Done!\r\n");
PidAutoTuneChrSopdt *chr[2] = { &chr1, &chr2 };
// todo: is it correct?
double dTime = 1.0 / data.settings.timeScale;
const int numSimPoints = 1024;
PidAutoTune *chr[2] = { &chr1, &chr2 };
for (int k = 0; k < 2; k++) {
const double *p = chr[k]->getParams();
printf("Model-%d Params: K=%g T1=%g T2=%g L=%g\r\n", (k + 1), p[PARAM_K], p[PARAM_T], p[PARAM_T2], p[PARAM_L]);
const pid_s & pid = chr[k]->getPid();
printf(" PID: P=%f I=%f D=%f offset=%f\r\n", pid.pFactor, pid.iFactor, pid.dFactor, pid.offset);
const pid_s pid0 = chr[k]->getPid0();
const pid_s pid = chr[k]->getPid();
printf(" PID0: P=%.8f I=%.8f D=%.8f offset=%.8f\r\n", pid0.pFactor, pid0.iFactor, pid0.dFactor, pid0.offset);
// todo: is it correct?
double dTime = 1.0 / data.settings.timeScale;
PidAccuracyMetric metric = PidAutoTuneChrSopdt::simulatePid<2048>(chr[k]->getMethodOrder(method),
chr[k]->getAvgMeasuredMin(), chr[k]->getAvgMeasuredMax(), dTime, pid, p);
printf(" Metric result: ITAE=%g ISE=%g Overshoot=%g%%\r\n", metric.getItae(), metric.getIse(), metric.getMaxOvershoot() * 100.0);
PidSimulator<numSimPoints> sim(chr[k]->getMethodOrder(method), chr[k]->getAvgMeasuredMin(), chr[k]->getAvgMeasuredMax(), dTime, true);
PidAccuracyMetric metric0 = sim.simulate(numSimPoints, pid0, p);
printf(" Metric0 result: ITAE=%g ISE=%g Overshoot=%g%%\r\n", metric0.getItae(), metric0.getIse(), metric0.getMaxOvershoot() * 100.0);
printf(" PID: P=%.8f I=%.8f D=%.8f offset=%.8f\r\n", pid.pFactor, pid.iFactor, pid.dFactor, pid.offset);
PidAccuracyMetric metric = sim.simulate(numSimPoints, pid, p);
printf(" Metric result: ITAE=%g ISE=%g Overshoot=%g%%\r\n", metric.getItae(), metric.getIse(), metric.getMaxOvershoot() * 100.0);
}
return 0;

14
pid_functions.h
View File

@ -28,6 +28,14 @@ enum {
};
// 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 minParamKL = 0.00001;
@ -90,6 +98,10 @@ template <int numPoints>
class StoredDataInputFunction : public InputFunction {
public:
StoredDataInputFunction(double timeScale_) : InputFunction(timeScale_) {
reset();
}
void reset() {
inputData.init();
}
@ -125,7 +137,7 @@ public:
virtual double getEstimatedValueAtPoint(int i, const double *params) const = 0;
// Get the total number of data points
virtual double getNumPoints() const {
virtual int getNumPoints() const {
return numPoints;
}

BIN
pid_open_loop_models.h
View File

Binary file not shown.

138
pid_sim.h Normal file
View File

@ -0,0 +1,138 @@
/*
* @file pid_sim.h
*
* PID Controller simulator.
* Used
*
* @date Oct 10, 2019
* @author andreika, (c) 2019
*/
#pragma once
#include "global.h"
template <int maxPoints>
class PidSimulator {
public:
PidSimulator(int order, double target1_, double target2_, double dTime_, bool showOutput_) :
target1(target1_), target2(target2_), dTime(dTime_), methodOrder(order), showOutput(showOutput_),
plantInput(1.0 / dTime_), plant1(&plantInput, nullptr, 0, minParamT0), plant2(&plantInput, nullptr, 0, minParamT0) {
}
PidAccuracyMetric simulate(int numPoints, const pid_s & pidParams, const double *params) {
LMSFunction<4> *plant;
if (methodOrder == 1)
plant = (LMSFunction<4> *)&plant1;
else
plant = &plant2;
PidParallelController pid(pidParams);
//PidDerivativeFilterController pid(pidParams, 10);
plantInput.reset();
// guess a previous state to minimize the system "shock"
plantInput.addDataPoint((float)(getTarget(0) / params[PARAM_K]) - pidParams.offset);
// "calm down" the PID controller to avoid huge spikes at the beginning
pid.getOutput(getTarget(0), plant->getEstimatedValueAtPoint(0, params), dTime);
metric.reset();
// simulate over time
for (int i = 0; i < numPoints; i++) {
// make a step in the middle
double target = getTarget(i);
// "measure" the current value of the plant
double pidInput = plant->getEstimatedValueAtPoint(i, params);
// wait for the controller reaction
double pidOutput = pid.getOutput(target, pidInput, dTime);
// apply the reaction to the plant's pidInput
plantInput.addDataPoint((float)pidOutput);
metric.addPoint((double)i / (double)numPoints, pidInput, target);
#ifdef PID_DEBUG
if (showOutput)
output_csv((methodOrder == 1) ? "pid_test.csv" : "pid_test2.csv", (double)i, pidInput, pidOutput, target);
#endif
}
return metric;
}
double getTarget(int i) const {
return (i > maxPoints / 2) ? target2 : target1;
}
protected:
int methodOrder;
bool showOutput;
StoredDataInputFunction<maxPoints> plantInput;
FirstOrderPlusDelayLineFunction plant1;
SecondOrderPlusDelayLineOverdampedFunction plant2;
PidAccuracyMetric metric;
double target1, target2, dTime;
};
// A working copy of simulator and PID controller params. Used by PidAutoTune::solveModel().
// We don't want to create a simulator instance each time we call getEstimatedValueAtPoint() from PidCoefsFinderFunction.
// So we create an instance of this class and store temporary allocated data.
template <int numPoints>
class PidSimulatorFactory {
public:
PidSimulatorFactory(int methodOrder_, double target1_, double target2_, double dTime, const pid_s & pid_) :
sim(methodOrder_, target1_, target2_, dTime, false), pid(pid_) {
}
public:
PidSimulator<numPoints> sim;
pid_s pid;
};
// A special function used for PID controller analytic simulation
// This method doesn't use any "magic" formulas, instead it uses Levenberg-Marquardt solver as a "brute-force".
template <int numPoints>
class PidCoefsFinderFunction : public LMSFunction<numParamsForPid> {
public:
PidCoefsFinderFunction(PidSimulatorFactory<numPoints> *simFactory_, const double *modelParams_) :
simFactory(simFactory_), modelParams(modelParams_) {
}
virtual void justifyParams(double *params) const {
// todo: limit PID coefs somehow?
}
// Get the total number of data points
virtual int getNumPoints() const {
return numPoints;
}
// Calculate the sum of the squares of the residuals
virtual double calcMerit(double *params) const {
return simulate(numPoints - 1, params).getItae();
}
virtual double getResidual(int i, const double *params) const {
return simFactory->sim.getTarget(i) - getEstimatedValueAtPoint(i, params);
}
virtual double getEstimatedValueAtPoint(int i, const double *params) const {
return simulate(i, params).getLastValue();
}
PidAccuracyMetric simulate(int i, const double *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];
// simulate PID controller response
return simFactory->sim.simulate(i + 1, simFactory->pid, modelParams);
}
protected:
const double *modelParams;
PidSimulatorFactory<numPoints> *simFactory;
};

6
test_chs_pid.cpp
View File

@ -73,7 +73,7 @@ void printSOPDT() {
}
TEST(pidAutoTune, chsSopdtPid) {
PidAutoTuneChrSopdt chr;
PidAutoTune chr;
double params0[4];
// todo: find better initial values?
@ -127,8 +127,10 @@ TEST(pidAutoTune, testPidCoefs) {
// todo: is it correct?
double dTime = 1;
const int numSimPoints = 2048;
PidSimulator<numSimPoints> sim(2, 13.0, 14.0, dTime, true);
for (int idx = 0; idx <= 4; idx++) {
PidAccuracyMetric metric = PidAutoTuneChrSopdt::simulatePid<2048>(2, 13.0, 14.0, dTime, pidParams[idx], params);
PidAccuracyMetric metric = sim.simulate(numSimPoints, pidParams[idx], params);
#ifdef PID_DEBUG
printf("Metric result: ITAE=%g ISE=%g Overshoot=%g%%\r\n", metric.getItae(), metric.getIse(), metric.getMaxOvershoot() * 100.0);
#endif

2
test_levenberg_marquardt_solver.cpp
View File

@ -21,7 +21,7 @@ public:
}
// Get the total number of data points
virtual double getNumPoints() const {
virtual int getNumPoints() const {
return numPoints;
}