GPA.cpp

00001 /*
00002     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
00003 
00004          Hint:        If the plant has a pole at zero, is unstable or weakly damped the measurement has to be perfomed 
00005                       in closed loop (this is NOT tfestimate, the algorithm is based on the one point DFT).
00006          Assumption:  The system is and remains at the desired steady state of interest when the measurment starts
00007 
00008     Instantiate option 0: ("Not a Jedi yet" users, for logarithmic equaly spaced frequency points)
00009     
00010         GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts)
00011         
00012             fMin:       Minimal desired frequency that should be measured in Hz
00013             fMax:       Maximal desired frequency that should be measured in Hz
00014             NfexcDes:   Number of logarithmic equaly spaced frequency points between fMin and fMax
00015             Aexc0:      Excitation amplitude at fMin
00016             Aexc1:      Excitation amplitude at fMax
00017             Ts:         Sampling time in sec
00018             
00019             Default values are as follows:
00020             int NperMin  = 3;
00021             int NmeasMin = (int)ceil(1.0f/Ts);
00022             int Nstart   = (int)ceil(3.0f/Ts);
00023             int Nsweep   = (int)ceil(0.0f/Ts);
00024 
00025     Instantiate option 1: ("Jedi or Sith Lord", for logarithmic equaly spaced frequency points)
00026     
00027         GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep)
00028         
00029             fMin:       Minimal desired frequency that should be measured in Hz
00030             fMax:       Maximal desired frequency that should be measured in Hz
00031             NfexcDes:   Number of logarithmic equaly spaced frequency points
00032             NperMin:    Minimal number of periods that are used for each frequency point
00033             NmeasMin:   Minimal number of samples that are used for each frequency point
00034             Ts:         Sampling time in sec
00035             Aexc0:      Excitation amplitude at fMin
00036             Aexc1:      Excitation amplitude at fMax
00037             Nstart:     Minimal number of samples to sweep to the first frequency point (can be equal 0)
00038             Nsweep:     Minimal number of samples to sweep from freq. point to freq. point (can be equal 0)
00039 
00040  
00041     Instantiate option 2: (for a second, refined frequency grid measurement)
00042     
00043         GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep)
00044         
00045             f0:         Frequency point for the calculation of Aexc0 in Hz (should be chosen like in the first measurement)
00046             f1:         Frequency point for the calculation of Aexc1 in Hz (should be chosen like in the first measurement)
00047             *fexcDes:   Sorted frequency point array in Hz
00048             NfexcDes:   Length of fexcDes
00049             
00050             For the other parameters see above.
00051 
00052     Instantiate option 3: (for an arbitary but sorted frequency grid measurement)
00053     
00054         GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep)
00055         
00056             *fexcDes:   Sorted frequency point array in Hz
00057             Aexc0:      Excitation amplitude at fexcDes[0]
00058             Aexc1:      Excitation amplitude at fexcDes[NfexcDes-1]
00059             NfexcDes:   Length of fexcDes
00060             
00061             For the other parameters see above.
00062 
00063     Note:   The amplitude drops with 1/fexc, if you're using Axc1 = Aexc0/fMax then d/dt exc = const.,
00064             this is recommended if your controller does not have a rolloff. If a desired frequency point
00065             is not measured (could not be reached) try to increase Nmeas.
00066               
00067         
00068     Block diagram:
00069 
00070                 w (const.)    exc(2)                C: controller
00071                 |              |                    P: plant
00072                 v   e          v
00073      exc(1) --> o   ->| C |--->o------->| P |----------> out (y)
00074                 ^ -                |             |
00075                 |                   --> inp (u)  |  exc (R): excitation signal
00076                 |                                |  inp (U): input plant
00077                  --------------------------------   out (Y): output plant
00078               
00079               
00080     Pseudo code for an open loop measurement:
00081 
00082         - Measuring the plant P = Gyu = Gyr:
00083         
00084             u = w + exc;
00085             ... write output u here! it follows exc(k+1) ...
00086             exc = Wobble(exc, y);
00087             
00088             Closed loop FRF calculus with a stabilizing controller C:
00089                 S  = 1/(1 + C*P);  % ( exc1 -> e ,   1/(1 + C*P) ) sensitivity, contr. error rejection, robustness (1/max|S|)
00090                 T  = 1 - S;        % ( w -> y    , C*P/(1 + C*P) ) compl. sensitivity, tracking performance
00091                 CS = C*S;          % ( exc1 -> u ,   C/(1 + C*P) ) control effort, disturbance plant output on plant input
00092                 PS = P*S;          % ( exc2 -> y ,   P/(1 + C*P) ) compliance, disturbance plant input on plant output
00093 
00094 
00095     Pseudo code for a closed loop measurement with stabilizing controller C:
00096 
00097         Excitation at excitation input (1):   
00098         
00099         - Measuring the plant P = Gyu and the closed loop tf T = PC/(1 + PC) = Gyr:
00100         
00101             u = C(w - y + exc);
00102             ... write output u here! it follows exc(k+1) ...
00103             exc = Wobble(u, y);
00104                         
00105             Closed loop FRF calculus:
00106                 S  = 1 - T;
00107                 PS = P*S;
00108                 CS = T/P;
00109                 C  = C/S;
00110 
00111         Excitation at excitation input (2):
00112         
00113         - Measuring the plant P = Gyu and the closed loop tf PS = P/(1 + PC) = Gyr:
00114         
00115             u = C(w - y) + exc;
00116             ... write output u here! it follows exc(k+1) ...
00117             exc = Wobble(u, y);
00118         
00119             Closed loop FRF calculus:
00120                 S  = PS/P;
00121                 T  = 1 - S;
00122                 CS = T/P;
00123                 C  = C/S;
00124 
00125 
00126     Usage:
00127         exc(k+1) = myGPA(inp(k), out(k)) does update the internal states of the 
00128         gpa at the timestep k and returns the excitation signal for the timestep
00129         k+1. The FRF data are plotted to a terminal (Putty) over a serial 
00130         connection and look as follows:
00131         
00132 --------------------------------------------------------------------------------
00133   fexc[Hz]    |Gyu|    deg(Gyu)  |Gyr|    deg(Gyr)   |U|       |Y|       |R|
00134 --------------------------------------------------------------------------------
00135  5.0000e-02 1.001e+00   -0.309 1.001e+00   -0.309 4.000e-01 4.000e-01 4.005e-01
00136     .           .         .        .         .        .         .         .
00137     .           .         .        .         .        .         .         .
00138     .           .         .        .         .        .         .         .
00139   
00140     In Matlab you can use the editor as follows:
00141         data = [... insert measurement data here ...];
00142         gyu = frd(data(:,2).*exp(1i*data(:,3)*pi/180), data(:,1), Ts, 'Units', 'Hz');
00143         gyr = frd(data(:,4).*exp(1i*data(:,5)*pi/180), data(:,1), Ts, 'Units', 'Hz');
00144 
00145     If you're evaluating more than one measurement which contain equal frequency points use:
00146         data = [data1; data2];
00147         [~, ind] = unique(data(:,1), 'stable');
00148         data = data(ind,:);
00149 
00150 
00151     Autor and Copyrigth: 2018 / 2019 / M.E. Peter
00152     
00153 */
00154 
00155 #include "GPA.h"
00156 #include "mbed.h"
00157 #include "math.h"
00158 #define   pi 3.141592653589793
00159 
00160 using namespace std;
00161 
00162 // -----------------------------------------------------------------------------
00163 //      instantiate
00164 // -----------------------------------------------------------------------------
00165 
00166 GPA::GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts)
00167 {
00168     int NperMin = 3;
00169     int NmeasMin = (int)ceil(1.0f/Ts);
00170     int Nstart = (int)ceil(3.0f/Ts);
00171     int Nsweep = (int)ceil(0.0f/Ts);
00172     
00173     doPrint = true;
00174     assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, Nstart, Nsweep);
00175 
00176     // calculate logarithmic spaced frequency points
00177     fexcDes = (double*)malloc(NfexcDes*sizeof(double));
00178     fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
00179 
00180     calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
00181     initializeConstants((double)Ts);
00182     assignFilterStorage();
00183     reset();
00184 }
00185 
00186 GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep)
00187 {
00188     doPrint = true;
00189     assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, Nstart, Nsweep);
00190 
00191     // calculate logarithmic spaced frequency points
00192     fexcDes = (double*)malloc(NfexcDes*sizeof(double));
00193     fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
00194 
00195     calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
00196     initializeConstants((double)Ts);
00197     assignFilterStorage();
00198     reset();
00199 }
00200 
00201 GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep)
00202 {
00203     doPrint = true;
00204     assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, Nstart, Nsweep);
00205     
00206     // convert fexcDes from float to double, it is assumed that it is sorted
00207     this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
00208     for(int i = 0; i < NfexcDes; i++) {
00209         this->fexcDes[i] = (double)fexcDes[i];
00210     }
00211     
00212     calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
00213     initializeConstants((double)Ts);
00214     assignFilterStorage();
00215     reset();
00216 }
00217 
00218 GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep)
00219 {
00220     doPrint = true;
00221     assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, Nstart, Nsweep);
00222     
00223     // convert fexcDes from float to double, it is assumed that it is sorted
00224     this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
00225     for(int i = 0; i < NfexcDes; i++) {
00226         this->fexcDes[i] = (double)fexcDes[i];
00227     }
00228 
00229     calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
00230     initializeConstants((double)Ts);
00231     assignFilterStorage();
00232     reset();
00233 }
00234 
00235 GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep, bool doPrint)
00236 {
00237     this->doPrint = doPrint;
00238     assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, Nstart, Nsweep);
00239 
00240     // calculate logarithmic spaced frequency points
00241     fexcDes = (double*)malloc(NfexcDes*sizeof(double));
00242     fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
00243 
00244     calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
00245     initializeConstants((double)Ts);
00246     assignFilterStorage();
00247     reset();
00248 }
00249 
00250 // -----------------------------------------------------------------------------
00251 //      virtual and reset
00252 // -----------------------------------------------------------------------------
00253 
00254 GPA::~GPA() {}
00255 
00256 void GPA::reset()
00257 {
00258     Nmeas = 0;
00259     Nper = 0;
00260     dfexc = 0.0;
00261     fexc = 0.0;
00262     fexcPast = 0.0;
00263     dfexcj = 0.0;
00264     i = 1; // iterating through desired frequency points
00265     j = 1; // iterating through measurement points w.r.t. reachable frequency
00266     scaleG = 0.0;
00267     cr = 0.0;
00268     ci = 0.0;
00269     for(int i = 0; i < 3; i++) {
00270         sU[i] = 0.0;
00271         sY[i] = 0.0;
00272     }
00273     sinarg = 0.0;
00274     sinargR = 0.0;
00275     NmeasTotal = 0;
00276     Aexc = 0.0;
00277     pi2Tsfexc = 0.0;
00278     Nsweep_i = Nstart;
00279     AexcOut = 0.0;
00280     gpaData.fexc = 0.0f;
00281     gpaData.absGyu = 0.0f;
00282     gpaData.angGyu = 0.0f;
00283     gpaData.absGyr = 0.0f;
00284     gpaData.angGyr = 0.0f;
00285     gpaData.Umag = 0.0f;
00286     gpaData.Ymag = 0.0f;
00287     gpaData.Rmag = 0.0f;
00288     gpaData.MeasPointFinished = false;
00289     
00290 }
00291 
00292 // -----------------------------------------------------------------------------
00293 //      update (operator)
00294 // -----------------------------------------------------------------------------  
00295 
00296 float GPA::update(double inp, double out)
00297 {
00298     // a new frequency point has been reached
00299     if(j == 1) {
00300         // user info
00301         if(i == 1 & doPrint) {
00302             printLine();
00303             printf("  fexc[Hz]    |Gyu|    deg(Gyu)  |Gyr|    deg(Gyr)   |U|       |Y|       |R|\r\n");
00304             printLine();
00305         }
00306         // get a new unique frequency point
00307         while(fexc == fexcPast) {
00308             // measurement finished
00309             if(i > NfexcDes) {
00310                 gpaData.MeasPointFinished = false;
00311                 return 0.0f;
00312             }
00313             calcGPAmeasPara(fexcDes[i - 1]);
00314             // secure fexc is not higher or equal to nyquist frequency
00315             if(fexc >= fnyq) {
00316                 fexc = fexcPast;
00317             }
00318             // no frequency found
00319             if(fexc == fexcPast) {
00320                 i += 1;
00321             } else {
00322                 Aexc = aAexcDes/fexc + bAexcDes;
00323                 pi2Tsfexc = pi2Ts*fexc;
00324             }
00325         }
00326         // filter scaling
00327         scaleG = 1.0/sqrt((double)Nmeas);
00328         // filter coefficients
00329         cr = cos(pi2Tsfexc);
00330         ci = sin(pi2Tsfexc);
00331         // set filter storage zero
00332         for(int i = 0; i < 3; i++) {
00333             sU[i] = 0.0;
00334             sY[i] = 0.0;
00335         }
00336         gpaData.MeasPointFinished = false;
00337     }
00338     // perfomre the sweep or measure
00339     if(j <= Nsweep_i) {
00340         dfexcj = ((double)j - 1.0)/((double)Nsweep_i - 1.0);
00341         dfexcj = div12pi*sin(pi4*dfexcj) - div812pi*sin(pi2*dfexcj) + dfexcj;
00342         dfexc = fexcPast + (fexc - fexcPast)*dfexcj;
00343         AexcOut = AexcPast + (Aexc - AexcPast)*dfexcj;
00344     } else {
00345         dfexc = fexc;
00346         AexcOut = Aexc;
00347         // one point DFT filter step for signal su
00348         sU[0] = scaleG*inp + 2.0*cr*sU[1] - sU[2];
00349         sU[2] = sU[1];
00350         sU[1] = sU[0];
00351         // one point DFT filter step for signal sy
00352         sY[0] = scaleG*out + 2.0*cr*sY[1] - sY[2];
00353         sY[2] = sY[1];
00354         sY[1] = sY[0];
00355     }
00356     // copy starting value for ang(R)
00357     if(j == 1 || j == Nsweep_i + 1) sinargR = sinarg;
00358     // measurement of frequencypoint is finished
00359     if(j == Nmeas + Nsweep_i) {
00360         fexcPast = fexc;
00361         AexcPast = Aexc;
00362         Nsweep_i = Nsweep;
00363         // calculate the one point dft
00364         double Ureal = 2.0*scaleG*(cr*sU[1] - sU[2]);
00365         double Uimag = 2.0*scaleG*ci*sU[1];
00366         double Yreal = 2.0*scaleG*(cr*sY[1] - sY[2]);
00367         double Yimag = 2.0*scaleG*ci*sY[1];
00368         // calculate magnitude and angle
00369         double Umag = sqrt(Ureal*Ureal + Uimag*Uimag);
00370         double Ymag = sqrt(Yreal*Yreal + Yimag*Yimag);
00371         gpaData.fexc = (float)fexc;
00372         gpaData.absGyu = (float)(Ymag/Umag);
00373         gpaData.angGyu = (float)(wrapAngle(atan2(Yimag, Yreal) - atan2(Uimag, Ureal))*rad2deg);
00374         gpaData.absGyr = (float)(Ymag/Aexc);
00375         gpaData.angGyr = (float)(wrapAngle(atan2(Yimag, Yreal) + fmod(piDiv2 - sinargR, pi2))*rad2deg);
00376         gpaData.Umag = (float)(sqrt(Ureal*Ureal + Uimag*Uimag));
00377         gpaData.Ymag = (float)(sqrt(Yreal*Yreal + Yimag*Yimag));
00378         gpaData.Rmag = (float)Aexc;
00379         gpaData.MeasPointFinished = true;
00380         // user info
00381         if(doPrint) {
00382         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);
00383         }
00384         i += 1;
00385         j = 1;
00386     } else {
00387         j += 1;
00388     }
00389     // calculate the excitation
00390     sinarg = fmod(sinarg + pi2Ts*dfexc, pi2);
00391     NmeasTotal += 1;
00392     return (float)(AexcOut*sin(sinarg));
00393 }
00394 
00395 // -----------------------------------------------------------------------------
00396 //      private functions
00397 // -----------------------------------------------------------------------------  
00398 
00399 void GPA::assignParameters(int NfexcDes, int NperMin, int NmeasMin, double Ts, int Nstart, int Nsweep)
00400 {
00401     this->NfexcDes = NfexcDes;
00402     this->NperMin = NperMin;
00403     this->NmeasMin = NmeasMin;
00404     this->Ts = Ts;
00405     this->Nstart = Nstart;
00406     this->Nsweep = Nsweep;   
00407 }
00408 
00409 void GPA::calculateDecreasingAmplitudeCoefficients(double Aexc0, double Aexc1)
00410 {
00411     // calculate coefficients for decreasing amplitude (1/fexc)
00412     this->aAexcDes = (Aexc1 - Aexc0)/(1.0/fexcDes[NfexcDes-1] - 1.0/fexcDes[0]);
00413     this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0];   
00414 }
00415 
00416 void GPA::initializeConstants(double Ts)
00417 {
00418     fnyq = 1.0/2.0/Ts;
00419     pi2 = 2.0*pi;
00420     pi4 = 4.0*pi;
00421     pi2Ts = 2.0*pi*Ts;
00422     piDiv2 = pi/2.0;
00423     rad2deg = 180.0/pi;
00424     div12pi = 1.0/(12.0*pi);
00425     div812pi = 8.0/(12.0*pi);
00426 }
00427 
00428 void GPA::assignFilterStorage()
00429 {
00430     sU = (double*)malloc(3*sizeof(double));
00431     sY = (double*)malloc(3*sizeof(double));   
00432 }
00433 
00434 void GPA::fexcDesLogspace(double fMin, double fMax, int NfexcDes)
00435 {
00436     // calculate logarithmic spaced frequency points
00437     double Gain = log10(fMax/fMin)/((double)NfexcDes - 1.0);
00438     double expon = 0.0;
00439     for(int i = 0; i < NfexcDes; i++) {
00440         fexcDes[i] = fMin*pow(10.0, expon);
00441         expon += Gain;
00442     }
00443 }
00444 
00445 void GPA::calcGPAmeasPara(double fexcDes_i)
00446 {
00447     // Nmeas has to be an integer
00448     Nper = NperMin;
00449     Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5);
00450     //  secure that the minimal number of measurements is fullfilled
00451     int Ndelta = NmeasMin - Nmeas;
00452     if(Ndelta > 0) {
00453         Nper = (int)ceil((double)NmeasMin*fexcDes_i*Ts);
00454         Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5);
00455     }
00456     // evaluating reachable frequency
00457     fexc = (double)Nper/(double)Nmeas/Ts;
00458 }
00459 
00460 double GPA::wrapAngle(double angle)
00461 {
00462     // wrap angle from (-2pi,2pi) into (-pi,pi)
00463     if(abs(angle) > pi) angle -= copysign(-pi2, angle); // -1*sign(angle)*2*pi + angle;
00464     return angle;
00465 }
00466 
00467 void GPA::printLongLine()
00468 {
00469     printf("-------------------------------------------------------------------------------------------------------\r\n");
00470 }
00471 
00472 // -----------------------------------------------------------------------------
00473 //      public functions (mainly for debugging)
00474 // -----------------------------------------------------------------------------     
00475 
00476 void GPA::printGPAfexcDes()
00477 {
00478     printLine();
00479     for(int i = 0; i < NfexcDes; i++) {
00480         printf("%9.4f\r\n", (float)fexcDes[i]);
00481     }
00482 }
00483 
00484 void GPA::printGPAmeasPara()
00485 {
00486     printLine();
00487     printf(" fexcDes[Hz]   fexc[Hz]     Aexc      Nmeas   Nper  Nsweep_i\r\n");
00488     printLine();
00489     for(int i = 0; i < NfexcDes; i++) {
00490         calcGPAmeasPara(fexcDes[i]);
00491         if(fexc == fexcPast || fexc >= fnyq) {
00492             fexc = 0.0;
00493             Aexc = 0.0;
00494             Nmeas = 0;
00495             Nper = 0;
00496             Nsweep_i = 0;
00497         } else {
00498             Aexc = aAexcDes/fexc + bAexcDes;
00499             fexcPast = fexc;
00500             AexcPast = Aexc;
00501         }
00502         NmeasTotal += Nmeas;
00503         NmeasTotal += Nsweep_i;
00504         printf("%11.4e %12.4e %10.3e %7i %6i %7i\r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper, Nsweep_i);
00505         Nsweep_i = Nsweep;
00506     }
00507     printGPAmeasTime();
00508     reset();
00509 }
00510 
00511 void GPA::printGPAmeasTime()
00512 {
00513     printLine();
00514     printf(" Number of data points :  %11i\r\n", NmeasTotal);
00515     printf(" Measurment time in sec: %12.2f\r\n", (float)((double)NmeasTotal*Ts));
00516 }
00517 
00518 void GPA::printNfexcDes()
00519 {
00520     printLine();
00521     printf("  Number of frequancy points:  %3i\r\n", NfexcDes);
00522 }
00523 
00524 void GPA::printLine()
00525 {
00526     printf("--------------------------------------------------------------------------------\r\n");
00527 }
00528 
00529 GPA::gpadata_t GPA::getGPAdata()
00530 {
00531     return gpaData;
00532 }