[1464] | 1 | #include "ekf_mm.h" |
---|
| 2 | #include "qmath.h" |
---|
| 3 | |
---|
| 4 | |
---|
| 5 | |
---|
| 6 | void ekfCh2(ekf_data *E, int16 ux, int16 uy, int16 isx, int16 isy, int16 *detS, int16 *rem) |
---|
| 7 | { |
---|
| 8 | int16 t_sin,t_cos; |
---|
| 9 | int32 tmp; |
---|
| 10 | |
---|
| 11 | //x_est[0]=x_est[0]; |
---|
| 12 | // q15 + q15*q13 |
---|
| 13 | (E->x_est)[1]=(((int32)(E->x_est[1])<<15)+(int32)(E->cG)*(E->x_est[0]))>>15; |
---|
| 14 | |
---|
| 15 | mmultACh(E->PSI,E->Chf,E->PSICh,2,2); |
---|
| 16 | |
---|
| 17 | givens_fast(E->PSICh,E->Q,2); // pracuje inplace |
---|
| 18 | for (int i=0;i<4;i++) E->Chf[i]=E->PSICh[i]; |
---|
| 19 | |
---|
| 20 | // implementace v PC |
---|
| 21 | t_sin=qsin(E->x_est[1]); |
---|
| 22 | t_cos=qcos(E->x_est[1]); |
---|
| 23 | |
---|
| 24 | { |
---|
| 25 | E->C[0]=((int32)(E->cB)*t_sin)>>15; |
---|
| 26 | tmp=((int32)(E->cH)*(E->x_est[0]))>>15; // ! cH =cB with different scale!! |
---|
| 27 | E->C[1]=((int32)tmp*t_cos)>>15; |
---|
| 28 | E->C[2]=-((int32)(E->cB)*t_cos)>>15; |
---|
| 29 | E->C[3]=((int32)tmp*t_sin)>>15; |
---|
| 30 | } |
---|
| 31 | |
---|
| 32 | |
---|
| 33 | tmp=((int32)(E->cB)*(E->x_est[0]))>>15; // q15*q13 -> q13 |
---|
| 34 | // q15*q13 + q13*q15 + q15*q13?? |
---|
| 35 | E->y_est[0]=((int32)(E->cA)*(E->y_old[0])+(int32)tmp*t_sin+(int32)(E->cC)*(ux))>>15; // in q13 |
---|
| 36 | E->y_est[1]=((int32)(E->cA)*(E->y_old[1])-(int32)tmp*t_cos+(int32)(E->cC)*(uy))>>15; // q13 |
---|
| 37 | |
---|
| 38 | |
---|
| 39 | int16 difz[2]; |
---|
| 40 | difz[0]=(isx-(E->y_est[0])); // shift to q15!! |
---|
| 41 | difz[1]=(isy-(E->y_est[1]));//<<2; |
---|
| 42 | |
---|
| 43 | E->y_old[0] = isx; |
---|
| 44 | E->y_old[1] = isy; |
---|
| 45 | |
---|
| 46 | *detS = 32767; *rem=0; |
---|
| 47 | carlson_fastC(difz,E->x_est,E->Chf,E->C,E->dR,2,2, detS, rem); |
---|
| 48 | } |
---|
| 49 | |
---|
| 50 | void init_ekfCh2(ekf_data *E, double Tv) |
---|
| 51 | { |
---|
| 52 | // Tuning of matrix Q |
---|
| 53 | E->Q[0]=prevod(0.005,15); // 1e-3 |
---|
| 54 | E->Q[3]=prevod(0.001,15); // 1e-3 |
---|
| 55 | |
---|
| 56 | E->Chf[0]=0x1FFF;// ! // 0.05 |
---|
| 57 | E->Chf[1]=E->Chf[2]=0; |
---|
| 58 | E->Chf[3]=0x1FFF;//! |
---|
| 59 | |
---|
| 60 | // Tuning of matrix R |
---|
| 61 | E->dR[0]=prevod(0.001,15); // 0.05 |
---|
| 62 | E->dR[1]=E->dR[0]; |
---|
| 63 | |
---|
| 64 | // Motor model parameters |
---|
| 65 | E->cA=prevod(1-Tv*Rs/Ls,15); |
---|
| 66 | E->cB=prevod(Tv*Wref*Fmag/Iref/Ls,15); |
---|
| 67 | E->cC=prevod(Tv/Ls/Iref*Uref,15); |
---|
| 68 | // cD=prevod(1-Tv*Bf/J,15); |
---|
| 69 | // cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15); |
---|
| 70 | // cF=prevod(p*Tv*Mref/J/Wref,15); |
---|
| 71 | E->cG=prevod(Tv*Wref/Thetaref,15); //in q15! |
---|
| 72 | //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15); |
---|
| 73 | // cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // < =========== |
---|
| 74 | E->cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); // |
---|
| 75 | // cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref); |
---|
| 76 | |
---|
| 77 | /* Init matrix PSI with permanently constant terms */ |
---|
| 78 | E->PSI[0]=0x7FFF; |
---|
| 79 | E->PSI[2]=E->cG; |
---|
| 80 | E->PSI[3]=0x7FFF; |
---|
| 81 | } |
---|
| 82 | |
---|
| 83 | void ekfmm(ekf_data* E1, ekf_data* E2, int16 ux, int16 uy, int16 isx, int16 isy) |
---|
| 84 | { |
---|
| 85 | int16 det1, det2, rem1, rem2; |
---|
| 86 | ekfCh2(E1, ux,uy,isx,isy,&det1,&rem1); |
---|
| 87 | ekfCh2(E2, ux,uy,isx,isy,&det2,&rem2); |
---|
| 88 | |
---|
| 89 | |
---|
| 90 | } |
---|