GPA.cpp

00001 /*
00002     GPA: frequency point wise gain and phase analyser to measure the frequency
00003          respone of a dynamical system
00004 
00005         hint:       the measurements should only be perfomed in closed loop
00006         assumption: the system is at the desired steady state of interest when
00007                     the measurment starts
00008 
00009                              exc(2)                 C: controller
00010                                |                    P: plant
00011                                v
00012      exc(1) --> o   ->| C |--->o------->| P |----------> out
00013                 ^ -                |             |
00014                 |                   --> inp      |  exc: excitation signal (E)
00015                 |                                |  inp: input plant (U)
00016                  --------------------------------   out: output plant (Y)
00017 
00018     instantiate option 1: (logarithmic equaly spaced frequency points)
00019     
00020     GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
00021     
00022         fMin:     minimal desired frequency that should be measured in Hz
00023         fMax:     maximal desired frequency that should be measured in Hz
00024         NfexcDes: number of logarithmic equaly spaced frequency points
00025         NperMin:  minimal number of periods that are used for each frequency point
00026         NmeasMin: minimal number of samples that are used for each frequency point
00027         Ts:       sampling time
00028         Aexc0:    excitation amplitude at fMin
00029         Aexc1:    excitation amplitude at fMax
00030            
00031     instantiate option 2: (for a second, refined frequency grid measurement)
00032     
00033     GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
00034     
00035         f0:       frequency point for the calculation of Aexc0 in Hz (should be chosen like in the first measurement)
00036         f1:       frequency point for the calculation of Aexc1 in Hz (should be chosen like in the first measurement)
00037         *fexcDes: sorted frequency point array in Hz
00038         NfexcDes: length of fexcDes
00039         
00040     instantiate option 3: (for an arbitary but sorted frequency grid measurement)
00041     
00042     GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
00043     
00044         *fexcDes: sorted frequency point array in Hz
00045         Aexc0:    excitation amplitude at fexcDes[0]
00046         Aexc1:    excitation amplitude at fexcDes[NfexcDes-1]
00047         NfexcDes: length of fexcDes
00048 
00049     hints:    the amplitude drops with 1/fexc, if you're using Axc1 = Aexc0/fMax then d/dt exc = const.,
00050               this is recommended if your controller does not have a rolloff. if a desired frequency point
00051               is not measured try to increase Nmeas.
00052 
00053     pseudo code for a closed loop measurement with a controller C:
00054 
00055         excitation input at (1):
00056        
00057         - measuring the plant P and the closed loop tf T = PC/(1 + PC):
00058             desTorque = pi_w(omega_desired - omega + excWobble);
00059             inpWobble = desTorque;
00060             outWobble = omega;
00061             excWobble = Wobble(excWobble, outWobble);
00062                         
00063         - measuring the controller C and the closed loop tf SC = C/(1 + PC)
00064             desTorque = pi_w(omega_desired - omega + excWobble);
00065             inpWobble = n_soll + excWobble - omega;
00066             outWobble = desTorque;
00067             excWobble = Wobble(inpWobble, outWobble);
00068 
00069         excitation input at (2):
00070         
00071         - measuring the plant P and the closed loop tf SP = P/(1 + PC):
00072             desTorque = pi_w(omega_desired - omega) + excWobble;
00073             inpWobble = desTorque;
00074             outWobble = omega;
00075             excWobble = Wobble(excWobble, outWobble);    
00076 
00077     usage:
00078         exc(k+1) = myGPA(inp(k), out(k)) does update the internal states of the 
00079         gpa at the timestep k and returns the excitation signal for the timestep
00080         k+1. the results are plotted to a terminal (putty) over a serial 
00081         connection and look as follows:
00082  -----------------------------------------------------------------------------------------
00083    fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|
00084  -----------------------------------------------------------------------------------------
00085    1.000e+00  2.924e+01 -1.572e+00  1.017e+00 -4.983e-02  5.000e+00  1.739e-01  5.084e+00
00086 
00087     in matlab you can use:
00088         dataNucleo = [... insert measurement data here ...];
00089         g = frd(dataNucleo(:,2).*exp(1i*dataNucleo(:,3)), dataNucleo(:,1), Ts, 'Units', 'Hz');
00090         gcl = frd(dataNucleo(:,4).*exp(1i*dataNucleo(:,5)), dataNucleo(:,1), Ts, 'Units', 'Hz');
00091 
00092     if you're evaluating more than one measurement which contain equal frequency points try:
00093         dataNucleo = [dataNucleo1; dataNucleo2];
00094         [~, ind] = unique(dataNucleo(:,1),'stable');
00095         dataNucleo = dataNucleo(ind,:);
00096 
00097         autor: M.E. Peter
00098 */
00099 
00100 #include "GPA.h"
00101 #include "mbed.h"
00102 #include "math.h"
00103 #define   pi 3.141592653589793
00104 
00105 using namespace std;
00106 
00107 GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
00108 {
00109     this->NfexcDes = NfexcDes;
00110     this->NperMin = NperMin;
00111     this->NmeasMin = NmeasMin;
00112     this->Ts = (double)Ts;
00113 
00114     // calculate logarithmic spaced frequency points
00115     fexcDes = (double*)malloc(NfexcDes*sizeof(double));
00116     fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
00117 
00118     // calculate coefficients for decreasing amplitude (1/fexc)
00119     this->aAexcDes = ((double)Aexc1 - (double)Aexc0)/(1.0/fexcDes[NfexcDes-1] - 1.0/fexcDes[0]);
00120     this->bAexcDes = (double)Aexc0 - aAexcDes/fexcDes[0];
00121 
00122     fnyq = 1.0/2.0/(double)Ts;
00123     pi2 = 2.0*pi;
00124     pi2Ts = pi2*(double)Ts;
00125     piDiv2 = pi/2.0;
00126     
00127     sU = (double*)malloc(3*sizeof(double));
00128     sY = (double*)malloc(3*sizeof(double));
00129     reset();
00130 }
00131 
00132 GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
00133 {
00134     // convert fexcDes from float to double, it is assumed that it is sorted
00135     this->NfexcDes = NfexcDes;
00136     this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
00137     for(int i = 0; i < NfexcDes; i++) {
00138         this->fexcDes[i] = (double)fexcDes[i];
00139     }
00140     this->NperMin = NperMin;
00141     this->NmeasMin = NmeasMin;
00142     this->Ts = (double)Ts;
00143 
00144     // calculate coefficients for decreasing amplitude (1/fexc)
00145     this->aAexcDes = ((double)Aexc1 - (double)Aexc0)/(1.0/(double)f1 - 1.0/(double)f0);
00146     this->bAexcDes = (double)Aexc0 - aAexcDes/fexcDes[0];
00147 
00148     fnyq = 1.0/2.0/(double)Ts;
00149     pi2 = 2.0*pi;
00150     pi2Ts = pi2*(double)Ts;
00151     piDiv2 = pi/2.0;
00152 
00153     sU = (double*)malloc(3*sizeof(double));
00154     sY = (double*)malloc(3*sizeof(double));
00155     reset();
00156 }
00157 
00158 GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
00159 {
00160     // convert fexcDes from float to double, it is assumed that it is sorted
00161     this->NfexcDes = NfexcDes;
00162     this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
00163     for(int i = 0; i < NfexcDes; i++) {
00164         this->fexcDes[i] = (double)fexcDes[i];
00165     }
00166     this->NperMin = NperMin;
00167     this->NmeasMin = NmeasMin;
00168     this->Ts = (double)Ts;
00169 
00170     // calculate coefficients for decreasing amplitude (1/fexc)
00171     this->aAexcDes = ((double)Aexc1 - (double)Aexc0)/(1.0/this->fexcDes[NfexcDes-1] - 1.0/this->fexcDes[0]);
00172     this->bAexcDes = (double)Aexc0 - aAexcDes/fexcDes[0];
00173 
00174     fnyq = 1.0/2.0/(double)Ts;
00175     pi2 = 2.0*pi;
00176     pi2Ts = pi2*(double)Ts;
00177     piDiv2 = pi/2.0;
00178 
00179     sU = (double*)malloc(3*sizeof(double));
00180     sY = (double*)malloc(3*sizeof(double));
00181     reset();
00182 }
00183 
00184 GPA::~GPA() {}
00185 
00186 void GPA::reset()
00187 {
00188     Nmeas = 0;
00189     Nper = 0;
00190     fexc = 0.0;
00191     fexcPast = 0.0;
00192     ii = 1; // iterating through desired frequency points
00193     jj = 1; // iterating through measurement points w.r.t. reachable frequency
00194     scaleG = 0.0;
00195     scaleH = 2.0;
00196     wk = 0.0;
00197     cr = 0.0;
00198     ci = 0.0;
00199     for(int i = 0; i < 3; i++) {
00200         sU[i] = 0.0;
00201         sY[i] = 0.0;
00202     }
00203     sinarg = 0.0;
00204     NmeasTotal = 0;
00205     Aexc = 0.0;
00206     pi2Tsfexc = 0.0;
00207 }
00208 
00209 float GPA::update(double inp, double out)
00210 {
00211     // a new frequency point has been reached
00212     if(jj == 1) { 
00213         // get a new unique frequency point
00214         while(fexc == fexcPast) {
00215             // measurement finished
00216             if(ii > NfexcDes) {
00217                 return 0.0f;
00218             }
00219             calcGPAmeasPara(fexcDes[ii - 1]);
00220             // secure fexc is not higher or equal to nyquist frequency
00221             if(fexc >= fnyq) {
00222                 fexc = fexcPast;
00223             }
00224             // no frequency found
00225             if(fexc == fexcPast) {
00226                 ii += 1;
00227             } else {
00228                 Aexc = aAexcDes/fexc + bAexcDes;
00229                 pi2Tsfexc = pi2Ts*fexc;
00230             }
00231         }
00232         // secure sinarg starts at 0 (numerically maybe not given)
00233         sinarg = 0.0;
00234         // filter scaling
00235         scaleG = 1.0/sqrt((double)Nmeas);
00236         // filter coefficients
00237         cr = cos(pi2Tsfexc);
00238         ci = sin(pi2Tsfexc);
00239         // filter storage
00240         for(int i = 0; i < 3; i++) {
00241             sU[i] = 0.0;
00242             sY[i] = 0.0;
00243         }
00244     }
00245     // update hann window
00246     calcHann();
00247     // filter step for signal su
00248     sU[0] = wk*scaleG*inp + 2.0*cr*sU[1] - sU[2];
00249     sU[2] = sU[1];
00250     sU[1] = sU[0];
00251     // filter step for signal sy
00252     sY[0] = wk*scaleG*out + 2.0*cr*sY[1] - sY[2];
00253     sY[2] = sY[1];
00254     sY[1] = sY[0];
00255     // measurement of frequencypoint is finished
00256     if(jj == Nmeas) {
00257         jj = 1;
00258         ii += 1;
00259         fexcPast = fexc;
00260         // calculate the one point dft
00261         double Ureal = 2.0*scaleH*scaleG*(cr*sU[1] - sU[2]);
00262         double Uimag = 2.0*scaleH*scaleG*ci*sU[1];
00263         double Yreal = 2.0*scaleH*scaleG*(cr*sY[1] - sY[2]);
00264         double Yimag = 2.0*scaleH*scaleG*ci*sY[1];
00265         // calculate magnitude and angle
00266         float Umag = (float)(sqrt(Ureal*Ureal + Uimag*Uimag));
00267         float Ymag = (float)(sqrt(Yreal*Yreal + Yimag*Yimag));
00268         float absGyu = (float)(Ymag/Umag);
00269         float angGyu = (float)(atan2(Yimag, Yreal) - atan2(Uimag, Ureal));
00270         float absGye = (float)(Ymag/Aexc);
00271         float angGye = (float)(atan2(Yimag, Yreal) + piDiv2);
00272         // user info
00273         if(ii == 2) {
00274             printLine();
00275             printf("   fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|\r\n");
00276             printLine();
00277         }
00278         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);
00279     } else {
00280         jj += 1;
00281     }
00282     sinarg = fmod(sinarg + pi2Tsfexc, pi2);
00283     NmeasTotal += 1;
00284     return (float)(Aexc*sin(sinarg));
00285 }
00286 
00287 void GPA::fexcDesLogspace(double fMin, double fMax, int NfexcDes)
00288 {
00289     // calculate logarithmic spaced frequency points
00290     double Gain = log10(fMax/fMin)/((double)NfexcDes - 1.0);
00291     double expon = 0.0;
00292     for(int i = 0; i < NfexcDes; i++) {
00293         fexcDes[i] = fMin*pow(10.0, expon);
00294         expon += Gain;
00295     }
00296 }
00297 
00298 void GPA::calcGPAmeasPara(double fexcDes_i)
00299 {
00300     // Nmeas has to be an integer
00301     Nper = NperMin;
00302     Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5);
00303     //  secure that the minimal number of measurements is fullfilled
00304     int Ndelta = NmeasMin - Nmeas;
00305     if(Ndelta > 0) {
00306         Nper = (int)ceil((double)NmeasMin*fexcDes_i*Ts);
00307         Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5);
00308     }
00309     // evaluating reachable frequency
00310     fexc = (double)Nper/(double)Nmeas/Ts;
00311 }
00312 
00313 void GPA::calcHann()
00314 {
00315     wk = 0.5 - 0.5*cos(2.0*pi*((double)jj-1.0)/(double)Nmeas);
00316 }
00317 
00318 void GPA::printLine()
00319 {
00320     printf("-----------------------------------------------------------------------------------------\r\n");
00321 }
00322 
00323 void GPA::printGPAfexcDes()
00324 {
00325     printLine();
00326     for(int i = 0; i < NfexcDes; i++) {
00327         printf("%9.4f\r\n", (float)fexcDes[i]);
00328     }
00329 }
00330 
00331 void GPA::printGPAmeasPara()
00332 {
00333     printLine();
00334     printf(" fexcDes[Hz]  fexc[Hz]       Aexc   Nmeas   Nper\r\n");
00335     printLine();
00336     for(int i = 0; i < NfexcDes; i++) {
00337         calcGPAmeasPara(fexcDes[i]);
00338         if(fexc == fexcPast || fexc >= fnyq) {
00339             fexc = 0.0;
00340             Nmeas = 0;
00341             Nper = 0;
00342             Aexc = 0.0;
00343         } else {
00344             Aexc = aAexcDes/fexc + bAexcDes;
00345             fexcPast = fexc;
00346         }
00347         NmeasTotal += Nmeas;
00348         printf("%12.2e %9.2e %10.2e %7i %6i \r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper);
00349     }
00350     printGPAmeasTime();
00351     reset();
00352 }
00353 
00354 void GPA::printGPAmeasTime()
00355 {
00356     printLine();
00357     printf(" number of data points:  %9i\r\n", NmeasTotal);
00358     printf(" measurment time in sec: %9.2f\r\n", (float)((double)NmeasTotal*Ts));
00359 }
00360 
00361 void GPA::printNfexcDes()
00362 {
00363     printLine();
00364     printf(" number of frequancy points:  %2i\r\n", NfexcDes);
00365 }