Rev 966 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
966 | MikeW | 1 | /* |
2 | Copyright 2008, by Michael Walter |
||
3 | |||
4 | All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser |
||
5 | General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but |
||
6 | WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||
7 | See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public |
||
8 | License along with this program. If not, see <http://www.gnu.org/licenses/>. |
||
9 | |||
10 | Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
||
11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
||
12 | unless it is stated otherwise. |
||
13 | */ |
||
14 | |||
838 | MikeW | 15 | /***************************************************************************** |
16 | INCLUDES |
||
17 | **************************************************************************** */ |
||
18 | #include "kafi.h" |
||
966 | MikeW | 19 | #include "analog.h" |
20 | #include "main.h" |
||
21 | #include "FlightControl.h" |
||
22 | #include "mm3.h" |
||
838 | MikeW | 23 | |
24 | /***************************************************************************** |
||
25 | (SYMBOLIC) CONSTANTS |
||
26 | *****************************************************************************/ |
||
27 | |||
966 | MikeW | 28 | /***************************************************************************** |
29 | VARIABLES |
||
30 | *****************************************************************************/ |
||
31 | volatile int KompassValue = 0; |
||
838 | MikeW | 32 | |
966 | MikeW | 33 | /* Kalman filter */ |
34 | f32_t Phi, Theta, sinPhi, cosPhi, tanTheta, secTheta; |
||
35 | f32_t sinPhi, cosPhi, sinTheta, cosTheta; |
||
36 | f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P, sinPsi_P, cosPsi_P; |
||
838 | MikeW | 37 | |
966 | MikeW | 38 | f32_t **matPs; /* Predicted system Noise */ |
39 | f32_t **matP; /* Predicted system Noise */ |
||
40 | f32_t **matP_tmp; |
||
41 | f32_t **matQ; /* system noise covariance */ |
||
42 | f32_t **matR; /* measurement noise covariance */ |
||
43 | f32_t **matXd; /* estimated state */ |
||
44 | f32_t **matXs; /* predicted state */ |
||
45 | f32_t **matX_tmp1; |
||
46 | f32_t **matX_tmp2; |
||
47 | |||
48 | f32_t **matPhi; /* transition matrix to predict new state from current state */ |
||
49 | f32_t **matB ; /* control matrix to predict new state from ext. control vector U */ |
||
50 | f32_t **matU; /* control vector */ |
||
51 | f32_t **matC; /* Jacobi matrix */ |
||
52 | f32_t **matY; /* real measurements */ |
||
53 | f32_t **matYs; /* predicted measurements */ |
||
54 | f32_t **matdY; /* innovation */ |
||
55 | f32_t **temp_meas_meas; |
||
56 | f32_t **temp_meas_state; |
||
57 | f32_t **temp_state_meas; |
||
58 | f32_t **temp_meas_2; |
||
59 | f32_t **temp_state_state; |
||
60 | f32_t **matK; /* the Kalman gain matrix */ |
||
61 | |||
62 | char fYValid[KAFI_DIM_Y]; /* measurement validity flag */ |
||
63 | |||
64 | |||
65 | |||
66 | /* **************************************************************************** |
||
67 | Functionname: AttitudeEstimation */ /*! |
||
68 | Description: Main Kalman Filter Loop |
||
69 | |||
70 | @param[in] |
||
71 | |||
72 | @return void |
||
73 | @pre - |
||
74 | @post - |
||
75 | @author Michael Walter |
||
76 | **************************************************************************** */ |
||
77 | void FlightAttitudeEstimation() |
||
78 | { |
||
79 | /* Predict the Measurent */ |
||
80 | trPredictMeasurement(); |
||
81 | |||
82 | /* Update the Jacobi Matrix */ |
||
83 | trUpdateJacobiMatrix(); |
||
84 | |||
85 | /* Extract measurements and store them in vector matY. |
||
86 | Measurement validity is indicated by fYValid[] */ |
||
87 | trMeasure(); |
||
88 | |||
89 | /* Innovation: calculate Xd, and Pd */ |
||
90 | trInnovate(); |
||
91 | |||
92 | /* Update transition matrix Phi */ |
||
93 | //trUpdatePhi(); // Is an Identity matrix which will not change |
||
94 | |||
95 | /* Update control matrix B */ |
||
96 | trUpdateBU(); |
||
97 | |||
98 | /* Predict new state Xs and Ps */ |
||
99 | KAFIPrediction(); |
||
100 | |||
101 | /* store innovated state vector in current state */ |
||
102 | matGetFull(matXs, _Phi , 0, &status.Phi); |
||
103 | matGetFull(matXs, _Theta , 0, &status.Theta); |
||
104 | matGetFull(matXs, _Psi , 0, &status.Psi); |
||
105 | |||
106 | /* Limit Angles to [-2Pi;...;+2Pi] */ |
||
107 | trLimitAngles(); |
||
108 | } |
||
109 | |||
110 | /* **************************************************************************** |
||
111 | Functionname: Kafi_Init */ /*! |
||
112 | Description: Generate Kalman Matrizes |
||
113 | |||
114 | @param[in] |
||
115 | |||
116 | @return void |
||
117 | @pre - |
||
118 | @post - |
||
119 | @author Michael Walter |
||
120 | **************************************************************************** */ |
||
121 | void Kafi_Init() |
||
122 | { |
||
123 | /* create kalman filter and associated matrizes */ |
||
124 | ui32_t i, j; |
||
125 | matPs = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X ); |
||
126 | matQ = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X); |
||
127 | matR = matrix( 1, KAFI_DIM_Y, 1,KAFI_DIM_Y); |
||
128 | matXd = matrix( 1, KAFI_DIM_X, 1,1); /* estimated state */ |
||
129 | matXs = matrix( 1, KAFI_DIM_X, 1,1); /* predicted state */ |
||
130 | matX_tmp1 = matrix( 1, KAFI_DIM_X,1, 1); |
||
131 | matX_tmp2 = matrix( 1, KAFI_DIM_X, 1,1); |
||
132 | matPhi = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X);/* transition matrix to predict new state from current state */ |
||
133 | matB = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_U); /* control matrix to predict new state from ext. control vector U */ |
||
134 | matU = matrix( 1, KAFI_DIM_U, 1,1); /* control vector */ |
||
135 | matC = matrix( 1, KAFI_DIM_Y, 1,KAFI_DIM_X); /* Jacobi matrix */ |
||
136 | matY = matrix( 1, KAFI_DIM_Y, 1,1); /* real measurements */ |
||
137 | matdY = matrix( 1, KAFI_DIM_Y, 1,1); /* innovation */ |
||
138 | matYs = matrix( 1, KAFI_DIM_Y, 1,1); /* predicted measurements */ |
||
139 | matP = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X); |
||
140 | matK = matrix(1, KAFI_DIM_X, 1, KAFI_DIM_Y); |
||
141 | temp_state_state = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X ); |
||
142 | temp_meas_state = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_X ); |
||
143 | temp_state_meas = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_Y ); |
||
144 | temp_meas_meas = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_Y ); |
||
145 | temp_meas_2 = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_Y ); |
||
146 | matP_tmp = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X); |
||
147 | |||
148 | /* set phi to identity matrix */ |
||
149 | for ( j = 0; j < KAFI_DIM_X; j++ ) |
||
150 | { |
||
151 | for ( i = 0; i < KAFI_DIM_X; i++ ) |
||
152 | { |
||
153 | matSetFull(matPhi, j, i, (j == i) ? 1.F : 0.F); |
||
154 | } |
||
155 | } |
||
156 | |||
157 | /* set diagonal of measurement noise covariance R */ |
||
158 | matSetDiagonal(matR, _ax, _ax, 10.0F); |
||
159 | matSetDiagonal(matR, _ay, _ay, 10.0F); |
||
160 | matSetDiagonal(matR, _az, _az, 10.0F); |
||
161 | matSetDiagonal(matR, _compass, _compass, 5.0F); |
||
162 | |||
163 | #ifdef MELEXIS_GYRO |
||
164 | /* set diagonal of system noise covariance Q, dim(X,X) */ |
||
165 | matSetDiagonal(matQ, _Phi, _Phi, 0.000006F); |
||
166 | matSetDiagonal(matQ, _Theta, _Theta, 0.000006F); |
||
167 | matSetDiagonal(matQ, _Psi, _Psi, 0.000006F); |
||
168 | #else |
||
169 | /* set diagonal of system noise covariance Q */ |
||
170 | matSetDiagonal(matQ, _Phi, _Phi, 0.00001F); |
||
171 | matSetDiagonal(matQ, _Theta, _Theta, 0.00001F); |
||
172 | matSetDiagonal(matQ, _Psi, _Psi, 0.00005F); |
||
173 | #endif |
||
174 | |||
175 | /* set diagonal init. estimation error (covariance) P0 */ |
||
176 | matSetDiagonal(matPs, _Phi, _Phi, 0.00001F); |
||
177 | matSetDiagonal(matPs, _Theta, _Theta, 0.00001F); |
||
178 | matSetDiagonal(matPs, _Psi, _Psi, 0.00001F); |
||
179 | |||
180 | matSetFull(matXd, _Phi, 0, 0.0F); |
||
181 | matSetFull(matXd, _Theta, 0, 0.0F); |
||
182 | matSetFull(matXd, _Psi, 0, 0.0F); |
||
183 | |||
184 | matSetFull(matXs, _Phi, 0, 0.0F); |
||
185 | matSetFull(matXs, _Theta, 0, 0.0F); |
||
186 | matSetFull(matXs, _Psi, 0, 0.0F); |
||
187 | } |
||
188 | |||
189 | /* **************************************************************************** |
||
190 | Functionname: KAFIPrediction */ /*! |
||
191 | Description: Predict new state Xs and Ps |
||
192 | |||
193 | X_k = A * X_(k-1) + B * u_k |
||
194 | P_k = A * P_(k-1) * A^T + Q |
||
195 | |||
196 | @param[in] |
||
197 | |||
198 | @return void |
||
199 | @pre - |
||
200 | @post - |
||
201 | @author Michael Walter |
||
202 | **************************************************************************** */ |
||
203 | void KAFIPrediction() |
||
204 | { |
||
205 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
206 | |||
207 | /*--- VARIABLES ---*/ |
||
208 | |||
209 | /* Innovation: calculate Xd, and Pd */ |
||
210 | |||
211 | /* X_k = A * X_(k-1) + B * u_k */ |
||
212 | |||
213 | /* matX_tmp1 = matPhi * matXd */ |
||
214 | /* |
||
215 | matrix_mult(matPhi,matXd, matX_tmp1, |
||
216 | KAFI_DIM_X, KAFI_DIM_X, 1); |
||
217 | */ |
||
218 | /* matX_tmp2 = matB * matU */ |
||
219 | matrix_mult( matB,matU, matX_tmp2, |
||
220 | KAFI_DIM_X, KAFI_DIM_U, 1); |
||
221 | |||
222 | /* matXs = matPhi * matXd + matB * matU */ |
||
223 | /* |
||
224 | matrix_add( matX_tmp1, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
||
225 | matrix_add( matXd, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
||
226 | */ |
||
227 | |||
228 | matXs[1][1] = matXd[1][1] + matX_tmp2[1][1]; |
||
229 | matXs[2][1] = matXd[2][1] + matX_tmp2[2][1]; |
||
230 | matXs[3][1] = matXd[3][1] + matX_tmp2[3][1]; |
||
231 | |||
232 | /*P_k = A * P_(k-1)*A^T + Q */ |
||
233 | /* |
||
234 | matrix_mult_transpose( matP, matPhi, matP_tmp, // Can be skiped PHI is a ident matrix |
||
235 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X ); |
||
236 | matrix_mult( matPhi, matP_tmp, matP, |
||
237 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X ); |
||
238 | matrix_add( matP, matQ, matPs, KAFI_DIM_X, KAFI_DIM_X ); |
||
239 | */ |
||
240 | |||
241 | matPs[1][1] = matP[1][1] + matQ[1][1]; |
||
242 | matPs[2][2] = matP[2][2] + matQ[2][2]; |
||
243 | matPs[3][3] = matP[3][3] + matQ[3][3]; |
||
244 | } |
||
245 | |||
246 | /* **************************************************************************** |
||
247 | Functionname: trInnovate */ /*! |
||
248 | Description: Innovation: calculate Xd, and Pd |
||
249 | |||
250 | K = Ps * C^T * (C * Ps * C^T + R)^-1 |
||
251 | X = Xs + K (Y - Ys) |
||
252 | |||
253 | @param[in] |
||
254 | |||
255 | @return void |
||
256 | @pre - |
||
257 | @post - |
||
258 | @author Michael Walter |
||
259 | **************************************************************************** */ |
||
260 | void trInnovate() |
||
261 | { |
||
262 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
263 | |||
264 | /*--- VARIABLES ---*/ |
||
265 | #if 0 /* Runtime optimised. To be verified */ |
||
266 | /* temp_meas_state = matC * matPs */ |
||
267 | matrix_mult( matC, matPs, temp_meas_state, |
||
268 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
||
269 | |||
270 | /* temp_meas_meas = matC * matPs^T * matC^T */ |
||
271 | matrix_mult_transpose( matC, temp_meas_state, temp_meas_meas, |
||
272 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
||
273 | |||
274 | /* temp_meas_meas = matC * matPs^T * matC^T + matR */ |
||
275 | /* |
||
276 | matrix_add( temp_meas_meas, matR, temp_meas_meas, |
||
277 | KAFI_DIM_Y, KAFI_DIM_Y ); |
||
278 | */ |
||
279 | temp_meas_meas[1][1] += matR[1][1]; |
||
280 | temp_meas_meas[2][2] += matR[2][2]; |
||
281 | temp_meas_meas[3][3] += matR[3][3]; |
||
282 | temp_meas_meas[4][4] += matR[4][4]; |
||
283 | |||
284 | /* temp_meas_2 = (matC * matPs^T * matC^T + matR)^-1 */ |
||
285 | take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
||
286 | |||
287 | /* matK = matPs^T * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
||
288 | matrix_transpose_mult( temp_meas_state, temp_meas_2, matK, |
||
289 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
||
290 | |||
291 | /* temp_state_state = matK * matC * matPs */ |
||
292 | matrix_mult( matK, temp_meas_state, temp_state_state, |
||
293 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
||
294 | |||
295 | /* matP = matPs - matK * matC * matPs */ |
||
296 | matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
||
297 | #else |
||
298 | /* temp_state_meas = matPs * matC^T */ |
||
299 | matrix_mult_transpose( matPs, matC, temp_state_meas, |
||
300 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_Y ); |
||
301 | |||
302 | /* temp_meas_meas = matC * matPs * matC^T */ |
||
303 | matrix_mult( matC, temp_state_meas, temp_meas_meas, |
||
304 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
||
305 | |||
306 | /* temp_meas_meas = matC * matPs * matC^T + matR */ |
||
307 | /* |
||
308 | matrix_add( temp_meas_meas, matR, temp_meas_meas, |
||
309 | KAFI_DIM_Y, KAFI_DIM_Y ); |
||
310 | */ |
||
311 | temp_meas_meas[1][1] += matR[1][1]; |
||
312 | temp_meas_meas[2][2] += matR[2][2]; |
||
313 | temp_meas_meas[3][3] += matR[3][3]; |
||
314 | temp_meas_meas[4][4] += matR[4][4]; |
||
315 | |||
316 | /* temp_meas_2 = (matC * matPs * matC^T + matR)^-1 */ |
||
317 | take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
||
318 | |||
319 | /* matK = matPs * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
||
320 | matrix_mult( temp_state_meas, temp_meas_2, matK, |
||
321 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
||
322 | |||
323 | /* temp_meas_state = matC * matPs*/ |
||
324 | matrix_mult( matC, matPs, temp_meas_state, |
||
325 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
||
326 | |||
327 | /* temp_state_state = matK * matC * matPs */ |
||
328 | matrix_mult( matK, temp_meas_state, temp_state_state, |
||
329 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
||
330 | |||
331 | /* matP = matPs - matK * matC * matPs */ |
||
332 | matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
||
333 | #endif |
||
334 | |||
335 | /* matdY = matY - matYs */ |
||
336 | /* |
||
337 | matrix_sub( matY, matYs, matdY, |
||
338 | KAFI_DIM_Y, 1 ); |
||
339 | */ |
||
340 | matdY[1][1] = matY[1][1] - matYs[1][1]; |
||
341 | matdY[2][1] = matY[2][1] - matYs[2][1]; |
||
342 | matdY[3][1] = matY[3][1] - matYs[3][1]; |
||
343 | |||
344 | /* We will only update the Heading if we got a valid compass signal */ |
||
345 | if(fYValid[_compass] == 1) |
||
346 | { |
||
347 | matdY[4][1] = matY[4][1] - matYs[4][1]; |
||
348 | } |
||
349 | else |
||
350 | { |
||
351 | matdY[4][1] = 0.0F; |
||
352 | } |
||
353 | |||
354 | /* matX_tmp1 = matK * (Y -Ys) */ |
||
355 | matrix_mult( matK, matdY, matX_tmp1, |
||
356 | KAFI_DIM_X, KAFI_DIM_Y, 1 ); |
||
357 | |||
358 | /* matXd = matXs + matK * (Y -Ys) */ |
||
359 | /* |
||
360 | matrix_add( matXs, matX_tmp1, matXd, |
||
361 | KAFI_DIM_X, 1 ); |
||
362 | */ |
||
363 | matXd[1][1] = matXs[1][1] + matX_tmp1[1][1]; |
||
364 | matXd[2][1] = matXs[2][1] + matX_tmp1[2][1]; |
||
365 | matXd[3][1] = matXs[3][1] + matX_tmp1[3][1]; |
||
366 | matXd[4][1] = matXs[4][1] + matX_tmp1[4][1]; |
||
367 | } |
||
368 | |||
369 | |||
370 | /* **************************************************************************** |
||
371 | Functionname: trUpdatePhi */ /*! |
||
372 | Description: Setup matPhi, used to calculate the predicted state |
||
373 | from the current state |
||
374 | |||
375 | @param[in] |
||
376 | @param[in] fCycleTime |
||
377 | |||
378 | @return void |
||
379 | @pre - |
||
380 | @post - |
||
381 | @author Michael Walter |
||
382 | **************************************************************************** */ |
||
383 | void trUpdatePhi() |
||
384 | { |
||
385 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
386 | |||
387 | /*--- VARIABLES ---*/ |
||
388 | matSetFull(matPhi, _Phi, _Phi, 1.0F); |
||
389 | matSetFull(matPhi, _Theta, _Theta, 1.0F); |
||
390 | matSetFull(matPhi, _Psi, _Psi, 1.0F); |
||
391 | } |
||
392 | |||
393 | /* **************************************************************************** |
||
394 | Functionname: trUpdateJacobiMatrix */ /*! |
||
395 | Description: |
||
396 | |||
397 | @param[in] void |
||
398 | |||
399 | @return void |
||
400 | @pre - |
||
401 | @post - |
||
402 | @author Michael Walter |
||
403 | *****************************************************************************/ |
||
404 | void trUpdateJacobiMatrix(void) |
||
405 | { |
||
406 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
407 | /* |
||
408 | Simplified Version d/dt |
||
409 | Pre_ax = du - sinTheta * g; |
||
410 | Pre_ay = dv + cosTheta_sinPhi * g; |
||
411 | Pre_az = dw + cosTheta_cosPhi * g; |
||
412 | */ |
||
413 | |||
414 | /*--- VARIABLES ---*/ |
||
415 | f32_t Phi, Theta, Psi; |
||
416 | f32_t g = 9.81F; |
||
417 | |||
418 | Phi = status.Phi; |
||
419 | Theta = status.Theta; |
||
420 | Psi = status.Psi; |
||
421 | |||
422 | sinPhi = sin(Phi); |
||
423 | cosPhi = cos(Phi); |
||
424 | sinTheta = sin(Theta); |
||
425 | cosTheta = cos(Theta); |
||
426 | |||
427 | //Pre_ax = du - sinTheta * g; |
||
428 | //matSetFull(matC, _ax, _Phi , 0); |
||
429 | matSetFull(matC, _ax, _Theta, -cosTheta * g); |
||
430 | //matSetFull(matC, _ax, _Psi , 0); |
||
431 | |||
432 | //Pre_ay = dv + cosTheta_sinPhi * g; |
||
433 | matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g); |
||
434 | matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g); |
||
435 | //matSetFull(matC, _ay, _Psi , 0); |
||
436 | |||
437 | //Pre_az = dw + cosTheta_cosPhi * g; |
||
438 | matSetFull(matC, _az, _Phi , -cosTheta * sinPhi * g); |
||
439 | matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g); |
||
440 | //matSetFull(matC, _az, _Psi , 0); |
||
441 | |||
442 | matSetFull(matC, _compass, _Psi , 1.0F); |
||
443 | } |
||
444 | |||
838 | MikeW | 445 | /***************************************************************************** |
966 | MikeW | 446 | Functionname: trPredictMeasurement */ /*! |
447 | Description:Predict Measurements: AX, AY, AZ, Compass |
||
448 | |||
449 | |||
450 | @param[in] void |
||
451 | |||
452 | @return void |
||
453 | @pre - |
||
454 | @post - |
||
455 | @author Michael Walter |
||
838 | MikeW | 456 | *****************************************************************************/ |
966 | MikeW | 457 | void trPredictMeasurement(void) |
458 | { |
||
459 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
460 | |||
461 | /* |
||
462 | Simplified Version |
||
463 | Pre_ax = du - sinTheta * g; |
||
464 | Pre_ay = dv + cosTheta_sinPhi * g; |
||
465 | Pre_az = dw + cosTheta_cosPhi * g; |
||
466 | */ |
||
467 | |||
468 | /*--- VARIABLES ---*/ |
||
469 | f32_t g = 9.81F; |
||
470 | f32_t Pre_ax, Pre_ay, Pre_az; |
||
471 | f32_t Phi, Theta, Psi; |
||
472 | |||
473 | matGetFull(matXs, _Phi, 0, &Phi); |
||
474 | matGetFull(matXs, _Theta, 0, &Theta); |
||
475 | matGetFull(matXs, _Psi, 0, &Psi); |
||
476 | |||
477 | sinPhi_P = sin(Phi); |
||
478 | cosPhi_P = cos(Phi); |
||
479 | sinTheta_P = sin(Theta); |
||
480 | cosTheta_P = cos(Theta); |
||
481 | |||
482 | /* Simplified Version */ |
||
483 | Pre_ax = -sinTheta_P * g; |
||
484 | Pre_ay = cosTheta_P * sinPhi_P * g; |
||
485 | Pre_az = cosTheta_P * cosPhi_P * g; |
||
486 | |||
487 | matSetFull(matYs, _ax, 0, Pre_ax); |
||
488 | matSetFull(matYs, _ay, 0, Pre_ay); |
||
489 | matSetFull(matYs, _az, 0, Pre_az); |
||
490 | matSetFull(matYs, _compass, 0, Psi); |
||
491 | } |
||
492 | |||
493 | /* **************************************************************************** |
||
494 | Functionname: trMeasure */ /*! |
||
495 | Description: |
||
496 | |||
497 | @param[in] |
||
498 | |||
499 | @return void |
||
500 | @pre - |
||
501 | @post - |
||
502 | @author Michael Walter |
||
503 | **************************************************************************** */ |
||
504 | void trMeasure() |
||
505 | { |
||
506 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
507 | |||
508 | /*--- VARIABLES ---*/ |
||
509 | #ifdef USE_COMPASS |
||
510 | static int updCompass = 10; |
||
511 | f32_t Psi, Compass; |
||
512 | #endif |
||
513 | |||
514 | f32_t ADC_ax = 0.0F; |
||
515 | f32_t ADC_ay = 0.0F; |
||
516 | f32_t ADC_az = 0.0F; |
||
517 | |||
518 | fYValid[_ax] = 1; |
||
519 | fYValid[_ay] = 1; |
||
520 | fYValid[_az] = 1; |
||
521 | |||
522 | #ifdef INTERNAL_REFERENCE |
||
523 | ADC_ax = AdWertAccNick * 0.044F; |
||
524 | ADC_ay = -AdWertAccRoll * 0.044F; |
||
525 | ADC_az = AdWertAccHoch * 0.044F; |
||
526 | #else |
||
527 | ADC_ax = AdWertAccNick * 0.047F; |
||
528 | ADC_ay = -AdWertAccRoll * 0.047F; |
||
529 | ADC_az = AdWertAccHoch * 0.047F; |
||
530 | #endif |
||
531 | |||
532 | matSetFull(matY, _ax, 0, ADC_ax); |
||
533 | matSetFull(matY, _ay, 0, ADC_ay); |
||
534 | matSetFull(matY, _az, 0, ADC_az); |
||
535 | |||
536 | #ifdef USE_COMPASS |
||
537 | if (!updCompass--) |
||
538 | { |
||
539 | updCompass = 10; // update only at 2ms*50 = 100ms (10Hz) |
||
540 | KompassValue = MM3_Heading(); |
||
541 | } |
||
542 | Compass = KompassValue / 180.F * PI; |
||
543 | |||
544 | matGetFull(matXs, _Psi, 0, &Psi); |
||
545 | |||
546 | if (fabs(Compass + 2*PI - Psi) < fabs(Compass - Psi)) |
||
547 | { |
||
548 | Compass += 2 * PI; |
||
549 | } |
||
550 | else if (fabs(Compass - 2*PI - Psi) < fabs(Compass - Psi)) |
||
551 | { |
||
552 | Compass -= 2 * PI; |
||
553 | } |
||
554 | |||
555 | matSetFull(matY, _compass, 0, Compass); |
||
556 | |||
557 | /* Roll and Yaw angle are smaller 8 Deg*/ |
||
558 | if ((abs(status.iPhi10) < 80 ) && (abs(status.iTheta10) < 80)) |
||
559 | { |
||
560 | fYValid[_compass] = 1; |
||
561 | } |
||
562 | else |
||
563 | { |
||
564 | fYValid[_compass] = 0; |
||
565 | } |
||
566 | #else |
||
567 | fYValid[_compass] = 0; |
||
568 | #endif |
||
569 | } |
||
570 | |||
571 | /* **************************************************************************** |
||
572 | Functionname: trUpdateBU */ /*! |
||
573 | Description: Update control matrix B and vector U |
||
574 | |||
575 | |||
576 | @param[in] |
||
577 | |||
578 | @return void |
||
579 | @pre - |
||
580 | @post - |
||
581 | @author Michael Walter |
||
582 | **************************************************************************** */ |
||
583 | void trUpdateBU() |
||
584 | { |
||
585 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
586 | |||
587 | /*--- VARIABLES ---*/ |
||
588 | /* |
||
589 | dPhi | 1 sinPhi_tanTheta cosPhi_tanTheta | p |
||
590 | dTheta = | 0 cosPhi -sinPhi | * q |
||
591 | dPsi | 0 sinPhi_secTheta cosPhi_secTheta | w |
||
592 | */ |
||
593 | |||
594 | static f32_t fSumGier = 0.0F; |
||
595 | static f32_t fSumNick = 0.0F; |
||
596 | static f32_t fSumRoll = 0.0F; |
||
597 | |||
598 | f32_t ADC_p = 0.0F; |
||
599 | f32_t ADC_q = 0.0F; |
||
600 | f32_t ADC_r = 0.0F; |
||
601 | |||
602 | /* store predicted state vector in current state */ |
||
603 | matGetFull(matXd, _Phi , 0, &Phi); |
||
604 | matGetFull(matXd, _Theta , 0, &Theta); |
||
605 | |||
606 | sinPhi = sin(Phi); |
||
607 | cosPhi = cos(Phi); |
||
608 | |||
609 | tanTheta = tan(Theta); |
||
610 | secTheta = 1.0F / cos(Theta); |
||
611 | |||
612 | matSetFull(matB, _Phi, _p, 1.0F); |
||
613 | matSetFull(matB, _Phi, _q, sinPhi * tanTheta ); |
||
614 | matSetFull(matB, _Phi, _r, cosPhi * tanTheta ); |
||
615 | |||
616 | //matSetFull(matB, _Theta, _p, 0.0F); |
||
617 | matSetFull(matB, _Theta, _q, cosPhi ); |
||
618 | matSetFull(matB, _Theta, _r, -sinPhi ); |
||
619 | |||
620 | //matSetFull(matB, _Psi, _p, 0.0F); |
||
621 | matSetFull(matB, _Psi, _q, sinPhi * secTheta ); |
||
622 | matSetFull(matB, _Psi, _r, cosPhi * secTheta ); |
||
623 | |||
973 | MikeW | 624 | ADC_p = -(float) AverageRoll * 0.00001F * fCycleTime; |
625 | ADC_q = -(float) AverageNick * 0.00001F * fCycleTime; |
||
626 | ADC_r = -(float) AverageGier * 0.00001F * fCycleTime; |
||
966 | MikeW | 627 | |
628 | fSumGier += ADC_r * 180.F / 3.14F; |
||
629 | fSumNick += ADC_q * 180.F / 3.14F; |
||
630 | fSumRoll += ADC_p * 180.F / 3.14F; |
||
631 | |||
632 | DebugOut.Analog[3] = fSumNick; |
||
633 | DebugOut.Analog[4] = fSumRoll; |
||
634 | DebugOut.Analog[5] = fSumGier; |
||
635 | |||
636 | matSetFull(matU, _p, 0, ADC_p ); |
||
637 | matSetFull(matU, _q, 0, ADC_q); |
||
638 | matSetFull(matU, _r, 0, ADC_r); |
||
639 | } |
||
640 | |||
641 | |||
642 | |||
643 | /* **************************************************************************** |
||
644 | Functionname: trLimitAngles */ /*! |
||
645 | Description: |
||
646 | |||
647 | @param[in] |
||
648 | |||
649 | @return void |
||
650 | @pre - |
||
651 | @post - |
||
652 | @author |
||
653 | **************************************************************************** */ |
||
654 | void trLimitAngles() |
||
655 | { |
||
656 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
657 | |||
658 | /*--- VARIABLES ---*/ |
||
659 | f32_t Psi; |
||
660 | |||
661 | if (status.Phi > 2.0F * PI) |
||
662 | { |
||
663 | status.Phi -= 2.0F * PI; |
||
664 | matSetFull(matXs, _Phi, 0, status.Phi); |
||
665 | } |
||
666 | if (status.Phi < -2.0F * PI) |
||
667 | { |
||
668 | status.Phi += 2.0F * PI; |
||
669 | matSetFull(matXs, _Phi, 0, status.Phi); |
||
670 | } |
||
671 | if (status.Theta > 2.0F * PI) |
||
672 | { |
||
673 | status.Theta -= 2.0F * PI; |
||
674 | matSetFull(matXs, _Theta, 0, status.Theta); |
||
675 | } |
||
676 | if (status.Theta < -2.0F * PI) |
||
677 | { |
||
678 | status.Theta += 2.0F * PI; |
||
679 | matSetFull(matXs, _Theta, 0, status.Theta); |
||
680 | } |
||
681 | if (status.Psi > 2.0F * PI) |
||
682 | { |
||
683 | status.Psi -= 2.0F * PI; |
||
684 | sollGier -= 3600; |
||
685 | |||
686 | matGetFull(matXs, _Psi, 0, &Psi); |
||
687 | matSetFull(matXs, _Psi , 0, Psi - 2.0F * PI); |
||
688 | matGetFull(matXd, _Psi, 0, &Psi); |
||
689 | matSetFull(matXd, _Psi , 0, Psi - 2.0F * PI); |
||
690 | } |
||
691 | if (status.Psi < 0.0F * PI) |
||
692 | { |
||
693 | status.Psi += 2.0F * PI; |
||
694 | sollGier += 3600; |
||
695 | |||
696 | matGetFull(matXs, _Psi, 0, &Psi); |
||
697 | matSetFull(matXs, _Psi , 0, Psi + 2.0F * PI); |
||
698 | matGetFull(matXd, _Psi, 0, &Psi); |
||
699 | matSetFull(matXd, _Psi , 0, Psi + 2.0F * PI); |
||
700 | } |
||
701 | |||
702 | status.iPhi10 = (int) (status.Phi * 573.0F); |
||
703 | status.iTheta10 = (int) (status.Theta * 573.0F); |
||
704 | status.iPsi10 = (int) (status.Psi * 573.0F); |
||
705 | |||
706 | if ((sollGier - status.iPsi10) > 3600) |
||
707 | { |
||
708 | sollGier -= 3600; |
||
709 | } |
||
710 | |||
711 | if ((sollGier - status.iPsi10) < -3600) |
||
712 | { |
||
713 | sollGier += 3600; |
||
714 | } |
||
715 | } |
||
716 | |||
717 | |||
718 | /* **************************************************************************** |
||
719 | Functionname: trEstimateVelocity */ /*! |
||
720 | Description: |
||
721 | |||
722 | @param[in] |
||
723 | |||
724 | @return void |
||
725 | @pre - |
||
726 | @post - |
||
727 | @author |
||
728 | **************************************************************************** */ |
||
729 | void trEstimateVelocity() |
||
730 | { |
||
731 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
732 | |||
733 | /*--- VARIABLES ---*/ |
||
734 | |||
735 | } |