Ruprecht Altenburger
template for students for mirror actuator
Diff: Lib_Cntrl/GPA.cpp
- Revision:
- 0:d2e117716219
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Lib_Cntrl/GPA.cpp Sun May 02 19:32:30 2021 +0000
@@ -0,0 +1,548 @@
+/*
+ GPA: Frequency point wise gain and phase analyser to measure a frequency respone function (FRF) of a dynamical system
+
+ Hint: If the plant has a pole at zero, is unstable or weakly damped the measurement has to be perfomed
+ in closed loop (this is NOT tfestimate, the algorithm is based on the one point DFT).
+ Assumption: The system is and remains at the desired steady state when the measurment starts
+
+ Note: The amplitude drops with 1/fexc, if you're using Axc1 = Aexc0/fMax then d/dt exc = const.,
+ this is recommended if your controller does not have a rolloff. If a desired frequency point
+ is not measured (could not be reached) try to increase Nmeas.
+
+
+ Instantiate option 0: ("Not a Jedi yet" users, for logarithmic equaly spaced frequency points)
+
+ GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts)
+
+ fMin: Minimal desired frequency that should be measured in Hz
+ fMax: Maximal desired frequency that should be measured in Hz
+ NfexcDes: Number of logarithmic equaly spaced frequency points between fMin and fMax
+ Aexc0: Excitation amplitude at fMin
+ Aexc1: Excitation amplitude at fMax
+ Ts: Sampling time in sec
+
+ Default values are as follows:
+ int NperMin = 3;
+ int NmeasMin = (int)ceil(1.0f/Ts);
+ int Nstart = (int)ceil(3.0f/Ts);
+ int Nsweep = (int)ceil(0.0f/Ts);
+
+ Instantiate option 1: ("Jedi or Sith Lord", for logarithmic equaly spaced frequency points)
+
+ GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep)
+
+ fMin: Minimal desired frequency that should be measured in Hz
+ fMax: Maximal desired frequency that should be measured in Hz
+ NfexcDes: Number of logarithmic equaly spaced frequency points
+ NperMin: Minimal number of periods that are used for each frequency point
+ NmeasMin: Minimal number of samples that are used for each frequency point
+ Ts: Sampling time in sec
+ Aexc0: Excitation amplitude at fMin
+ Aexc1: Excitation amplitude at fMax
+ Nstart: Minimal number of samples to sweep to the first frequency point (can be equal 0)
+ Nsweep: Minimal number of samples to sweep from freq. point to freq. point (can be equal 0)
+
+
+ Instantiate option 2: (for a second, refined frequency grid measurement)
+
+ GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep)
+
+ f0: Frequency point for the calculation of Aexc0 in Hz (should be chosen like in the first measurement)
+ f1: Frequency point for the calculation of Aexc1 in Hz (should be chosen like in the first measurement)
+ *fexcDes: Sorted frequency point array in Hz
+ NfexcDes: Length of fexcDes
+
+ For the other parameters see above.
+
+ Instantiate option 3: (for an arbitary but sorted frequency grid measurement)
+
+ GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep)
+
+ *fexcDes: Sorted frequency point array in Hz
+ Aexc0: Excitation amplitude at fexcDes[0]
+ Aexc1: Excitation amplitude at fexcDes[NfexcDes-1]
+ NfexcDes: Length of fexcDes
+
+ For the other parameters see above.
+
+
+ Block diagram:
+
+ w (const.) exc(2) C: controller
+ | | P: plant
+ v e v
+ exc(1) --> o ->| C |--->o------->| P |----------> out (y)
+ ^ - | |
+ | --> inp (u) | exc (R): excitation signal
+ | | inp (U): input plant
+ -------------------------------- out (Y): output plant
+
+
+ Pseudo code for an open loop measurement:
+
+ - Measuring the plant P = Gyu = Gyr:
+
+ u = w + exc;
+ ... write output u here! it follows exc(k+1) ...
+ exc = Wobble(exc, y);
+
+ Closed loop FRF calculus with a stabilizing controller C:
+ S = 1/(1 + C*P); % ( exc1 -> e , 1/(1 + C*P) ) sensitivity, contr. error rejection, robustness (1/max|S|)
+ T = 1 - S; % ( w -> y , C*P/(1 + C*P) ) compl. sensitivity, tracking performance
+ CS = C*S; % ( exc1 -> u , C/(1 + C*P) ) control effort, disturbance plant output on plant input
+ PS = P*S; % ( exc2 -> y , P/(1 + C*P) ) compliance, disturbance plant input on plant output
+
+
+ Pseudo code for a closed loop measurement with stabilizing controller C:
+
+ Excitation at excitation input (1):
+
+ - Measuring the plant P = Gyu and the closed loop tf T = PC/(1 + PC) = Gyr:
+
+ u = C(w - y + exc);
+ ... write output u here! it follows exc(k+1) ...
+ exc = Wobble(u, y);
+
+ Closed loop FRF calculus:
+ S = 1 - T;
+ PS = P*S;
+ CS = T/P;
+ C = C/S;
+
+ Excitation at excitation input (2):
+
+ - Measuring the plant P = Gyu and the closed loop tf PS = P/(1 + PC) = Gyr:
+
+ u = C(w - y) + exc;
+ ... write output u here! it follows exc(k+1) ...
+ exc = Wobble(u, y);
+
+ Closed loop FRF calculus:
+ S = PS/P;
+ T = 1 - S;
+ CS = T/P;
+ C = C/S;
+
+
+ Usage:
+ exc(k+1) = myGPA(inp(k), out(k)) does update the internal states of the
+ gpa at the timestep k and returns the excitation signal for the timestep
+ k+1. The FRF data are plotted to a terminal (Putty) over a serial
+ connection and look as follows:
+
+--------------------------------------------------------------------------------
+ fexc[Hz] |Gyu| deg(Gyu) |Gyr| deg(Gyr) |U| |Y| |R|
+--------------------------------------------------------------------------------
+ 5.0000e-02 1.001e+00 -0.309 1.001e+00 -0.309 4.000e-01 4.000e-01 4.005e-01
+ . . . . . . . .
+ . . . . . . . .
+ . . . . . . . .
+
+ In Matlab you can use the editor as follows:
+ data = [... insert measurement data here ...];
+ gyu = frd(data(:,2).*exp(1i*data(:,3)*pi/180), data(:,1), Ts, 'Units', 'Hz');
+ gyr = frd(data(:,4).*exp(1i*data(:,5)*pi/180), data(:,1), Ts, 'Units', 'Hz');
+
+ If you're evaluating more than one measurement which contain equal frequency points use:
+ data = [data1; data2];
+ [~, ind] = unique(data(:,1), 'stable');
+ data = data(ind,:);
+
+
+ Autor and Copyrigth: 2018 / 2019 / 2020 / M.E. Peter
+
+*/
+
+#include "GPA.h"
+#include "mbed.h"
+#include "math.h"
+#define pi 3.14159265358979323846 // M_PI
+
+using namespace std;
+
+// -----------------------------------------------------------------------------
+// instantiate
+// -----------------------------------------------------------------------------
+
+GPA::GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts)
+{
+ doPrint = true;
+ int NperMin = 3;
+ int NmeasMin = (int)ceil(1.0f/Ts);
+ int Nstart = (int)ceil(3.0f/Ts);
+ int Nsweep = (int)ceil(0.0f/Ts);
+ new_data_available = false;
+ meas_is_finished = false;
+ start_now = false;
+
+ setParameters(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, Nstart, Nsweep, doPrint);
+}
+
+GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep)
+{
+ doPrint = true;
+ new_data_available = false;
+ meas_is_finished = false;
+ start_now = false;
+ setParameters(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, Nstart, Nsweep, doPrint);
+}
+
+GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep)
+{
+ doPrint = true;
+ new_data_available = false;
+ meas_is_finished = false;
+
+ assignParameters(NfexcDes, NperMin, NmeasMin, Ts, Nstart, Nsweep);
+
+ // convert fexcDes from float to float, it is assumed that it is sorted
+ this->fexcDes = (float*)malloc(NfexcDes*sizeof(float));
+ for(int i = 0; i < NfexcDes; i++) {
+ this->fexcDes[i] = (float)fexcDes[i];
+ }
+
+ calculateDecreasingAmplitudeCoefficients(Aexc0, Aexc1);
+ initializeConstants(Ts);
+ assignFilterStorage();
+ reset();
+}
+
+GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep)
+{
+ doPrint = true;
+ assignParameters(NfexcDes, NperMin, NmeasMin, Ts, Nstart, Nsweep);
+
+ // convert fexcDes from float to float, it is assumed that it is sorted
+ this->fexcDes = (float*)malloc(NfexcDes*sizeof(float));
+ for(int i = 0; i < NfexcDes; i++) {
+ this->fexcDes[i] = fexcDes[i];
+ }
+
+ calculateDecreasingAmplitudeCoefficients(Aexc0, Aexc1);
+ initializeConstants(Ts);
+ assignFilterStorage();
+ reset();
+}
+
+GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep, bool doPrint)
+{
+ setParameters(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, Nstart, Nsweep, doPrint);
+}
+
+// -----------------------------------------------------------------------------
+// virtual, reset and set
+// -----------------------------------------------------------------------------
+
+GPA::~GPA() {}
+
+void GPA::setParameters(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep, bool doPrint)
+{
+ this->doPrint = doPrint;
+ assignParameters(NfexcDes, NperMin, NmeasMin, Ts, Nstart, Nsweep);
+
+ // calculate logarithmic spaced frequency points
+ fexcDes = (float*)malloc(NfexcDes*sizeof(float));
+ fexcDesLogspace(fMin, fMax, NfexcDes);
+
+ calculateDecreasingAmplitudeCoefficients(Aexc0, Aexc1);
+ initializeConstants(Ts);
+ assignFilterStorage();
+ reset();
+
+}
+
+void GPA::reset()
+{
+ Nmeas = 0;
+ Nper = 0;
+ dfexc = 0.0;
+ fexc = 0.0;
+ fexcPast = 0.0f;
+ dfexcj = 0.0f;
+ i = 1; // iterating through desired frequency points
+ j = 1; // iterating through measurement points w.r.t. reachable frequency
+ scaleG = 0.0;
+ cr = 0.0;
+ ci = 0.0;
+ for(int i = 0; i < 3; i++) {
+ sU[i] = 0.0;
+ sY[i] = 0.0;
+ }
+ sinarg = 0.0;
+ sinargR = 0.0f;
+ NmeasTotal = 0;
+ Aexc = 0.0f;
+ pi2Tsfexc = 0.0;
+ Nsweep_i = Nstart;
+ AexcOut = 0.0f;
+ gpaData.fexc = 0.0f;
+ gpaData.absGyu = 0.0f;
+ gpaData.angGyu = 0.0f;
+ gpaData.absGyr = 0.0f;
+ gpaData.angGyr = 0.0f;
+ gpaData.Umag = 0.0f;
+ gpaData.Ymag = 0.0f;
+ gpaData.Rmag = 0.0f;
+ gpaData.MeasPointFinished = false;
+ gpaData.MeasFinished = false;
+ gpaData.ind = -1;
+
+}
+
+// -----------------------------------------------------------------------------
+// update (operator)
+// -----------------------------------------------------------------------------
+
+float GPA::update(float inp, float out)
+{
+ // a new frequency point has been reached
+ if(j == 1) {
+ // user info
+ if(i == 1 && doPrint) {
+ printLine();
+ //printf(" fexc[Hz] |Gyu| deg(Gyu) |Gyr| deg(Gyr) |U| |Y| |R|\r\n");
+ printLine();
+ start_now = true;
+ //uart_com.send_char_data(250,2,0);
+ }
+
+ // get a new unique frequency point
+ while(fexc == fexcPast) {
+ // measurement finished
+ if(i > NfexcDes) {
+ gpaData.MeasPointFinished = false;
+ gpaData.MeasFinished = true;
+ meas_is_finished = true;
+ return 0.0f;
+ }
+ calcGPAmeasPara(fexcDes[i - 1]);
+ // secure fexc is not higher or equal to nyquist frequency
+ if(fexc >= fnyq) {
+ fexc = fexcPast;
+ }
+ // no frequency found
+ if(fexc == fexcPast) {
+ i += 1;
+ } else {
+ Aexc = aAexcDes/fexc + bAexcDes;
+ pi2Tsfexc = pi2Ts*fexc;
+ }
+ }
+ // filter scaling
+ scaleG = 1.0/sqrt((double)Nmeas);
+ // filter coefficients
+ cr = cos(pi2Tsfexc);
+ ci = sin(pi2Tsfexc);
+ // set filter storage zero
+ for(int i = 0; i < 3; i++) {
+ sU[i] = 0.0;
+ sY[i] = 0.0;
+ }
+ gpaData.MeasPointFinished = false;
+ }
+ // perfomre the sweep or measure
+ if(j <= Nsweep_i) {
+ dfexcj = ((float)j - 1.0f)/((float)Nsweep_i - 1.0f);
+ dfexcj = div12pi*sinf(pi4*dfexcj) - div812pi*sinf((float)pi2*dfexcj) + dfexcj;
+ dfexc = fexcPast + (fexc - fexcPast)*dfexcj;
+ AexcOut = AexcPast + (Aexc - AexcPast)*dfexcj;
+ } else {
+ dfexc = fexc;
+ AexcOut = Aexc;
+ // one point DFT filter step for signal su
+ sU[0] = scaleG*inp + 2.0*cr*sU[1] - sU[2];
+ sU[2] = sU[1];
+ sU[1] = sU[0];
+ // one point DFT filter step for signal sy
+ sY[0] = scaleG*out + 2.0*cr*sY[1] - sY[2];
+ sY[2] = sY[1];
+ sY[1] = sY[0];
+ }
+ // copy starting value for ang(R)
+ if(j == 1 || j == Nsweep_i + 1) sinargR = sinarg;
+ // measurement of frequencypoint is finished
+ if(j == Nmeas + Nsweep_i) {
+ fexcPast = fexc;
+ AexcPast = Aexc;
+ Nsweep_i = Nsweep;
+ // calculate the one point dft
+ float Ureal = (float)(2.0*scaleG*(cr*sU[1] - sU[2]));
+ float Uimag = (float)(2.0*scaleG*ci*sU[1]);
+ float Yreal = (float)(2.0*scaleG*(cr*sY[1] - sY[2]));
+ float Yimag = (float)(2.0*scaleG*ci*sY[1]);
+ // calculate magnitude and angle
+ float Umag = sqrtf(Ureal*Ureal + Uimag*Uimag);
+ float Ymag = sqrtf(Yreal*Yreal + Yimag*Yimag);
+ gpaData.fexc = (float)fexc;
+ gpaData.absGyu = Ymag/Umag;
+ gpaData.angGyu = wrapAngle(atan2f(Yimag, Yreal) - atan2f(Uimag, Ureal))*rad2deg;
+ gpaData.absGyr = Ymag/Aexc;
+ gpaData.angGyr = wrapAngle(atan2f(Yimag, Yreal) + fmod(piDiv2 - sinargR, (float)pi2))*rad2deg;
+ gpaData.Umag = Umag;
+ gpaData.Ymag = Ymag;
+ gpaData.Rmag = Aexc;
+ gpaData.MeasPointFinished = true;
+ gpaData.ind++;
+ // user info
+ if(doPrint) {
+ //printf("%11.4e %9.3e %8.3f %9.3e %8.3f %9.3e %9.3e %9.3e\r\n", gpaData.fexc, gpaData.absGyu, gpaData.angGyu, gpaData.absGyr, gpaData.angGyr, gpaData.Umag, gpaData.Ymag, gpaData.Rmag);
+ new_data_available = true;
+ }
+ i += 1;
+ j = 1;
+ } else {
+ j += 1;
+ }
+ // calculate the excitation
+ sinarg = fmod(sinarg + pi2Ts*dfexc, pi2);
+ NmeasTotal += 1;
+ return AexcOut*sinf(sinarg);
+}
+
+// -----------------------------------------------------------------------------
+// private functions
+// -----------------------------------------------------------------------------
+
+void GPA::assignParameters(int NfexcDes, int NperMin, int NmeasMin, float Ts, int Nstart, int Nsweep)
+{
+ this->NfexcDes = NfexcDes;
+ this->NperMin = NperMin;
+ this->NmeasMin = NmeasMin;
+ this->Ts = Ts;
+ this->Nstart = Nstart;
+ this->Nsweep = Nsweep;
+}
+
+void GPA::calculateDecreasingAmplitudeCoefficients(float Aexc0, float Aexc1)
+{
+ // calculate coefficients for decreasing amplitude (1/fexc)
+ this->aAexcDes = (Aexc1 - Aexc0)/(1.0f/fexcDes[NfexcDes-1] - 1.0f/fexcDes[0]);
+ this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0];
+}
+
+void GPA::initializeConstants(float Ts)
+{
+ fnyq = 1.0f/2.0f/Ts;
+ pi2 = 2.0*pi;
+ pi4 = 4.0f*(float)pi;
+ pi2Ts = 2.0*pi*(double)Ts;// 2.0*pi*Ts;
+ piDiv2 = (float)pi/2.0f;
+ rad2deg = 180.0f/(float)pi;
+ div12pi = 1.0f/(12.0f*(float)pi);
+ div812pi = 8.0f/(12.0f*(float)pi);
+}
+
+void GPA::assignFilterStorage()
+{
+ sU = (double*)malloc(3*sizeof(double));
+ sY = (double*)malloc(3*sizeof(double));
+}
+
+void GPA::fexcDesLogspace(float fMin, float fMax, int NfexcDes)
+{
+ // calculate logarithmic spaced frequency points
+ float Gain = log10f(fMax/fMin)/((float)NfexcDes - 1.0f);
+ float expon = 0.0;
+ for(int i = 0; i < NfexcDes; i++) {
+ fexcDes[i] = fMin*powf(10.0f, expon);
+ expon += Gain;
+ }
+}
+
+void GPA::calcGPAmeasPara(float fexcDes_i)
+{
+ // Nmeas has to be an integer
+ Nper = NperMin;
+ Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
+ // secure that the minimal number of measurements is fullfilled
+ int Ndelta = NmeasMin - Nmeas;
+ if(Ndelta > 0) {
+ Nper = (int)ceil((float)NmeasMin*fexcDes_i*Ts);
+ Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
+ }
+ // evaluating reachable frequency
+ fexc = (float)((double)Nper/(double)Nmeas/(double)Ts);
+}
+
+float GPA::wrapAngle(float angle)
+{
+ // wrap angle from (-2pi,2pi) into (-pi,pi)
+ if(abs(angle) > (float)pi) angle -= copysignf(-(float)pi2, angle); // -1*sign(angle)*2*pi + angle;
+ return angle;
+}
+
+void GPA::printLongLine()
+{
+ //printf("-------------------------------------------------------------------------------------------------------------------------------\r\n");
+}
+
+// -----------------------------------------------------------------------------
+// public functions (mainly for debugging)
+// -----------------------------------------------------------------------------
+
+void GPA::printGPAfexcDes()
+{
+ printLine();
+ for(int i = 0; i < NfexcDes; i++) {
+ printf("%9.4f\r\n", fexcDes[i]);
+ }
+}
+
+void GPA::printGPAmeasPara()
+{
+ printLine();
+ printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper Nsweep_i\r\n");
+ printLine();
+ for(int i = 0; i < NfexcDes; i++) {
+ calcGPAmeasPara(fexcDes[i]);
+ if(fexc == fexcPast || fexc >= fnyq) {
+ fexc = 0.0;
+ Aexc = 0.0f;
+ Nmeas = 0;
+ Nper = 0;
+ Nsweep_i = 0;
+ } else {
+ Aexc = aAexcDes/fexc + bAexcDes;
+ fexcPast = fexc;
+ AexcPast = Aexc;
+ }
+ NmeasTotal += Nmeas;
+ NmeasTotal += Nsweep_i;
+ printf("%11.4e %12.4e %10.3e %7i %6i %7i\r\n", fexcDes[i], (float)fexc, Aexc, Nmeas, Nper, Nsweep_i);
+// wait(0.01);
+ Nsweep_i = Nsweep;
+ }
+ printGPAmeasTime();
+ reset();
+}
+
+void GPA::printGPAmeasTime()
+{
+ printLine();
+ printf(" Number of data points : %11i\r\n", NmeasTotal);
+ printf(" Measurment time in sec: %12.2f\r\n", (float)NmeasTotal*Ts);
+}
+
+void GPA::printNfexcDes()
+{
+ printLine();
+ printf(" Number of frequancy points: %3i\r\n", NfexcDes);
+}
+
+void GPA::printLine()
+{
+ printf("--------------------------------------------------------------------------------\r\n");
+}
+
+void GPA::getGPAdata(float *val)
+{
+ val[0] = gpaData.fexc;
+ val[1] = gpaData.absGyu;
+ val[2] = gpaData.angGyu;
+ val[3] = gpaData.absGyr;
+ val[4] = gpaData.angGyr;
+ val[5] = gpaData.Umag;
+ val[6] = gpaData.Ymag;
+ val[7] = gpaData.Rmag;
+ new_data_available = false;
+}
\ No newline at end of file