commit 1fea0fe1f76c87468e5b3f8bf7f676c8a6c3bb5f Author: Andrei Date: Sat Oct 5 17:54:26 2019 +0300 initial diff --git a/global.h b/global.h new file mode 100644 index 0000000..da2a267 --- /dev/null +++ b/global.h @@ -0,0 +1,38 @@ +// Dummy file replacement + +#pragma once + + +#include +#include +#include +#include +#include + +#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 diff --git a/levenberg_marquardt_solver.h b/levenberg_marquardt_solver.h new file mode 100644 index 0000000..5014a09 --- /dev/null +++ b/levenberg_marquardt_solver.h @@ -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 +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 +class LevenbergMarquardtSolver { +public: + // ctor + LevenbergMarquardtSolver(LMSFunction *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::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 *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; +}; diff --git a/matrix_helper.h b/matrix_helper.h new file mode 100644 index 0000000..17da04d --- /dev/null +++ b/matrix_helper.h @@ -0,0 +1,95 @@ +/* +* @file matrix_helper.h +* +* @date Sep 27, 2019 +* @author andreika, (c) 2019 +*/ + +#pragma once + +#include "global.h" + + +template +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]; + } + } + } + } + +}; + + diff --git a/pid_avg_buf.h b/pid_avg_buf.h new file mode 100644 index 0000000..fb9565b --- /dev/null +++ b/pid_avg_buf.h @@ -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 +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; +}; + diff --git a/pid_controller.h b/pid_controller.h new file mode 100644 index 0000000..1408352 --- /dev/null +++ b/pid_controller.h @@ -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; +}; \ No newline at end of file diff --git a/pid_from_msl.cpp b/pid_from_msl.cpp new file mode 100644 index 0000000..f294f0a --- /dev/null +++ b/pid_from_msl.cpp @@ -0,0 +1,117 @@ +/* +* @file pid_from_msl.cpp +* +* @date Oct 5, 2019 +* @author andreika, (c) 2019 +*/ + +#include "global.h" +#include +#include + +#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 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 diff --git a/pid_functions.h b/pid_functions.h new file mode 100644 index 0000000..ec55655 --- /dev/null +++ b/pid_functions.h @@ -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 +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 inputData; +}; + +// Abstract indirect transfer function used for step response analytic simulation +template +class AbstractDelayLineFunction : public LMSFunction { +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; + } +}; + diff --git a/pid_open_loop_models.h b/pid_open_loop_models.h new file mode 100644 index 0000000..f59c853 Binary files /dev/null and b/pid_open_loop_models.h differ diff --git a/test_chs_pid.cpp b/test_chs_pid.cpp new file mode 100644 index 0000000..ab4e938 --- /dev/null +++ b/test_chs_pid.cpp @@ -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 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 diff --git a/test_levenberg_marquardt_solver.cpp b/test_levenberg_marquardt_solver.cpp new file mode 100644 index 0000000..e812b86 --- /dev/null +++ b/test_levenberg_marquardt_solver.cpp @@ -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 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::inverseMatrix(ai, a); + ASSERT_EQ(true, ret); + + double mul[n][n]; + MatrixHelper::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]); + } + } +} +