.
Fork of Cntrlol_Lib by
GPAf.cpp
- Committer:
- altb
- Date:
- 2018-10-26
- Revision:
- 7:15ea5021288d
- Child:
- 8:32445aab4589
File content as of revision 7:15ea5021288d:
/* 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.14159f using namespace std; GPAf::GPAf(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 = (float)Ts; // calculate logarithmic spaced frequency points fexcDes = (float*)malloc(NfexcDes*sizeof(float)); fexcDesLogspace((float)fMin, (float)fMax, NfexcDes); // calculate coefficients for decreasing amplitude (1/fexc) this->aAexcDes = ((float)Aexc1 - (float)Aexc0)/(1.0f/fexcDes[NfexcDes-1] - 1.0f/fexcDes[0]); this->bAexcDes = (float)Aexc0 - aAexcDes/fexcDes[0]; fnyq = 1.0f/2.0f/(float)Ts; pi2 = 2.0f*pi; pi2Ts = pi2*(float)Ts; piDiv2 = pi/2.0f; sU = (float*)malloc(3*sizeof(float)); sY = (float*)malloc(3*sizeof(float)); reset(); } GPAf::GPAf(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) { // convert fexcDes from float to float, it is assumed that it is sorted this->NfexcDes = NfexcDes; this->fexcDes = (float*)malloc(NfexcDes*sizeof(float)); for(int i = 0; i < NfexcDes; i++) { this->fexcDes[i] = (float)fexcDes[i]; } this->NperMin = NperMin; this->NmeasMin = NmeasMin; this->Ts = (float)Ts; // calculate coefficients for decreasing amplitude (1/fexc) this->aAexcDes = ((float)Aexc1 - (float)Aexc0)/(1.0f/(float)f1 - 1.0f/(float)f0); this->bAexcDes = (float)Aexc0 - aAexcDes/fexcDes[0]; fnyq = 1.0f/2.0f/(float)Ts; pi2 = 2.0f*pi; pi2Ts = pi2*(float)Ts; piDiv2 = pi/2.0f; sU = (float*)malloc(3*sizeof(float)); sY = (float*)malloc(3*sizeof(float)); reset(); } GPAf::GPAf(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1) { // convert fexcDes from float to float, it is assumed that it is sorted this->NfexcDes = NfexcDes; this->fexcDes = (float*)malloc(NfexcDes*sizeof(float)); for(int i = 0; i < NfexcDes; i++) { this->fexcDes[i] = (float)fexcDes[i]; } this->NperMin = NperMin; this->NmeasMin = NmeasMin; this->Ts = (float)Ts; // calculate coefficients for decreasing amplitude (1/fexc) this->aAexcDes = ((float)Aexc1 - (float)Aexc0)/(1.0f/this->fexcDes[NfexcDes-1] - 1.0f/this->fexcDes[0]); this->bAexcDes = (float)Aexc0 - aAexcDes/fexcDes[0]; fnyq = 1.0f/2.0f/(float)Ts; pi2 = 2.0f*pi; pi2Ts = pi2*(float)Ts; piDiv2 = pi/2.0f; sU = (float*)malloc(3*sizeof(float)); sY = (float*)malloc(3*sizeof(float)); reset(); } GPAf::~GPAf() {} void GPAf::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; scaleH = 2.0; wk = 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 GPAf::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; } } // secure sinarg starts at 0 (numerically maybe not given) sinarg = 0.0; // 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.0; sY[i] = 0.0; } } // update hann window calcHann(); // filter step for signal su sU[0] = wk*scaleG*inp + 2.0f*cr*sU[1] - sU[2]; sU[2] = sU[1]; sU[1] = sU[0]; // filter step for signal sy sY[0] = wk*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; fexcPast = fexc; // calculate the one point dft float Ureal = 2.0f*scaleH*scaleG*(cr*sU[1] - sU[2]); float Uimag = 2.0f*scaleH*scaleG*ci*sU[1]; float Yreal = 2.0f*scaleH*scaleG*(cr*sY[1] - sY[2]); float Yimag = 2.0f*scaleH*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 GPAf::fexcDesLogspace(float fMin, float fMax, int NfexcDes) { // calculate logarithmic spaced frequency points float Gain = log10(fMax/fMin)/((float)NfexcDes - 1.0f); float expon = 0.0; for(int i = 0; i < NfexcDes; i++) { fexcDes[i] = fMin*pow(10.0f, expon); expon += Gain; } } void GPAf::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 GPAf::calcHann() { wk = 0.5f - 0.5f*cos(2.0f*pi*((float)jj-1.0f)/(float)Nmeas); } void GPA::printLine() { printf("-----------------------------------------------------------------------------------------\r\n"); } void GPAf::printGPAfexcDes() { printLine(); for(int i = 0; i < NfexcDes; i++) { printf("%9.4f\r\n", (float)fexcDes[i]); } } void GPAf::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 GPAf::printGPAmeasTime() { printLine(); printf(" number of data points: %9i\r\n", NmeasTotal); printf(" measurment time in sec: %9.2f\r\n", (float)((float)NmeasTotal*Ts)); } void GPAf::printNfexcDes() { printLine(); printf(" number of frequancy points: %2i\r\n", NfexcDes); }