.
Fork of Cntrlol_Lib by
Diff: GPAf.cpp
- Revision:
- 7:15ea5021288d
- Child:
- 8:32445aab4589
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GPAf.cpp Fri Oct 26 12:31:41 2018 +0000 @@ -0,0 +1,365 @@ +/* + GPA: frequency point wise gain and phase analyser to measure the frequency + respone of a dynamical system + + hint: the measurements should only be perfomed in closed loop + assumption: the system is at the desired steady state of interest when + the measurment starts + + exc(2) C: controller + | P: plant + v + exc(1) --> o ->| C |--->o------->| P |----------> out + ^ - | | + | --> inp | exc: excitation signal (E) + | | inp: input plant (U) + -------------------------------- out: output plant (Y) + + instantiate option 1: (logarithmic equaly spaced frequency points) + + GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) + + 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 + Aexc0: excitation amplitude at fMin + Aexc1: excitation amplitude at fMax + + 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) + + 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 + + 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) + + *fexcDes: sorted frequency point array in Hz + Aexc0: excitation amplitude at fexcDes[0] + Aexc1: excitation amplitude at fexcDes[NfexcDes-1] + NfexcDes: length of fexcDes + + hints: 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 try to increase Nmeas. + + pseudo code for a closed loop measurement with a controller C: + + excitation input at (1): + + - measuring the plant P and the closed loop tf T = PC/(1 + PC): + desTorque = pi_w(omega_desired - omega + excWobble); + inpWobble = desTorque; + outWobble = omega; + excWobble = Wobble(excWobble, outWobble); + + - measuring the controller C and the closed loop tf SC = C/(1 + PC) + desTorque = pi_w(omega_desired - omega + excWobble); + inpWobble = n_soll + excWobble - omega; + outWobble = desTorque; + excWobble = Wobble(inpWobble, outWobble); + + excitation input at (2): + + - measuring the plant P and the closed loop tf SP = P/(1 + PC): + desTorque = pi_w(omega_desired - omega) + excWobble; + inpWobble = desTorque; + outWobble = omega; + excWobble = Wobble(excWobble, outWobble); + + 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 results are plotted to a terminal (putty) over a serial + connection and look as follows: + ----------------------------------------------------------------------------------------- + fexc[Hz] |Gyu| ang(Gyu) |Gye| ang(Gye) |E| |U| |Y| + ----------------------------------------------------------------------------------------- + 1.000e+00 2.924e+01 -1.572e+00 1.017e+00 -4.983e-02 5.000e+00 1.739e-01 5.084e+00 + + in matlab you can use: + dataNucleo = [... insert measurement data here ...]; + g = frd(dataNucleo(:,2).*exp(1i*dataNucleo(:,3)), dataNucleo(:,1), Ts, 'Units', 'Hz'); + gcl = frd(dataNucleo(:,4).*exp(1i*dataNucleo(:,5)), dataNucleo(:,1), Ts, 'Units', 'Hz'); + + if you're evaluating more than one measurement which contain equal frequency points try: + dataNucleo = [dataNucleo1; dataNucleo2]; + [~, ind] = unique(dataNucleo(:,1),'stable'); + dataNucleo = dataNucleo(ind,:); + + autor: M.E. Peter +*/ + +#include "GPA.h" +#include "mbed.h" +#include "math.h" +#define pi 3.14159f + +using namespace std; + +GPAf::GPAf(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) +{ + this->NfexcDes = NfexcDes; + this->NperMin = NperMin; + this->NmeasMin = NmeasMin; + this->Ts = (float)Ts; + + // calculate logarithmic spaced frequency points + fexcDes = (float*)malloc(NfexcDes*sizeof(float)); + fexcDesLogspace((float)fMin, (float)fMax, NfexcDes); + + // calculate coefficients for decreasing amplitude (1/fexc) + this->aAexcDes = ((float)Aexc1 - (float)Aexc0)/(1.0f/fexcDes[NfexcDes-1] - 1.0f/fexcDes[0]); + this->bAexcDes = (float)Aexc0 - aAexcDes/fexcDes[0]; + + fnyq = 1.0f/2.0f/(float)Ts; + pi2 = 2.0f*pi; + pi2Ts = pi2*(float)Ts; + piDiv2 = pi/2.0f; + + sU = (float*)malloc(3*sizeof(float)); + sY = (float*)malloc(3*sizeof(float)); + reset(); +} + +GPAf::GPAf(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) +{ + // convert fexcDes from float to float, it is assumed that it is sorted + this->NfexcDes = NfexcDes; + this->fexcDes = (float*)malloc(NfexcDes*sizeof(float)); + for(int i = 0; i < NfexcDes; i++) { + this->fexcDes[i] = (float)fexcDes[i]; + } + this->NperMin = NperMin; + this->NmeasMin = NmeasMin; + this->Ts = (float)Ts; + + // calculate coefficients for decreasing amplitude (1/fexc) + this->aAexcDes = ((float)Aexc1 - (float)Aexc0)/(1.0f/(float)f1 - 1.0f/(float)f0); + this->bAexcDes = (float)Aexc0 - aAexcDes/fexcDes[0]; + + fnyq = 1.0f/2.0f/(float)Ts; + pi2 = 2.0f*pi; + pi2Ts = pi2*(float)Ts; + piDiv2 = pi/2.0f; + + sU = (float*)malloc(3*sizeof(float)); + sY = (float*)malloc(3*sizeof(float)); + reset(); +} + +GPAf::GPAf(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) +{ + // convert fexcDes from float to float, it is assumed that it is sorted + this->NfexcDes = NfexcDes; + this->fexcDes = (float*)malloc(NfexcDes*sizeof(float)); + for(int i = 0; i < NfexcDes; i++) { + this->fexcDes[i] = (float)fexcDes[i]; + } + this->NperMin = NperMin; + this->NmeasMin = NmeasMin; + this->Ts = (float)Ts; + + // calculate coefficients for decreasing amplitude (1/fexc) + this->aAexcDes = ((float)Aexc1 - (float)Aexc0)/(1.0f/this->fexcDes[NfexcDes-1] - 1.0f/this->fexcDes[0]); + this->bAexcDes = (float)Aexc0 - aAexcDes/fexcDes[0]; + + fnyq = 1.0f/2.0f/(float)Ts; + pi2 = 2.0f*pi; + pi2Ts = pi2*(float)Ts; + piDiv2 = pi/2.0f; + + sU = (float*)malloc(3*sizeof(float)); + sY = (float*)malloc(3*sizeof(float)); + reset(); +} + +GPAf::~GPAf() {} + +void GPAf::reset() +{ + Nmeas = 0; + Nper = 0; + fexc = 0.0; + fexcPast = 0.0; + ii = 1; // iterating through desired frequency points + jj = 1; // iterating through measurement points w.r.t. reachable frequency + scaleG = 0.0; + scaleH = 2.0; + wk = 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; + NmeasTotal = 0; + Aexc = 0.0; + pi2Tsfexc = 0.0; +} + +float GPAf::update(float inp, float out) +{ + // a new frequency point has been reached + if(jj == 1) { + // get a new unique frequency point + while(fexc == fexcPast) { + // measurement finished + if(ii > NfexcDes) { + return 0.0f; + } + calcGPAmeasPara(fexcDes[ii - 1]); + // secure fexc is not higher or equal to nyquist frequency + if(fexc >= fnyq) { + fexc = fexcPast; + } + // no frequency found + if(fexc == fexcPast) { + ii += 1; + } else { + Aexc = aAexcDes/fexc + bAexcDes; + pi2Tsfexc = pi2Ts*fexc; + } + } + // secure sinarg starts at 0 (numerically maybe not given) + sinarg = 0.0; + // filter scaling + scaleG = 1.0f/sqrt((float)Nmeas); + // filter coefficients + cr = cos(pi2Tsfexc); + ci = sin(pi2Tsfexc); + // filter storage + for(int i = 0; i < 3; i++) { + sU[i] = 0.0; + sY[i] = 0.0; + } + } + // update hann window + calcHann(); + // filter step for signal su + sU[0] = wk*scaleG*inp + 2.0f*cr*sU[1] - sU[2]; + sU[2] = sU[1]; + sU[1] = sU[0]; + // filter step for signal sy + sY[0] = wk*scaleG*out + 2.0f*cr*sY[1] - sY[2]; + sY[2] = sY[1]; + sY[1] = sY[0]; + // measurement of frequencypoint is finished + if(jj == Nmeas) { + jj = 1; + ii += 1; + fexcPast = fexc; + // calculate the one point dft + float Ureal = 2.0f*scaleH*scaleG*(cr*sU[1] - sU[2]); + float Uimag = 2.0f*scaleH*scaleG*ci*sU[1]; + float Yreal = 2.0f*scaleH*scaleG*(cr*sY[1] - sY[2]); + float Yimag = 2.0f*scaleH*scaleG*ci*sY[1]; + // calculate magnitude and angle + float Umag = (float)(sqrt(Ureal*Ureal + Uimag*Uimag)); + float Ymag = (float)(sqrt(Yreal*Yreal + Yimag*Yimag)); + float absGyu = (float)(Ymag/Umag); + float angGyu = (float)(atan2(Yimag, Yreal) - atan2(Uimag, Ureal)); + float absGye = (float)(Ymag/Aexc); + float angGye = (float)(atan2(Yimag, Yreal) + piDiv2); + // user info + if(ii == 2) { + printLine(); + printf(" fexc[Hz] |Gyu| ang(Gyu) |Gye| ang(Gye) |E| |U| |Y|\r\n"); + printLine(); + } + //printf("%11.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e\r\n", (float)fexc, absGyu, angGyu, absGye, angGye, (float)Aexc, Umag, Ymag); + } else { + jj += 1; + } + sinarg = fmod(sinarg + pi2Tsfexc, pi2); + NmeasTotal += 1; + return (float)(Aexc*sin(sinarg)); +} + +void GPAf::fexcDesLogspace(float fMin, float fMax, int NfexcDes) +{ + // calculate logarithmic spaced frequency points + float Gain = log10(fMax/fMin)/((float)NfexcDes - 1.0f); + float expon = 0.0; + for(int i = 0; i < NfexcDes; i++) { + fexcDes[i] = fMin*pow(10.0f, expon); + expon += Gain; + } +} + +void GPAf::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)Nper/(float)Nmeas/Ts; +} + +void GPAf::calcHann() +{ + wk = 0.5f - 0.5f*cos(2.0f*pi*((float)jj-1.0f)/(float)Nmeas); +} + +void GPA::printLine() +{ + printf("-----------------------------------------------------------------------------------------\r\n"); +} + +void GPAf::printGPAfexcDes() +{ + printLine(); + for(int i = 0; i < NfexcDes; i++) { + printf("%9.4f\r\n", (float)fexcDes[i]); + } +} + +void GPAf::printGPAmeasPara() +{ + printLine(); + printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper\r\n"); + printLine(); + for(int i = 0; i < NfexcDes; i++) { + calcGPAmeasPara(fexcDes[i]); + if(fexc == fexcPast || fexc >= fnyq) { + fexc = 0.0; + Nmeas = 0; + Nper = 0; + Aexc = 0.0; + } else { + Aexc = aAexcDes/fexc + bAexcDes; + fexcPast = fexc; + } + NmeasTotal += Nmeas; + printf("%12.2e %9.2e %10.2e %7i %6i \r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper); + } + printGPAmeasTime(); + reset(); +} + +void GPAf::printGPAmeasTime() +{ + printLine(); + printf(" number of data points: %9i\r\n", NmeasTotal); + printf(" measurment time in sec: %9.2f\r\n", (float)((float)NmeasTotal*Ts)); +} + +void GPAf::printNfexcDes() +{ + printLine(); + printf(" number of frequancy points: %2i\r\n", NfexcDes); +}