enhanced functionality in V01 vs. V00, V02 finished, conversion to double precsision in V03

Dependencies:   mbed

Committer:
pmic
Date:
Mon Apr 09 17:50:45 2018 +0000
Revision:
22:c895fa4d7319
Parent:
21:55b11670959e
all in float!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 10:b7f142952df9 1 /*
pmic 10:b7f142952df9 2 GPA: frequency point wise gain and phase analyser to measure the frequency
pmic 21:55b11670959e 3 respone of a dynamical system
pmic 12:e54941459353 4
pmic 10:b7f142952df9 5 hint: the measurements should only be perfomed in closed loop
pmic 10:b7f142952df9 6 assumption: the system is at the desired steady state of interest when
pmic 10:b7f142952df9 7 the measurment starts
pmic 12:e54941459353 8
pmic 10:b7f142952df9 9 exc(2) C: controller
pmic 10:b7f142952df9 10 | P: plant
pmic 12:e54941459353 11 v
pmic 10:b7f142952df9 12 exc(1) --> o ->| C |--->o------->| P |----------> out
pmic 10:b7f142952df9 13 ^ | |
pmic 10:b7f142952df9 14 | --> inp | exc: excitation signal (E)
pmic 10:b7f142952df9 15 | | inp: input plant (U)
pmic 10:b7f142952df9 16 -------------------------------- out: output plant (Y)
pmic 12:e54941459353 17
pmic 10:b7f142952df9 18 instantiate option 1:
pmic 10:b7f142952df9 19 GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
pmic 12:e54941459353 20
pmic 10:b7f142952df9 21 fMin: minimal desired frequency that should be measured in Hz
pmic 10:b7f142952df9 22 fMax: maximal desired frequency that should be measured in Hz
pmic 10:b7f142952df9 23 NfexcDes: number of logarithmic equaly spaced frequency points
pmic 10:b7f142952df9 24 NperMin: minimal number of periods that are used for each frequency point
pmic 21:55b11670959e 25 NmeasMin: minimal number of samples that are used for each frequency point
pmic 10:b7f142952df9 26 Ts: sampling time
pmic 12:e54941459353 27 Aexc0: excitation amplitude at fMin
pmic 10:b7f142952df9 28 Aexc1: excitation amplitude at fMax
pmic 12:e54941459353 29
pmic 10:b7f142952df9 30 hints: the amplitude drops with 1/fexc, if you're using
pmic 21:55b11670959e 31 Axc1 = Aexc0/fMax then d/dt exc = const., this is recommended
pmic 10:b7f142952df9 32 if your controller does not have a rolloff.
pmic 11:3b55cce7a2f3 33 if a desired frequency point is not measured try to increase Nmeas.
pmic 12:e54941459353 34
pmic 21:55b11670959e 35 pseudo code for a closed loop measurement with a controller C:
pmic 12:e54941459353 36
pmic 10:b7f142952df9 37 excitation input at (1):
pmic 16:9627301c7983 38
pmic 16:9627301c7983 39 - measuring the plant P and the closed loop tf T = PC/(1 + PC):
pmic 16:9627301c7983 40 desTorque = pi_w(omega_desired - omega + excWobble);
pmic 16:9627301c7983 41 inpWobble = desTorque;
pmic 16:9627301c7983 42 outWobble = omega;
pmic 16:9627301c7983 43 excWobble = Wobble(excWobble, outWobble);
pmic 16:9627301c7983 44
pmic 16:9627301c7983 45 - measuring the controller C and the closed loop tf SC = C/(1 + PC)
pmic 16:9627301c7983 46 desTorque = pi_w(omega_desired - omega + excWobble);
pmic 16:9627301c7983 47 inpWobble = n_soll + excWobble - omega;
pmic 16:9627301c7983 48 outWobble = desTorque;
pmic 16:9627301c7983 49 excWobble = Wobble(inpWobble, outWobble);
pmic 12:e54941459353 50
pmic 10:b7f142952df9 51 excitation input at (2):
pmic 16:9627301c7983 52
pmic 16:9627301c7983 53 - measuring the plant P and the closed loop tf SP = P/(1 + PC):
pmic 16:9627301c7983 54 desTorque = pi_w(omega_desired - omega) + excWobble;
pmic 16:9627301c7983 55 inpWobble = desTorque;
pmic 16:9627301c7983 56 outWobble = omega;
pmic 16:9627301c7983 57 excWobble = Wobble(excWobble, outWobble);
pmic 12:e54941459353 58
pmic 10:b7f142952df9 59 usage:
pmic 16:9627301c7983 60 exc(k+1) = myGPA(inp(k), out(k)) does update the internal states of the
pmic 16:9627301c7983 61 gpa at the timestep k and returns the excitation signal for the timestep
pmic 16:9627301c7983 62 k+1. the results are plotted to a terminal (putty) over a serial
pmic 16:9627301c7983 63 connection and look as follows:
pmic 10:b7f142952df9 64 -----------------------------------------------------------------------------------------
pmic 10:b7f142952df9 65 fexc[Hz] |Gyu| ang(Gyu) |Gye| ang(Gye) |E| |U| |Y|
pmic 10:b7f142952df9 66 -----------------------------------------------------------------------------------------
pmic 16:9627301c7983 67 1.000e+00 2.924e+01 -1.572e+00 1.017e+00 -4.983e-02 5.000e+00 1.739e-01 5.084e+00
pmic 12:e54941459353 68
pmic 10:b7f142952df9 69 in matlab you can use:
pmic 10:b7f142952df9 70 dataNucleo = [... insert measurement data here ...];
pmic 10:b7f142952df9 71 g = frd(dataNucleo(:,2).*exp(1i*dataNucleo(:,3)), dataNucleo(:,1), Ts, 'Units', 'Hz');
pmic 10:b7f142952df9 72 gcl = frd(dataNucleo(:,4).*exp(1i*dataNucleo(:,5)), dataNucleo(:,1), Ts, 'Units', 'Hz');
pmic 12:e54941459353 73
pmic 10:b7f142952df9 74 if you're evaluating more than one measurement which contain equal frequency points try:
pmic 10:b7f142952df9 75 dataNucleo = [dataNucleo1; dataNucleo2];
pmic 10:b7f142952df9 76 [~, ind] = unique(dataNucleo(:,1),'stable');
pmic 10:b7f142952df9 77 dataNucleo = dataNucleo(ind,:);
pmic 12:e54941459353 78
pmic 10:b7f142952df9 79 autor: M.E. Peter
pmic 12:e54941459353 80 */
pmic 10:b7f142952df9 81
pmic 8:3b98b6e1ead3 82 #include "GPA.h"
pmic 8:3b98b6e1ead3 83 #include "mbed.h"
pmic 8:3b98b6e1ead3 84 #include "math.h"
pmic 22:c895fa4d7319 85 #define pi 3.1415927f
pmic 8:3b98b6e1ead3 86
pmic 8:3b98b6e1ead3 87 using namespace std;
pmic 8:3b98b6e1ead3 88
pmic 9:4e6f3404d473 89 GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
pmic 8:3b98b6e1ead3 90 {
pmic 9:4e6f3404d473 91 this->NfexcDes = NfexcDes;
pmic 9:4e6f3404d473 92 this->NperMin = NperMin;
pmic 8:3b98b6e1ead3 93 this->NmeasMin = NmeasMin;
pmic 22:c895fa4d7319 94 this->Ts = Ts;
pmic 9:4e6f3404d473 95
pmic 9:4e6f3404d473 96 // calculate logarithmic spaced frequency points
pmic 22:c895fa4d7319 97 fexcDes = (float*)malloc(NfexcDes*sizeof(float));
pmic 22:c895fa4d7319 98 fexcDesLogspace(fMin, fMax, NfexcDes);
pmic 9:4e6f3404d473 99
pmic 9:4e6f3404d473 100 // calculate coefficients for decreasing amplitude (1/fexc)
pmic 22:c895fa4d7319 101 this->aAexcDes = (Aexc1 - Aexc0)/(1.0f/fexcDes[NfexcDes-1] - 1.0f/fexcDes[0]);
pmic 22:c895fa4d7319 102 this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0];
pmic 8:3b98b6e1ead3 103
pmic 22:c895fa4d7319 104 fnyq = 1/2.0f/Ts;
pmic 22:c895fa4d7319 105 pi2 = 2.0f*pi;
pmic 22:c895fa4d7319 106 pi2Ts = pi2*Ts;
pmic 22:c895fa4d7319 107 piDiv2 = pi/2.0f;
pmic 8:3b98b6e1ead3 108
pmic 22:c895fa4d7319 109 sU = (float*)malloc(3*sizeof(float));
pmic 22:c895fa4d7319 110 sY = (float*)malloc(3*sizeof(float));
pmic 8:3b98b6e1ead3 111 reset();
pmic 8:3b98b6e1ead3 112 }
pmic 8:3b98b6e1ead3 113
pmic 8:3b98b6e1ead3 114 GPA::~GPA() {}
pmic 8:3b98b6e1ead3 115
pmic 8:3b98b6e1ead3 116 void GPA::reset()
pmic 8:3b98b6e1ead3 117 {
pmic 8:3b98b6e1ead3 118 Nmeas = 0;
pmic 9:4e6f3404d473 119 Nper = 0;
pmic 22:c895fa4d7319 120 fexc = 0.0f;
pmic 22:c895fa4d7319 121 fexcPast = 0.0f;
pmic 8:3b98b6e1ead3 122 ii = 1; // iterating through desired frequency points
pmic 8:3b98b6e1ead3 123 jj = 1; // iterating through measurement points w.r.t. reachable frequency
pmic 22:c895fa4d7319 124 scaleG = 0.0f;
pmic 22:c895fa4d7319 125 cr = 0.0f;
pmic 22:c895fa4d7319 126 ci = 0.0f;
pmic 8:3b98b6e1ead3 127 for(int i = 0; i < 3; i++) {
pmic 22:c895fa4d7319 128 sU[i] = 0.0f;
pmic 22:c895fa4d7319 129 sY[i] = 0.0f;
pmic 8:3b98b6e1ead3 130 }
pmic 22:c895fa4d7319 131 sinarg = 0.0f;
pmic 8:3b98b6e1ead3 132 NmeasTotal = 0;
pmic 22:c895fa4d7319 133 Aexc = 0.0f;
pmic 22:c895fa4d7319 134 pi2Tsfexc = 0.0f;
pmic 8:3b98b6e1ead3 135 }
pmic 8:3b98b6e1ead3 136
pmic 22:c895fa4d7319 137 float GPA::update(float inp, float out)
pmic 8:3b98b6e1ead3 138 {
pmic 8:3b98b6e1ead3 139 // a new frequency point has been reached
pmic 8:3b98b6e1ead3 140 if(jj == 1) {
pmic 9:4e6f3404d473 141 // get a new unique frequency point
pmic 8:3b98b6e1ead3 142 while(fexc == fexcPast) {
pmic 8:3b98b6e1ead3 143 // measurement finished
pmic 9:4e6f3404d473 144 if(ii > NfexcDes) {
pmic 8:3b98b6e1ead3 145 return 0.0f;
pmic 8:3b98b6e1ead3 146 }
pmic 8:3b98b6e1ead3 147 calcGPAmeasPara(fexcDes[ii - 1]);
pmic 8:3b98b6e1ead3 148 // secure fexc is not higher or equal to nyquist frequency
pmic 8:3b98b6e1ead3 149 if(fexc >= fnyq) {
pmic 8:3b98b6e1ead3 150 fexc = fexcPast;
pmic 8:3b98b6e1ead3 151 }
pmic 8:3b98b6e1ead3 152 // no frequency found
pmic 8:3b98b6e1ead3 153 if(fexc == fexcPast) {
pmic 8:3b98b6e1ead3 154 ii += 1;
pmic 9:4e6f3404d473 155 } else {
pmic 9:4e6f3404d473 156 Aexc = aAexcDes/fexc + bAexcDes;
pmic 9:4e6f3404d473 157 pi2Tsfexc = pi2Ts*fexc;
pmic 8:3b98b6e1ead3 158 }
pmic 8:3b98b6e1ead3 159 }
pmic 12:e54941459353 160 // secure sinarg starts at 0 (numerically maybe not given)
pmic 22:c895fa4d7319 161 sinarg = 0.0f;
pmic 8:3b98b6e1ead3 162 // filter scaling
pmic 22:c895fa4d7319 163 scaleG = 1.0f/sqrt((float)Nmeas);
pmic 8:3b98b6e1ead3 164 // filter coefficients
pmic 9:4e6f3404d473 165 cr = cos(pi2Tsfexc);
pmic 9:4e6f3404d473 166 ci = sin(pi2Tsfexc);
pmic 8:3b98b6e1ead3 167 // filter storage
pmic 8:3b98b6e1ead3 168 for(int i = 0; i < 3; i++) {
pmic 22:c895fa4d7319 169 sU[i] = 0.0f;
pmic 22:c895fa4d7319 170 sY[i] = 0.0f;
pmic 8:3b98b6e1ead3 171 }
pmic 8:3b98b6e1ead3 172 }
pmic 8:3b98b6e1ead3 173 // filter step for signal su
pmic 22:c895fa4d7319 174 sU[0] = scaleG*inp + 2.0f*cr*sU[1] - sU[2];
pmic 8:3b98b6e1ead3 175 sU[2] = sU[1];
pmic 8:3b98b6e1ead3 176 sU[1] = sU[0];
pmic 8:3b98b6e1ead3 177 // filter step for signal sy
pmic 22:c895fa4d7319 178 sY[0] = scaleG*out + 2.0f*cr*sY[1] - sY[2];
pmic 8:3b98b6e1ead3 179 sY[2] = sY[1];
pmic 8:3b98b6e1ead3 180 sY[1] = sY[0];
pmic 9:4e6f3404d473 181 // measurement of frequencypoint is finished
pmic 8:3b98b6e1ead3 182 if(jj == Nmeas) {
pmic 8:3b98b6e1ead3 183 jj = 1;
pmic 10:b7f142952df9 184 ii += 1;
pmic 12:e54941459353 185 fexcPast = fexc;
pmic 8:3b98b6e1ead3 186 // calculate the one point dft
pmic 22:c895fa4d7319 187 float Ureal = 2.0f*scaleG*(cr*sU[1] - sU[2]);
pmic 22:c895fa4d7319 188 float Uimag = 2.0f*scaleG*ci*sU[1];
pmic 22:c895fa4d7319 189 float Yreal = 2.0f*scaleG*(cr*sY[1] - sY[2]);
pmic 22:c895fa4d7319 190 float Yimag = 2.0f*scaleG*ci*sY[1];
pmic 8:3b98b6e1ead3 191 // calculate magnitude and angle
pmic 22:c895fa4d7319 192 float Umag = sqrt(Ureal*Ureal + Uimag*Uimag);
pmic 22:c895fa4d7319 193 float Ymag = sqrt(Yreal*Yreal + Yimag*Yimag);
pmic 22:c895fa4d7319 194 float absGyu = Ymag/Umag;
pmic 22:c895fa4d7319 195 float angGyu = atan2(Yimag, Yreal) - atan2(Uimag, Ureal);
pmic 22:c895fa4d7319 196 float absGye = Ymag/Aexc;
pmic 22:c895fa4d7319 197 float angGye = (atan2(Yimag, Yreal) + piDiv2);
pmic 8:3b98b6e1ead3 198 // user info
pmic 12:e54941459353 199 if(ii == 2) {
pmic 8:3b98b6e1ead3 200 printLine();
pmic 9:4e6f3404d473 201 printf(" fexc[Hz] |Gyu| ang(Gyu) |Gye| ang(Gye) |E| |U| |Y|\r\n");
pmic 8:3b98b6e1ead3 202 printLine();
pmic 8:3b98b6e1ead3 203 }
pmic 22:c895fa4d7319 204 printf("%11.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e\r\n", fexc, absGyu, angGyu, absGye, angGye, Aexc, Umag, Ymag);
pmic 8:3b98b6e1ead3 205 } else {
pmic 8:3b98b6e1ead3 206 jj += 1;
pmic 8:3b98b6e1ead3 207 }
pmic 9:4e6f3404d473 208 sinarg = fmod(sinarg + pi2Tsfexc, pi2);
pmic 8:3b98b6e1ead3 209 NmeasTotal += 1;
pmic 22:c895fa4d7319 210 return Aexc*sin(sinarg);
pmic 8:3b98b6e1ead3 211 }
pmic 8:3b98b6e1ead3 212
pmic 22:c895fa4d7319 213 void GPA::fexcDesLogspace(float fMin, float fMax, int NfexcDes)
pmic 9:4e6f3404d473 214 {
pmic 9:4e6f3404d473 215 // calculate logarithmic spaced frequency points
pmic 22:c895fa4d7319 216 float Gain = log10(fMax/fMin)/((float)NfexcDes - 1.0f);
pmic 22:c895fa4d7319 217 float expon = 0.0f;
pmic 9:4e6f3404d473 218 for(int i = 0; i < NfexcDes; i++) {
pmic 22:c895fa4d7319 219 fexcDes[i] = fMin*pow(10.0f, expon);
pmic 9:4e6f3404d473 220 expon += Gain;
pmic 9:4e6f3404d473 221 }
pmic 9:4e6f3404d473 222 }
pmic 9:4e6f3404d473 223
pmic 22:c895fa4d7319 224 void GPA::calcGPAmeasPara(float fexcDes_i)
pmic 9:4e6f3404d473 225 {
pmic 9:4e6f3404d473 226 // Nmeas has to be an integer
pmic 9:4e6f3404d473 227 Nper = NperMin;
pmic 22:c895fa4d7319 228 Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
pmic 9:4e6f3404d473 229 // secure that the minimal number of measurements is fullfilled
pmic 9:4e6f3404d473 230 int Ndelta = NmeasMin - Nmeas;
pmic 9:4e6f3404d473 231 if(Ndelta > 0) {
pmic 22:c895fa4d7319 232 Nper = (int)ceil((float)NmeasMin*fexcDes_i*Ts);
pmic 22:c895fa4d7319 233 Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
pmic 9:4e6f3404d473 234 }
pmic 9:4e6f3404d473 235 // evaluating reachable frequency
pmic 22:c895fa4d7319 236 fexc = (float)Nper/(float)Nmeas/Ts;
pmic 9:4e6f3404d473 237 }
pmic 9:4e6f3404d473 238
pmic 9:4e6f3404d473 239 void GPA::printLine()
pmic 9:4e6f3404d473 240 {
pmic 9:4e6f3404d473 241 printf("-----------------------------------------------------------------------------------------\r\n");
pmic 9:4e6f3404d473 242 }
pmic 9:4e6f3404d473 243
pmic 9:4e6f3404d473 244 void GPA::printGPAfexcDes()
pmic 9:4e6f3404d473 245 {
pmic 9:4e6f3404d473 246 printLine();
pmic 9:4e6f3404d473 247 for(int i = 0; i < NfexcDes; i++) {
pmic 22:c895fa4d7319 248 printf("%9.4f\r\n", fexcDes[i]);
pmic 9:4e6f3404d473 249 }
pmic 9:4e6f3404d473 250 }
pmic 9:4e6f3404d473 251
pmic 9:4e6f3404d473 252 void GPA::printGPAmeasPara()
pmic 9:4e6f3404d473 253 {
pmic 9:4e6f3404d473 254 printLine();
pmic 9:4e6f3404d473 255 printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper\r\n");
pmic 9:4e6f3404d473 256 printLine();
pmic 9:4e6f3404d473 257 for(int i = 0; i < NfexcDes; i++) {
pmic 9:4e6f3404d473 258 calcGPAmeasPara(fexcDes[i]);
pmic 9:4e6f3404d473 259 if(fexc == fexcPast || fexc >= fnyq) {
pmic 22:c895fa4d7319 260 fexc = 0.0f;
pmic 9:4e6f3404d473 261 Nmeas = 0;
pmic 9:4e6f3404d473 262 Nper = 0;
pmic 22:c895fa4d7319 263 Aexc = 0;
pmic 9:4e6f3404d473 264 } else {
pmic 9:4e6f3404d473 265 Aexc = aAexcDes/fexc + bAexcDes;
pmic 9:4e6f3404d473 266 fexcPast = fexc;
pmic 9:4e6f3404d473 267 }
pmic 9:4e6f3404d473 268 NmeasTotal += Nmeas;
pmic 22:c895fa4d7319 269 printf("%12.2e %9.2e %10.2e %7i %6i \r\n", fexcDes[i], fexc, Aexc, Nmeas, Nper);
pmic 9:4e6f3404d473 270 }
pmic 9:4e6f3404d473 271 printGPAmeasTime();
pmic 9:4e6f3404d473 272 reset();
pmic 9:4e6f3404d473 273 }
pmic 9:4e6f3404d473 274
pmic 8:3b98b6e1ead3 275 void GPA::printGPAmeasTime()
pmic 8:3b98b6e1ead3 276 {
pmic 8:3b98b6e1ead3 277 printLine();
pmic 8:3b98b6e1ead3 278 printf(" number of data points: %9i\r\n", NmeasTotal);
pmic 22:c895fa4d7319 279 printf(" measurment time in sec: %9.2f\r\n", (float)NmeasTotal*Ts);
pmic 9:4e6f3404d473 280 }