[61] | 1 | #include <itpp/itbase.h> |
---|
| 2 | #include <estim/libKF.h> |
---|
| 3 | |
---|
| 4 | #include "ekf_obj.h" |
---|
| 5 | |
---|
| 6 | double minQ(double Q){if (Q>1.0){ return 1.0;} else {return Q;};}; |
---|
| 7 | |
---|
| 8 | /////////////// |
---|
| 9 | void EKFfixed::bayes(const vec &dt){ |
---|
| 10 | ekf(dt(2),dt(3),dt(0),dt(1)); |
---|
| 11 | |
---|
| 12 | if ( BM::evalll ) { |
---|
| 13 | //from enorm |
---|
| 14 | vec ydif(2); |
---|
| 15 | ydif(0)=dt(0)-zprevod(x_pred[0],Qm)*Iref; |
---|
| 16 | ydif(1)=dt(1)-zprevod(x_pred[1],Qm)*Iref; |
---|
| 17 | |
---|
| 18 | BM::ll = -0.5* ( 2 * 1.83787706640935 +log ( det ( Ry ) ) +ydif* ( inv(Ry)*ydif ) ); |
---|
| 19 | } |
---|
| 20 | }; |
---|
| 21 | |
---|
| 22 | |
---|
| 23 | void EKFfixed::update_psi(void) |
---|
| 24 | { |
---|
| 25 | int t_sin,t_cos,tmp; |
---|
| 26 | |
---|
| 27 | // implementace v PC |
---|
| 28 | t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); |
---|
| 29 | t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15); |
---|
| 30 | |
---|
| 31 | PSI[2]=((long)cB*t_sin)>>15; |
---|
| 32 | tmp=((long)cH*x_est[2])>>15; |
---|
| 33 | PSI[3]=((long)tmp*t_cos)>>15; |
---|
| 34 | PSI[6]=-((long)cB*t_cos)>>15; |
---|
| 35 | PSI[7]=((long)tmp*t_sin)>>15; |
---|
| 36 | } |
---|
| 37 | |
---|
| 38 | |
---|
| 39 | void EKFfixed::prediction(int *ux) |
---|
| 40 | { |
---|
| 41 | int t_sin,t_cos, tmp; |
---|
| 42 | |
---|
| 43 | // implementace v PC |
---|
| 44 | t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); |
---|
| 45 | t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15); |
---|
| 46 | |
---|
| 47 | tmp=((long)cB*x_est[2])>>15; |
---|
| 48 | x_pred[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*ux[0])>>15; |
---|
| 49 | x_pred[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*ux[1])>>15; |
---|
| 50 | x_pred[2]=x_est[2]; |
---|
| 51 | x_pred[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15; |
---|
| 52 | |
---|
| 53 | update_psi(); |
---|
| 54 | |
---|
| 55 | mmult(PSI,P_est,temp15a,3,3,3); |
---|
| 56 | // mtrans(PSI,temp15b,5,5); |
---|
| 57 | mmultt(temp15a,PSI,P_pred,3,3,3); |
---|
| 58 | maddD(P_pred,Q,3,3); |
---|
| 59 | } |
---|
| 60 | |
---|
| 61 | void EKFfixed::correction(void) |
---|
| 62 | { |
---|
| 63 | int Y_error[2]; |
---|
| 64 | long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */ |
---|
| 65 | |
---|
| 66 | choice_P(P_pred,temp15a,3); |
---|
| 67 | maddD(temp15a,R,1,1); |
---|
| 68 | minv2(temp15a,temp30a); |
---|
| 69 | Ry(0,0)=zprevod(temp15a[0],15); |
---|
| 70 | Ry(0,1)=zprevod(temp15a[1],15); |
---|
| 71 | Ry(1,0)=zprevod(temp15a[2],15); |
---|
| 72 | Ry(1,1)=zprevod(temp15a[3],15); |
---|
| 73 | |
---|
| 74 | mmultDr(P_pred,temp15a,3,3,1,1); |
---|
| 75 | mmult1530(temp15a,temp30a,Kalm,3,1,1); |
---|
| 76 | |
---|
| 77 | |
---|
| 78 | /* estimate the state system */ |
---|
| 79 | choice_x(x_pred, temp15a); |
---|
| 80 | msub(Y_mes,temp15a,Y_error,1,0); |
---|
| 81 | mmult(Kalm,Y_error,temp15a,3,1,0); |
---|
| 82 | madd(x_pred,temp15a,x_est,3,0); |
---|
| 83 | |
---|
| 84 | /* matrix of covariances - version without MMULTDL() */ |
---|
| 85 | |
---|
| 86 | /* Version with MMULTDL() */ |
---|
| 87 | mmultDl(P_pred,temp15a,1,3,3,1); |
---|
| 88 | |
---|
| 89 | mmult(Kalm,temp15a,P_est,3,1,3); |
---|
| 90 | msub(P_pred,P_est,P_est,3,3); |
---|
| 91 | /* END */ |
---|
| 92 | } |
---|
| 93 | |
---|
| 94 | |
---|
| 95 | void EKFfixed::ekf(double ux, double uy, double isxd, double isyd) |
---|
| 96 | { |
---|
| 97 | // vypocet napeti v systemu (x,y) |
---|
| 98 | ukalm[0]=prevod(ux/Uref,Qm); |
---|
| 99 | ukalm[1]=prevod(uy/Uref,Qm); |
---|
| 100 | |
---|
| 101 | // zadani mereni |
---|
| 102 | Y_mes[0]=prevod(isxd/Iref,Qm); |
---|
| 103 | Y_mes[1]=prevod(isyd/Iref,Qm); |
---|
| 104 | |
---|
| 105 | ////// vlastni rutina EKF ///////////////////////// |
---|
| 106 | prediction(ukalm); |
---|
| 107 | correction(); |
---|
| 108 | |
---|
| 109 | // navrat estimovanych hodnot regulatoru |
---|
| 110 | vec* mu = E._mu(); |
---|
| 111 | (*mu)(0)=zprevod(x_est[0],Qm)*Iref; |
---|
| 112 | (*mu)(1)=zprevod(x_est[1],Qm)*Iref; |
---|
| 113 | (*mu)(2)=zprevod(x_est[2],Qm)*Wref; |
---|
| 114 | (*mu)(3)=zprevod(x_est[3],15)*Thetaref; |
---|
| 115 | } |
---|
| 116 | |
---|
| 117 | void EKFfixed::init_ekf(double Tv) |
---|
| 118 | { |
---|
| 119 | // Tuning of matrix Q |
---|
| 120 | Q[0]=prevod(.01,15); // 0.05 |
---|
| 121 | Q[5]=Q[0]; |
---|
| 122 | Q[10]=prevod(0.0001,15); // 1e-3 |
---|
| 123 | Q[15]=prevod(0.0001,15); // 1e-3 |
---|
| 124 | |
---|
| 125 | // Tuning of matrix R |
---|
| 126 | R[0]=prevod(0.05,15); // 0.05 |
---|
| 127 | R[3]=R[0]; |
---|
| 128 | |
---|
| 129 | // Motor model parameters |
---|
| 130 | cA=prevod(1-Tv*Rs/Ls,15); |
---|
| 131 | cB=prevod(Tv*Wref*Fmag/Iref/Ls,15); |
---|
| 132 | cC=prevod(Tv/Ls/Iref*Uref,15); |
---|
| 133 | // cD=prevod(1-Tv*Bf/J,15); |
---|
| 134 | // cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15); |
---|
| 135 | // cF=prevod(p*Tv*Mref/J/Wref,15); |
---|
| 136 | cG=prevod(Tv*Wref*4/Thetaref,15); |
---|
| 137 | cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15); |
---|
| 138 | // cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref); |
---|
| 139 | |
---|
| 140 | /* Init matrix PSI with permanently constant terms */ |
---|
| 141 | PSI[0]=cA; |
---|
| 142 | PSI[5]=PSI[0]; |
---|
| 143 | PSI[10]=0x7FFF; |
---|
| 144 | PSI[14]=cG; |
---|
| 145 | PSI[15]=0x7FFF; |
---|
| 146 | } |
---|