RT2_Cuboid_demo
Library for control puposes
Fork of
Cntrl_Lib
by Ruprecht Altenburger
Diff: GPA.cpp
- Revision:
- 0:e2a7d7f91e49
- Child:
- 5:d00752fcad65
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GPA.cpp Fri Sep 28 08:34:20 2018 +0000
@@ -0,0 +1,356 @@
+/*
+ GPA: frequency point wise gain and phase analyser to measure the frequency
+ respone of a 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: (logarithmic equaly spaced frequency points)
+
+ 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: minimal number of samples that are used for each frequency point
+ Ts: sampling time
+ Aexc0: excitation amplitude at fMin
+ Aexc1: excitation amplitude at fMax
+
+ 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)
+
+ 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)
+
+ *fexcDes: sorted frequency point array in Hz
+ Aexc0: excitation amplitude at fexcDes[0]
+ Aexc1: excitation amplitude at fexcDes[NfexcDes-1]
+ NfexcDes: length of fexcDes
+
+ hints: 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.
+
+ pseudo code for a closed loop measurement with a controller C:
+
+ excitation input at (1):
+
+ - measuring the plant P and the closed loop tf T = PC/(1 + PC):
+ desTorque = pi_w(omega_desired - omega + excWobble);
+ inpWobble = desTorque;
+ outWobble = omega;
+ excWobble = Wobble(excWobble, outWobble);
+
+ - measuring the controller C and the closed loop tf SC = C/(1 + PC)
+ desTorque = pi_w(omega_desired - omega + excWobble);
+ inpWobble = n_soll + excWobble - omega;
+ outWobble = desTorque;
+ excWobble = Wobble(inpWobble, outWobble);
+
+ excitation input at (2):
+
+ - measuring the plant P and the closed loop tf SP = P/(1 + PC):
+ desTorque = pi_w(omega_desired - omega) + excWobble;
+ inpWobble = desTorque;
+ outWobble = omega;
+ excWobble = Wobble(excWobble, outWobble);
+
+ 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 results are plotted to a terminal (putty) over a serial
+ connection and look as follows:
+ -----------------------------------------------------------------------------------------
+ fexc[Hz] |Gyu| ang(Gyu) |Gye| ang(Gye) |E| |U| |Y|
+ -----------------------------------------------------------------------------------------
+ 1.000e+00 2.924e+01 -1.572e+00 1.017e+00 -4.983e-02 5.000e+00 1.739e-01 5.084e+00
+
+ 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.141592653589793
+
+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 = (double)Ts;
+
+ // calculate logarithmic spaced frequency points
+ fexcDes = (double*)malloc(NfexcDes*sizeof(double));
+ fexcDesLogspace((double)fMin, (double)fMax, NfexcDes);
+
+ // calculate coefficients for decreasing amplitude (1/fexc)
+ this->aAexcDes = ((double)Aexc1 - (double)Aexc0)/(1.0/fexcDes[NfexcDes-1] - 1.0/fexcDes[0]);
+ this->bAexcDes = (double)Aexc0 - aAexcDes/fexcDes[0];
+
+ fnyq = 1.0/2.0/(double)Ts;
+ pi2 = 2.0*pi;
+ pi2Ts = pi2*(double)Ts;
+ piDiv2 = pi/2.0;
+
+ sU = (double*)malloc(3*sizeof(double));
+ sY = (double*)malloc(3*sizeof(double));
+ reset();
+}
+
+GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
+{
+ // convert fexcDes from float to double, it is assumed that it is sorted
+ this->NfexcDes = NfexcDes;
+ this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
+ for(int i = 0; i < NfexcDes; i++) {
+ this->fexcDes[i] = (double)fexcDes[i];
+ }
+ this->NperMin = NperMin;
+ this->NmeasMin = NmeasMin;
+ this->Ts = (double)Ts;
+
+ // calculate coefficients for decreasing amplitude (1/fexc)
+ this->aAexcDes = ((double)Aexc1 - (double)Aexc0)/(1.0/(double)f1 - 1.0/(double)f0);
+ this->bAexcDes = (double)Aexc0 - aAexcDes/fexcDes[0];
+
+ fnyq = 1.0/2.0/(double)Ts;
+ pi2 = 2.0*pi;
+ pi2Ts = pi2*(double)Ts;
+ piDiv2 = pi/2.0;
+
+ sU = (double*)malloc(3*sizeof(double));
+ sY = (double*)malloc(3*sizeof(double));
+ reset();
+}
+
+GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
+{
+ // convert fexcDes from float to double, it is assumed that it is sorted
+ this->NfexcDes = NfexcDes;
+ this->fexcDes = (double*)malloc(NfexcDes*sizeof(double));
+ for(int i = 0; i < NfexcDes; i++) {
+ this->fexcDes[i] = (double)fexcDes[i];
+ }
+ this->NperMin = NperMin;
+ this->NmeasMin = NmeasMin;
+ this->Ts = (double)Ts;
+
+ // calculate coefficients for decreasing amplitude (1/fexc)
+ this->aAexcDes = ((double)Aexc1 - (double)Aexc0)/(1.0/this->fexcDes[NfexcDes-1] - 1.0/this->fexcDes[0]);
+ this->bAexcDes = (double)Aexc0 - aAexcDes/fexcDes[0];
+
+ fnyq = 1.0/2.0/(double)Ts;
+ pi2 = 2.0*pi;
+ pi2Ts = pi2*(double)Ts;
+ piDiv2 = pi/2.0;
+
+ sU = (double*)malloc(3*sizeof(double));
+ sY = (double*)malloc(3*sizeof(double));
+ reset();
+}
+
+GPA::~GPA() {}
+
+void GPA::reset()
+{
+ Nmeas = 0;
+ Nper = 0;
+ fexc = 0.0;
+ fexcPast = 0.0;
+ ii = 1; // iterating through desired frequency points
+ jj = 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;
+}
+
+float GPA::update(double inp, double 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;
+ }
+ }
+ // secure sinarg starts at 0 (numerically maybe not given)
+ sinarg = 0.0;
+ // filter scaling
+ scaleG = 1.0/sqrt((double)Nmeas);
+ // filter coefficients
+ cr = cos(pi2Tsfexc);
+ ci = sin(pi2Tsfexc);
+ // filter storage
+ for(int i = 0; i < 3; i++) {
+ sU[i] = 0.0;
+ sY[i] = 0.0;
+ }
+ }
+ // filter step for signal su
+ sU[0] = scaleG*inp + 2.0*cr*sU[1] - sU[2];
+ sU[2] = sU[1];
+ sU[1] = sU[0];
+ // filter step for signal sy
+ sY[0] = scaleG*out + 2.0*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;
+ fexcPast = fexc;
+ // 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)(atan2(Yimag, Yreal) - atan2(Uimag, Ureal));
+ float absGye = (float)(Ymag/Aexc);
+ float angGye = (float)(atan2(Yimag, Yreal) + piDiv2);
+ // user info
+ if(ii == 2) {
+ printLine();
+ printf(" fexc[Hz] |Gyu| ang(Gyu) |Gye| ang(Gye) |E| |U| |Y|\r\n");
+ printLine();
+ }
+ printf("%11.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e\r\n", (float)fexc, absGyu, angGyu, absGye, angGye, (float)Aexc, Umag, Ymag);
+ } else {
+ jj += 1;
+ }
+ sinarg = fmod(sinarg + pi2Tsfexc, pi2);
+ NmeasTotal += 1;
+ return (float)(Aexc*sin(sinarg));
+}
+
+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::printLine()
+{
+ printf("-----------------------------------------------------------------------------------------\r\n");
+}
+
+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\r\n");
+ printLine();
+ for(int i = 0; i < NfexcDes; i++) {
+ calcGPAmeasPara(fexcDes[i]);
+ if(fexc == fexcPast || fexc >= fnyq) {
+ fexc = 0.0;
+ Nmeas = 0;
+ Nper = 0;
+ Aexc = 0.0;
+ } else {
+ Aexc = aAexcDes/fexc + bAexcDes;
+ fexcPast = fexc;
+ }
+ NmeasTotal += Nmeas;
+ printf("%12.2e %9.2e %10.2e %7i %6i \r\n", (float)fexcDes[i], (float)fexc, (float)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)((double)NmeasTotal*Ts));
+}
+
+void GPA::printNfexcDes()
+{
+ printLine();
+ printf(" number of frequancy points: %2i\r\n", NfexcDes);
+}