[278] | 1 | /************************************ |
---|
| 2 | Extended Kalman Filter |
---|
| 3 | Kalman Observer |
---|
| 4 | |
---|
| 5 | Z. Peroutka |
---|
| 6 | |
---|
| 7 | Rev. 15.3.2008 |
---|
| 8 | |
---|
| 9 | EKF pocitan s daty ve formatu Q15, zatimco regulace pracuje v Q13 -> |
---|
| 10 | resenim je vynasobit referencni hodnoty v EKF 4-mi (tim je automaticky |
---|
| 11 | zajisten prechod mezi formaty Q13 a Q15). Realizovano konstantou K_PREVOD_FORM. |
---|
| 12 | |
---|
| 13 | 15.3.2008 Kontrola kodu + zamena datovych typu q15->int a q30->long. |
---|
| 14 | |
---|
| 15 | *************************************/ |
---|
| 16 | |
---|
| 17 | #include <math.h> |
---|
| 18 | #include "fixed.h" |
---|
| 19 | //#include "parametry_motoru.h" // aktivovat v DSP |
---|
| 20 | #include "reference.h" |
---|
| 21 | #include "matrix.h" |
---|
| 22 | #include "ekf.h" |
---|
| 23 | |
---|
| 24 | #define K_PREVOD_FORM 4 // 2^(15-Qm) |
---|
| 25 | |
---|
| 26 | /* Declaration of global functions */ |
---|
| 27 | void init_ekf(double Tv, double *param); |
---|
| 28 | void ekf(double *x_estimation, double Umd, double beta, double Ucnd, double Ucd, double isxd, double isyd); |
---|
| 29 | |
---|
| 30 | /* Declaration of local functions */ |
---|
| 31 | static void prediction(int *ux); |
---|
| 32 | static void correction(void); |
---|
| 33 | static void update_psi(void); |
---|
| 34 | |
---|
| 35 | // global variables |
---|
| 36 | double ladeni_EKF[10]; |
---|
| 37 | |
---|
| 38 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
| 39 | static int H[8]={0x7FFF,0,0,0,0,0x7FFF,0,0}; /* matrix [2,4] */ |
---|
| 40 | static int Ht[8]={0x7FFF,0,0,0x7FFF,0,0,0,0};/* matrix [4,2] */ |
---|
| 41 | //static int Q[4][4]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */ |
---|
| 42 | //static int R[2][2]={0,0,0,0}; /* matrix [2,2] */ |
---|
| 43 | static int Q[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */ |
---|
| 44 | static int R[4]={0,0,0,0}; /* matrix [2,2] */ |
---|
| 45 | |
---|
| 46 | /* Initial conditions and variables */ |
---|
| 47 | static int x_est[4]={0,0,0,0}; |
---|
| 48 | static int x_pred[4]={0,0,0,0}; |
---|
| 49 | static int P_pred[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */ |
---|
| 50 | //static int P_est[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */ |
---|
| 51 | static int P_est[16]={0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF}; /* matrix [4,4] */ |
---|
| 52 | static int Y_mes[2]={0,0}; |
---|
| 53 | static int ukalm[2]={0,0}; |
---|
| 54 | static int Kalm[8]; /* matrix [5,2] */ |
---|
| 55 | |
---|
| 56 | static int PSI[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */ |
---|
| 57 | |
---|
| 58 | /* Temporary variables */ |
---|
| 59 | static int temp15a[16]; |
---|
| 60 | |
---|
| 61 | // constants of PMSM mathematical model |
---|
| 62 | static int cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
| 63 | |
---|
| 64 | ////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
| 65 | void init_ekf(double Tv, double *param) |
---|
| 66 | { |
---|
| 67 | // parametry simulace - v DSP vyhodit (nahrazeno "parametry_motoru.h") |
---|
| 68 | double Rs, Ls, Fmag, kp, p; |
---|
| 69 | |
---|
| 70 | // param=[Rs, Ls, Fmag, Bf, p, kp, J = 0.04?]; |
---|
| 71 | Rs=*param; |
---|
| 72 | Ls=*(param+1); |
---|
| 73 | Fmag=*(param+2); |
---|
| 74 | p=*(param+4); |
---|
| 75 | kp=*(param+5); |
---|
| 76 | /// KONEC inicializace pro PC - az sem vyhodit v DSP /// |
---|
| 77 | |
---|
| 78 | // Tuning of matrix Q |
---|
| 79 | /* Q[0][0]=prevod(.05,15); // 1e-2 |
---|
| 80 | Q[1][1]=Q[0][0]; |
---|
| 81 | Q[2][2]=prevod(1e-3,15); // 10e-5 |
---|
| 82 | Q[3][3]=prevod(1e-3,15); // model(ureg) ... 4e-3 |
---|
| 83 | |
---|
| 84 | // Tuning of matrix R |
---|
| 85 | R[0][0]=prevod(0.05,15); // model(ureg) ... 0.1 |
---|
| 86 | R[1][1]=R[0][0]; |
---|
| 87 | /**/ |
---|
| 88 | |
---|
| 89 | // Tuning of matrix Q |
---|
| 90 | Q[0]=prevod(.01,15); // 0.05 |
---|
| 91 | Q[5]=Q[0]; |
---|
| 92 | Q[10]=prevod(0.0001,15); // 1e-3 |
---|
| 93 | Q[15]=prevod(0.0001,15); // 1e-3 |
---|
| 94 | |
---|
| 95 | // Tuning of matrix R |
---|
| 96 | R[0]=prevod(0.05,15); // 0.05 |
---|
| 97 | R[3]=R[0]; |
---|
| 98 | /**/ |
---|
| 99 | |
---|
| 100 | /* Tuning of matrix P_est - initial values */ |
---|
| 101 | /* P_est[0][0]=prevod(0.999,15); |
---|
| 102 | P_est[1][1]=prevod(0.999,15); |
---|
| 103 | P_est[2][2]=prevod(0.999,15); |
---|
| 104 | P_est[3][3]=prevod(0.999,15); |
---|
| 105 | P_est[4][4]=prevod(0.999,15); |
---|
| 106 | /**/ |
---|
| 107 | |
---|
| 108 | // Motor model parameters |
---|
| 109 | cA=prevod(1-Tv*Rs/Ls,15); |
---|
| 110 | cB=prevod(Tv*Wref*Fmag/Iref/Ls,15); |
---|
| 111 | cC=prevod(Tv/Ls/Iref*Uref,15); |
---|
| 112 | // cD=prevod(1-Tv*Bf/J,15); |
---|
| 113 | // cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15); |
---|
| 114 | // cF=prevod(p*Tv*Mref/J/Wref,15); |
---|
| 115 | cG=prevod(Tv*Wref*K_PREVOD_FORM/Thetaref,15); |
---|
| 116 | cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15); |
---|
| 117 | // cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref); |
---|
| 118 | |
---|
| 119 | /* Init matrix PSI with permanently constant terms */ |
---|
| 120 | PSI[0]=cA; |
---|
| 121 | PSI[5]=PSI[0]; |
---|
| 122 | PSI[10]=0x7FFF; |
---|
| 123 | PSI[14]=cG; |
---|
| 124 | PSI[15]=0x7FFF; |
---|
| 125 | } |
---|
| 126 | |
---|
| 127 | |
---|
| 128 | static void update_psi(void) |
---|
| 129 | { |
---|
| 130 | int t_sin,t_cos,tmp; |
---|
| 131 | |
---|
| 132 | // implementace v PC |
---|
| 133 | t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); |
---|
| 134 | t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15); |
---|
| 135 | |
---|
| 136 | // implementace v DSP |
---|
| 137 | // t_sin=qsin(x_est[3]); |
---|
| 138 | // t_cos=qcos(x_est[3]); |
---|
| 139 | |
---|
| 140 | PSI[2]=((long)cB*t_sin)>>15; |
---|
| 141 | tmp=((long)cH*x_est[2])>>15; |
---|
| 142 | PSI[3]=((long)tmp*t_cos)>>15; |
---|
| 143 | PSI[6]=-((long)cB*t_cos)>>15; |
---|
| 144 | PSI[7]=((long)tmp*t_sin)>>15; |
---|
| 145 | } |
---|
| 146 | |
---|
| 147 | |
---|
| 148 | void prediction(int *ux) |
---|
| 149 | { |
---|
| 150 | int t_sin,t_cos, tmp; |
---|
| 151 | |
---|
| 152 | // implementace v PC |
---|
| 153 | t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); |
---|
| 154 | t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15); |
---|
| 155 | |
---|
| 156 | // implementace v DSP |
---|
| 157 | // t_sin=qsin(x_est[3]); |
---|
| 158 | // t_cos=qcos(x_est[3]); |
---|
| 159 | |
---|
| 160 | tmp=((long)cB*x_est[2])>>15; |
---|
| 161 | x_pred[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*ux[0])>>15; |
---|
| 162 | x_pred[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*ux[1])>>15; |
---|
| 163 | x_pred[2]=x_est[2]; |
---|
| 164 | x_pred[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15; |
---|
| 165 | |
---|
| 166 | update_psi(); |
---|
| 167 | |
---|
| 168 | mmult(PSI,P_est,temp15a,3,3,3); |
---|
| 169 | // mtrans(PSI,temp15b,5,5); |
---|
| 170 | mmultt(temp15a,PSI,P_pred,3,3,3); |
---|
| 171 | maddD(P_pred,Q,3,3); |
---|
| 172 | } |
---|
| 173 | |
---|
| 174 | void correction(void) |
---|
| 175 | { |
---|
| 176 | int Y_error[2]; |
---|
| 177 | long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */ |
---|
| 178 | |
---|
| 179 | /* Kalman gain calculation */ |
---|
| 180 | // mmult(P_pred,Ht,temp15a,5,5,2); |
---|
| 181 | /* mmultt(P_pred,H,temp15a,4,4,1); |
---|
| 182 | mmult(H,temp15a,temp15b,1,4,1); these lines are replaced by choice_P */ |
---|
| 183 | |
---|
| 184 | choice_P(P_pred,temp15a,3); |
---|
| 185 | maddD(temp15a,R,1,1); |
---|
| 186 | minv2(temp15a,temp30a); |
---|
| 187 | /* mmultt(P_pred,H,temp15a,4,4,1); /* remove this line if choice_P is not used */ |
---|
| 188 | // mmultDr15(P_pred,Ht,temp15a,4,4,1,1); |
---|
| 189 | mmultDr(P_pred,temp15a,3,3,1,1); |
---|
| 190 | mmult1530(temp15a,temp30a,Kalm,3,1,1); |
---|
| 191 | |
---|
| 192 | /* estimate the state system */ |
---|
| 193 | // mmult(H,x_pred,temp15a,1,4,0); |
---|
| 194 | choice_x(x_pred, temp15a); |
---|
| 195 | msub(Y_mes,temp15a,Y_error,1,0); |
---|
| 196 | mmult(Kalm,Y_error,temp15a,3,1,0); |
---|
| 197 | madd(x_pred,temp15a,x_est,3,0); |
---|
| 198 | |
---|
| 199 | /* matrix of covariances - version without MMULTDL() */ |
---|
| 200 | /* mmult(Kalm,H,temp15a,4,1,4); |
---|
| 201 | mmult(temp15a,P_pred,P_est,4,4,4); |
---|
| 202 | msub(P_pred,P_est,P_est,4,4); |
---|
| 203 | /* END */ |
---|
| 204 | |
---|
| 205 | /* Version with MMULTDL() */ |
---|
| 206 | mmultDl(P_pred,temp15a,1,3,3,1); |
---|
| 207 | /* if result matrix has more terms than DIAG matrix,it is necessary to enable |
---|
| 208 | erase sequence in definition of function MMULTDL() that is currently disabled. */ |
---|
| 209 | |
---|
| 210 | mmult(Kalm,temp15a,P_est,3,1,3); |
---|
| 211 | msub(P_pred,P_est,P_est,3,3); |
---|
| 212 | /* END */ |
---|
| 213 | } |
---|
| 214 | |
---|
| 215 | |
---|
| 216 | void ekf(double *x_estimation, double Umd, double beta, double Ucnd, double Ucd, double isxd, double isyd) |
---|
| 217 | { |
---|
| 218 | int Umk, ua, ub; |
---|
| 219 | |
---|
| 220 | /// prechod ze simulace do fixed-pointu - vyradit v DSP /////////// |
---|
| 221 | int Um, Ucn, Uc, isx, isy, t_cos; |
---|
| 222 | |
---|
| 223 | Um=prevod(Umd/Uref,Qm); |
---|
| 224 | Ucn=prevod(Ucnd/Uref,Qm); |
---|
| 225 | Uc=prevod(Ucd/Uref,Qm); |
---|
| 226 | isx=prevod(isxd/Iref,Qm); |
---|
| 227 | isy=prevod(isyd/Iref,Qm); |
---|
| 228 | /// KONEC emulace FIXED-POINTu - az sem vyradit v DSP //////////// |
---|
| 229 | |
---|
| 230 | // prepocet napeti pro EKF dle skutecneho napeti v ss obvodu |
---|
| 231 | Umk=((long)Um*Uc)/Ucn; |
---|
| 232 | |
---|
| 233 | // vypocet fazovych napeti stridace |
---|
| 234 | ua=((long)Umk*prevod(cos(beta),15))>>15; |
---|
| 235 | ub=((long)Umk*prevod(cos(beta-2./3.*M_PI),15))>>15; |
---|
| 236 | // uc=((long)Umk*prevod(cos(beta+2./3.*M_PI),15))>>15; |
---|
| 237 | |
---|
| 238 | // vypocet napeti v systemu (x,y) |
---|
| 239 | ukalm[0]=ua; |
---|
| 240 | ukalm[1]=(((long)ua+ub<<1)*18917)>>15; // usy=(ua+2*ub)/sqrt(3) |
---|
| 241 | |
---|
| 242 | // zadani mereni |
---|
| 243 | Y_mes[0]=isx; |
---|
| 244 | Y_mes[1]=isy; |
---|
| 245 | |
---|
| 246 | ////// vlastni rutina EKF ///////////////////////// |
---|
| 247 | prediction(ukalm); |
---|
| 248 | correction(); |
---|
| 249 | |
---|
| 250 | // navrat estimovanych hodnot regulatoru |
---|
| 251 | *x_estimation=zprevod(x_est[2],Qm)*Wref; |
---|
| 252 | // x_est[3]=(short int)x_est[3]; |
---|
| 253 | *(x_estimation+1)=zprevod((short int)x_est[3],15)*Thetaref; |
---|
| 254 | } |
---|