Ruprecht Altenburger
/
mirror_actuator_V1
Mirror actuator for RT2 lab
Diff: Library_Cntrl/GPA.cpp
- Revision:
- 12:8ea956a5c1d8
diff -r d43f8b421d6d -r 8ea956a5c1d8 Library_Cntrl/GPA.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Library_Cntrl/GPA.cpp Wed Apr 28 13:10:16 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