Revision 4:37df0f6a1bc3, committed 2019-05-10

Comitter:

pmic

Date:

Fri May 10 18:15:41 2019 +0000

Parent:

3:477db0d9895e

Commit message:

Introduce GPA and implement two sets of measurements which are apropriate for a frequency reasponse measurements, rember, the GPA is written to be used in CL, especially for weakly damped systems. It's just an example for future usage

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPA.cpp	Fri May 10 18:15:41 2019 +0000
@@ -0,0 +1,550 @@
+/*
+    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
+
+         Hint:        If the plant has a pole at zero, is unstable or weakly damped the measurement has to be perfomed 
+                      in closed loop (this is NOT tfestimate, the algorithm is based on the one point DFT).
+         Assumption:  The system is and remains at the desired steady state of interest when the measurment starts
+
+    Instantiate option 0: ("Not a Jedi yet" users, for logarithmic equaly spaced frequency points)
+    
+        GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts)
+        
+            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 between fMin and fMax
+            Aexc0:      Excitation amplitude at fMin
+            Aexc1:      Excitation amplitude at fMax
+            Ts:         Sampling time in sec
+            
+            Default values are as follows:
+            int NperMin   = 3;
+            int NmeasMin  = (int)ceil(1.0f/Ts);
+            int NstartMin = (int)ceil(3.0f/Ts);
+            int NsweepMin = 0;
+
+    Instantiate option 1: ("Jedi or Sith Lord", for logarithmic equaly spaced frequency points)
+    
+        GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
+        
+            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:   Minimal number of samples that are used for each frequency point
+            Ts:         Sampling time in sec
+            Aexc0:      Excitation amplitude at fMin
+            Aexc1:      Excitation amplitude at fMax
+            NstartMin:  Minimal number of samples to sweep to the first frequency point (can be equal 0)
+            NsweepMin:  Minimal number of samples to sweep from freq. point to freq. point (can be equal 0)
+
+ 
+    Instantiate option 2: (for a second, refined frequency grid measurement)
+    
+        GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
+        
+            f0:         Frequency point for the calculation of Aexc0 in Hz (should be chosen like in the first measurement)
+            f1:         Frequency point for the calculation of Aexc1 in Hz (should be chosen like in the first measurement)
+            *fexcDes:   Sorted frequency point array in Hz
+            NfexcDes:   Length of fexcDes
+            
+            For the other parameters see above.
+
+    Instantiate option 3: (for an arbitary but sorted frequency grid measurement)
+    
+        GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
+        
+            *fexcDes:   Sorted frequency point array in Hz
+            Aexc0:      Excitation amplitude at fexcDes[0]
+            Aexc1:      Excitation amplitude at fexcDes[NfexcDes-1]
+            NfexcDes:   Length of fexcDes
+            
+            For the other parameters see above.
+
+    Note:   The amplitude drops with 1/fexc, if you're using Axc1 = Aexc0/fMax then d/dt exc = const.,
+            this is recommended if your controller does not have a rolloff. If a desired frequency point
+            is not measured (could not be reached) try to increase Nmeas.
+              
+        
+    Block diagram:
+
+                w (const.)    exc(2)                C: controller
+                |              |                    P: plant
+                v   e          v
+     exc(1) --> o   ->| C |--->o------->| P |----------> out (y)
+                ^ -                |             |
+                |                   --> inp (u)  |  exc (R): excitation signal
+                |                                |  inp (U): input plant
+                 --------------------------------   out (Y): output plant
+              
+              
+    Pseudo code for an open loop measurement:
+
+        - Measuring the plant P = Gyu = Gyr:
+        
+            u = w + exc;
+            ... write output u here! it follows exc(k+1) ...
+            exc = Wobble(exc, y);
+            
+            Closed loop FRF calculus with a stabilizing controller C:
+                S  = 1/(1 + C*P);  % ( exc1 -> e ,   1/(1 + C*P) ) contr. error rejection, robustness (1/max|S|)
+                T  = 1 - S;        % ( w -> y    , C*P/(1 + C*P) ) tracking
+                CS = C*S;          % ( exc1 -> u ,   C/(1 + C*P) ) disturbance plant output
+                PS = P*S;          % ( exc2 -> y ,   P/(1 + C*P) ) disturbance plant input
+
+
+    Pseudo code for a closed loop measurement with stabilizing controller C:
+
+        Excitation at excitation input (1):   
+        
+        - Measuring the plant P = Gyu and the closed loop tf T = PC/(1 + PC) = Gyr:
+        
+            u = C(w - y + exc);
+            ... write output u here! it follows exc(k+1) ...
+            exc = Wobble(u, y);
+                        
+            Closed loop FRF calculus:
+                S  = 1 - T;
+                PS = P*S;
+                CS = T/P;
+                C  = C/S;
+
+        Excitation at excitation input (2):
+        
+        - Measuring the plant P = Gyu and the closed loop tf PS = P/(1 + PC) = Gyr:
+        
+            u = C(w - y) + exc;
+            ... write output u here! it follows exc(k+1) ...
+            exc = Wobble(u, y);
+        
+            Closed loop FRF calculus:
+                S  = PS/P;
+                T  = 1 - S;
+                CS = T/P;
+                C  = C/S;
+
+
+    Usage:
+        exc(k+1) = myGPA(inp(k), out(k)) does update the internal states of the 
+        gpa at the timestep k and returns the excitation signal for the timestep
+        k+1. The FRF data are plotted to a terminal (Putty) over a serial 
+        connection and look as follows:
+        
+--------------------------------------------------------------------------------
+  fexc[Hz]    |Gyu|    deg(Gyu)  |Gyr|    deg(Gyr)   |U|       |Y|       |R|
+--------------------------------------------------------------------------------
+ 5.0000e-02 1.001e+00   -0.309 1.001e+00   -0.309 4.000e-01 4.000e-01 4.005e-01
+    .           .         .        .         .        .         .         .
+    .           .         .        .         .        .         .         .
+    .           .         .        .         .        .         .         .
+  
+    In Matlab you can use the editor as follows:
+        data = [... insert measurement data here ...];
+        gyu = frd(data(:,2).*exp(1i*data(:,3)*pi/180), data(:,1), Ts, 'Units', 'Hz');
+        gyr = frd(data(:,4).*exp(1i*data(:,5)*pi/180), data(:,1), Ts, 'Units', 'Hz');
+
+    If you're evaluating more than one measurement which contain equal frequency points use:
+        data = [data1; data2];
+        [~, ind] = unique(data(:,1), 'stable');
+        data = data(ind,:);
+
+
+    Autor and Copyrigth: 2018 / 2019 / M.E. Peter
+    
+*/
+
+#include "GPA.h"
+#include "mbed.h"
+#include "math.h"
+#define   pi 3.141592653589793
+
+using namespace std;
+
+// -----------------------------------------------------------------------------
+//      instantiate
+// -----------------------------------------------------------------------------
+
+GPA::GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts)
+{
+    int NperMin = 3;
+    int NmeasMin = (int)ceil(1.0f/Ts);
+    int NstartMin = (int)ceil(3.0f/Ts);
+    int NsweepMin = 0;
+    
+    assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
+
+    // calculate logarithmic spaced frequency points
+    fexcDes = (double*)malloc(NfexcDes*sizeof(double));
+    fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
+
+    calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
+    initializeConstants((double)Ts);
+    assignFilterStorage();
+    reset();
+}
+
+GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
+{
+    assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
+
+    // calculate logarithmic spaced frequency points
+    fexcDes = (double*)malloc(NfexcDes*sizeof(double));
+    fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
+
+    calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
+    initializeConstants((double)Ts);
+    assignFilterStorage();
+    reset();
+}
+
+GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
+{
+    assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
+    
+    // convert fexcDes from float to double, it is assumed that it is sorted
+    this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
+    for(int i = 0; i < NfexcDes; i++) {
+        this->fexcDes[i] = (double)fexcDes[i];
+    }
+    
+    calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
+    initializeConstants((double)Ts);
+    assignFilterStorage();
+    reset();
+}
+
+GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin)
+{
+    assignParameters(NfexcDes, NperMin, NmeasMin, (double)Ts, NstartMin, NsweepMin);
+    
+    // convert fexcDes from float to double, it is assumed that it is sorted
+    this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
+    for(int i = 0; i < NfexcDes; i++) {
+        this->fexcDes[i] = (double)fexcDes[i];
+    }
+
+    calculateDecreasingAmplitudeCoefficients((double)Aexc0, (double)Aexc1);
+    initializeConstants((double)Ts);
+    assignFilterStorage();
+    reset();
+}
+
+// -----------------------------------------------------------------------------
+//      virtual and reset
+// -----------------------------------------------------------------------------
+
+GPA::~GPA() {}
+
+void GPA::reset()
+{
+    Nmeas = 0;
+    Nper = 0;
+    dfexc = 0.0;
+    fexc = 0.0;
+    fexcPast = 0.0;
+    i = 1; // iterating through desired frequency points
+    j = 1; // iterating through measurement points w.r.t. reachable frequency
+    scaleG = 0.0;
+    cr = 0.0;
+    ci = 0.0;
+    for(int i = 0; i < 3; i++) {
+        sU[i] = 0.0;
+        sY[i] = 0.0;
+    }
+    sinarg = 0.0;
+    NmeasTotal = 0;
+    Aexc = 0.0;
+    pi2Tsfexc = 0.0;
+    Nsweep = NstartMin;
+    bfexc = 0.0;
+    afexc = 0.0;
+    aAexc = 0.0;
+    bAexc = 0.0;
+    AexcOut = 0.0;
+}
+
+// -----------------------------------------------------------------------------
+//      update (operator)
+// -----------------------------------------------------------------------------  
+
+float GPA::update(double inp, double out)
+{
+    // a new frequency point has been reached
+    if(j == 1) {
+        // user info
+        if(i == 1) {
+            printLine();
+            printf("  fexc[Hz]    |Gyu|    deg(Gyu)  |Gyr|    deg(Gyr)   |U|       |Y|       |R|\r\n");
+            printLine();
+        }
+        // get a new unique frequency point
+        while(fexc == fexcPast) {
+            // measurement finished
+            if(i > NfexcDes) {
+                return 0.0f;
+            }
+            calcGPAmeasPara(fexcDes[i - 1]);
+            // secure fexc is not higher or equal to nyquist frequency
+            if(fexc >= fnyq) {
+                fexc = fexcPast;
+            }
+            // no frequency found
+            if(fexc == fexcPast) {
+                i += 1;
+            } else {
+                Aexc = aAexcDes/fexc + bAexcDes;
+                pi2Tsfexc = pi2Ts*fexc;
+            }
+        }
+        // filter scaling
+        scaleG = 1.0/sqrt((double)Nmeas);
+        // filter coefficients
+        cr = cos(pi2Tsfexc);
+        ci = sin(pi2Tsfexc);
+        // set filter storage zero
+        for(int i = 0; i < 3; i++) {
+            sU[i] = 0.0;
+            sY[i] = 0.0;
+        }
+        // calculate the parameters for the frequency sweep from fexcPast to fexc
+        if(Nsweep > 0) calcGPAsweepPara();
+    }
+    // perfomre the sweep or measure
+    if(j <= Nsweep) {
+        dfexc = afexc*(double)j + bfexc;
+        AexcOut = aAexc*(double)j + bAexc;   
+    } else {
+        dfexc = fexc;
+        AexcOut = Aexc;
+        // one point DFT filter step for signal su
+        sU[0] = scaleG*inp + 2.0*cr*sU[1] - sU[2];
+        sU[2] = sU[1];
+        sU[1] = sU[0];
+        // one point DFT filter step for signal sy
+        sY[0] = scaleG*out + 2.0*cr*sY[1] - sY[2];
+        sY[2] = sY[1];
+        sY[1] = sY[0];
+    }
+    // secure sinarg starts at 0 (numerically maybe not given)
+    if(j == 1 || j == Nsweep + 1) sinarg = 0.0;
+    // measurement of frequencypoint is finished
+    if(j == Nmeas + Nsweep) {
+        fexcPast = fexc;
+        AexcPast = Aexc;
+        Nsweep = NsweepMin;
+        // calculate the one point dft
+        double Ureal = 2.0*scaleG*(cr*sU[1] - sU[2]);
+        double Uimag = 2.0*scaleG*ci*sU[1];
+        double Yreal = 2.0*scaleG*(cr*sY[1] - sY[2]);
+        double Yimag = 2.0*scaleG*ci*sY[1];
+        // calculate magnitude and angle
+        float Umag = (float)(sqrt(Ureal*Ureal + Uimag*Uimag));
+        float Ymag = (float)(sqrt(Yreal*Yreal + Yimag*Yimag));
+        float absGyu = (float)(Ymag/Umag);
+        float angGyu = (float)wrapAngle(atan2(Yimag, Yreal) - atan2(Uimag, Ureal));
+        float absGyr = (float)(Ymag/Aexc);
+        float angGyr = (float)wrapAngle(atan2(Yimag, Yreal) + piDiv2);
+        // user info
+        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);
+        i += 1;
+        j = 1;
+    } else {
+        j += 1;
+    }
+    // calculate the excitation
+    sinarg = fmod(sinarg + pi2Ts*dfexc, pi2);
+    NmeasTotal += 1;
+    return (float)(AexcOut*sin(sinarg));
+}
+
+// -----------------------------------------------------------------------------
+//      private functions
+// -----------------------------------------------------------------------------  
+
+void GPA::assignParameters(int NfexcDes, int NperMin, int NmeasMin, double Ts, int NstartMin, int NsweepMin)
+{
+    this->NfexcDes = NfexcDes;
+    this->NperMin = NperMin;
+    this->NmeasMin = NmeasMin;
+    this->Ts = Ts;
+    this->NstartMin = NstartMin;
+    this->NsweepMin = NsweepMin;   
+}
+
+void GPA::calculateDecreasingAmplitudeCoefficients(double Aexc0, double Aexc1)
+{
+    // calculate coefficients for decreasing amplitude (1/fexc)
+    this->aAexcDes = (Aexc1 - Aexc0)/(1.0/fexcDes[NfexcDes-1] - 1.0/fexcDes[0]);
+    this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0];   
+}
+
+void GPA::initializeConstants(double Ts)
+{
+    fnyq = 1.0/2.0/Ts;
+    pi2 = 2.0*pi;
+    pi2Ts = pi2*Ts;
+    piDiv2 = pi/2.0;
+    rad2deg = 180.0f/(float)pi;
+}
+
+void GPA::assignFilterStorage()
+{
+    sU = (double*)malloc(3*sizeof(double));
+    sY = (double*)malloc(3*sizeof(double));   
+}
+
+void GPA::fexcDesLogspace(double fMin, double fMax, int NfexcDes)
+{
+    // calculate logarithmic spaced frequency points
+    double Gain = log10(fMax/fMin)/((double)NfexcDes - 1.0);
+    double expon = 0.0;
+    for(int i = 0; i < NfexcDes; i++) {
+        fexcDes[i] = fMin*pow(10.0, expon);
+        expon += Gain;
+    }
+}
+
+void GPA::calcGPAmeasPara(double fexcDes_i)
+{
+    // Nmeas has to be an integer
+    Nper = NperMin;
+    Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5);
+    //  secure that the minimal number of measurements is fullfilled
+    int Ndelta = NmeasMin - Nmeas;
+    if(Ndelta > 0) {
+        Nper = (int)ceil((double)NmeasMin*fexcDes_i*Ts);
+        Nmeas = (int)floor((double)Nper/fexcDes_i/Ts + 0.5);
+    }
+    // evaluating reachable frequency
+    fexc = (double)Nper/(double)Nmeas/Ts;
+}
+
+void GPA::calcGPAsweepPara()
+{
+    // calculate linear frequency sweep parameters
+    double ksta = ceil(Ts*(double)Nsweep/2.0*(fexc + fexcPast));
+    Nsweep = (int)floor(2.0*ksta/Ts/(fexc + fexcPast) + 0.5);
+    bfexc = 2.0*ksta/Ts/(double)Nsweep - fexc;
+    afexc = (fexc - bfexc)/((double)Nsweep + 1.0);
+    aAexc = (Aexc - AexcPast)/((double)Nsweep + 1.0);
+    bAexc = AexcPast;
+}
+
+double GPA::wrapAngle(double angle)
+{
+    // wrap angle from (-2pi,2pi) into (-pi,pi)
+    if(abs(angle) > pi) angle -= copysign(-pi2, angle); // -1*sign(angle)*2*pi + angle;
+    return angle;
+}
+
+void GPA::printLine()
+{
+    printf("--------------------------------------------------------------------------------\r\n");
+}
+
+void GPA::printLongLine()
+{
+    printf("-------------------------------------------------------------------------------------------------------\r\n");
+}
+
+// -----------------------------------------------------------------------------
+//      public functions
+// -----------------------------------------------------------------------------     
+
+void GPA::printGPAfexcDes()
+{
+    printLine();
+    for(int i = 0; i < NfexcDes; i++) {
+        printf("%9.4f\r\n", (float)fexcDes[i]);
+    }
+}
+
+void GPA::printGPAmeasPara()
+{
+    printLine();
+    printf(" fexcDes[Hz]   fexc[Hz]     Aexc      Nmeas   Nper  Nsweep\r\n");
+    printLine();
+    for(int i = 0; i < NfexcDes; i++) {
+        calcGPAmeasPara(fexcDes[i]);
+        if(fexc == fexcPast || fexc >= fnyq) {
+            fexc = 0.0;
+            Aexc = 0.0;
+            Nmeas = 0;
+            Nper = 0;
+            Nsweep = 0;
+            afexc = 0.0;
+            bfexc = 0.0;
+            aAexc = 0.0;
+            bAexc = 0.0;
+            
+        } else {
+            Aexc = aAexcDes/fexc + bAexcDes;
+            if(Nsweep > 0)  calcGPAsweepPara();
+            else{
+                afexc = 0.0;
+                bfexc = 0.0;
+                aAexc = 0.0;
+                bAexc = 0.0; 
+            }
+            fexcPast = fexc;
+            AexcPast = Aexc;
+        }
+        NmeasTotal += Nmeas;
+        NmeasTotal += Nsweep;
+        printf("%11.4e %12.4e %10.3e %7i %6i %7i\r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper, Nsweep);
+        Nsweep = NsweepMin;
+    }
+    printGPAmeasTime();
+    reset();
+}
+
+void GPA::printFullGPAmeasPara()
+{
+    printLongLine();
+    printf(" fexcDes[Hz]   fexc[Hz]     Aexc      Nmeas   Nper  Nsweep    afexc      bfexc      aAexc      bAexc\r\n");
+    printLongLine();
+    for(int i = 0; i < NfexcDes; i++) {
+        calcGPAmeasPara(fexcDes[i]);
+        if(fexc == fexcPast || fexc >= fnyq) {
+            fexc = 0.0;
+            Aexc = 0.0;
+            Nmeas = 0;
+            Nper = 0;
+            Nsweep = 0;
+            afexc = 0.0;
+            bfexc = 0.0;
+            aAexc = 0.0;
+            bAexc = 0.0;
+            
+        } else {
+            Aexc = aAexcDes/fexc + bAexcDes;
+            if(Nsweep > 0)  calcGPAsweepPara();
+            else{
+                afexc = 0.0;
+                bfexc = 0.0;
+                aAexc = 0.0;
+                bAexc = 0.0; 
+            }
+            fexcPast = fexc;
+            AexcPast = Aexc;
+        }
+        NmeasTotal += Nmeas;
+        NmeasTotal += Nsweep;
+        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);
+        Nsweep = NsweepMin;
+    }
+    printGPAmeasTime();
+    reset();
+}
+
+void GPA::printGPAmeasTime()
+{
+    printLine();
+    printf(" Number of data points :  %11i\r\n", NmeasTotal);
+    printf(" Measurment time in sec: %12.2f\r\n", (float)((double)NmeasTotal*Ts));
+}
+
+void GPA::printNfexcDes()
+{
+    printLine();
+    printf("  Number of frequancy points:  %3i\r\n", NfexcDes);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPA.h	Fri May 10 18:15:41 2019 +0000
@@ -0,0 +1,78 @@
+class GPA
+{
+public:
+    
+    GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts);
+    GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin);
+    GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin);
+    GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int NstartMin, int NsweepMin);
+
+    float operator()(float inp, float out) {
+        return update((double)inp, (double)out);
+    }
+
+    virtual ~GPA();
+
+    void     reset();
+    float    update(double inp, double out);
+
+    void     printGPAfexcDes();
+    void     printGPAmeasPara();
+    void     printFullGPAmeasPara();
+    void     printGPAmeasTime();
+    void     printNfexcDes();
+
+private:
+
+    int     NfexcDes;
+    int     NperMin;
+    int     NmeasMin;
+    double  Ts;
+    double *fexcDes;
+    double  aAexcDes;
+    double  bAexcDes;
+
+    double  fnyq;
+    double  pi2;
+    double  pi2Ts;
+    double  piDiv2;
+    float   rad2deg;
+
+    int     Nmeas;
+    int     Nper;
+    double  dfexc;
+    double  fexc;
+    double  fexcPast;
+    int     i;
+    int     j;
+    double  scaleG;
+    double  cr;
+    double  ci;
+    double *sU;
+    double *sY;
+    double  sinarg;
+    int     NmeasTotal;
+    double  Aexc;
+    double  AexcPast;
+    double  pi2Tsfexc;
+    int     NstartMin;
+    int     NsweepMin;
+    int     Nsweep;
+    double  bfexc;
+    double  afexc;
+    double  aAexc;
+    double  bAexc;
+    double  AexcOut;
+    
+    void    assignParameters(int NfexcDes, int NperMin, int NmeasMin, double Ts, int NstartMin, int NsweepMin);
+    void    calculateDecreasingAmplitudeCoefficients(double Aexc0, double Aexc1);
+    void    initializeConstants(double Ts);
+    void    assignFilterStorage();
+    void    fexcDesLogspace(double fMin, double fMax, int NfexcDes);
+    void    calcGPAmeasPara(double fexcDes_i);
+    void    calcGPAsweepPara();
+    double  wrapAngle(double angle);
+    void    printLongLine();
+    void    printLine();
+
+};

--- a/main.cpp	Fri May 10 14:24:29 2019 +0000
+++ b/main.cpp	Fri May 10 18:15:41 2019 +0000
@@ -2,6 +2,7 @@
 #include "EncoderCounter.h"
 #include "PID_Cntrl.h"
 #include "LinearCharacteristics.h"
+#include "GPA.h"
 //------------------------------------------
 #define PI 3.1415927f
 //------------------------------------------
@@ -35,6 +36,28 @@
 PID_Cntrl dt1(0.0f, 0.0f, 0.0126f, 0.00067f, Ts, -3.0f, 3.0f);
 PID_Cntrl pi1(3.16f, 15.1f, 0.0f, 1.0f, Ts, -3.0f, 3.0f);
 float w = 0.5f;
+/*
+// low frequency region until 20 Hz with more amplitude
+float fMin = 0.3f;
+float fMax = 20.0f;
+int   NfexcDes = 60;
+float Aexc0 = 3.0f;
+float Aexc1 = 0.8f; // Aexc0/fMax;
+GPA   gpa(fMin, fMax, NfexcDes, Aexc0, Aexc1, Ts);
+float w = 0.0f;
+float exc = 0.0f;
+*/
+/*
+// all frequencies with less amplitude (until approx. 20 Hz bad quality)
+float fMin = 1.0f;
+float fMax = 0.99f/2.0f/Ts;
+int   NfexcDes = 200;
+float Aexc0 = 0.8f;
+float Aexc1 = 0.2f; // Aexc0/fMax;
+GPA   gpa(fMin, fMax, NfexcDes, Aexc0, Aexc1, Ts);
+float w = 0.0f;
+float exc = 0.0f;
+*/
 //******************************************************************************
 //---------- main loop -------------
 //******************************************************************************
@@ -59,6 +82,8 @@
     if(controller_active) {
         // controller update
         i_des = pi1(w - x) - dt1(x);
+        // i_des = pi1(exc - x) - dt1(x);
+        // exc = gpa(i_des, x);
     }
     out.write(i2u(i_des));
     if(++k>1000) {
@@ -90,6 +115,8 @@
             // reset controller here!!!
             dt1.reset(0.0);
             pi1.reset(0.0);
+            // gpa.reset();
+            // exc = 0.0f;
         } else
             pc.printf("Controller disabled\r\n");
     }