2nd Library
GPA.cpp
- Committer:
- altb2
- Date:
- 2019-03-08
- Revision:
- 2:35e1cf78ea6f
- Parent:
- 0:a201a6cd4c0c
File content as of revision 2:35e1cf78ea6f:
/* GPA: Frequency point wise gain and phase analyser to measure the frequency respone function (FRF) of a dynamical system, based on the one point DFT 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 of interest when the measurment starts 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 NstartMin = (int)ceil(3.0f/Ts); int NsweepMin = 0; 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 NstartMin, int NsweepMin) 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 NstartMin: Minimal number of samples to sweep to the first frequency point (can be equal 0) NsweepMin: 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 NstartMin, int NsweepMin) 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 NstartMin, int NsweepMin) *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. 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. 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) ) contr. error rejection, robustness (1/max|S|) T = 1 - S; % ( w -> y , C*P/(1 + C*P) ) tracking CS = C*S; % ( exc1 -> u , C/(1 + C*P) ) disturbance plant output PS = P*S; % ( exc2 -> y , P/(1 + C*P) ) disturbance plant input 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 / M.E. Peter */ #include "GPA.h" #include <mbed.h> #include "math.h" #define pi 3.141592653589793 using namespace std; // ----------------------------------------------------------------------------- // instantiate // ----------------------------------------------------------------------------- GPA::GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts) { int NperMin = 3; int NmeasMin = (int)ceil(1.0f/Ts); int NstartMin = (int)ceil(3.0f/Ts); int NsweepMin = 0; assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin); // calculate logarithmic spaced frequency points fexcDes = (double*)malloc(NfexcDes*sizeof(double)); fexcDesLogspace((double)fMin, (double)fMax, NfexcDes); calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1); initializeConstants((double)Ts); assignFilterStorage(); reset(); } GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin) { assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin); // calculate logarithmic spaced frequency points fexcDes = (double*)malloc(NfexcDes*sizeof(double)); fexcDesLogspace((double)fMin, (double)fMax, NfexcDes); calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1); initializeConstants((double)Ts); assignFilterStorage(); reset(); } GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin) { assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin); // convert fexcDes from float to double, it is assumed that it is sorted this->fexcDes = (double*)malloc(NfexcDes*sizeof(double)); for(int i = 0; i < NfexcDes; i++) { this->fexcDes[i] = (double)fexcDes[i]; } calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1); initializeConstants((double)Ts); assignFilterStorage(); reset(); } GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin) { assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin); // convert fexcDes from float to double, it is assumed that it is sorted this->fexcDes = (double*)malloc(NfexcDes*sizeof(double)); for(int i = 0; i < NfexcDes; i++) { this->fexcDes[i] = (double)fexcDes[i]; } calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1); initializeConstants((double)Ts); assignFilterStorage(); reset(); } // ----------------------------------------------------------------------------- // virtual and reset // ----------------------------------------------------------------------------- GPA::~GPA() {} void GPA::reset() { Nmeas = 0; Nper = 0; dfexc = 0.0; fexc = 0.0; fexcPast = 0.0; 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; NmeasTotal = 0; Aexc = 0.0; pi2Tsfexc = 0.0; Nsweep = NstartMin; bfexc = 0.0; afexc = 0.0; aAexc = 0.0; bAexc = 0.0; AexcOut = 0.0; } // ----------------------------------------------------------------------------- // update (operator) // ----------------------------------------------------------------------------- float GPA::update(double inp, double out) { // a new frequency point has been reached if(j == 1) { // user info if(i == 1) { printLine(); printf(" fexc[Hz] |Gyu| deg(Gyu) |Gyr| deg(Gyr) |U| |Y| |R|\r\n"); printLine(); } // get a new unique frequency point while(fexc == fexcPast) { // measurement finished if(i > NfexcDes) { 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; } // calculate the parameters for the frequency sweep from fexcPast to fexc if(Nsweep > 0) calcGPAsweepPara(); } // perfomre the sweep or measure if(j <= Nsweep) { dfexc = afexc*(double)j + bfexc; AexcOut = aAexc*(double)j + bAexc; } 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]; } // secure sinarg starts at 0 (numerically maybe not given) if(j == 1 || j == Nsweep + 1) sinarg = 0.0; // measurement of frequencypoint is finished if(j == Nmeas + Nsweep) { fexcPast = fexc; AexcPast = Aexc; Nsweep = NsweepMin; // calculate the one point dft double Ureal = 2.0*scaleG*(cr*sU[1] - sU[2]); double Uimag = 2.0*scaleG*ci*sU[1]; double Yreal = 2.0*scaleG*(cr*sY[1] - sY[2]); double Yimag = 2.0*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)wrapAngle(atan2(Yimag, Yreal) - atan2(Uimag, Ureal)); float absGyr = (float)(Ymag/Aexc); float angGyr = (float)wrapAngle(atan2(Yimag, Yreal) + piDiv2); // user info printf("%11.4e %9.3e %8.3f %9.3e %8.3f %9.3e %9.3e %9.3e\r\n", (float)fexc, absGyu, angGyu*rad2deg, absGyr, angGyr*rad2deg, Umag, Ymag, (float)Aexc); i += 1; j = 1; } else { j += 1; } // calculate the excitation sinarg = fmod(sinarg + pi2Ts*dfexc, pi2); NmeasTotal += 1; return (float)(AexcOut*sin(sinarg)); } // ----------------------------------------------------------------------------- // private functions // ----------------------------------------------------------------------------- void GPA::assignParameters(int NfexcDes, int NperMin, int NmeasMin, double Ts, int NstartMin, int NsweepMin) { this->NfexcDes = NfexcDes; this->NperMin = NperMin; this->NmeasMin = NmeasMin; this->Ts = Ts; this->NstartMin = NstartMin; this->NsweepMin = NsweepMin; } void GPA::calculateDecreasingAmplitudeCoefficients(double Aexc0, double Aexc1) { // calculate coefficients for decreasing amplitude (1/fexc) this->aAexcDes = (Aexc1 - Aexc0)/(1.0/fexcDes[NfexcDes-1] - 1.0/fexcDes[0]); this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0]; } void GPA::initializeConstants(double Ts) { fnyq = 1.0/2.0/Ts; pi2 = 2.0*pi; pi2Ts = pi2*Ts; piDiv2 = pi/2.0; rad2deg = 180.0f/(float)pi; } void GPA::assignFilterStorage() { sU = (double*)malloc(3*sizeof(double)); sY = (double*)malloc(3*sizeof(double)); } void GPA::fexcDesLogspace(double fMin, double fMax, int NfexcDes) { // calculate logarithmic spaced frequency points double Gain = log10(fMax/fMin)/((double)NfexcDes - 1.0); double expon = 0.0; for(int i = 0; i < NfexcDes; i++) { fexcDes[i] = fMin*pow(10.0, expon); expon += Gain; } } void GPA::calcGPAmeasPara(double fexcDes_i) { // Nmeas has to be an integer Nper = NperMin; Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5); // secure that the minimal number of measurements is fullfilled int Ndelta = NmeasMin - Nmeas; if(Ndelta > 0) { Nper = (int)ceil((double)NmeasMin*fexcDes_i*Ts); Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5); } // evaluating reachable frequency fexc = (double)Nper/(double)Nmeas/Ts; } void GPA::calcGPAsweepPara() { // calculate linear frequency sweep parameters double ksta = ceil(Ts*(double)Nsweep/2.0*(fexc + fexcPast)); Nsweep = (int)floor(2.0*ksta/Ts/(fexc + fexcPast) + 0.5); bfexc = 2.0*ksta/Ts/(double)Nsweep - fexc; afexc = (fexc - bfexc)/((double)Nsweep + 1.0); aAexc = (Aexc - AexcPast)/((double)Nsweep + 1.0); bAexc = AexcPast; } double GPA::wrapAngle(double angle) { // wrap angle from (-2pi,2pi) into (-pi,pi) if(abs(angle) > pi) angle -= copysign(-pi2, angle); // -1*sign(angle)*2*pi + angle; return angle; } void GPA::printLine() { printf("--------------------------------------------------------------------------------\r\n"); } void GPA::printLongLine() { printf("-------------------------------------------------------------------------------------------------------\r\n"); } // ----------------------------------------------------------------------------- // public functions // ----------------------------------------------------------------------------- void GPA::printGPAfexcDes() { printLine(); for(int i = 0; i < NfexcDes; i++) { printf("%9.4f\r\n", (float)fexcDes[i]); } } void GPA::printGPAmeasPara() { printLine(); printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper Nsweep\r\n"); printLine(); for(int i = 0; i < NfexcDes; i++) { calcGPAmeasPara(fexcDes[i]); if(fexc == fexcPast || fexc >= fnyq) { fexc = 0.0; Aexc = 0.0; Nmeas = 0; Nper = 0; Nsweep = 0; afexc = 0.0; bfexc = 0.0; aAexc = 0.0; bAexc = 0.0; } else { Aexc = aAexcDes/fexc + bAexcDes; if(Nsweep > 0) calcGPAsweepPara(); else{ afexc = 0.0; bfexc = 0.0; aAexc = 0.0; bAexc = 0.0; } fexcPast = fexc; AexcPast = Aexc; } NmeasTotal += Nmeas; NmeasTotal += Nsweep; printf("%11.4e %12.4e %10.3e %7i %6i %7i\r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper, Nsweep); Nsweep = NsweepMin; } printGPAmeasTime(); reset(); } void GPA::printFullGPAmeasPara() { printLongLine(); printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper Nsweep afexc bfexc aAexc bAexc\r\n"); printLongLine(); for(int i = 0; i < NfexcDes; i++) { calcGPAmeasPara(fexcDes[i]); if(fexc == fexcPast || fexc >= fnyq) { fexc = 0.0; Aexc = 0.0; Nmeas = 0; Nper = 0; Nsweep = 0; afexc = 0.0; bfexc = 0.0; aAexc = 0.0; bAexc = 0.0; } else { Aexc = aAexcDes/fexc + bAexcDes; if(Nsweep > 0) calcGPAsweepPara(); else{ afexc = 0.0; bfexc = 0.0; aAexc = 0.0; bAexc = 0.0; } fexcPast = fexc; AexcPast = Aexc; } NmeasTotal += Nmeas; NmeasTotal += Nsweep; printf("%11.4e %12.4e %10.3e %7i %6i %7i %10.3e %10.3e %10.3e %10.3e\r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper, Nsweep, (float)afexc, (float)bfexc, (float)aAexc, (float)bAexc); Nsweep = NsweepMin; } 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)((double)NmeasTotal*Ts)); } void GPA::printNfexcDes() { printLine(); printf(" Number of frequancy points: %3i\r\n", NfexcDes); }