GPA.cpp

00001 /*
00002     GPA: frequency point wise gain and phase analyser to measure the frequency
00003          respone of 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:
00019     GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
00020     
00021         fMin:     minimal desired frequency that should be measured in Hz
00022         fMax:     maximal desired frequency that should be measured in Hz
00023         NfexcDes: number of logarithmic equaly spaced frequency points
00024         NperMin:  minimal number of periods that are used for each frequency point
00025         NmeasMin: maximal number of samples that are used for each frequency point
00026         Ts:       sampling time
00027         Aexc0:    excitation amplitude at fMin 
00028         Aexc1:    excitation amplitude at fMax
00029     
00030         hints:    the amplitude drops with 1/fexc, if you're using
00031                   Axc1 = Aexc0/fMax the d/dt exc = const., this is recommended
00032                   if your controller does not have a rolloff.
00033                   if a desired frequency point is not measured try to increase Nmeas.
00034     
00035     pseudo code for a closed loop measurement with a proportional controller Kp:
00036     
00037         float inp = "measurement of inputsignal";
00038         float out = "mesurement of outputsignal";
00039         float exc = myGPA(inp, out);
00040         float off = "desired steady state off the system";
00041         
00042         excitation input at (1):
00043         inp = Kp*(exc + off - out);
00044         
00045         excitation input at (2):
00046         inp = Kp*(off - out) + exc;
00047         
00048     usage:
00049         exc = myGPA(inp, out) does update the internal states of the gpa at the
00050         timestep k and returns the excitation signal for the timestep k+1. the 
00051         results are plotted to a terminal (putty) over serial cennection and look
00052         as follows:
00053  -----------------------------------------------------------------------------------------
00054    fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|
00055  -----------------------------------------------------------------------------------------
00056    7.01e-01   1.08e+01  -7.45e-02   1.08e+01  -7.38e-02   9.99e-01   9.99e-01   1.08e+01
00057         
00058     in matlab you can use:
00059         dataNucleo = [... insert measurement data here ...];
00060         g = frd(dataNucleo(:,2).*exp(1i*dataNucleo(:,3)), dataNucleo(:,1), Ts, 'Units', 'Hz');
00061         gcl = frd(dataNucleo(:,4).*exp(1i*dataNucleo(:,5)), dataNucleo(:,1), Ts, 'Units', 'Hz');
00062         
00063     if you're evaluating more than one measurement which contain equal frequency points try:
00064         dataNucleo = [dataNucleo1; dataNucleo2];
00065         [~, ind] = unique(dataNucleo(:,1),'stable');
00066         dataNucleo = dataNucleo(ind,:);
00067          
00068         autor: M.E. Peter
00069 */ 
00070 
00071 #include "GPA.h"
00072 #include "mbed.h"
00073 #include "math.h"
00074 #define   pi 3.1415927f
00075 
00076 using namespace std;
00077 
00078 GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
00079 {
00080     this->NfexcDes = NfexcDes;
00081     this->NperMin = NperMin;
00082     this->NmeasMin = NmeasMin;
00083     this->Ts = Ts;
00084 
00085     // calculate logarithmic spaced frequency points
00086     fexcDes = (float*)malloc(NfexcDes*sizeof(float));
00087     fexcDesLogspace(fMin, fMax, NfexcDes);
00088 
00089     // calculate coefficients for decreasing amplitude (1/fexc)
00090     this->aAexcDes = (Aexc1 - Aexc0)/(1.0f/fexcDes[NfexcDes-1] - 1.0f/fexcDes[0]);
00091     this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0];
00092 
00093     fnyq = 1/2.0f/Ts;
00094     pi2 = 2.0f*pi;
00095     pi2Ts = pi2*Ts;
00096     piDiv2 = pi/2.0f;
00097 
00098     sU = (float*)malloc(3*sizeof(float));
00099     sY = (float*)malloc(3*sizeof(float));
00100     reset();
00101 }
00102 
00103 GPA::~GPA() {}
00104 
00105 void GPA::reset()
00106 {
00107     Nmeas = 0;
00108     Nper = 0;
00109     fexc = 0.0f;
00110     fexcPast = 0.0f;
00111     ii = 1; // iterating through desired frequency points
00112     jj = 1; // iterating through measurement points w.r.t. reachable frequency
00113     scaleG = 0.0f;
00114     cr = 0.0f;
00115     ci = 0.0f;
00116     for(int i = 0; i < 3; i++) {
00117         sU[i] = 0.0f;
00118         sY[i] = 0.0f;
00119     }
00120     sinarg = 0.0f;
00121     NmeasTotal = 0;
00122     Aexc = 0.0f;
00123     pi2Tsfexc = 0.0f;
00124 }
00125 
00126 float GPA::update(float inp, float out)
00127 {
00128     // a new frequency point has been reached
00129     if(jj == 1) {
00130         // get a new unique frequency point
00131         while(fexc == fexcPast) {
00132             // measurement finished
00133             if(ii > NfexcDes) {
00134                 return 0.0f;
00135             }
00136             calcGPAmeasPara(fexcDes[ii - 1]);
00137             // secure fexc is not higher or equal to nyquist frequency
00138             if(fexc >= fnyq) {
00139                 fexc = fexcPast;
00140             }
00141             // no frequency found
00142             if(fexc == fexcPast) {
00143                 ii += 1;
00144             } else {
00145                 Aexc = aAexcDes/fexc + bAexcDes;
00146                 pi2Tsfexc = pi2Ts*fexc;
00147             }
00148         }
00149         fexcPast = fexc;
00150         // filter scaling
00151         scaleG = 1.0f/sqrt((float)Nmeas);
00152         // filter coefficients
00153         cr = cos(pi2Tsfexc);
00154         ci = sin(pi2Tsfexc);
00155         // filter storage
00156         for(int i = 0; i < 3; i++) {
00157             sU[i] = 0.0f;
00158             sY[i] = 0.0f;
00159         }
00160     }
00161     // filter step for signal su
00162     sU[0] = scaleG*inp + 2.0f*cr*sU[1] - sU[2];
00163     sU[2] = sU[1];
00164     sU[1] = sU[0];
00165     // filter step for signal sy
00166     sY[0] = scaleG*out + 2.0f*cr*sY[1] - sY[2];
00167     sY[2] = sY[1];
00168     sY[1] = sY[0];
00169     // measurement of frequencypoint is finished
00170     if(jj == Nmeas) {
00171         jj = 1;
00172         ii += 1;
00173         // calculate the one point dft
00174         float Ureal = 2.0f*scaleG*(cr*sU[1] - sU[2]);
00175         float Uimag = 2.0f*scaleG*ci*sU[1];
00176         float Yreal = 2.0f*scaleG*(cr*sY[1] - sY[2]);
00177         float Yimag = 2.0f*scaleG*ci*sY[1];
00178         // calculate magnitude and angle
00179         float Umag = sqrt(Ureal*Ureal + Uimag*Uimag);
00180         float Ymag = sqrt(Yreal*Yreal + Yimag*Yimag);
00181         float absGyu = Ymag/Umag;
00182         float angGyu = atan2(Yimag, Yreal) - atan2(Uimag, Ureal);
00183         float absGye = Ymag/Aexc;
00184         float angGye = (atan2(Yimag, Yreal) + piDiv2);
00185         // user info
00186         if(ii == 1) {
00187             printLine();
00188             printf("   fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|\r\n");
00189             printLine();
00190         }
00191         printf("%11.2e %10.2e %10.2e %10.2e %10.2e %10.2e %10.2e %10.2e\r\n", fexc, absGyu, angGyu, absGye, angGye, Aexc, Umag, Ymag);
00192     } else {
00193         jj += 1;
00194     }
00195     sinarg = fmod(sinarg + pi2Tsfexc, pi2);
00196     NmeasTotal += 1;
00197     return Aexc*sin(sinarg);
00198 }
00199 
00200 void GPA::fexcDesLogspace(float fMin, float fMax, int NfexcDes)
00201 {
00202     // calculate logarithmic spaced frequency points
00203     float Gain = log10(fMax/fMin)/((float)NfexcDes - 1.0f);
00204     float expon = 0.0f;
00205     for(int i = 0; i < NfexcDes; i++) {
00206         fexcDes[i] = fMin*pow(10.0f, expon);
00207         expon += Gain;
00208     }
00209 }
00210 
00211 void GPA::calcGPAmeasPara(float fexcDes_i)
00212 {
00213     // Nmeas has to be an integer
00214     Nper = NperMin;
00215     Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
00216     //  secure that the minimal number of measurements is fullfilled
00217     int Ndelta = NmeasMin - Nmeas;
00218     if(Ndelta > 0) {
00219         Nper = (int)ceil((float)NmeasMin*fexcDes_i*Ts);
00220         Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
00221     }
00222     // evaluating reachable frequency
00223     fexc = (float)Nper/(float)Nmeas/Ts;
00224 }
00225 
00226 void GPA::printLine()
00227 {
00228     printf("-----------------------------------------------------------------------------------------\r\n");
00229 }
00230 
00231 void GPA::printGPAfexcDes()
00232 {
00233     printLine();
00234     for(int i = 0; i < NfexcDes; i++) {
00235         printf("%9.4f\r\n", fexcDes[i]);
00236     }
00237 }
00238 
00239 void GPA::printGPAmeasPara()
00240 {
00241     printLine();
00242     printf(" fexcDes[Hz]  fexc[Hz]       Aexc   Nmeas   Nper\r\n");
00243     printLine();
00244     for(int i = 0; i < NfexcDes; i++) {
00245         calcGPAmeasPara(fexcDes[i]);
00246         if(fexc == fexcPast || fexc >= fnyq) {
00247             fexc = 0.0f;
00248             Nmeas = 0;
00249             Nper = 0;
00250             Aexc = 0;
00251         } else {
00252             Aexc = aAexcDes/fexc + bAexcDes;
00253             fexcPast = fexc;
00254         }
00255         NmeasTotal += Nmeas;
00256         printf("%12.2e %9.2e %10.2e %7i %6i \r\n", fexcDes[i], fexc, Aexc, Nmeas, Nper);
00257     }
00258     printGPAmeasTime();
00259     reset();
00260 }
00261 
00262 void GPA::printGPAmeasTime()
00263 {
00264     printLine();
00265     printf(" number of data points:  %9i\r\n", NmeasTotal);
00266     printf(" measurment time in sec: %9.2f\r\n", (float)NmeasTotal*Ts);
00267 }