File content as of revision 7:7715f92c5182:

/*
    GPA: Frequency point wise gain and phase analyser to measure the frequency
         respone function (FRF) of a dynamical system

         Hint:       If the plant has a pole at zero, is unstable or weakly damped the measurement has to be perfomed in closed loop
         Assumption: The system is at the desired steady state of interest when the measurment starts


    Instantiate option 1: (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
            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


    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


    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 try to increase Nmeas (could not be reached).
              
        
    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, 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);
}