File content as of revision 0:d49418189c5c:

/*
    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);
}