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

initial

This commit is contained in:
Andrei 2019-10-05 17:54:26 +03:00
commit 1fea0fe1f7
10 changed files with 1107 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
global.h Normal file
View File

@ -0,0 +1,38 @@
// Dummy file replacement
#pragma once
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "gtest/gtest.h"
#include "gmock/gmock.h"
#ifndef CONTROLLERS_GENERATED_ENGINE_CONFIGURATION_GENERATED_STRUCTURES_H
struct pid_s {
float pFactor;
float iFactor;
float dFactor;
float offset;
int16_t minValue;
int16_t maxValue;
};
typedef struct pid_s pid_s;
#endif
#if 1 // remove!
#define LMS_DEBUG
#define PID_DEBUG
#endif
#define EPS1D 0.1
#define EPS2D 0.01
#define EPS3D 0.001
#define EPS4D 0.0001
#define EPS5D 0.00001

185
levenberg_marquardt_solver.h Normal file
View File

@ -0,0 +1,185 @@
/*
* @file levenberg_marquardt_solver.h
*
* Levenberg-Marquardt Algorithm, and efficient non-linear optimization solver used for regression analysis ("least squares problem").
* It basically combines Gauss-Newton method and gradient descent, but using an approximation for computing a Hessian matrix!
* The code is based on "W.Press, S.Teukolsky, W.Vetterling, B.Flannery. Numerical Recipes in Fortran 77, 1992."
*
* @date Sep 27, 2019
* @author andreika, (c) 2019
*/
#pragma once
#include "global.h"
#include "matrix_helper.h"
template <int numParams>
class LMSFunction {
public:
// Get the total number of data points
virtual double getNumPoints() = 0;
virtual void justifyParams(double *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;
/// Returns the residual (error delta) of the function (return (dataPoints[i] - estimatedPoint(i)))
virtual double getResidual(int i, const double *params) const = 0;
/// 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 {
// 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
double tmpParams[numParams];
for (int k = 0; k < numParams; k++)
tmpParams[k] = params[k];
tmpParams[pIndex] = params[pIndex] + delta;
double dplusResult = getEstimatedValueAtPoint(i, tmpParams);
tmpParams[pIndex] = params[pIndex] - delta;
double dminusResult = getEstimatedValueAtPoint(i, tmpParams);
return (dplusResult - dminusResult) / (delta * 2.0);
}
};
template<int numParams>
class LevenbergMarquardtSolver {
public:
// ctor
LevenbergMarquardtSolver(LMSFunction<numParams> *func, double parameters[numParams]) {
this->func = func;
this->parameters = parameters;
}
// 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) {
this->lambda = lambda_;
iterationCount = 0;
double delta = 0;
do {
double merit = calcMerit(parameters);
calcGradient();
calcHessian();
bool isSolved = calcNewParameters();
double newMerit = calcMerit(newParameters);
if (!isSolved) {
return -1;
}
// if we don't like the new parameters
if (newMerit >= merit) {
// decrease the step
lambda *= lambdaMultiplier;
}
// if we're good, accept them
else {
// update via copy
memcpy(parameters, newParameters, sizeof(newParameters));
// let's increase the step even more
lambda /= lambdaMultiplier;
}
// 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);
#endif
iterationCount++;
} while (delta > minDelta && iterationCount < maxIterations);
return iterationCount;
}
double *getParameters() const {
return parameters;
}
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);
if (!ret)
return false;
for (int row = 0; row < numParams; row++) {
double increment = 0;
for (int col = 0; col < numParams; col++) {
increment += hinv[row][col] * gradient[col];
}
newParameters[row] = parameters[row] + increment;
}
func->justifyParams(newParameters);
return true;
}
// 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;
}
/// Calculate the Hessian matrix (2nd derivative) approximation
void calcHessian() {
for (int row = 0; row < numParams; row++) {
for (int col = 0; col < numParams; col++) {
double res = 0;
for (int i = 0; i < func->getNumPoints(); i++) {
res += func->getPartialDerivative(i, parameters, row) * func->getPartialDerivative(i, parameters, col);
}
hessian[row][col] = (row == col) ? res * (lambda + 1.0) : res;
}
}
}
// Calculate the 1st derivatives of the residual func
void calcGradient() {
for (int row = 0; row < numParams; row++) {
double res = 0;
for (int i = 0; i < func->getNumPoints(); i++) {
res += func->getResidual(i, parameters) * func->getPartialDerivative(i, parameters, row);
}
gradient[row] = res;
}
}
private:
// optimization function
LMSFunction<numParams> *func;
// Current (accepted) parameters vector
double *parameters; // [numParams]
// Incremented (next step) parameters vector
double newParameters[numParams];
// Hessian matrix
double hessian[numParams][numParams];
// Gradients vector
double gradient[numParams];
// some magic number
const double lambdaMultiplier = 10.0;
// coeff used to adapt the descent step (and speed)
double lambda;
// Total number of iterations
int iterationCount;
};

95
matrix_helper.h Normal file
View File

@ -0,0 +1,95 @@
/*
* @file matrix_helper.h
*
* @date Sep 27, 2019
* @author andreika, (c) 2019
*/
#pragma once
#include "global.h"
template<typename FLOAT, int N>
class MatrixHelper {
public:
// Gauss-Jordan elimination method
static bool inverseMatrix(FLOAT dst[N][N], FLOAT src[N][N]) {
// a working copy of src (needs modification)
FLOAT tmp[N][N];
for (int i = 0; i < N; i++) {
for (int j = 0; j < N; j++) {
tmp[i][j] = src[i][j];
dst[i][j] = (i == j) ? 1.0 : 0; // fill as an identity matrix first
}
}
// determinant
FLOAT det = 1.0;
// for each pass, find the maximum element in the pivot column.
for (int pass = 0; pass < N; pass++) {
int row, maxRow = pass;
// find the largest element in the column
for (row = pass; row < N; row++) {
if (fabs(tmp[row][pass]) > fabs(tmp[maxRow][pass]))
maxRow = row;
}
// interchange the elements of these rows
if (maxRow != pass) {
for (int col = 0; col < N; col++) {
FLOAT temp = dst[pass][col];
dst[pass][col] = dst[maxRow][col];
dst[maxRow][col] = temp;
if (col >= pass) {
temp = tmp[pass][col];
tmp[pass][col] = tmp[maxRow][col];
tmp[maxRow][col] = temp;
}
}
}
// calculate the determinant as the product of the elements
FLOAT coef = tmp[pass][pass];
if (coef == 0.0)
return false;
det *= coef;
// normalize
for (int col = 0; col < N; col++) {
dst[pass][col] /= coef;
if (col >= pass)
tmp[pass][col] /= coef;
}
// add a multiple of the pivot row to each row
for (row = 0; row < N; row++) {
if (row != pass) {
coef = tmp[row][pass];
for (int col = 0; col < N; col++) {
dst[row][col] -= coef * dst[pass][col];
tmp[row][col] -= coef * tmp[pass][col];
}
}
}
}
return true;
}
static void multiplyMatrix(FLOAT dst[N][N], FLOAT src1[N][N], FLOAT src2[N][N]) {
for (int i = 0; i < N; i++) {
for (int j = 0; j < N; j++) {
dst[i][j] = 0;
for (int k = 0; k < N; k++) {
dst[i][j] += src1[i][k] * src2[k][j];
}
}
}
}
};

94
pid_avg_buf.h Normal file
View File

@ -0,0 +1,94 @@
/*
* @file pid_avg_buf.h
*
* Chien-Hrones-Reswick Algorithm, a PID coefficient finder method using step-response measured data
*
* @date Sep 27, 2019
* @author andreika, (c) 2019
*/
#pragma once
#include "global.h"
// Used to store measured data in the memory-limited buffer.
// The buffer adapts to the data size automatically by averaging stored values.
template<int maxPoints>
class AveragingDataBuffer {
public:
AveragingDataBuffer() {
// zero buffer
memset(buf, 0, sizeof(buf));
num = 0;
}
void addDataPoint(float v) {
int idx;
for (;;) {
idx = num >> scaleShift;
if (idx < maxPoints)
break;
// we're here because the buffer size is too small to hold the new point
int idxHalf = idx / 2;
// shrink the buffer twice using averaging, and clear the rest of the buffer
for (int i = 0; i < maxPoints; i++) {
buf[i] = (i < idxHalf) ? (buf[i * 2] + buf[i * 2 + 1]) * 0.5f : 0;
}
scaleShift++;
}
float numInTheCell = (float)(num - ((num >> scaleShift) << scaleShift));
buf[idx] *= numInTheCell;
buf[idx] += v;
buf[idx] /= (numInTheCell + 1.0f);
num++;
}
int getNumDataPoints() const {
return (num < 1) ? 1 : ((num - 1) >> scaleShift) + 1;
}
float const *getBuf() const {
return buf;
}
// This is a robust method for all rational indices
float getValue(float i) const {
// todo: this works only for scale=1 :(
assert(scaleShift == 0);
// reject empty buffer
if (num < 1)
return 0.0f;
// singular?
if (num == 1)
return buf[0];
int I = (int)i;
// extrapolate to the left?
if (I < 0)
return buf[0];
// extrapolate to the right?
if (I >= num - 1)
return buf[num - 1];
// get 2 closest values and interpolate
float fract = i - I;
return buf[I + 1] * fract + buf[I] * (1.0f - fract);
}
float getAveragedData(int from, int to) const {
float avg = 0.0f;
for (int i = from; i <= to; i++) {
avg += buf[i];
}
avg /= (float)(to - from + 1);
return avg;
}
protected:
float buf[maxPoints];
int num = 0;
int scaleShift = 0;
};

122
pid_controller.h Normal file
View File

@ -0,0 +1,122 @@
/*
* @file pid_controller.h
*
* PID Controller models needed to verify the parameters.
*
* @date Oct 02, 2019
* @author andreika, (c) 2019
*/
#pragma once
#include "global.h"
class PidController {
public:
PidController(const pid_s & p_) : p(p_) {
}
double limitOutput(double v) {
if (v < p.minValue)
v = p.minValue;
if (v > p.maxValue)
v = p.maxValue;
return v;
}
protected:
const pid_s p;
};
class PidParallelController : public PidController {
public:
PidParallelController(const pid_s & p_) : PidController(p_) {
pTerm = iTerm = dTerm = 0.0;
}
double getOutput(double target, double input, double dTime) {
double error = target - input;
pTerm = p.pFactor * error;
iTerm += p.iFactor * dTime * error;
dTerm = p.dFactor / dTime * (error - previousError);
previousError = error;
return limitOutput(pTerm + iTerm + dTerm + p.offset);
}
protected:
double pTerm, iTerm, dTerm;
double previousError = 0;
};
// C(s) = Kp + (Ki / s) + (N * Kd * s / (1 + N / s))
// The Integral term is discretized using backward Euler method
// 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_) {
}
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 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 ku1 = a1 / a0;
double ku2 = a2 / a0;
double ke0 = b0 / a0;
double ke1 = b1 / a0;
double ke2 = b2 / a0;
e2 = e1;
e1 = e0;
u2 = u1;
u1 = u0;
e0 = error;
u0 = -ku1 * u1 - ku2 * u2 + ke0 * e0 + ke1 * e1 + ke2 * e2;
u0 = limitOutput(u0);
return u0;
}
protected:
double e2 = 0, e1 = 0, e0 = 0, u2 = 0, u1 = 0, u0 = 0;
double N = 1;
};
// Calculate ITAE/ISE and Overshoot
class PidAccuracyMetric {
public:
void addPoint(double i, double value, double target) {
double e = target - value;
itae += i * fabs(e);
ise += e * e;
double overshoot = (value - target) / target;
if (overshoot > 0 && overshoot > maxOvershoot)
maxOvershoot = overshoot;
}
double getItae() const {
return itae;
}
double getIse() const {
return ise;
}
double getMaxOvershoot() const {
return maxOvershoot;
}
private:
double itae = 0; // Integral time-weighted absolute error
double ise = 0; // Integral square error
double maxOvershoot = 0;
};

117
pid_from_msl.cpp Normal file
View File

@ -0,0 +1,117 @@
/*
* @file pid_from_msl.cpp
*
* @date Oct 5, 2019
* @author andreika, (c) 2019
*/
#include "global.h"
#include <fstream>
#include <vector>
#include "pid_open_loop_models.h"
#include "pid_controller.h"
class MslData {
public:
bool readMsl(const char *fname, double startTime, double endTime, int inputIdx, int outputIdx) {
std::ifstream fp(fname);
if (!fp)
return false;
curIdx = -1;
stepPoint = -1.0;
std::string str;
for (int i = 0; std::getline(fp, str); i++) {
// data starts at 4th line
if (i < 4)
continue;
parseLine(str, startTime, endTime, inputIdx, outputIdx);
}
maxPoint = curIdx - 1;
assert(data.size() == curIdx);
fp.close();
return true;
}
bool parseLine(const std::string & str, double startTime, double 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());
// the first column is timestamp
if (j == 0) {
if (v < startTime || v > endTime)
return false;
} else if (j == inputIdx) {
// this is an input step, we should find it
if (curIdx < 0) {
minValue = v;
curIdx = 0;
} else if (v != minValue && stepPoint < 0) {
maxValue = v;
stepPoint = curIdx;
}
curIdx++;
} else if (j == outputIdx) {
data.push_back((float)v);
}
}
return true;
}
public:
std::vector<float> data;
double minValue = 0, maxValue = 0, stepPoint = -1.0, maxPoint = 0;
int curIdx = -1;
};
#if 0
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");
return -1;
}
printf("PID_FROM_MSL: 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]))) {
return -2;
}
printf("PID_FROM_MSL: Calculating...\r\n");
PidAutoTuneChrSopdt chr;
double params0[4];
// todo: find better initial values?
params0[PARAM_K] = 0.1;
params0[PARAM_T] = 1;
params0[PARAM_T2] = 1;
params0[PARAM_L] = 1;
for (size_t i = 0; i < data.data.size(); i++) {
chr.addData(data.data[i]);
}
double stepPoint = 178; // todo: adjust for the buffer scale
double maxPoint = 460;
chr.findPid(PID_TUNE_CHR2, data.minValue, data.maxValue, data.stepPoint, data.maxPoint, params0);
printf("Done!\r\n");
const double *p = chr.getParams();
printf("Model 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);
return 0;
}
#endif

193
pid_functions.h Normal file
View File

@ -0,0 +1,193 @@
/*
* @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
};
static const double minParamValue = 0.01;
class InputFunction {
public:
virtual double getValue(double i) const = 0;
};
// Heaviside step function interpolated between 'min' and 'max' values with 'stepPoint' time offset
class StepFunction : public InputFunction
{
public:
StepFunction(double minValue_, double maxValue_, double offset_, double stepPoint_) :
minValue(minValue_), maxValue(maxValue_), offset(offset_), stepPoint(stepPoint_) {
}
virtual double getValue(double i) const {
#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)i;
double fract = i - 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;
// interpolate
return offset + vFar * fract + vNear * (1.0f - fract);
#else
return offset + ((i < stepPoint) ? minValue : maxValue);
#endif
}
private:
double minValue, maxValue;
// stepPoint is float because we have AveragingDataBuffer, and the time axis may be scaled
double stepPoint;
// needed to use PARAM_K coefficient properly; also offset is used by PID
double offset;
};
template <int numPoints>
class StoredDataInputFunction : public InputFunction {
public:
void addDataPoint(float v) {
// todo: support data scaling
assert(inputData.getNumDataPoints() <= numPoints);
inputData.addDataPoint(v);
}
virtual double getValue(double i) const {
return inputData.getValue((float)i);
}
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 *measuredOutput, int numDataPoints) {
dataPoints = measuredOutput;
inputFunc = input;
numPoints = numDataPoints;
}
virtual double getResidual(int i, const double *params) const {
return dataPoints[i] - getEstimatedValueAtPoint(i, params);
}
virtual double getEstimatedValueAtPoint(int i, const double *params) const = 0;
// Get the total number of data points
virtual double getNumPoints() {
return numPoints;
}
protected:
const InputFunction *inputFunc;
const float *dataPoints;
int numPoints;
};
// FODPT indirect transfer function used for step response analytic simulation.
// Used mostly as an approximate model for chemical processes?
// 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) :
AbstractDelayLineFunction(input, measuredOutput, numDataPoints) {
}
virtual void justifyParams(double *params) const {
params[PARAM_L] = fmax(params[PARAM_L], minParamValue);
params[PARAM_T] = fmax(params[PARAM_T], minParamValue);
}
// Creating a state-space representation using Rosenbrock system matrix
virtual double getEstimatedValueAtPoint(int i, const double *params) const {
// only positive values allowed (todo: choose the limits)
double pL = fmax(params[PARAM_L], minParamValue);
double pT = fmax(params[PARAM_T], minParamValue);
// state-space params
double lambda = exp(-1.0 / pT);
// todo: find better initial value?
double y = inputFunc->getValue(0) * params[PARAM_K];
// 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);
// indirect model response in Controllable Canonical Form (1st order CCF)
y = lambda * y + params[PARAM_K] * (1.0 - lambda) * inp;
}
return y;
}
};
// "Overdamped" SODPT indirect transfer function used for step response analytic simulation (xi > 1)
// 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) :
AbstractDelayLineFunction(input, measuredOutput, numDataPoints) {
}
virtual void justifyParams(double *params) const {
params[PARAM_L] = fmax(params[PARAM_L], minParamValue);
params[PARAM_T] = fmax(params[PARAM_T], minParamValue);
params[PARAM_T2] = fmax(params[PARAM_T2], minParamValue);
}
// Creating a state-space representation using Rosenbrock system matrix
virtual double getEstimatedValueAtPoint(int i, const double *params) const {
// only positive values allowed (todo: choose the limits)
double pL = fmax(params[PARAM_L], minParamValue);
double pT = fmax(params[PARAM_T], minParamValue);
double pT2 = fmax(params[PARAM_T2], minParamValue);
// state-space params
double lambda = exp(-1.0 / pT);
double lambda2 = exp(-1.0 / pT2);
// todo: find better initial values?
double x = inputFunc->getValue(0) * params[PARAM_K];
double y = inputFunc->getValue(0) * params[PARAM_K];
// 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);
// indirect model response in Controllable Canonical Form (2nd order CCF)
y = lambda2 * y + (1.0 - lambda2) * x;
x = lambda * x + params[PARAM_K] * (1.0 - lambda) * inp;
}
return y;
}
};

BIN
pid_open_loop_models.h Normal file
View File

Binary file not shown.

156
test_chs_pid.cpp Normal file
View File

@ -0,0 +1,156 @@
/*
* @file test_chs_pid.cpp
*
* @date Sep 27, 2019
* @author andreika, (c) 2019
*/
#include "global.h"
#include "pid_open_loop_models.h"
#include "pid_controller.h"
extern void testGaussianFunction();
extern void testMatrixInverse();
TEST(pidAutoTune, testMeasuredDataBuffer) {
const int numPoints = 2;
AveragingDataBuffer<numPoints> buf;
for (int i = 0; i < 16; i++) {
buf.addDataPoint((float)(i + 1));
float v = (float)(1 << (int)(log(i) / log(2.0)));
float v1 = 0.5f * (1.0f + v);
float v2 = (i < 1) ? 0 : v1 * 2.0f + (i - v) * 0.5f;
ASSERT_EQ(v1, buf.getBuf()[0]);
ASSERT_EQ(v2, buf.getBuf()[1]);
ASSERT_EQ((i == 0) ? 1 : 2, buf.getNumDataPoints());
}
}
TEST(pidAutoTune, testFOPDT) {
StepFunction stepFunc(0, 100, 0, 10);
FirstOrderPlusDelayLineFunction func(&stepFunc, nullptr, 0);
double params[3];
params[PARAM_K] = 2.0;
params[PARAM_T] = 3.0;
params[PARAM_L] = 4.0;
double v = func.getEstimatedValueAtPoint(24, params);
ASSERT_DOUBLE_EQ(25.200031003972988, v);
}
TEST(pidAutoTune, testSOPDTOverdamped) {
StepFunction stepFunc(0, 100, 0, 10);
SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, nullptr, 0);
double params[4];
params[PARAM_K] = 2.0;
params[PARAM_T] = 3.0;
params[PARAM_T2] = 0.3;
params[PARAM_L] = 4.0;
double v = func.getEstimatedValueAtPoint(24, params);
ASSERT_DOUBLE_EQ(25.200031003972988, v);
}
static const float outputData[] = { 13.29, 13.29, 13.33, 13.33, 13.33, 13.33, 13.33, 13.22, 13.22, 13.22, 13.22, 13.3, 13.3, 13.3, 13.3, 13.3, 13.34, 13.34, 13.34, 13.34, 13.34, 13.2, 13.2, 13.2, 13.2, 13.2, 13.29, 13.29, 13.29, 13.29, 13.29, 13.32, 13.32, 13.32, 13.32, 13.32, 13.19, 13.19, 13.19, 13.19, 13.19, 13.28, 13.28, 13.28, 13.28, 13.28, 13.32, 13.32, 13.32, 13.32, 13.32, 13.18, 13.18, 13.18, 13.18, 13.18, 13.27, 13.27, 13.27, 13.27, 13.27, 13.32, 13.32, 13.32, 13.32, 13.17, 13.17, 13.17, 13.17, 13.17, 13.27, 13.27, 13.27, 13.27, 13.27, 13.32, 13.32, 13.32, 13.32, 13.32, 13.16, 13.16, 13.16, 13.16, 13.16, 13.25, 13.25, 13.25, 13.25, 13.25, 13.3, 13.3, 13.3, 13.3, 13.3, 13.14, 13.14, 13.14, 13.14, 13.23, 13.23, 13.23, 13.23, 13.23, 13.28, 13.28, 13.28, 13.28, 13.28, 13.16, 13.16, 13.16, 13.16, 13.16, 13.25, 13.25, 13.25, 13.25, 13.25, 13.28, 13.28, 13.28, 13.28, 13.28, 13.2, 13.2, 13.2, 13.2, 13.2, 13.27, 13.27, 13.27, 13.27, 13.27, 13.29, 13.29, 13.29, 13.29, 13.24, 13.24, 13.24, 13.24, 13.24, 13.3, 13.3, 13.3, 13.3, 13.3, 13.3, 13.3, 13.3, 13.3, 13.29, 13.29, 13.29, 13.29, 13.29, 13.33, 13.33, 13.33, 13.33, 13.33, 13.3, 13.3, 13.3, 13.3, 13.3, 13.33, 13.33, 13.33, 13.33, 13.33, 13.36, 13.36, 13.36, 13.36, 13.36, 13.31, 13.37, 13.37, 13.37, 13.44, 13.44, 13.44, 13.44, 13.44, 13.44, 13.44, 13.44, 13.44, 13.44, 13.54, 13.54, 13.54, 13.54, 13.54, 13.62, 13.62, 13.62, 13.62, 13.62, 13.56, 13.56, 13.56, 13.56, 13.56, 13.68, 13.68, 13.68, 13.68, 13.76, 13.76, 13.76, 13.76, 13.76, 13.65, 13.65, 13.65, 13.65, 13.65, 13.78, 13.78, 13.78, 13.78, 13.78, 13.84, 13.84, 13.84, 13.84, 13.84, 13.95, 13.95, 13.95, 13.95, 13.95, 14.04, 14.04, 14.04, 14.04, 14.04, 13.91, 13.91, 13.91, 13.91, 13.91, 14.06, 14.06, 14.06, 14.06, 14.06, 14.11, 14.11, 14.11, 14.11, 14.23, 14.23, 14.23, 14.23, 14.23, 14.33, 14.33, 14.33, 14.33, 14.33, 14.37, 14.37, 14.37, 14.37, 14.37, 14.48, 14.48, 14.48, 14.48, 14.48, 14.36, 14.36, 14.36, 14.36, 14.36, 14.53, 14.53, 14.53, 14.53, 14.53, 14.59, 14.59, 14.59, 14.59, 14.59, 14.74, 14.74, 14.74, 14.74, 14.74, 14.85, 14.85, 14.85, 14.85, 14.85, 14.94, 14.94, 14.94, 14.94, 15.05, 15.05, 15.05, 15.05, 15.05, 14.91, 14.91, 14.91, 14.91, 14.91, 15.06, 15.06, 15.06, 15.06, 15.06, 15.05, 15.05, 15.05, 15.05, 15.05, 15.18, 15.18, 15.18, 15.18, 15.18, 15.23, 15.23, 15.23, 15.23, 15.23, 15.34, 15.34, 15.34, 15.34, 15.34, 15.4, 15.4, 15.4, 15.4, 15.4, 15.42, 15.42, 15.42, 15.42, 15.49, 15.49, 15.49, 15.49, 15.49, 15.32, 15.32, 15.32, 15.32, 15.32, 15.45, 15.45, 15.45, 15.45, 15.45, 15.43, 15.43, 15.43, 15.43, 15.43, 15.53, 15.53, 15.53, 15.53, 15.53, 15.58, 15.58, 15.58, 15.58, 15.58, 15.63, 15.63, 15.63, 15.63, 15.63, 15.67, 15.67, 15.67, 15.67, 15.67, 15.5, 15.5, 15.5, 15.5, 15.5, 15.61, 15.61, 15.61, 15.61, 15.61, 15.57, 15.57, 15.57, 15.57, 15.57, 15.66, 15.66, 15.66, 15.66, 15.66, 15.7, 15.7, 15.7, 15.7, 15.74, 15.74, 15.74, 15.74, 15.74, 15.77, 15.77, 15.77, 15.77, 15.77, 15.63, 15.63, 15.63, 15.63, 15.63, 15.7, 15.7, 15.7, 15.7, 15.59, 15.59, 15.59, 15.59, 15.68, 15.68, 15.68, 15.68, 15.68, 15.68, 15.68, 15.68, 15.68, 15.68, 15.75, 15.75, 15.75, 15.75, 15.75, 15.77, 15.77, 15.77, 15.77, 15.77, 15.8, 15.8, 15.8, 15.8, 15.8, 15.83, 15.83, 15.83, 15.83, 15.83, 15.71, 15.71, 15.71, 15.71, 15.77, 15.77, 15.77, 15.77, 15.77, 15.57, 15.57, 15.57, 15.57, 15.57, 15.68, 15.68, 15.68, 15.68, 15.68, 15.61, 15.61, 15.61, 15.61, 15.61, 15.71, 15.71, 15.71, 15.71, 15.71, 15.7, 15.7, 15.7, 15.7, 15.7, 15.77, 15.77, 15.77, 15.77, 15.79, 15.79, 15.79, 15.79, 15.79, 15.82, 15.82, 15.82, 15.82, 15.82, 15.84, 15.84, 15.84, 15.78, 15.78, 15.78, 15.78, 15.83, 15.83, 15.83, 15.83, 15.83, 15.62, 15.62, 15.62, 15.62, 15.73, 15.73, 15.73, 15.73, 15.73, 15.66, 15.66, 15.66, 15.66, 15.66, 15.75, 15.75, 15.75, 15.75, 15.75, 15.75, 15.75, 15.75, 15.75, 15.75, 15.81, 15.81, 15.81, 15.81, 15.81, 15.84, 15.84, 15.84, 15.84, 15.84, 15.86, 15.86, 15.86, 15.86, 15.86, 15.88, 15.88, 15.88, 15.88, 15.88, 15.74, 15.74, 15.74, 15.74, 15.74, 15.81, 15.81, 15.81, 15.81, 15.81, 15.69, 15.69, 15.69, 15.69, 15.69, 15.77, 15.77, 15.77, 15.77, 15.77, 15.79, 15.79, 15.79, 15.79, 15.86, 15.86, 15.86, 15.86, 15.86, 15.88, 15.88, 15.88, 15.88, 15.88, 15.82, 15.82, 15.82, 15.82, 15.82, 15.88, 15.88, 15.88, 15.88, 15.88, 15.69, 15.69, 15.69, 15.69, 15.69, 15.78, 15.78, 15.78, 15.78, 15.78, 15.79, 15.79, 15.79, 15.79, 15.79, 15.88, 15.88, 15.88, 15.88, 15.88, 15.91, 15.91, 15.91, 15.91, 15.91, 15.88, 15.88, 15.88, 15.88, 15.88, 15.91, 15.91, 15.91, 15.91, 15.91, 15.7, 15.7, 15.7, 15.7, 15.7, 15.79, 15.79, 15.79, 15.79, 15.79, 15.74, 15.74, 15.74, 15.74, 15.74, 15.81, 15.81, 15.81, 15.81, 15.81, 15.81, 15.81, 15.81, 15.81, 15.81, 15.87, 15.87 };
const int numData = sizeof(outputData) / sizeof(outputData[0]);
void printSOPDT() {
StepFunction stepFunc(20.0, 30.0, 32.823277, 178);
SecondOrderPlusDelayLineOverdampedFunction func(&stepFunc, nullptr, 0);
double 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);
printf("%d,%f,%f,%f\r\n", i, outputData[i], v, stepFunc.getValue((float)i));
}
}
TEST(pidAutoTune, chsSopdtPid) {
PidAutoTuneChrSopdt chr;
double params0[4];
// todo: find better initial values?
params0[PARAM_K] = 0.1;
params0[PARAM_T] = 1;
params0[PARAM_T2] = 1;
params0[PARAM_L] = 1;
for (int tries = 0; tries < 10; tries ++) {
for (int i = 0; i < numData; i++) {
chr.addData(outputData[i]);
}
double stepPoint = 178; // todo: adjust for the buffer scale
double maxPoint = 460;
if (chr.findPid(PID_TUNE_CHR2, 20.0, 30.0, stepPoint, maxPoint, params0))
break;
// todo: the solver has failed. Choose other initial params?
// params0[0] = ; params0[1] = ; params0[2] = ;
break;
}
#ifdef PID_DEBUG
const double *p = chr.getParams();
printf("Params: K=%g T1=%g T2=%g L=%g\r\n", p[PARAM_K], p[PARAM_T], p[PARAM_T2], p[PARAM_L]);
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);
#endif
// todo: check results
}
TEST(pidAutoTune, testPidCoefs) {
pid_s pidParams[] = {
{ 2.378598f, 0.011108f, 0.063678f, 32.823277f, 0, 100 }, // CHR1 ITAE=102.008 ISE=787.356 Overshoot=2.41016%
{ 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%
};
double 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 = 2;
for (int idx = 0; idx <= 4; idx++) {
PidAccuracyMetric metric = PidAutoTuneChrSopdt::simulatePid<1024>(13.0, 16.0, dTime, 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
}
// todo: check results
}
#if 1
GTEST_API_ int main(int argc, char **argv) {
//testPidCoefs();
//printSOPDT();
testing::InitGoogleTest(&argc, argv);
// uncomment if you only want to run selected tests
#if 0
testMeasuredDataBuffer();
testMatrixInverse();
testGaussianFunction();
testFOPDT();
testSOPDTOverdamped();
testChsSopdtPid();
#endif
//::testing::GTEST_FLAG(filter) = "*testPidCoefs*";
int result = RUN_ALL_TESTS();
// windows ERRORLEVEL in Jenkins batch file seems to want negative value to detect failure
return result == 0 ? 0 : -1;
}
#endif

107
test_levenberg_marquardt_solver.cpp Normal file
View File

@ -0,0 +1,107 @@
/*
* @file test_levenberg_marquardt_solver.cpp
*
* @date Sep 27, 2019
* @author andreika, (c) 2019
*/
#include "global.h"
#include "matrix_helper.h"
#include "levenberg_marquardt_solver.h"
// Use simple and well-known Gaussian function to test the solver
class GaussianFunction : public LMSFunction<6> {
const int numParams = 6;
public:
GaussianFunction(int numPoints_, double *xValues_, double *yValues_) : numPoints(numPoints_), xValues(xValues_), yValues(yValues_) {
}
virtual void justifyParams(double *params) const {
}
// Get the total number of data points
virtual double getNumPoints() {
return numPoints;
}
virtual double getEstimatedValueAtPoint(int pi, const double *params) const {
double 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];
val += params[i] * exp(-arg * arg);
}
return val;
}
virtual double getResidual(int i, const double *params) const {
return yValues[i] - getEstimatedValueAtPoint(i, params);
}
private:
int numPoints;
double *xValues;
double *yValues;
};
void testGaussianFunction() {
int i;
const int numParams = 6;
const int numPoints = 100;
const double goodParams[numParams] = { 5, 2, 3, 2, 5, 3 };
double xValues[numPoints];
for (i = 0; i < numPoints; i++) {
xValues[i] = 0.1 * (double)(i + 1);
}
double yValues[numPoints];
GaussianFunction func(numPoints, xValues, yValues);
// imitate "real" data by using our ideal function
for (i = 0; i < numPoints; i++) {
yValues[i] = func.getEstimatedValueAtPoint(i, goodParams);
}
double params[numParams] = { 4, 2, 2, 2, 5, 2 };
LevenbergMarquardtSolver<numParams> solver(&func, params);
int iterationCount = solver.solve(0.001, 1e-15, 100);
for (i = 0; i < numParams; i++) {
ASSERT_DOUBLE_EQ(goodParams[i], params[i]);
}
ASSERT_EQ(9, iterationCount);
}
void testMatrixInverse() {
int i, j;
const int n = 4;
double a[n][n];
// fill with some arbitrary values
for (i = 0; i < n; i++) {
for (j = 0; j < n; j++) {
a[i][j] = 1.0 / (i * n + j + 1);
}
}
double ai[n][n];
bool ret = MatrixHelper<double, n>::inverseMatrix(ai, a);
ASSERT_EQ(true, ret);
double mul[n][n];
MatrixHelper<double, 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
ASSERT_DOUBLE_EQ(va, mul[i][j]);
}
}
}