1 | /************************************ |
---|
2 | Extended Kalman Filter |
---|
3 | Kalman Observer |
---|
4 | |
---|
5 | Z. Peroutka |
---|
6 | |
---|
7 | Rev. 29.10.2010 |
---|
8 | |
---|
9 | EKF pocitan dle Grewala - varianta Choleski |
---|
10 | |
---|
11 | 29.10.2010 Predelano na Choleski (puvodne Bierman rev. 4). |
---|
12 | |
---|
13 | |
---|
14 | *************************************/ |
---|
15 | |
---|
16 | #include "funkce.h" |
---|
17 | #include "parametry_motoru.h" // aktivovat v DSP |
---|
18 | #include "reference_Q15.h" |
---|
19 | //#include "reference.h" |
---|
20 | #include "matrix_vs.h" |
---|
21 | #include "ekf_choleski.h" |
---|
22 | #include "parametry_motoru.h" |
---|
23 | #include "qmath.h" |
---|
24 | |
---|
25 | #define K_PREVOD_FORM (1<<(15-Qm)) // 2^(15-Qm) |
---|
26 | #define pi2_3 21845 // 2/3*PI |
---|
27 | |
---|
28 | #define INIT_CH (1<<qCh) |
---|
29 | |
---|
30 | /* Declaration of global functions */ |
---|
31 | void init_ekf(double Tv); |
---|
32 | void ekf(int *x_estimation, int Um, int beta, int Ucn, int Uc, int isx, int isy); |
---|
33 | |
---|
34 | /* Declaration of local functions */ |
---|
35 | static void prediction(int *ux); |
---|
36 | static void correction(void); |
---|
37 | static void update_psi(void); |
---|
38 | |
---|
39 | // Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli? |
---|
40 | static int Q[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */ |
---|
41 | static int R[4]={0,0,0,0}; /* matrix [2,2] */ |
---|
42 | |
---|
43 | static int PSI[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */ |
---|
44 | |
---|
45 | /* Initial conditions and variables */ |
---|
46 | static int x_est[4]={0,0,0,0}; |
---|
47 | static int x_pred[4]={0,0,0,0}; |
---|
48 | |
---|
49 | static int Y_mes[2]={0,0}; |
---|
50 | static int ukalm[2]={0,0}; |
---|
51 | |
---|
52 | // matice pro Biermana |
---|
53 | static int Ch[16]={INIT_CH,0,0,0,0,INIT_CH,0,0,0,0,INIT_CH,0,0,0,0,INIT_CH}; /* matrix [4,4] */ |
---|
54 | static int PSIU[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* matrix [4,4] */ |
---|
55 | |
---|
56 | // constants of PMSM mathematical model |
---|
57 | static int cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
58 | |
---|
59 | ////////////////////////////////////////////////////////////////////////////////////////////////////// |
---|
60 | void init_ekf(double Tv) |
---|
61 | { |
---|
62 | // Tuning of matrix Q |
---|
63 | // Tuning of matrix Q ... 0.05, 2e-4, 1e-3 // Vencovi dle MPF vyslo, q11=q22=0.023, q33=2e-4, q44=7e-4 |
---|
64 | /* Q[0]=prevod(0.02,15); // 0.05 |
---|
65 | Q[5]=Q[0]; |
---|
66 | Q[10]=prevod(0.0002,15); // 0.0002 |
---|
67 | Q[15]=prevod(0.001,15); // 1e-3 |
---|
68 | |
---|
69 | // Tuning of matrix R |
---|
70 | R[0]=prevod(0.1,15); // 0.1 |
---|
71 | R[3]=R[0]; |
---|
72 | /**/ |
---|
73 | // 29.10.2010: 0.01; 0.008; 0.005 |
---|
74 | Q[0]=prevod(0.01,15); // puvodne: Q11=0.02; Q33=0.0002; Q44=0.001 |
---|
75 | Q[5]=Q[0]; // VS: 0.001; 0.001; 0.0001 |
---|
76 | Q[10]=prevod(0.008,15); // 0.01 na vsech variancich |
---|
77 | Q[15]=prevod(0.005,15); |
---|
78 | |
---|
79 | // Tuning of matrix R |
---|
80 | R[0]=prevod(0.05,15); // puvodni: 0.1; test: 0.08 |
---|
81 | R[3]=R[0]; |
---|
82 | /**/ |
---|
83 | |
---|
84 | // Motor model parameters |
---|
85 | cA=prevod(1-Tv*Rs/Ls,15); |
---|
86 | cB=prevod(Tv*Wref*Fmag/Iref/Ls,15); |
---|
87 | cC=prevod(Tv/Ls/Iref*Uref,15); |
---|
88 | // cD=prevod(1-Tv*Bf/J,15); |
---|
89 | // cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15); |
---|
90 | // cF=prevod(p*Tv*Mref/J/Wref,15); |
---|
91 | cG=prevod(Tv*Wref*K_PREVOD_FORM/Thetaref,15); |
---|
92 | // cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15); |
---|
93 | cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); |
---|
94 | // cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref); |
---|
95 | |
---|
96 | /* Init matrix PSI with permanently constant terms */ |
---|
97 | PSI[0]=cA; |
---|
98 | PSI[5]=PSI[0]; |
---|
99 | PSI[10]=0x7FFF; |
---|
100 | PSI[14]=cG; |
---|
101 | PSI[15]=0x7FFF; |
---|
102 | } |
---|
103 | |
---|
104 | |
---|
105 | static void update_psi(void) |
---|
106 | { |
---|
107 | int t_sin,t_cos,tmp; |
---|
108 | |
---|
109 | // implementace v DSP |
---|
110 | t_sin=qsin(x_est[3]); |
---|
111 | t_cos=qcos(x_est[3]); |
---|
112 | // t_sin=prevod(qsin(Thetaref*x_est[3]/32768.),15); |
---|
113 | // t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15); |
---|
114 | |
---|
115 | PSI[2]=((long)cB*t_sin)>>15; |
---|
116 | tmp=((long)cH*x_est[2])>>15; |
---|
117 | PSI[3]=((long)tmp*t_cos)>>15; |
---|
118 | PSI[6]=-((long)cB*t_cos)>>15; |
---|
119 | PSI[7]=((long)tmp*t_sin)>>15; |
---|
120 | } |
---|
121 | |
---|
122 | |
---|
123 | static void prediction(int *ux) |
---|
124 | { |
---|
125 | int t_sin,t_cos, tmp; |
---|
126 | |
---|
127 | int G[16]; |
---|
128 | int Dold[4]; |
---|
129 | |
---|
130 | // implementace v DSP |
---|
131 | t_sin=qsin(x_est[3]); |
---|
132 | t_cos=qcos(x_est[3]); |
---|
133 | // t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); |
---|
134 | // t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15); |
---|
135 | |
---|
136 | tmp=((long)cB*x_est[2])>>15; |
---|
137 | x_pred[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*ux[0])>>15; |
---|
138 | x_pred[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*ux[1])>>15; |
---|
139 | x_pred[2]=x_est[2]; |
---|
140 | x_pred[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15; |
---|
141 | |
---|
142 | update_psi(); |
---|
143 | |
---|
144 | // Householder |
---|
145 | mmultACh(PSI,Ch,PSIU,4,4); |
---|
146 | // householder(PSIU,Q,4); |
---|
147 | givens(PSIU,Q,4); |
---|
148 | for (tmp=0; tmp<16;tmp++) |
---|
149 | Ch[tmp]=PSIU[tmp]; |
---|
150 | } |
---|
151 | |
---|
152 | static void correction(void) |
---|
153 | { |
---|
154 | int Y_error[2]; |
---|
155 | unsigned int ii; |
---|
156 | int dR[2]; |
---|
157 | |
---|
158 | // prediction error |
---|
159 | Y_error[0]=Y_mes[0]-x_pred[0]; |
---|
160 | Y_error[1]=Y_mes[1]-x_pred[1]; |
---|
161 | |
---|
162 | // copy prediction to state exstimation vector |
---|
163 | for (ii=0;ii<4;ii++) x_est[ii]=x_pred[ii]; |
---|
164 | |
---|
165 | // copy diagonal from matrix R |
---|
166 | dR[0]=R[0]; |
---|
167 | dR[1]=R[3]; |
---|
168 | |
---|
169 | // Carlson - correction |
---|
170 | carlson(Y_error,x_est,Ch,dR,2,4); |
---|
171 | } |
---|
172 | |
---|
173 | |
---|
174 | void ekf(int *x_estimation, int Um, int beta, int Ucn, int Uc, int isx, int isy) |
---|
175 | { |
---|
176 | int Umk, ua, ub; |
---|
177 | |
---|
178 | // prepocet napeti pro EKF dle skutecneho napeti v ss obvodu |
---|
179 | Umk=((long)Um*Uc)/Ucn; |
---|
180 | |
---|
181 | // vypocet napeti v systemu (x,y) |
---|
182 | ukalm[0]=((long)Umk*qcos(beta))>>15; // usx |
---|
183 | ukalm[1]=((long)Umk*qsin(beta))>>15; // usy |
---|
184 | |
---|
185 | // zadani mereni |
---|
186 | Y_mes[0]=isx; |
---|
187 | Y_mes[1]=isy; |
---|
188 | |
---|
189 | ////// vlastni rutina EKF ///////////////////////// |
---|
190 | prediction(ukalm); |
---|
191 | correction(); |
---|
192 | |
---|
193 | // navrat estimovanych hodnot regulatoru |
---|
194 | *x_estimation=x_est[2]; |
---|
195 | *(x_estimation+1)=x_est[3]; |
---|
196 | } |
---|
197 | |
---|
198 | void reset_ekf(void) |
---|
199 | { |
---|
200 | int ii; |
---|
201 | |
---|
202 | for (ii=0;ii<4;ii++) x_est[ii]=0; |
---|
203 | for (ii=0;ii<4;ii++) x_pred[ii]=0; |
---|
204 | |
---|
205 | for (ii=0;ii<16;ii++) PSIU[ii]=0; |
---|
206 | for (ii=0;ii<16;ii++) Ch[ii]=0; |
---|
207 | Ch[0]=INIT_CH; |
---|
208 | Ch[5]=INIT_CH; |
---|
209 | Ch[10]=INIT_CH; |
---|
210 | Ch[15]=INIT_CH; |
---|
211 | /**/ |
---|
212 | } |
---|