[1469] | 1 | #include <math.h> |
---|
[1380] | 2 | #include <stdio.h> |
---|
[1442] | 3 | #include "fast_exp.h" |
---|
| 4 | #include "fastexp.h" |
---|
| 5 | #include "mpf_double.h" |
---|
[1380] | 6 | |
---|
[1469] | 7 | //Parameters of PMSM |
---|
| 8 | #define Lsd (3.465e-3*0.9) // *0.9)//H ... zajimave - chodi to i pri Lsd = Lsq |
---|
| 9 | #define Lsq 3.465e-3//H |
---|
| 10 | //#define Ls 3.465e-3//H |
---|
| 11 | #define Rs 0.28//Ohm |
---|
| 12 | #define Fpm (0.1989*1.1) //*1.15) //V.s // !!xx!! |
---|
| 13 | #define pp 4 |
---|
| 14 | #define Ism 35.//40. |
---|
| 15 | |
---|
[1439] | 16 | static floatx om[N]; |
---|
| 17 | static floatx Pt[N]; |
---|
| 18 | static floatx sth[N]; |
---|
| 19 | static floatx cth[N]; |
---|
| 20 | static floatx isdm[N]; |
---|
| 21 | static floatx isqm[N]; |
---|
| 22 | static floatx th[N]; |
---|
| 23 | static floatx w[N]; |
---|
| 24 | static floatx lwi[N]; |
---|
[1469] | 25 | static int i_best; |
---|
[1380] | 26 | |
---|
[1439] | 27 | static floatx qth; |
---|
| 28 | static floatx qom; |
---|
| 29 | static floatx r; |
---|
[1442] | 30 | static floatx rinv; |
---|
[1380] | 31 | |
---|
[1469] | 32 | // staticke promenne pro model |
---|
| 33 | static float _ad; |
---|
| 34 | static float _aq; |
---|
| 35 | static float _b; |
---|
| 36 | static float _cd; |
---|
| 37 | static float _cq; |
---|
| 38 | static float _dd; |
---|
| 39 | static float _dq; |
---|
[1380] | 40 | |
---|
| 41 | |
---|
| 42 | void resample() { |
---|
| 43 | int N_babies[N]; |
---|
[1439] | 44 | floatx cumdist; |
---|
[1380] | 45 | int i; |
---|
[1439] | 46 | floatx ui; |
---|
| 47 | int i_from=0; |
---|
| 48 | int i_to=0; |
---|
[1380] | 49 | |
---|
[1439] | 50 | floatx u0 = qrandu(); |
---|
[1380] | 51 | |
---|
| 52 | int j=0; |
---|
| 53 | N_babies[0]=0; |
---|
| 54 | cumdist = w[0]; |
---|
| 55 | for ( i = 0; i < N; i++ ) { |
---|
| 56 | ui = ( i + u0 ) / N; |
---|
| 57 | while ( ui > cumdist ) { |
---|
| 58 | j++; |
---|
| 59 | N_babies[j]=0; |
---|
| 60 | cumdist+=w[j]; |
---|
| 61 | } |
---|
| 62 | N_babies [j] ++; |
---|
| 63 | } |
---|
| 64 | while (j++<=(N-1)) { // delete all N_babies after j |
---|
| 65 | N_babies[j]=0; |
---|
| 66 | } |
---|
| 67 | |
---|
| 68 | // COPY |
---|
[1439] | 69 | |
---|
[1380] | 70 | while ( i_from<N ) { |
---|
[1469] | 71 | w[i_from]=1.0/N; |
---|
[1380] | 72 | while ( N_babies[i_from]>1 ) { // 1 baby stays where it is |
---|
| 73 | |
---|
| 74 | while ( N_babies[i_to]>=1 ) i_to++; // find first empty slot |
---|
| 75 | |
---|
| 76 | //copy it |
---|
| 77 | Pt[i_to]=Pt[i_from]; |
---|
| 78 | om[i_to]=om[i_from]; |
---|
| 79 | th[i_to]=th[i_from]; |
---|
| 80 | isdm[i_to]=isdm[i_from]; |
---|
| 81 | isqm[i_to]=isqm[i_from]; |
---|
| 82 | |
---|
| 83 | N_babies[i_to]++; |
---|
| 84 | N_babies[i_from]--; |
---|
| 85 | } |
---|
| 86 | i_from++; |
---|
| 87 | } |
---|
| 88 | } |
---|
| 89 | |
---|
[1439] | 90 | void mpf_bayes ( const floatx isa, const floatx isb , const floatx usa, const floatx usb ) { |
---|
[1380] | 91 | |
---|
[1439] | 92 | floatx isd; |
---|
| 93 | floatx isq; |
---|
| 94 | floatx usd; |
---|
| 95 | floatx usq; |
---|
[1380] | 96 | |
---|
[1439] | 97 | floatx Cd; |
---|
| 98 | floatx Cq; |
---|
| 99 | floatx CC; |
---|
| 100 | floatx oCC; |
---|
[1380] | 101 | |
---|
[1439] | 102 | floatx difid; |
---|
| 103 | floatx difiq; |
---|
[1380] | 104 | |
---|
[1439] | 105 | floatx zeta; |
---|
| 106 | floatx Kd; |
---|
| 107 | floatx Kq; |
---|
| 108 | floatx ro; |
---|
| 109 | floatx ypd; |
---|
| 110 | floatx ypq; |
---|
| 111 | floatx detRy; |
---|
| 112 | floatx ydiffd; |
---|
| 113 | floatx ydiffq; |
---|
| 114 | floatx ydC; |
---|
[1380] | 115 | |
---|
[1469] | 116 | floatx maxlwi, sumlwi,sumw2,neff_th; |
---|
[1380] | 117 | int i; |
---|
[1439] | 118 | |
---|
| 119 | floatx *th_i, *Pt_i, *om_i, *isdm_i, *isqm_i; |
---|
| 120 | floatx *w_i, *lw_i; |
---|
| 121 | floatx *sth_i, *cth_i; |
---|
[1469] | 122 | floatx sumw_inv; |
---|
[1380] | 123 | // implementation starts here |
---|
[1439] | 124 | th_i = &th[0]; |
---|
| 125 | Pt_i = &Pt[0]; |
---|
| 126 | om_i = &om[0]; |
---|
| 127 | isdm_i = &isdm[0]; |
---|
| 128 | isqm_i = &isqm[0]; |
---|
| 129 | sth_i = &sth[0]; |
---|
| 130 | cth_i = &cth[0]; |
---|
[1380] | 131 | for ( i=0; i<N; i++ ) { |
---|
| 132 | |
---|
[1469] | 133 | /////////////////////////// HACK!!!!!!!!!!! |
---|
| 134 | //om[i] = 0; |
---|
| 135 | /////////////////////////// HACK!!!!!!!!!!! |
---|
| 136 | |
---|
[1439] | 137 | // time update |
---|
| 138 | *Pt_i = 1.0*1.0*(*Pt_i)+qom; // Pt is now predictive variance |
---|
[1469] | 139 | floatx ff =qrandn() ; |
---|
| 140 | // printf("%f\n", ff); |
---|
| 141 | *th_i += *om_i*_dt+qth * ff; |
---|
[1439] | 142 | while ( *th_i>M_PI ) *th_i -=2*M_PI; |
---|
| 143 | while ( *th_i<-M_PI ) *th_i+=2*M_PI; |
---|
[1380] | 144 | |
---|
[1439] | 145 | *sth_i=sin ( *th_i ); |
---|
| 146 | *cth_i=cos ( *th_i ); |
---|
| 147 | |
---|
| 148 | isd = *cth_i*isa+*sth_i*isb; |
---|
| 149 | isq = -*sth_i*isa+*cth_i*isb; |
---|
| 150 | // process old voltage |
---|
| 151 | usd = (*cth_i)*usa+(*sth_i)*usb; |
---|
| 152 | usq = -(*sth_i)*usa+(*cth_i)*usb; |
---|
[1380] | 153 | |
---|
[1469] | 154 | Cd = isq*_dd; |
---|
| 155 | Cq = -_b - isd* _dq; |
---|
[1380] | 156 | |
---|
[1439] | 157 | difid=isd- _ad * (*isdm_i) - _cd *(usd); |
---|
| 158 | difiq=isq- _aq * (*isqm_i) - _cq *(usq); |
---|
[1380] | 159 | |
---|
| 160 | CC=Cd*Cd+Cq*Cq; |
---|
[1439] | 161 | zeta = *Pt_i/ ( r+*Pt_i*CC ); |
---|
[1380] | 162 | oCC = ( 1-zeta*CC ); |
---|
[1442] | 163 | ro = oCC*rinv; |
---|
[1380] | 164 | |
---|
[1439] | 165 | Kd = *Pt_i*Cd*ro; |
---|
| 166 | Kq = *Pt_i*Cq*ro; |
---|
[1380] | 167 | |
---|
[1440] | 168 | (*Pt_i)*=( 1.0- ( Kd*Cd+Kq*Cq ) ); |
---|
[1380] | 169 | |
---|
[1439] | 170 | ypd = Cd**om_i; |
---|
| 171 | ypq = Cq**om_i; |
---|
[1380] | 172 | |
---|
[1439] | 173 | //detRy = ro/r; |
---|
| 174 | *om_i += Kd* ( difid - ypd ) +Kq* ( difiq-ypq ); |
---|
[1380] | 175 | ydiffd = ( ypd-difid ); |
---|
| 176 | ydiffq = ( ypq-difiq ); |
---|
| 177 | ydC = ydiffd*Cd + ydiffq*Cq; |
---|
| 178 | |
---|
[1439] | 179 | //lwi[i] = 0.5* ( log ( detRy ) + ( ydC*ydC*zeta -(ydiffd*ydiffd+ydiffq*ydiffq) ) /r ) ; |
---|
[1442] | 180 | lwi[i] = 0.5*( ydC*ydC*zeta -(ydiffd*ydiffd+ydiffq*ydiffq) ) *rinv ; |
---|
[1380] | 181 | |
---|
[1439] | 182 | *isdm_i=isd; |
---|
| 183 | *isqm_i=isq; |
---|
| 184 | |
---|
| 185 | // shift all counters |
---|
| 186 | th_i++; |
---|
| 187 | Pt_i++; |
---|
| 188 | om_i++; |
---|
| 189 | isdm_i++; |
---|
| 190 | isqm_i++; |
---|
| 191 | sth_i++; |
---|
| 192 | cth_i++; |
---|
| 193 | |
---|
[1380] | 194 | } |
---|
[1469] | 195 | maxlwi=-1e10; |
---|
[1439] | 196 | lw_i = &lwi[0]; |
---|
[1469] | 197 | i_best = 0; |
---|
[1439] | 198 | for ( i=0;i<N;i++,lw_i++ ) { |
---|
[1469] | 199 | if ( *lw_i>maxlwi ) {maxlwi=*lw_i;i_best=i;} |
---|
| 200 | } |
---|
| 201 | /*!!*/ |
---|
[1439] | 202 | lw_i = &lwi[0]; |
---|
| 203 | w_i = &w[0]; |
---|
| 204 | for ( i=0;i<N;i++,lw_i++,w_i++ ) { |
---|
[1469] | 205 | *lw_i-=maxlwi; |
---|
| 206 | (*w_i)*=fasterexp ( *lw_i ); // do not always resample |
---|
[1439] | 207 | } |
---|
[1380] | 208 | |
---|
| 209 | sumlwi=0.0; |
---|
[1469] | 210 | sumw2=0.0; |
---|
[1439] | 211 | w_i = &w[0]; |
---|
[1469] | 212 | for ( i=0;i<N;i++,w_i++ ) {sumlwi+=*w_i;sumw2+=*w_i**w_i;} |
---|
| 213 | sumw_inv = 1.0/sumlwi; // multiplication is faster than division! |
---|
[1439] | 214 | w_i = &w[0]; |
---|
[1469] | 215 | for ( i=0;i<N;i++,w_i++ ) *w_i*=sumw_inv; |
---|
[1439] | 216 | |
---|
[1469] | 217 | neff_th = 0.8*N*sumw2*sumw_inv*sumw_inv; |
---|
| 218 | if (1<neff_th) |
---|
| 219 | resample(); |
---|
[1380] | 220 | } |
---|
| 221 | |
---|
| 222 | |
---|
[1439] | 223 | void mpf_init(floatx qom0, floatx qth0, floatx r0) { |
---|
[1468] | 224 | |
---|
[1469] | 225 | // rng_init(); |
---|
[1468] | 226 | |
---|
[1439] | 227 | int i; |
---|
[1380] | 228 | r=r0; |
---|
[1442] | 229 | rinv = 1.0/r; |
---|
[1380] | 230 | qth=qth0; |
---|
| 231 | qom=qom0; |
---|
[1439] | 232 | |
---|
[1380] | 233 | for ( i=0; i<N; i++ ) { |
---|
[1439] | 234 | th[i]=qrandu() *2*M_PI - M_PI; |
---|
| 235 | sth[i]=sin ( th[i] ); |
---|
| 236 | cth[i]=cos ( th[i] ); |
---|
| 237 | |
---|
[1380] | 238 | om[i]=0; |
---|
[1439] | 239 | Pt[i]=10.; |
---|
[1380] | 240 | isdm[i]=0; |
---|
| 241 | isqm[i]=0; |
---|
| 242 | w[i]=1.0/N; |
---|
| 243 | } |
---|
| 244 | |
---|
[1469] | 245 | // inicializace promennych |
---|
| 246 | #if (VYBER_MODELU==0) |
---|
| 247 | // plny model s respektovanim Lsd/Lsq |
---|
| 248 | _ad = (1.-Rs*_dt/(Lsd)); |
---|
| 249 | _aq = (1.-Rs*_dt/(Lsq)); |
---|
| 250 | _b = Fpm/Lsq*_dt; |
---|
| 251 | _cd = _dt/(Lsd); |
---|
| 252 | _cq = _dt/(Lsq); |
---|
| 253 | _dd = _dt*Lsq/Lsd; |
---|
| 254 | _dq = _dt*Lsd/Lsq; /**/ |
---|
| 255 | #else |
---|
| 256 | // parametry modelu identifikovane z namerenych dat z prototypu v EL204 |
---|
| 257 | _ad = 0.9880; |
---|
| 258 | _aq = 0.9817; |
---|
| 259 | _b = 0.0076; |
---|
| 260 | _cd = 0.0289; |
---|
| 261 | _cq = 0.0292; |
---|
| 262 | _dd = _dt; |
---|
| 263 | _dq = _dt; |
---|
| 264 | #endif |
---|
| 265 | |
---|
[1380] | 266 | } |
---|
[1439] | 267 | void mpf_mean ( floatx *Ecosth, floatx *Esinth, floatx *Eome ) { |
---|
[1380] | 268 | int i; |
---|
[1440] | 269 | floatx *w_i=&w[0]; |
---|
| 270 | floatx *cth_i=&cth[0]; |
---|
| 271 | floatx *sth_i=&sth[0]; |
---|
| 272 | floatx *om_i=&om[0]; |
---|
[1381] | 273 | *Ecosth=0.0; |
---|
| 274 | *Esinth=0.0; |
---|
| 275 | *Eome=0.0; |
---|
[1440] | 276 | for ( i=0;i<N;i++,w_i++,cth_i++,sth_i++,om_i++ ) { |
---|
| 277 | *Ecosth+=(*w_i)*(*cth_i); |
---|
| 278 | *Esinth+=(*w_i)*(*sth_i); |
---|
| 279 | *Eome+=(*w_i)*(*om_i); |
---|
[1380] | 280 | } |
---|
| 281 | |
---|
| 282 | } |
---|
[1469] | 283 | |
---|
| 284 | void mpf_best( floatx *Ecosth, floatx *Esinth, floatx *Eome ) |
---|
| 285 | { |
---|
| 286 | *Ecosth=cth[i_best]; |
---|
| 287 | *Esinth=sth[i_best]; |
---|
| 288 | *Eome=om[i_best]; |
---|
| 289 | } |
---|
| 290 | |
---|
| 291 | |
---|
[1439] | 292 | void mpf_th(floatx th1[N]){ |
---|
| 293 | int i; |
---|
| 294 | for (i=0;i<N;i++) th1[i]=th[i]; |
---|
[1380] | 295 | } |
---|
[1468] | 296 | void mpf_om(floatx om1[N]){ |
---|
| 297 | int i; |
---|
| 298 | for (i=0;i<N;i++) om1[i]=om[i]; |
---|
| 299 | } |
---|
[1438] | 300 | |
---|
[1439] | 301 | floatx kalman_om( const floatx isa, const floatx isb , const floatx usa, const floatx usb, const floatx th ) { |
---|
[1438] | 302 | |
---|
[1439] | 303 | floatx isd; |
---|
| 304 | floatx isq; |
---|
| 305 | floatx usd; |
---|
| 306 | floatx usq; |
---|
[1438] | 307 | |
---|
[1439] | 308 | floatx Cd; |
---|
| 309 | floatx Cq; |
---|
| 310 | floatx CC; |
---|
| 311 | floatx oCC; |
---|
[1438] | 312 | |
---|
[1439] | 313 | floatx difid; |
---|
| 314 | floatx difiq; |
---|
[1438] | 315 | |
---|
[1439] | 316 | floatx zeta; |
---|
| 317 | floatx Kd; |
---|
| 318 | floatx Kq; |
---|
| 319 | floatx ro; |
---|
| 320 | floatx ypd; |
---|
| 321 | floatx ypq; |
---|
| 322 | floatx detRy; |
---|
| 323 | floatx ydiffd; |
---|
| 324 | floatx ydiffq; |
---|
| 325 | floatx ydC; |
---|
[1438] | 326 | |
---|
[1439] | 327 | static floatx Pt; |
---|
| 328 | static floatx om; |
---|
| 329 | floatx cth, sth; |
---|
| 330 | static floatx isdm, isqm; |
---|
[1438] | 331 | |
---|
| 332 | |
---|
| 333 | Pt = 1.0*1.0*Pt+qom; // Pt is now predictive variance |
---|
| 334 | //while ( th>M_PI ) th=th-2*M_PI; |
---|
| 335 | //while ( th<-M_PI ) th=th+2*M_PI; |
---|
| 336 | |
---|
| 337 | sth=sin ( th ); |
---|
| 338 | cth=cos ( th ); |
---|
| 339 | |
---|
| 340 | isd = cth*isa+sth*isb; |
---|
| 341 | isq = -sth*isa+cth*isb; |
---|
| 342 | usd = cth*usa+sth*usb; |
---|
| 343 | usq = -sth*usa+cth*usb; |
---|
| 344 | |
---|
[1469] | 345 | Cd = isq*_dd; |
---|
| 346 | Cq = -_b - isd* _dq; |
---|
[1438] | 347 | |
---|
| 348 | difid=isd- _ad *isdm - _cd *usd; |
---|
| 349 | difiq=isq- _aq *isqm - _cq *usq; |
---|
| 350 | |
---|
| 351 | CC=Cd*Cd+Cq*Cq; |
---|
| 352 | zeta = Pt/ ( r+Pt*CC ); |
---|
| 353 | oCC = ( 1-zeta*CC ); |
---|
[1442] | 354 | ro = oCC*rinv; |
---|
[1438] | 355 | |
---|
| 356 | Kd = Pt*Cd*ro; |
---|
| 357 | Kq = Pt*Cq*ro; |
---|
| 358 | |
---|
| 359 | Pt=Pt* ( 1- ( Kd*Cd+Kq*Cq ) ); |
---|
| 360 | |
---|
| 361 | ypd = Cd*om; |
---|
| 362 | ypq = Cq*om; |
---|
| 363 | |
---|
[1442] | 364 | detRy = ro*rinv; |
---|
[1438] | 365 | om = om + Kd* ( difid - ypd ) +Kq* ( difiq-ypq ); |
---|
| 366 | ydiffd = ( ypd-difid ); |
---|
| 367 | ydiffq = ( ypq-difiq ); |
---|
| 368 | ydC = ydiffd*Cd + ydiffq*Cq; |
---|
| 369 | |
---|
| 370 | isdm=isd; |
---|
| 371 | isqm=isq; |
---|
| 372 | |
---|
| 373 | return om; |
---|
| 374 | } |
---|