Diff: GPA.cpp

Revision:

0:78ca29b4c49e

Child:

2:769ce5f06d3e

diff -r 000000000000 -r 78ca29b4c49e GPA.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPA.cpp	Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,267 @@
+/*
+    GPA: frequency point wise gain and phase analyser to measure the frequency
+         respone of dynamical system
+    
+        hint:       the measurements should only be perfomed in closed loop
+        assumption: the system is at the desired steady state of interest when
+                    the measurment starts
+    
+                             exc(2)                 C: controller
+                               |                    P: plant
+                               v                     
+     exc(1) --> o   ->| C |--->o------->| P |----------> out
+                ^                  |             |
+                |                   --> inp      |  exc: excitation signal (E)
+                |                                |  inp: input plant (U)
+                 --------------------------------   out: output plant (Y)
+  
+    instantiate option 1:
+    GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
+    
+        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: maximal number of samples that are used for each frequency point
+        Ts:       sampling time
+        Aexc0:    excitation amplitude at fMin 
+        Aexc1:    excitation amplitude at fMax
+    
+        hints:    the amplitude drops with 1/fexc, if you're using
+                  Axc1 = Aexc0/fMax the d/dt exc = const., this is recommended
+                  if your controller does not have a rolloff.
+                  if a desired frequency point is not measured try to increase Nmeas.
+    
+    pseudo code for a closed loop measurement with a proportional controller Kp:
+    
+        float inp = "measurement of inputsignal";
+        float out = "mesurement of outputsignal";
+        float exc = myGPA(inp, out);
+        float off = "desired steady state off the system";
+        
+        excitation input at (1):
+        inp = Kp*(exc + off - out);
+        
+        excitation input at (2):
+        inp = Kp*(off - out) + exc;
+        
+    usage:
+        exc = myGPA(inp, out) does update the internal states of the gpa at the
+        timestep k and returns the excitation signal for the timestep k+1. the 
+        results are plotted to a terminal (putty) over serial cennection and look
+        as follows:
+ -----------------------------------------------------------------------------------------
+   fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|
+ -----------------------------------------------------------------------------------------
+   7.01e-01   1.08e+01  -7.45e-02   1.08e+01  -7.38e-02   9.99e-01   9.99e-01   1.08e+01
+        
+    in matlab you can use:
+        dataNucleo = [... insert measurement data here ...];
+        g = frd(dataNucleo(:,2).*exp(1i*dataNucleo(:,3)), dataNucleo(:,1), Ts, 'Units', 'Hz');
+        gcl = frd(dataNucleo(:,4).*exp(1i*dataNucleo(:,5)), dataNucleo(:,1), Ts, 'Units', 'Hz');
+        
+    if you're evaluating more than one measurement which contain equal frequency points try:
+        dataNucleo = [dataNucleo1; dataNucleo2];
+        [~, ind] = unique(dataNucleo(:,1),'stable');
+        dataNucleo = dataNucleo(ind,:);
+         
+        autor: M.E. Peter
+*/ 
+
+#include "GPA.h"
+#include "mbed.h"
+#include "math.h"
+#define   pi 3.1415927f
+
+using namespace std;
+
+GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
+{
+    this->NfexcDes = NfexcDes;
+    this->NperMin = NperMin;
+    this->NmeasMin = NmeasMin;
+    this->Ts = Ts;
+
+    // calculate logarithmic spaced frequency points
+    fexcDes = (float*)malloc(NfexcDes*sizeof(float));
+    fexcDesLogspace(fMin, fMax, NfexcDes);
+
+    // calculate coefficients for decreasing amplitude (1/fexc)
+    this->aAexcDes = (Aexc1 - Aexc0)/(1.0f/fexcDes[NfexcDes-1] - 1.0f/fexcDes[0]);
+    this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0];
+
+    fnyq = 1/2.0f/Ts;
+    pi2 = 2.0f*pi;
+    pi2Ts = pi2*Ts;
+    piDiv2 = pi/2.0f;
+
+    sU = (float*)malloc(3*sizeof(float));
+    sY = (float*)malloc(3*sizeof(float));
+    reset();
+}
+
+GPA::~GPA() {}
+
+void GPA::reset()
+{
+    Nmeas = 0;
+    Nper = 0;
+    fexc = 0.0f;
+    fexcPast = 0.0f;
+    ii = 1; // iterating through desired frequency points
+    jj = 1; // iterating through measurement points w.r.t. reachable frequency
+    scaleG = 0.0f;
+    cr = 0.0f;
+    ci = 0.0f;
+    for(int i = 0; i < 3; i++) {
+        sU[i] = 0.0f;
+        sY[i] = 0.0f;
+    }
+    sinarg = 0.0f;
+    NmeasTotal = 0;
+    Aexc = 0.0f;
+    pi2Tsfexc = 0.0f;
+}
+
+float GPA::update(float inp, float out)
+{
+    // a new frequency point has been reached
+    if(jj == 1) {
+        // get a new unique frequency point
+        while(fexc == fexcPast) {
+            // measurement finished
+            if(ii > NfexcDes) {
+                return 0.0f;
+            }
+            calcGPAmeasPara(fexcDes[ii - 1]);
+            // secure fexc is not higher or equal to nyquist frequency
+            if(fexc >= fnyq) {
+                fexc = fexcPast;
+            }
+            // no frequency found
+            if(fexc == fexcPast) {
+                ii += 1;
+            } else {
+                Aexc = aAexcDes/fexc + bAexcDes;
+                pi2Tsfexc = pi2Ts*fexc;
+            }
+        }
+        fexcPast = fexc;
+        // filter scaling
+        scaleG = 1.0f/sqrt((float)Nmeas);
+        // filter coefficients
+        cr = cos(pi2Tsfexc);
+        ci = sin(pi2Tsfexc);
+        // filter storage
+        for(int i = 0; i < 3; i++) {
+            sU[i] = 0.0f;
+            sY[i] = 0.0f;
+        }
+    }
+    // filter step for signal su
+    sU[0] = scaleG*inp + 2.0f*cr*sU[1] - sU[2];
+    sU[2] = sU[1];
+    sU[1] = sU[0];
+    // filter step for signal sy
+    sY[0] = scaleG*out + 2.0f*cr*sY[1] - sY[2];
+    sY[2] = sY[1];
+    sY[1] = sY[0];
+    // measurement of frequencypoint is finished
+    if(jj == Nmeas) {
+        jj = 1;
+        ii += 1;
+        // calculate the one point dft
+        float Ureal = 2.0f*scaleG*(cr*sU[1] - sU[2]);
+        float Uimag = 2.0f*scaleG*ci*sU[1];
+        float Yreal = 2.0f*scaleG*(cr*sY[1] - sY[2]);
+        float Yimag = 2.0f*scaleG*ci*sY[1];
+        // calculate magnitude and angle
+        float Umag = sqrt(Ureal*Ureal + Uimag*Uimag);
+        float Ymag = sqrt(Yreal*Yreal + Yimag*Yimag);
+        float absGyu = Ymag/Umag;
+        float angGyu = atan2(Yimag, Yreal) - atan2(Uimag, Ureal);
+        float absGye = Ymag/Aexc;
+        float angGye = (atan2(Yimag, Yreal) + piDiv2);
+        // user info
+        if(ii == 1) {
+            printLine();
+            printf("   fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|\r\n");
+            printLine();
+        }
+        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);
+    } else {
+        jj += 1;
+    }
+    sinarg = fmod(sinarg + pi2Tsfexc, pi2);
+    NmeasTotal += 1;
+    return Aexc*sin(sinarg);
+}
+
+void GPA::fexcDesLogspace(float fMin, float fMax, int NfexcDes)
+{
+    // calculate logarithmic spaced frequency points
+    float Gain = log10(fMax/fMin)/((float)NfexcDes - 1.0f);
+    float expon = 0.0f;
+    for(int i = 0; i < NfexcDes; i++) {
+        fexcDes[i] = fMin*pow(10.0f, expon);
+        expon += Gain;
+    }
+}
+
+void GPA::calcGPAmeasPara(float fexcDes_i)
+{
+    // Nmeas has to be an integer
+    Nper = NperMin;
+    Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
+    //  secure that the minimal number of measurements is fullfilled
+    int Ndelta = NmeasMin - Nmeas;
+    if(Ndelta > 0) {
+        Nper = (int)ceil((float)NmeasMin*fexcDes_i*Ts);
+        Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
+    }
+    // evaluating reachable frequency
+    fexc = (float)Nper/(float)Nmeas/Ts;
+}
+
+void GPA::printLine()
+{
+    printf("-----------------------------------------------------------------------------------------\r\n");
+}
+
+void GPA::printGPAfexcDes()
+{
+    printLine();
+    for(int i = 0; i < NfexcDes; i++) {
+        printf("%9.4f\r\n", fexcDes[i]);
+    }
+}
+
+void GPA::printGPAmeasPara()
+{
+    printLine();
+    printf(" fexcDes[Hz]  fexc[Hz]       Aexc   Nmeas   Nper\r\n");
+    printLine();
+    for(int i = 0; i < NfexcDes; i++) {
+        calcGPAmeasPara(fexcDes[i]);
+        if(fexc == fexcPast || fexc >= fnyq) {
+            fexc = 0.0f;
+            Nmeas = 0;
+            Nper = 0;
+            Aexc = 0;
+        } else {
+            Aexc = aAexcDes/fexc + bAexcDes;
+            fexcPast = fexc;
+        }
+        NmeasTotal += Nmeas;
+        printf("%12.2e %9.2e %10.2e %7i %6i \r\n", fexcDes[i], fexc, Aexc, Nmeas, Nper);
+    }
+    printGPAmeasTime();
+    reset();
+}
+
+void GPA::printGPAmeasTime()
+{
+    printLine();
+    printf(" number of data points:  %9i\r\n", NmeasTotal);
+    printf(" measurment time in sec: %9.2f\r\n", (float)NmeasTotal*Ts);
+}
\ No newline at end of file