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 NstartMin = (int)ceil(3.0f/Ts);
00023             int NsweepMin = 0;
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 NstartMin, int NsweepMin)
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             NstartMin:  Minimal number of samples to sweep to the first frequency point (can be equal 0)
00038             NsweepMin:  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 NstartMin, int NsweepMin)
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 NstartMin, int NsweepMin)
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) ) contr. error rejection, robustness (1/max|S|)
00090                 T  = 1 - S;        % ( w -> y    , C*P/(1 + C*P) ) tracking
00091                 CS = C*S;          % ( exc1 -> u ,   C/(1 + C*P) ) disturbance plant output
00092                 PS = P*S;          % ( exc2 -> y ,   P/(1 + C*P) ) disturbance plant input
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 NstartMin = (int)ceil(3.0f/Ts);
00171     int NsweepMin = 0;
00172     
00173     assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
00174 
00175     // calculate logarithmic spaced frequency points
00176     fexcDes = (double*)malloc(NfexcDes*sizeof(double));
00177     fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
00178 
00179     calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
00180     initializeConstants((double)Ts);
00181     assignFilterStorage();
00182     reset();
00183 }
00184 
00185 GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
00186 {
00187     assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
00188 
00189     // calculate logarithmic spaced frequency points
00190     fexcDes = (double*)malloc(NfexcDes*sizeof(double));
00191     fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
00192 
00193     calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
00194     initializeConstants((double)Ts);
00195     assignFilterStorage();
00196     reset();
00197 }
00198 
00199 GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
00200 {
00201     assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
00202     
00203     // convert fexcDes from float to double, it is assumed that it is sorted
00204     this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
00205     for(int i = 0; i < NfexcDes; i++) {
00206         this->fexcDes[i] = (double)fexcDes[i];
00207     }
00208     
00209     calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
00210     initializeConstants((double)Ts);
00211     assignFilterStorage();
00212     reset();
00213 }
00214 
00215 GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
00216 {
00217     assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
00218     
00219     // convert fexcDes from float to double, it is assumed that it is sorted
00220     this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
00221     for(int i = 0; i < NfexcDes; i++) {
00222         this->fexcDes[i] = (double)fexcDes[i];
00223     }
00224 
00225     calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
00226     initializeConstants((double)Ts);
00227     assignFilterStorage();
00228     reset();
00229 }
00230 
00231 // -----------------------------------------------------------------------------
00232 //      virtual and reset
00233 // -----------------------------------------------------------------------------
00234 
00235 GPA::~GPA() {}
00236 
00237 void GPA::reset()
00238 {
00239     Nmeas = 0;
00240     Nper = 0;
00241     dfexc = 0.0;
00242     fexc = 0.0;
00243     fexcPast = 0.0;
00244     i = 1; // iterating through desired frequency points
00245     j = 1; // iterating through measurement points w.r.t. reachable frequency
00246     scaleG = 0.0;
00247     cr = 0.0;
00248     ci = 0.0;
00249     for(int i = 0; i < 3; i++) {
00250         sU[i] = 0.0;
00251         sY[i] = 0.0;
00252     }
00253     sinarg = 0.0;
00254     NmeasTotal = 0;
00255     Aexc = 0.0;
00256     pi2Tsfexc = 0.0;
00257     Nsweep = NstartMin;
00258     bfexc = 0.0;
00259     afexc = 0.0;
00260     aAexc = 0.0;
00261     bAexc = 0.0;
00262     AexcOut = 0.0;
00263 }
00264 
00265 // -----------------------------------------------------------------------------
00266 //      update (operator)
00267 // -----------------------------------------------------------------------------  
00268 
00269 float GPA::update(double inp, double out)
00270 {
00271     // a new frequency point has been reached
00272     if(j == 1) {
00273         // user info
00274         if(i == 1) {
00275             printLine();
00276             printf("  fexc[Hz]    |Gyu|    deg(Gyu)  |Gyr|    deg(Gyr)   |U|       |Y|       |R|\r\n");
00277             printLine();
00278         }
00279         // get a new unique frequency point
00280         while(fexc == fexcPast) {
00281             // measurement finished
00282             if(i > NfexcDes) {
00283                 return 0.0f;
00284             }
00285             calcGPAmeasPara(fexcDes[i - 1]);
00286             // secure fexc is not higher or equal to nyquist frequency
00287             if(fexc >= fnyq) {
00288                 fexc = fexcPast;
00289             }
00290             // no frequency found
00291             if(fexc == fexcPast) {
00292                 i += 1;
00293             } else {
00294                 Aexc = aAexcDes/fexc + bAexcDes;
00295                 pi2Tsfexc = pi2Ts*fexc;
00296             }
00297         }
00298         // filter scaling
00299         scaleG = 1.0/sqrt((double)Nmeas);
00300         // filter coefficients
00301         cr = cos(pi2Tsfexc);
00302         ci = sin(pi2Tsfexc);
00303         // set filter storage zero
00304         for(int i = 0; i < 3; i++) {
00305             sU[i] = 0.0;
00306             sY[i] = 0.0;
00307         }
00308         // calculate the parameters for the frequency sweep from fexcPast to fexc
00309         if(Nsweep > 0) calcGPAsweepPara();
00310     }
00311     // perfomre the sweep or measure
00312     if(j <= Nsweep) {
00313         dfexc = afexc*(double)j + bfexc;
00314         AexcOut = aAexc*(double)j + bAexc;   
00315     } else {
00316         dfexc = fexc;
00317         AexcOut = Aexc;
00318         // one point DFT filter step for signal su
00319         sU[0] = scaleG*inp + 2.0*cr*sU[1] - sU[2];
00320         sU[2] = sU[1];
00321         sU[1] = sU[0];
00322         // one point DFT filter step for signal sy
00323         sY[0] = scaleG*out + 2.0*cr*sY[1] - sY[2];
00324         sY[2] = sY[1];
00325         sY[1] = sY[0];
00326     }
00327     // secure sinarg starts at 0 (numerically maybe not given)
00328     if(j == 1 || j == Nsweep + 1) sinarg = 0.0;
00329     // measurement of frequencypoint is finished
00330     if(j == Nmeas + Nsweep) {
00331         fexcPast = fexc;
00332         AexcPast = Aexc;
00333         Nsweep = NsweepMin;
00334         // calculate the one point dft
00335         double Ureal = 2.0*scaleG*(cr*sU[1] - sU[2]);
00336         double Uimag = 2.0*scaleG*ci*sU[1];
00337         double Yreal = 2.0*scaleG*(cr*sY[1] - sY[2]);
00338         double Yimag = 2.0*scaleG*ci*sY[1];
00339         // calculate magnitude and angle
00340         float Umag = (float)(sqrt(Ureal*Ureal + Uimag*Uimag));
00341         float Ymag = (float)(sqrt(Yreal*Yreal + Yimag*Yimag));
00342         float absGyu = (float)(Ymag/Umag);
00343         float angGyu = (float)wrapAngle(atan2(Yimag, Yreal) - atan2(Uimag, Ureal));
00344         float absGyr = (float)(Ymag/Aexc);
00345         float angGyr = (float)wrapAngle(atan2(Yimag, Yreal) + piDiv2);
00346         // user info
00347         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);
00348         i += 1;
00349         j = 1;
00350     } else {
00351         j += 1;
00352     }
00353     // calculate the excitation
00354     sinarg = fmod(sinarg + pi2Ts*dfexc, pi2);
00355     NmeasTotal += 1;
00356     return (float)(AexcOut*sin(sinarg));
00357 }
00358 
00359 // -----------------------------------------------------------------------------
00360 //      private functions
00361 // -----------------------------------------------------------------------------  
00362 
00363 void GPA::assignParameters(int NfexcDes, int NperMin, int NmeasMin, double Ts, int NstartMin, int NsweepMin)
00364 {
00365     this->NfexcDes = NfexcDes;
00366     this->NperMin = NperMin;
00367     this->NmeasMin = NmeasMin;
00368     this->Ts = Ts;
00369     this->NstartMin = NstartMin;
00370     this->NsweepMin = NsweepMin;   
00371 }
00372 
00373 void GPA::calculateDecreasingAmplitudeCoefficients(double Aexc0, double Aexc1)
00374 {
00375     // calculate coefficients for decreasing amplitude (1/fexc)
00376     this->aAexcDes = (Aexc1 - Aexc0)/(1.0/fexcDes[NfexcDes-1] - 1.0/fexcDes[0]);
00377     this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0];   
00378 }
00379 
00380 void GPA::initializeConstants(double Ts)
00381 {
00382     fnyq = 1.0/2.0/Ts;
00383     pi2 = 2.0*pi;
00384     pi2Ts = pi2*Ts;
00385     piDiv2 = pi/2.0;
00386     rad2deg = 180.0f/(float)pi;
00387 }
00388 
00389 void GPA::assignFilterStorage()
00390 {
00391     sU = (double*)malloc(3*sizeof(double));
00392     sY = (double*)malloc(3*sizeof(double));   
00393 }
00394 
00395 void GPA::fexcDesLogspace(double fMin, double fMax, int NfexcDes)
00396 {
00397     // calculate logarithmic spaced frequency points
00398     double Gain = log10(fMax/fMin)/((double)NfexcDes - 1.0);
00399     double expon = 0.0;
00400     for(int i = 0; i < NfexcDes; i++) {
00401         fexcDes[i] = fMin*pow(10.0, expon);
00402         expon += Gain;
00403     }
00404 }
00405 
00406 void GPA::calcGPAmeasPara(double fexcDes_i)
00407 {
00408     // Nmeas has to be an integer
00409     Nper = NperMin;
00410     Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5);
00411     //  secure that the minimal number of measurements is fullfilled
00412     int Ndelta = NmeasMin - Nmeas;
00413     if(Ndelta > 0) {
00414         Nper = (int)ceil((double)NmeasMin*fexcDes_i*Ts);
00415         Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5);
00416     }
00417     // evaluating reachable frequency
00418     fexc = (double)Nper/(double)Nmeas/Ts;
00419 }
00420 
00421 void GPA::calcGPAsweepPara()
00422 {
00423     // calculate linear frequency sweep parameters
00424     double ksta = ceil(Ts*(double)Nsweep/2.0*(fexc + fexcPast));
00425     Nsweep = (int)floor(2.0*ksta/Ts/(fexc + fexcPast) + 0.5);
00426     bfexc = 2.0*ksta/Ts/(double)Nsweep - fexc;
00427     afexc = (fexc - bfexc)/((double)Nsweep + 1.0);
00428     aAexc = (Aexc - AexcPast)/((double)Nsweep + 1.0);
00429     bAexc = AexcPast;
00430 }
00431 
00432 double GPA::wrapAngle(double angle)
00433 {
00434     // wrap angle from (-2pi,2pi) into (-pi,pi)
00435     if(abs(angle) > pi) angle -= copysign(-pi2, angle); // -1*sign(angle)*2*pi + angle;
00436     return angle;
00437 }
00438 
00439 void GPA::printLine()
00440 {
00441     printf("--------------------------------------------------------------------------------\r\n");
00442 }
00443 
00444 void GPA::printLongLine()
00445 {
00446     printf("-------------------------------------------------------------------------------------------------------\r\n");
00447 }
00448 
00449 // -----------------------------------------------------------------------------
00450 //      public functions
00451 // -----------------------------------------------------------------------------     
00452 
00453 void GPA::printGPAfexcDes()
00454 {
00455     printLine();
00456     for(int i = 0; i < NfexcDes; i++) {
00457         printf("%9.4f\r\n", (float)fexcDes[i]);
00458     }
00459 }
00460 
00461 void GPA::printGPAmeasPara()
00462 {
00463     printLine();
00464     printf(" fexcDes[Hz]   fexc[Hz]     Aexc      Nmeas   Nper  Nsweep\r\n");
00465     printLine();
00466     for(int i = 0; i < NfexcDes; i++) {
00467         calcGPAmeasPara(fexcDes[i]);
00468         if(fexc == fexcPast || fexc >= fnyq) {
00469             fexc = 0.0;
00470             Aexc = 0.0;
00471             Nmeas = 0;
00472             Nper = 0;
00473             Nsweep = 0;
00474             afexc = 0.0;
00475             bfexc = 0.0;
00476             aAexc = 0.0;
00477             bAexc = 0.0;
00478             
00479         } else {
00480             Aexc = aAexcDes/fexc + bAexcDes;
00481             if(Nsweep > 0)  calcGPAsweepPara();
00482             else{
00483                 afexc = 0.0;
00484                 bfexc = 0.0;
00485                 aAexc = 0.0;
00486                 bAexc = 0.0; 
00487             }
00488             fexcPast = fexc;
00489             AexcPast = Aexc;
00490         }
00491         NmeasTotal += Nmeas;
00492         NmeasTotal += Nsweep;
00493         printf("%11.4e %12.4e %10.3e %7i %6i %7i\r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper, Nsweep);
00494         Nsweep = NsweepMin;
00495     }
00496     printGPAmeasTime();
00497     reset();
00498 }
00499 
00500 void GPA::printFullGPAmeasPara()
00501 {
00502     printLongLine();
00503     printf(" fexcDes[Hz]   fexc[Hz]     Aexc      Nmeas   Nper  Nsweep    afexc      bfexc      aAexc      bAexc\r\n");
00504     printLongLine();
00505     for(int i = 0; i < NfexcDes; i++) {
00506         calcGPAmeasPara(fexcDes[i]);
00507         if(fexc == fexcPast || fexc >= fnyq) {
00508             fexc = 0.0;
00509             Aexc = 0.0;
00510             Nmeas = 0;
00511             Nper = 0;
00512             Nsweep = 0;
00513             afexc = 0.0;
00514             bfexc = 0.0;
00515             aAexc = 0.0;
00516             bAexc = 0.0;
00517             
00518         } else {
00519             Aexc = aAexcDes/fexc + bAexcDes;
00520             if(Nsweep > 0)  calcGPAsweepPara();
00521             else{
00522                 afexc = 0.0;
00523                 bfexc = 0.0;
00524                 aAexc = 0.0;
00525                 bAexc = 0.0; 
00526             }
00527             fexcPast = fexc;
00528             AexcPast = Aexc;
00529         }
00530         NmeasTotal += Nmeas;
00531         NmeasTotal += Nsweep;
00532         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);
00533         Nsweep = NsweepMin;
00534     }
00535     printGPAmeasTime();
00536     reset();
00537 }
00538 
00539 void GPA::printGPAmeasTime()
00540 {
00541     printLine();
00542     printf(" Number of data points :  %11i\r\n", NmeasTotal);
00543     printf(" Measurment time in sec: %12.2f\r\n", (float)((double)NmeasTotal*Ts));
00544 }
00545 
00546 void GPA::printNfexcDes()
00547 {
00548     printLine();
00549     printf("  Number of frequancy points:  %3i\r\n", NfexcDes);
00550 }