2nd Library
Embed:
(wiki syntax)
Show/hide line numbers
GPA.cpp
00001 /* 00002 GPA: Frequency point wise gain and phase analyser to measure a frequency respone function (FRF) of a dynamical system 00003 00004 Hint: If the plant has a pole at zero, is unstable or weakly damped the measurement has to be perfomed 00005 in closed loop (this is NOT tfestimate, the algorithm is based on the one point DFT). 00006 Assumption: The system is and remains at the desired steady state when the measurment starts 00007 00008 Note: The amplitude drops with 1/fexc, if you're using Axc1 = Aexc0/fMax then d/dt exc = const., 00009 this is recommended if your controller does not have a rolloff. If a desired frequency point 00010 is not measured (could not be reached) try to increase Nmeas. 00011 00012 00013 Instantiate option 0: ("Not a Jedi yet" users, for logarithmic equaly spaced frequency points) 00014 00015 GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts) 00016 00017 fMin: Minimal desired frequency that should be measured in Hz 00018 fMax: Maximal desired frequency that should be measured in Hz 00019 NfexcDes: Number of logarithmic equaly spaced frequency points between fMin and fMax 00020 Aexc0: Excitation amplitude at fMin 00021 Aexc1: Excitation amplitude at fMax 00022 Ts: Sampling time in sec 00023 00024 Default values are as follows: 00025 int NperMin = 3; 00026 int NmeasMin = (int)ceil(1.0f/Ts); 00027 int Nstart = (int)ceil(3.0f/Ts); 00028 int Nsweep = (int)ceil(0.0f/Ts); 00029 00030 Instantiate option 1: ("Jedi or Sith Lord", for logarithmic equaly spaced frequency points) 00031 00032 GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep) 00033 00034 fMin: Minimal desired frequency that should be measured in Hz 00035 fMax: Maximal desired frequency that should be measured in Hz 00036 NfexcDes: Number of logarithmic equaly spaced frequency points 00037 NperMin: Minimal number of periods that are used for each frequency point 00038 NmeasMin: Minimal number of samples that are used for each frequency point 00039 Ts: Sampling time in sec 00040 Aexc0: Excitation amplitude at fMin 00041 Aexc1: Excitation amplitude at fMax 00042 Nstart: Minimal number of samples to sweep to the first frequency point (can be equal 0) 00043 Nsweep: Minimal number of samples to sweep from freq. point to freq. point (can be equal 0) 00044 00045 00046 Instantiate option 2: (for a second, refined frequency grid measurement) 00047 00048 GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep) 00049 00050 f0: Frequency point for the calculation of Aexc0 in Hz (should be chosen like in the first measurement) 00051 f1: Frequency point for the calculation of Aexc1 in Hz (should be chosen like in the first measurement) 00052 *fexcDes: Sorted frequency point array in Hz 00053 NfexcDes: Length of fexcDes 00054 00055 For the other parameters see above. 00056 00057 Instantiate option 3: (for an arbitary but sorted frequency grid measurement) 00058 00059 GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep) 00060 00061 *fexcDes: Sorted frequency point array in Hz 00062 Aexc0: Excitation amplitude at fexcDes[0] 00063 Aexc1: Excitation amplitude at fexcDes[NfexcDes-1] 00064 NfexcDes: Length of fexcDes 00065 00066 For the other parameters see above. 00067 00068 00069 Block diagram: 00070 00071 w (const.) exc(2) C: controller 00072 | | P: plant 00073 v e v 00074 exc(1) --> o ->| C |--->o------->| P |----------> out (y) 00075 ^ - | | 00076 | --> inp (u) | exc (R): excitation signal 00077 | | inp (U): input plant 00078 -------------------------------- out (Y): output plant 00079 00080 00081 Pseudo code for an open loop measurement: 00082 00083 - Measuring the plant P = Gyu = Gyr: 00084 00085 u = w + exc; 00086 ... write output u here! it follows exc(k+1) ... 00087 exc = Wobble(exc, y); 00088 00089 Closed loop FRF calculus with a stabilizing controller C: 00090 S = 1/(1 + C*P); % ( exc1 -> e , 1/(1 + C*P) ) sensitivity, contr. error rejection, robustness (1/max|S|) 00091 T = 1 - S; % ( w -> y , C*P/(1 + C*P) ) compl. sensitivity, tracking performance 00092 CS = C*S; % ( exc1 -> u , C/(1 + C*P) ) control effort, disturbance plant output on plant input 00093 PS = P*S; % ( exc2 -> y , P/(1 + C*P) ) compliance, disturbance plant input on plant output 00094 00095 00096 Pseudo code for a closed loop measurement with stabilizing controller C: 00097 00098 Excitation at excitation input (1): 00099 00100 - Measuring the plant P = Gyu and the closed loop tf T = PC/(1 + PC) = Gyr: 00101 00102 u = C(w - y + exc); 00103 ... write output u here! it follows exc(k+1) ... 00104 exc = Wobble(u, y); 00105 00106 Closed loop FRF calculus: 00107 S = 1 - T; 00108 PS = P*S; 00109 CS = T/P; 00110 C = C/S; 00111 00112 Excitation at excitation input (2): 00113 00114 - Measuring the plant P = Gyu and the closed loop tf PS = P/(1 + PC) = Gyr: 00115 00116 u = C(w - y) + exc; 00117 ... write output u here! it follows exc(k+1) ... 00118 exc = Wobble(u, y); 00119 00120 Closed loop FRF calculus: 00121 S = PS/P; 00122 T = 1 - S; 00123 CS = T/P; 00124 C = C/S; 00125 00126 00127 Usage: 00128 exc(k+1) = myGPA(inp(k), out(k)) does update the internal states of the 00129 gpa at the timestep k and returns the excitation signal for the timestep 00130 k+1. The FRF data are plotted to a terminal (Putty) over a serial 00131 connection and look as follows: 00132 00133 -------------------------------------------------------------------------------- 00134 fexc[Hz] |Gyu| deg(Gyu) |Gyr| deg(Gyr) |U| |Y| |R| 00135 -------------------------------------------------------------------------------- 00136 5.0000e-02 1.001e+00 -0.309 1.001e+00 -0.309 4.000e-01 4.000e-01 4.005e-01 00137 . . . . . . . . 00138 . . . . . . . . 00139 . . . . . . . . 00140 00141 In Matlab you can use the editor as follows: 00142 data = [... insert measurement data here ...]; 00143 gyu = frd(data(:,2).*exp(1i*data(:,3)*pi/180), data(:,1), Ts, 'Units', 'Hz'); 00144 gyr = frd(data(:,4).*exp(1i*data(:,5)*pi/180), data(:,1), Ts, 'Units', 'Hz'); 00145 00146 If you're evaluating more than one measurement which contain equal frequency points use: 00147 data = [data1; data2]; 00148 [~, ind] = unique(data(:,1), 'stable'); 00149 data = data(ind,:); 00150 00151 00152 Autor and Copyrigth: 2018 / 2019 / 2020 / M.E. Peter 00153 00154 */ 00155 00156 #include "GPA.h" 00157 #include "mbed.h" 00158 #include "math.h" 00159 #define pi 3.14159265358979323846 // M_PI 00160 00161 using namespace std; 00162 00163 // ----------------------------------------------------------------------------- 00164 // instantiate 00165 // ----------------------------------------------------------------------------- 00166 00167 GPA::GPA(float fMin, float fMax, int NfexcDes, float Aexc0, float Aexc1, float Ts) 00168 { 00169 doPrint = true; 00170 int NperMin = 3; 00171 int NmeasMin = (int)ceil(1.0f/Ts); 00172 int Nstart = (int)ceil(3.0f/Ts); 00173 int Nsweep = (int)ceil(0.0f/Ts); 00174 new_data_available = false; 00175 meas_is_finished = false; 00176 start_now = false; 00177 00178 setParameters(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, Nstart, Nsweep, doPrint); 00179 } 00180 00181 GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep) 00182 { 00183 doPrint = true; 00184 new_data_available = false; 00185 meas_is_finished = false; 00186 start_now = false; 00187 setParameters(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, Nstart, Nsweep, doPrint); 00188 } 00189 00190 GPA::GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep) 00191 { 00192 doPrint = true; 00193 new_data_available = false; 00194 meas_is_finished = false; 00195 00196 assignParameters(NfexcDes, NperMin, NmeasMin, Ts, Nstart, Nsweep); 00197 00198 // convert fexcDes from float to float, it is assumed that it is sorted 00199 this->fexcDes = (float*)malloc(NfexcDes*sizeof(float)); 00200 for(int i = 0; i < NfexcDes; i++) { 00201 this->fexcDes[i] = (float)fexcDes[i]; 00202 } 00203 00204 calculateDecreasingAmplitudeCoefficients(Aexc0, Aexc1); 00205 initializeConstants(Ts); 00206 assignFilterStorage(); 00207 reset(); 00208 } 00209 00210 GPA::GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep) 00211 { 00212 doPrint = true; 00213 assignParameters(NfexcDes, NperMin, NmeasMin, Ts, Nstart, Nsweep); 00214 00215 // convert fexcDes from float to float, it is assumed that it is sorted 00216 this->fexcDes = (float*)malloc(NfexcDes*sizeof(float)); 00217 for(int i = 0; i < NfexcDes; i++) { 00218 this->fexcDes[i] = fexcDes[i]; 00219 } 00220 00221 calculateDecreasingAmplitudeCoefficients(Aexc0, Aexc1); 00222 initializeConstants(Ts); 00223 assignFilterStorage(); 00224 reset(); 00225 } 00226 00227 GPA::GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep, bool doPrint) 00228 { 00229 setParameters(fMin, fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1, Nstart, Nsweep, doPrint); 00230 } 00231 00232 // ----------------------------------------------------------------------------- 00233 // virtual, reset and set 00234 // ----------------------------------------------------------------------------- 00235 00236 GPA::~GPA() {} 00237 00238 void GPA::setParameters(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1, int Nstart, int Nsweep, bool doPrint) 00239 { 00240 this->doPrint = doPrint; 00241 assignParameters(NfexcDes, NperMin, NmeasMin, Ts, Nstart, Nsweep); 00242 00243 // calculate logarithmic spaced frequency points 00244 fexcDes = (float*)malloc(NfexcDes*sizeof(float)); 00245 fexcDesLogspace(fMin, fMax, NfexcDes); 00246 00247 calculateDecreasingAmplitudeCoefficients(Aexc0, Aexc1); 00248 initializeConstants(Ts); 00249 assignFilterStorage(); 00250 reset(); 00251 00252 } 00253 00254 void GPA::reset() 00255 { 00256 Nmeas = 0; 00257 Nper = 0; 00258 dfexc = 0.0; 00259 fexc = 0.0; 00260 fexcPast = 0.0f; 00261 dfexcj = 0.0f; 00262 i = 1; // iterating through desired frequency points 00263 j = 1; // iterating through measurement points w.r.t. reachable frequency 00264 scaleG = 0.0; 00265 cr = 0.0; 00266 ci = 0.0; 00267 for(int i = 0; i < 3; i++) { 00268 sU[i] = 0.0; 00269 sY[i] = 0.0; 00270 } 00271 sinarg = 0.0; 00272 sinargR = 0.0f; 00273 NmeasTotal = 0; 00274 Aexc = 0.0f; 00275 pi2Tsfexc = 0.0; 00276 Nsweep_i = Nstart; 00277 AexcOut = 0.0f; 00278 gpaData.fexc = 0.0f; 00279 gpaData.absGyu = 0.0f; 00280 gpaData.angGyu = 0.0f; 00281 gpaData.absGyr = 0.0f; 00282 gpaData.angGyr = 0.0f; 00283 gpaData.Umag = 0.0f; 00284 gpaData.Ymag = 0.0f; 00285 gpaData.Rmag = 0.0f; 00286 gpaData.MeasPointFinished = false; 00287 gpaData.MeasFinished = false; 00288 gpaData.ind = -1; 00289 00290 } 00291 00292 // ----------------------------------------------------------------------------- 00293 // update (operator) 00294 // ----------------------------------------------------------------------------- 00295 00296 float GPA::update(float inp, float out) 00297 { 00298 // a new frequency point has been reached 00299 if(j == 1) { 00300 // user info 00301 if(i == 1 && doPrint) { 00302 printLine(); 00303 //printf(" fexc[Hz] |Gyu| deg(Gyu) |Gyr| deg(Gyr) |U| |Y| |R|\r\n"); 00304 printLine(); 00305 start_now = true; 00306 //uart_com.send_char_data(250,2,0); 00307 } 00308 00309 // get a new unique frequency point 00310 while(fexc == fexcPast) { 00311 // measurement finished 00312 if(i > NfexcDes) { 00313 gpaData.MeasPointFinished = false; 00314 gpaData.MeasFinished = true; 00315 meas_is_finished = true; 00316 return 0.0f; 00317 } 00318 calcGPAmeasPara(fexcDes[i - 1]); 00319 // secure fexc is not higher or equal to nyquist frequency 00320 if(fexc >= fnyq) { 00321 fexc = fexcPast; 00322 } 00323 // no frequency found 00324 if(fexc == fexcPast) { 00325 i += 1; 00326 } else { 00327 Aexc = aAexcDes/fexc + bAexcDes; 00328 pi2Tsfexc = pi2Ts*fexc; 00329 } 00330 } 00331 // filter scaling 00332 scaleG = 1.0/sqrt((double)Nmeas); 00333 // filter coefficients 00334 cr = cos(pi2Tsfexc); 00335 ci = sin(pi2Tsfexc); 00336 // set filter storage zero 00337 for(int i = 0; i < 3; i++) { 00338 sU[i] = 0.0; 00339 sY[i] = 0.0; 00340 } 00341 gpaData.MeasPointFinished = false; 00342 } 00343 // perfomre the sweep or measure 00344 if(j <= Nsweep_i) { 00345 dfexcj = ((float)j - 1.0f)/((float)Nsweep_i - 1.0f); 00346 dfexcj = div12pi*sinf(pi4*dfexcj) - div812pi*sinf((float)pi2*dfexcj) + dfexcj; 00347 dfexc = fexcPast + (fexc - fexcPast)*dfexcj; 00348 AexcOut = AexcPast + (Aexc - AexcPast)*dfexcj; 00349 } else { 00350 dfexc = fexc; 00351 AexcOut = Aexc; 00352 // one point DFT filter step for signal su 00353 sU[0] = scaleG*inp + 2.0*cr*sU[1] - sU[2]; 00354 sU[2] = sU[1]; 00355 sU[1] = sU[0]; 00356 // one point DFT filter step for signal sy 00357 sY[0] = scaleG*out + 2.0*cr*sY[1] - sY[2]; 00358 sY[2] = sY[1]; 00359 sY[1] = sY[0]; 00360 } 00361 // copy starting value for ang(R) 00362 if(j == 1 || j == Nsweep_i + 1) sinargR = sinarg; 00363 // measurement of frequencypoint is finished 00364 if(j == Nmeas + Nsweep_i) { 00365 fexcPast = fexc; 00366 AexcPast = Aexc; 00367 Nsweep_i = Nsweep; 00368 // calculate the one point dft 00369 float Ureal = (float)(2.0*scaleG*(cr*sU[1] - sU[2])); 00370 float Uimag = (float)(2.0*scaleG*ci*sU[1]); 00371 float Yreal = (float)(2.0*scaleG*(cr*sY[1] - sY[2])); 00372 float Yimag = (float)(2.0*scaleG*ci*sY[1]); 00373 // calculate magnitude and angle 00374 float Umag = sqrtf(Ureal*Ureal + Uimag*Uimag); 00375 float Ymag = sqrtf(Yreal*Yreal + Yimag*Yimag); 00376 gpaData.fexc = (float)fexc; 00377 gpaData.absGyu = Ymag/Umag; 00378 gpaData.angGyu = wrapAngle(atan2f(Yimag, Yreal) - atan2f(Uimag, Ureal))*rad2deg; 00379 gpaData.absGyr = Ymag/Aexc; 00380 gpaData.angGyr = wrapAngle(atan2f(Yimag, Yreal) + fmod(piDiv2 - sinargR, (float)pi2))*rad2deg; 00381 gpaData.Umag = Umag; 00382 gpaData.Ymag = Ymag; 00383 gpaData.Rmag = Aexc; 00384 gpaData.MeasPointFinished = true; 00385 gpaData.ind++; 00386 // user info 00387 if(doPrint) { 00388 //printf("%11.4e %9.3e %8.3f %9.3e %8.3f %9.3e %9.3e %9.3e\r\n", gpaData.fexc, gpaData.absGyu, gpaData.angGyu, gpaData.absGyr, gpaData.angGyr, gpaData.Umag, gpaData.Ymag, gpaData.Rmag); 00389 new_data_available = true; 00390 } 00391 i += 1; 00392 j = 1; 00393 } else { 00394 j += 1; 00395 } 00396 // calculate the excitation 00397 sinarg = fmod(sinarg + pi2Ts*dfexc, pi2); 00398 NmeasTotal += 1; 00399 return AexcOut*sinf(sinarg); 00400 } 00401 00402 // ----------------------------------------------------------------------------- 00403 // private functions 00404 // ----------------------------------------------------------------------------- 00405 00406 void GPA::assignParameters(int NfexcDes, int NperMin, int NmeasMin, float Ts, int Nstart, int Nsweep) 00407 { 00408 this->NfexcDes = NfexcDes; 00409 this->NperMin = NperMin; 00410 this->NmeasMin = NmeasMin; 00411 this->Ts = Ts; 00412 this->Nstart = Nstart; 00413 this->Nsweep = Nsweep; 00414 } 00415 00416 void GPA::calculateDecreasingAmplitudeCoefficients(float Aexc0, float Aexc1) 00417 { 00418 // calculate coefficients for decreasing amplitude (1/fexc) 00419 this->aAexcDes = (Aexc1 - Aexc0)/(1.0f/fexcDes[NfexcDes-1] - 1.0f/fexcDes[0]); 00420 this->bAexcDes = Aexc0 - aAexcDes/fexcDes[0]; 00421 } 00422 00423 void GPA::initializeConstants(float Ts) 00424 { 00425 fnyq = 1.0f/2.0f/Ts; 00426 pi2 = 2.0*pi; 00427 pi4 = 4.0f*(float)pi; 00428 pi2Ts = 2.0*pi*(double)Ts;// 2.0*pi*Ts; 00429 piDiv2 = (float)pi/2.0f; 00430 rad2deg = 180.0f/(float)pi; 00431 div12pi = 1.0f/(12.0f*(float)pi); 00432 div812pi = 8.0f/(12.0f*(float)pi); 00433 } 00434 00435 void GPA::assignFilterStorage() 00436 { 00437 sU = (double*)malloc(3*sizeof(double)); 00438 sY = (double*)malloc(3*sizeof(double)); 00439 } 00440 00441 void GPA::fexcDesLogspace(float fMin, float fMax, int NfexcDes) 00442 { 00443 // calculate logarithmic spaced frequency points 00444 float Gain = log10f(fMax/fMin)/((float)NfexcDes - 1.0f); 00445 float expon = 0.0; 00446 for(int i = 0; i < NfexcDes; i++) { 00447 fexcDes[i] = fMin*powf(10.0f, expon); 00448 expon += Gain; 00449 } 00450 } 00451 00452 void GPA::calcGPAmeasPara(float fexcDes_i) 00453 { 00454 // Nmeas has to be an integer 00455 Nper = NperMin; 00456 Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f); 00457 // secure that the minimal number of measurements is fullfilled 00458 int Ndelta = NmeasMin - Nmeas; 00459 if(Ndelta > 0) { 00460 Nper = (int)ceil((float)NmeasMin*fexcDes_i*Ts); 00461 Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f); 00462 } 00463 // evaluating reachable frequency 00464 fexc = (float)((double)Nper/(double)Nmeas/(double)Ts); 00465 } 00466 00467 float GPA::wrapAngle(float angle) 00468 { 00469 // wrap angle from (-2pi,2pi) into (-pi,pi) 00470 if(abs(angle) > (float)pi) angle -= copysignf(-(float)pi2, angle); // -1*sign(angle)*2*pi + angle; 00471 return angle; 00472 } 00473 00474 void GPA::printLongLine() 00475 { 00476 //printf("-------------------------------------------------------------------------------------------------------------------------------\r\n"); 00477 } 00478 00479 // ----------------------------------------------------------------------------- 00480 // public functions (mainly for debugging) 00481 // ----------------------------------------------------------------------------- 00482 00483 void GPA::printGPAfexcDes() 00484 { 00485 printLine(); 00486 for(int i = 0; i < NfexcDes; i++) { 00487 printf("%9.4f\r\n", fexcDes[i]); 00488 } 00489 } 00490 00491 void GPA::printGPAmeasPara() 00492 { 00493 printLine(); 00494 printf(" fexcDes[Hz] fexc[Hz] Aexc Nmeas Nper Nsweep_i\r\n"); 00495 printLine(); 00496 for(int i = 0; i < NfexcDes; i++) { 00497 calcGPAmeasPara(fexcDes[i]); 00498 if(fexc == fexcPast || fexc >= fnyq) { 00499 fexc = 0.0; 00500 Aexc = 0.0f; 00501 Nmeas = 0; 00502 Nper = 0; 00503 Nsweep_i = 0; 00504 } else { 00505 Aexc = aAexcDes/fexc + bAexcDes; 00506 fexcPast = fexc; 00507 AexcPast = Aexc; 00508 } 00509 NmeasTotal += Nmeas; 00510 NmeasTotal += Nsweep_i; 00511 printf("%11.4e %12.4e %10.3e %7i %6i %7i\r\n", fexcDes[i], (float)fexc, Aexc, Nmeas, Nper, Nsweep_i); 00512 // wait(0.01); 00513 Nsweep_i = Nsweep; 00514 } 00515 printGPAmeasTime(); 00516 reset(); 00517 } 00518 00519 void GPA::printGPAmeasTime() 00520 { 00521 printLine(); 00522 printf(" Number of data points : %11i\r\n", NmeasTotal); 00523 printf(" Measurment time in sec: %12.2f\r\n", (float)NmeasTotal*Ts); 00524 } 00525 00526 void GPA::printNfexcDes() 00527 { 00528 printLine(); 00529 printf(" Number of frequancy points: %3i\r\n", NfexcDes); 00530 } 00531 00532 void GPA::printLine() 00533 { 00534 printf("--------------------------------------------------------------------------------\r\n"); 00535 } 00536 00537 void GPA::getGPAdata(float *val) 00538 { 00539 val[0] = gpaData.fexc; 00540 val[1] = gpaData.absGyu; 00541 val[2] = gpaData.angGyu; 00542 val[3] = gpaData.absGyr; 00543 val[4] = gpaData.angGyr; 00544 val[5] = gpaData.Umag; 00545 val[6] = gpaData.Ymag; 00546 val[7] = gpaData.Rmag; 00547 new_data_available = false; 00548 }
Generated on Sun Aug 21 2022 12:52:06 by
1.7.2