Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_float_lkf.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009 Felix Ruess <felix.ruess@gmail.com>
3  * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
30 #include "ahrs_float_lkf.h"
31 
32 #include "subsystems/imu.h"
34 
35 #include "generated/airframe.h"
37 
38 #include <stdio.h>
39 
40 
41 #define BAFL_DEBUG
42 
43 static void ahrs_do_update_accel(void);
44 static void ahrs_do_update_mag(void);
45 
46 
47 /* our estimated attitude (ltp <-> imu) */
49 /* our estimated gyro biases */
51 /* we get unbiased body rates as byproduct */
53 /* maintain a euler angles representation */
55 /* C n->b rotation matrix representation */
57 
58 /* estimated attitude errors from accel update */
60 /* estimated attitude errors from mag update */
62 /* our estimated gyro bias errors from accel update */
64 /* our estimated gyro bias errors from mag update */
66 /* temporary quaternion */
68 /* correction quaternion of strapdown computation */
70 /* norm of attitude quaternion */
71 float bafl_qnorm;
72 
73 /* pseude measured angles for debug */
76 
77 #ifndef BAFL_SSIZE
78 #define BAFL_SSIZE 6
79 #endif
80 /* error covariance matrix */
82 /* filter state */
84 
85 /*
86  * F represents the Jacobian of the derivative of the system with respect
87  * its states. We do not allocate the bottom three rows since we know that
88  * the derivatives of bias_dot are all zero.
89  */
90 float bafl_F[3][3];
91 /*
92  * T represents the discrete state transition matrix
93  * T = e^(F * dt)
94  */
95 float bafl_T[6][6];
96 
97 /*
98  * Kalman filter variables.
99  */
101 /* temporary variable used during state covariance update */
103 float bafl_K[6][3];
104 float bafl_tempK[6][3];
105 float bafl_S[3][3];
106 float bafl_tempS[3][6];
107 float bafl_invS[3][3];
110 
111 /*
112  * H represents the Jacobian of the measurements of the attitude
113  * with respect to the states of the filter. We do not allocate the bottom
114  * three rows since we know that the attitude measurements have no
115  * relationship to gyro bias.
116  * The last three columns always stay zero.
117  */
118 float bafl_H[3][3];
119 
120 /* low pass of accel measurements */
123 
125 
126 /* temporary variables for the strapdown computation */
127 float bafl_qom[4][4];
128 
129 #define BAFL_g 9.81
130 
131 #define BAFL_hx 1.0
132 #define BAFL_hy 0.0
133 #define BAFL_hz 1.0
135 
136 /*
137  * Q is our estimate noise variance. It is supposed to be an NxN
138  * matrix, but with elements only on the diagonals. Additionally,
139  * since the quaternion has no expected noise (we can't directly measure
140  * it), those are zero. For the gyro, we expect around 5 deg/sec noise,
141  * which is 0.08 rad/sec. The variance is then 0.08^2 ~= 0.0075.
142  */
143 /* I measured about 0.009 rad/s noise */
144 #define BAFL_Q_GYRO 1e-04
145 #define BAFL_Q_ATT 0
148 
149 /*
150  * R is our measurement noise estimate. Like Q, it is supposed to be
151  * an NxN matrix with elements on the diagonals. However, since we can
152  * not directly measure the gyro bias, we have no estimate for it.
153  * We only have an expected noise in the pitch and roll accelerometers
154  * and in the compass.
155  */
156 #define BAFL_SIGMA_ACCEL 1000.0
157 #define BAFL_SIGMA_MAG 20.
162 
163 #ifndef BAFL_DT
164 #define BAFL_DT (1./512.)
165 #endif
166 
167 
168 #define FLOAT_MAT33_INVERT(_mo, _mi) { \
169  float _det = 0.; \
170  _det = _mi[0][0] * (_mi[2][2] * _mi[1][1] - _mi[2][1] * _mi[1][2]) \
171  - _mi[1][0] * (_mi[2][2] * _mi[0][1] - _mi[2][1] * _mi[0][2]) \
172  + _mi[2][0] * (_mi[1][2] * _mi[0][1] - _mi[1][1] * _mi[0][2]); \
173  if (_det != 0.) { /* ? test for zero? */ \
174  _mo[0][0] = (_mi[1][1] * _mi[2][2] - _mi[1][2] * _mi[2][1]) / _det; \
175  _mo[0][1] = (_mi[0][2] * _mi[2][1] - _mi[0][1] * _mi[2][2]) / _det; \
176  _mo[0][2] = (_mi[0][1] * _mi[1][2] - _mi[0][2] * _mi[1][1]) / _det; \
177  \
178  _mo[1][0] = (_mi[1][2] * _mi[2][0] - _mi[1][0] * _mi[2][2]) / _det; \
179  _mo[1][1] = (_mi[0][0] * _mi[2][2] - _mi[0][2] * _mi[2][0]) / _det; \
180  _mo[1][2] = (_mi[0][2] * _mi[1][0] - _mi[0][0] * _mi[1][2]) / _det; \
181  \
182  _mo[2][0] = (_mi[1][0] * _mi[2][1] - _mi[1][1] * _mi[2][0]) / _det; \
183  _mo[2][1] = (_mi[0][1] * _mi[2][0] - _mi[0][0] * _mi[2][1]) / _det; \
184  _mo[2][2] = (_mi[0][0] * _mi[1][1] - _mi[0][1] * _mi[1][0]) / _det; \
185  } else { \
186  printf("ERROR! Not invertible!\n"); \
187  for (int _i=0; _i<3; _i++) { \
188  for (int _j=0; _j<3; _j++) { \
189  _mo[_i][_j] = 0.0; \
190  } \
191  } \
192  } \
193  }
194 
195 #define AHRS_TO_BFP() { \
196  /* IMU rate */ \
197  RATES_BFP_OF_REAL(ahrs.imu_rate, bafl_rates); \
198  /* LTP to IMU eulers */ \
199  EULERS_BFP_OF_REAL(ahrs.ltp_to_imu_euler, bafl_eulers); \
200  /* LTP to IMU quaternion */ \
201  QUAT_BFP_OF_REAL(ahrs.ltp_to_imu_quat, bafl_quat); \
202  /* LTP to IMU rotation matrix */ \
203  RMAT_BFP_OF_REAL(ahrs.ltp_to_imu_rmat, bafl_dcm); \
204  }
205 
206 #define AHRS_LTP_TO_BODY() { \
207  /* Compute LTP to BODY quaternion */ \
208  INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); \
209  /* Compute LTP to BODY rotation matrix */ \
210  INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); \
211  /* compute LTP to BODY eulers */ \
212  INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat); \
213  /* compute body rates */ \
214  INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); \
215  }
216 
217 
218 void ahrs_init(void) {
219  int i, j;
220 
221  for (i = 0; i < BAFL_SSIZE; i++) {
222  for (j = 0; j < BAFL_SSIZE; j++) {
223  bafl_T[i][j] = 0.;
224  bafl_P[i][j] = 0.;
225  }
226  /* Insert the diagonal elements of the T-Matrix. These will never change. */
227  bafl_T[i][i] = 1.0;
228  /* initial covariance values */
229  if (i<3) {
230  bafl_P[i][i] = 1.0;
231  } else {
232  bafl_P[i][i] = 0.1;
233  }
234  }
235 
237 
239  INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
240  INT_EULERS_ZERO(ahrs.ltp_to_imu_euler);
241  INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
242  INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
243  INT_RATES_ZERO(ahrs.body_rate);
244  INT_RATES_ZERO(ahrs.imu_rate);
245 
248 
251 
253 
254 }
255 
256 void ahrs_align(void) {
260 }
261 
262 static inline void ahrs_lowpass_accel(void) {
263  // get latest measurement
265  // low pass it
268 
269 #ifdef BAFL_DEBUG
272 #endif
273 }
274 
275 /*
276  * Propagate our dynamic system in time
277  *
278  * Run the strapdown computation and the predict step of the filter
279  *
280  *
281  * quat_dot = Wxq(pqr) * quat
282  * bias_dot = 0
283  *
284  * Wxq is the quaternion omega matrix:
285  *
286  * [ 0, -p, -q, -r ]
287  * 1/2 * [ p, 0, r, -q ]
288  * [ q, -r, 0, p ]
289  * [ r, q, -p, 0 ]
290  *
291  */
292 void ahrs_propagate(void) {
293  int i, j, k;
294 
296 
297  /* compute unbiased rates */
300 
301 
302  /* run strapdown computation
303  *
304  */
305 
306  /* multiplicative quaternion update */
307  /* compute correction quaternion */
309  /* normalize it. maybe not necessary?? */
310  float q_sq;
311  q_sq = (bafl_qr.qx * bafl_qr.qx +bafl_qr.qy * bafl_qr.qy + bafl_qr.qz * bafl_qr.qz) / 4;
312  if (q_sq > 1) { /* this should actually never happen */
313  FLOAT_QUAT_SMUL(bafl_qr, bafl_qr, 1 / sqrtf(1 + q_sq));
314  } else {
315  bafl_qr.qi = sqrtf(1 - q_sq);
316  }
317  /* propagate correction quaternion */
320 
322  //TODO check if broot force normalization is good, use lagrange normalization?
324 
325 
326  /*
327  * compute all representations
328  */
329  /* maintain rotation matrix representation */
331  /* maintain euler representation */
333  AHRS_TO_BFP();
335 
336 
337  /* error KF-Filter
338  * propagate only the error covariance matrix since error is corrected after each measurement
339  *
340  * F = [ 0 0 0 ]
341  * [ 0 0 0 -Cbn ]
342  * [ 0 0 0 ]
343  * [ 0 0 0 0 0 0 ]
344  * [ 0 0 0 0 0 0 ]
345  * [ 0 0 0 0 0 0 ]
346  *
347  * T = e^(dt * F)
348  *
349  * T = [ 1 0 0 ]
350  * [ 0 1 0 -Cbn*dt ]
351  * [ 0 0 1 ]
352  * [ 0 0 0 1 0 0 ]
353  * [ 0 0 0 0 1 0 ]
354  * [ 0 0 0 0 0 1 ]
355  *
356  * P_prio = T * P * T_T + Q
357  *
358  */
359 
360  /*
361  * compute state transition matrix T
362  *
363  * diagonal elements of T are always one
364  */
365  for (i = 0; i < 3; i++) {
366  for (j = 0; j < 3; j++) {
367  bafl_T[i][j + 3] = -RMAT_ELMT(bafl_dcm, j, i); /* inverted bafl_dcm */
368  }
369  }
370 
371 
372  /*
373  * estimate the a priori error covariance matrix P_k = T * P_k-1 * T_T + Q
374  */
375  /* Temporary multiplication temp(6x6) = T(6x6) * P(6x6) */
376  for (i = 0; i < BAFL_SSIZE; i++) {
377  for (j = 0; j < BAFL_SSIZE; j++) {
378  bafl_tempP[i][j] = 0.;
379  for (k = 0; k < BAFL_SSIZE; k++) {
380  bafl_tempP[i][j] += bafl_T[i][k] * bafl_P[k][j];
381  }
382  }
383  }
384 
385  /* P_k(6x6) = temp(6x6) * T_T(6x6) + Q */
386  for (i = 0; i < BAFL_SSIZE; i++) {
387  for (j = 0; j < BAFL_SSIZE; j++) {
388  bafl_P[i][j] = 0.;
389  for (k = 0; k < BAFL_SSIZE; k++) {
390  bafl_P[i][j] += bafl_tempP[i][k] * bafl_T[j][k]; //T[j][k] = T_T[k][j]
391  }
392  }
393  if (i<3) {
394  bafl_P[i][i] += bafl_Q_att;
395  } else {
396  bafl_P[i][i] += bafl_Q_gyro;
397  }
398  }
399 
400 #ifdef LKF_PRINT_P
401  printf("Pp =");
402  for (i = 0; i < 6; i++) {
403  printf("[");
404  for (j = 0; j < 6; j++) {
405  printf("%f\t", bafl_P[i][j]);
406  }
407  printf("]\n ");
408  }
409  printf("\n");
410 #endif
411 }
412 
413 void ahrs_update_accel(void) {
414  RunOnceEvery(50, ahrs_do_update_accel());
415 }
416 
417 static void ahrs_do_update_accel(void) {
418  int i, j, k;
419 
420 #ifdef BAFL_DEBUG2
421  printf("Accel update.\n");
422 #endif
423 
424  /* P_prio = P */
425  for (i = 0; i < BAFL_SSIZE; i++) {
426  for (j = 0; j < BAFL_SSIZE; j++) {
427  bafl_Pprio[i][j] = bafl_P[i][j];
428  }
429  }
430 
431  /*
432  * set up measurement matrix
433  *
434  * g = 9.81
435  * gx = [ 0 -g 0
436  * g 0 0
437  * 0 0 0 ]
438  * H = [ 0 0 0 ]
439  * [ -Cnb*gx 0 0 0 ]
440  * [ 0 0 0 ]
441  *
442  * */
443  bafl_H[0][0] = -RMAT_ELMT(bafl_dcm, 0, 1) * BAFL_g;
444  bafl_H[0][1] = RMAT_ELMT(bafl_dcm, 0, 0) * BAFL_g;
445  bafl_H[1][0] = -RMAT_ELMT(bafl_dcm, 1, 1) * BAFL_g;
446  bafl_H[1][1] = RMAT_ELMT(bafl_dcm, 1, 0) * BAFL_g;
447  bafl_H[2][0] = -RMAT_ELMT(bafl_dcm, 2, 1) * BAFL_g;
448  bafl_H[2][1] = RMAT_ELMT(bafl_dcm, 2, 0) * BAFL_g;
449  /* rest is zero
450  bafl_H[0][2] = 0.;
451  bafl_H[1][2] = 0.;
452  bafl_H[2][2] = 0.;*/
453 
454  /**********************************************
455  * compute Kalman gain K
456  *
457  * K = P_prio * H_T * inv(S)
458  * S = H * P_prio * HT + R
459  *
460  * K = P_prio * HT * inv(H * P_prio * HT + R)
461  *
462  **********************************************/
463 
464  /* covariance residual S = H * P_prio * HT + R */
465 
466  /* temp_S(3x6) = H(3x6) * P_prio(6x6)
467  * last 4 columns of H are zero
468  */
469  for (i = 0; i < 3; i++) {
470  for (j = 0; j < BAFL_SSIZE; j++) {
471  bafl_tempS[i][j] = bafl_H[i][0] * bafl_Pprio[0][j];
472  bafl_tempS[i][j] += bafl_H[i][1] * bafl_Pprio[1][j];
473  }
474  }
475 
476  /* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
477  *
478  * bottom 4 rows of HT are zero
479  */
480  for (i = 0; i < 3; i++) {
481  for (j = 0; j < 3; j++) {
482  bafl_S[i][j] = bafl_tempS[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
483  bafl_S[i][j] += bafl_tempS[i][1] * bafl_H[j][1]; /* H[j][0] = HT[0][j] */
484  }
485  bafl_S[i][i] += bafl_R_accel;
486  }
487 
488  /* invert S
489  */
491 
492 
493  /* temp_K(6x3) = P_prio(6x6) * HT(6x3)
494  *
495  * bottom 4 rows of HT are zero
496  */
497  for (i = 0; i < BAFL_SSIZE; i++) {
498  for (j = 0; j < 3; j++) {
499  /* not computing zero elements */
500  bafl_tempK[i][j] = bafl_Pprio[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
501  bafl_tempK[i][j] += bafl_Pprio[i][1] * bafl_H[j][1]; /* H[j][1] = HT[1][j] */
502  }
503  }
504 
505  /* K(6x3) = temp_K(6x3) * invS(3x3)
506  */
507  for (i = 0; i < BAFL_SSIZE; i++) {
508  for (j = 0; j < 3; j++) {
509  bafl_K[i][j] = bafl_tempK[i][0] * bafl_invS[0][j];
510  bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
511  bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
512  }
513  }
514 
515  /**********************************************
516  * Update filter state.
517  *
518  * The a priori filter state is zero since the errors are corrected after each update.
519  *
520  * X = X_prio + K (y - H * X_prio)
521  * X = K * y
522  **********************************************/
523 
524  /*printf("R = ");
525  for (i = 0; i < 3; i++) {
526  printf("[");
527  for (j = 0; j < 3; j++) {
528  printf("%f\t", RMAT_ELMT(bafl_dcm, i, j));
529  }
530  printf("]\n ");
531  }
532  printf("\n");*/
533 
534  /* innovation
535  * y = Cnb * -[0; 0; g] - accel
536  */
540 
541  /* X(6) = K(6x3) * y(3)
542  */
543  for (i = 0; i < BAFL_SSIZE; i++) {
544  bafl_X[i] = bafl_K[i][0] * bafl_ya.x;
545  bafl_X[i] += bafl_K[i][1] * bafl_ya.y;
546  bafl_X[i] += bafl_K[i][2] * bafl_ya.z;
547  }
548 
549  /**********************************************
550  * Update the filter covariance.
551  *
552  * P = P_prio - K * H * P_prio
553  * P = ( I - K * H ) * P_prio
554  *
555  **********************************************/
556 
557  /* temp(6x6) = I(6x6) - K(6x3)*H(3x6)
558  *
559  * last 4 columns of H are zero
560  */
561  for (i = 0; i < BAFL_SSIZE; i++) {
562  for (j = 0; j < BAFL_SSIZE; j++) {
563  if (i == j) {
564  bafl_tempP[i][j] = 1.;
565  } else {
566  bafl_tempP[i][j] = 0.;
567  }
568  if (j < 2) { /* omit the parts where H is zero */
569  for (k = 0; k < 3; k++) {
570  bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
571  }
572  }
573  }
574  }
575 
576  /* P(6x6) = temp(6x6) * P_prio(6x6)
577  */
578  for (i = 0; i < BAFL_SSIZE; i++) {
579  for (j = 0; j < BAFL_SSIZE; j++) {
580  bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
581  for (k = 1; k < BAFL_SSIZE; k++) {
582  bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
583  }
584  }
585  }
586 
587 #ifdef LKF_PRINT_P
588  printf("Pua=");
589  for (i = 0; i < 6; i++) {
590  printf("[");
591  for (j = 0; j < 6; j++) {
592  printf("%f\t", bafl_P[i][j]);
593  }
594  printf("]\n ");
595  }
596  printf("\n");
597 #endif
598 
599  /**********************************************
600  * Correct errors.
601  *
602  *
603  **********************************************/
604 
605  /* Error quaternion.
606  */
607  QUAT_ASSIGN(bafl_q_a_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
609 
610  /* normalize
611  * Is this needed? Or is the approximation good enough?*/
612  float q_sq;
614  if (q_sq > 1) { /* this should actually never happen */
615  FLOAT_QUAT_SMUL(bafl_q_a_err, bafl_q_a_err, 1 / sqrtf(1 + q_sq));
616  printf("Accel error quaternion q_sq > 1!!\n");
617  } else {
618  bafl_q_a_err.qi = sqrtf(1 - q_sq);
619  }
620 
621  /* correct attitude
622  */
625 
626 
627  /* correct gyro bias
628  */
631 
632  /*
633  * compute all representations
634  */
635  /* maintain rotation matrix representation */
637  /* maintain euler representation */
639  AHRS_TO_BFP();
641 }
642 
643 
644 void ahrs_update_mag(void) {
645  RunOnceEvery(10, ahrs_do_update_mag());
646 }
647 
648 static void ahrs_do_update_mag(void) {
649  int i, j, k;
650 
651 #ifdef BAFL_DEBUG2
652  printf("Mag update.\n");
653 #endif
654 
656 
657  /* P_prio = P */
658  for (i = 0; i < BAFL_SSIZE; i++) {
659  for (j = 0; j < BAFL_SSIZE; j++) {
660  bafl_Pprio[i][j] = bafl_P[i][j];
661  }
662  }
663 
664  /*
665  * set up measurement matrix
666  *
667  * h = [236.; -2.; 396.];
668  * hx = [ h(2); -h(1); 0]; //only psi (phi and theta = 0)
669  * Hm = Cnb * hx;
670  * H = [ 0 0 0 0 0
671  * 0 0 Cnb*hx 0 0 0
672  * 0 0 0 0 0 ];
673  * */
674  /*bafl_H[0][2] = RMAT_ELMT(bafl_dcm, 0, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 0, 1) * bafl_h.x;
675  bafl_H[1][2] = RMAT_ELMT(bafl_dcm, 1, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 1, 1) * bafl_h.x;
676  bafl_H[2][2] = RMAT_ELMT(bafl_dcm, 2, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 2, 1) * bafl_h.x;*/
677  bafl_H[0][2] = -RMAT_ELMT(bafl_dcm, 0, 1);
678  bafl_H[1][2] = -RMAT_ELMT(bafl_dcm, 1, 1);
679  bafl_H[2][2] = -RMAT_ELMT(bafl_dcm, 2, 1);
680  //rest is zero
681 
682  /**********************************************
683  * compute Kalman gain K
684  *
685  * K = P_prio * H_T * inv(S)
686  * S = H * P_prio * HT + R
687  *
688  * K = P_prio * HT * inv(H * P_prio * HT + R)
689  *
690  **********************************************/
691 
692  /* covariance residual S = H * P_prio * HT + R */
693 
694  /* temp_S(3x6) = H(3x6) * P_prio(6x6)
695  *
696  * only third column of H is non-zero
697  */
698  for (i = 0; i < 3; i++) {
699  for (j = 0; j < BAFL_SSIZE; j++) {
700  bafl_tempS[i][j] = bafl_H[i][2] * bafl_Pprio[2][j];
701  }
702  }
703 
704  /* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
705  *
706  * only third row of HT is non-zero
707  */
708  for (i = 0; i < 3; i++) {
709  for (j = 0; j < 3; j++) {
710  bafl_S[i][j] = bafl_tempS[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
711  }
712  bafl_S[i][i] += bafl_R_mag;
713  }
714 
715  /* invert S
716  */
718 
719  /* temp_K(6x3) = P_prio(6x6) * HT(6x3)
720  *
721  * only third row of HT is non-zero
722  */
723  for (i = 0; i < BAFL_SSIZE; i++) {
724  for (j = 0; j < 3; j++) {
725  /* not computing zero elements */
726  bafl_tempK[i][j] = bafl_Pprio[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
727  }
728  }
729 
730  /* K(6x3) = temp_K(6x3) * invS(3x3)
731  */
732  for (i = 0; i < BAFL_SSIZE; i++) {
733  for (j = 0; j < 3; j++) {
734  bafl_K[i][j] = bafl_tempK[i][0] * bafl_invS[0][j];
735  bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
736  bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
737  }
738  }
739 
740  /**********************************************
741  * Update filter state.
742  *
743  * The a priori filter state is zero since the errors are corrected after each update.
744  *
745  * X = X_prio + K (y - H * X_prio)
746  * X = K * y
747  **********************************************/
748 
749  /* innovation
750  * y = Cnb * [hx; hy; hz] - mag
751  */
752  FLOAT_RMAT_VECT3_MUL(bafl_ym, bafl_dcm, bafl_h); //can be optimized
754 
755  /* X(6) = K(6x3) * y(3)
756  */
757  for (i = 0; i < BAFL_SSIZE; i++) {
758  bafl_X[i] = bafl_K[i][0] * bafl_ym.x;
759  bafl_X[i] += bafl_K[i][1] * bafl_ym.y;
760  bafl_X[i] += bafl_K[i][2] * bafl_ym.z;
761  }
762 
763  /**********************************************
764  * Update the filter covariance.
765  *
766  * P = P_prio - K * H * P_prio
767  * P = ( I - K * H ) * P_prio
768  *
769  **********************************************/
770 
771  /* temp(6x6) = I(6x6) - K(6x3)*H(3x6)
772  *
773  * last 3 columns of H are zero
774  */
775  for (i = 0; i < 6; i++) {
776  for (j = 0; j < 6; j++) {
777  if (i == j) {
778  bafl_tempP[i][j] = 1.;
779  } else {
780  bafl_tempP[i][j] = 0.;
781  }
782  if (j == 2) { /* omit the parts where H is zero */
783  for (k = 0; k < 3; k++) {
784  bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
785  }
786  }
787  }
788  }
789 
790  /* P(6x6) = temp(6x6) * P_prio(6x6)
791  */
792  for (i = 0; i < BAFL_SSIZE; i++) {
793  for (j = 0; j < BAFL_SSIZE; j++) {
794  bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
795  for (k = 1; k < BAFL_SSIZE; k++) {
796  bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
797  }
798  }
799  }
800 
801 #ifdef LKF_PRINT_P
802  printf("Pum=");
803  for (i = 0; i < 6; i++) {
804  printf("[");
805  for (j = 0; j < 6; j++) {
806  printf("%f\t", bafl_P[i][j]);
807  }
808  printf("]\n ");
809  }
810  printf("\n");
811 #endif
812 
813  /**********************************************
814  * Correct errors.
815  *
816  *
817  **********************************************/
818 
819  /* Error quaternion.
820  */
821  QUAT_ASSIGN(bafl_q_m_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
823  /* normalize */
824  float q_sq;
826  if (q_sq > 1) { /* this should actually never happen */
827  FLOAT_QUAT_SMUL(bafl_q_m_err, bafl_q_m_err, 1 / sqrtf(1 + q_sq));
828  printf("mag error quaternion q_sq > 1!!\n");
829  } else {
830  bafl_q_m_err.qi = sqrtf(1 - q_sq);
831  }
832 
833  /* correct attitude
834  */
837 
838  /* correct gyro bias
839  */
842 
843  /*
844  * compute all representations
845  */
846  /* maintain rotation matrix representation */
848  /* maintain euler representation */
850  AHRS_TO_BFP();
852 }
853 
854 void ahrs_update(void) {
856  ahrs_update_mag();
857 }
#define BAFL_Q_ATT
#define FLOAT_QUAT_COPY(_qo, _qi)
#define FLOAT_VECT3_ASSIGN(_a, _x, _y, _z)
Interface to align the AHRS via low-passed measurements at startup.
#define ahrs_float_lkf_SetRaccel(_v)
struct FloatVect3 bafl_ym
rotation matrix
struct FloatVect3 bafl_h
#define AHRS_TO_BFP()
static void ahrs_do_update_mag(void)
#define FLOAT_QUAT_SMUL(_qo, _qi, _s)
struct FloatVect3 bafl_mag
struct FloatRates bafl_b_a_err
#define INT32_QUAT_ZERO(_q)
angular rates
#define BAFL_Q_GYRO
float bafl_phi_accel
float bafl_T[6][6]
float bafl_qnorm
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:41
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
float bafl_R_mag
#define BAFL_hx
#define INT_RATES_ZERO(_e)
#define INT_EULERS_ZERO(_e)
#define RATES_SUB(_a, _b)
Definition: pprz_algebra.h:315
#define FLOAT_VECT3_SUB(_a, _b)
#define BAFL_g
#define FLOAT_EULERS_OF_RMAT(_e, _rm)
struct FloatQuat bafl_qr
euler angles
void ahrs_init(void)
AHRS initialization.
float bafl_F[3][3]
float bafl_Q_att
#define BAFL_hy
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:653
#define BAFL_SSIZE
float p
in rad/s^2
Roation quaternion.
void ahrs_propagate(void)
Propagation.
#define BAFL_SIGMA_ACCEL
float bafl_tempS[3][6]
struct FloatEulers bafl_eulers
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:35
#define FLOAT_QUAT_NORM(_q)
float bafl_S[3][3]
Paparazzi floating point algebra.
#define BAFL_DT
static void ahrs_lowpass_accel(void)
struct FloatQuat bafl_q_a_err
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
float bafl_H[3][3]
float bafl_invS[3][3]
struct FloatQuat bafl_q_m_err
float bafl_Q_gyro
float bafl_P[BAFL_SSIZE][BAFL_SSIZE]
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:518
Inertial Measurement Unit interface.
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:617
void ahrs_update(void)
float r
in rad/s^2
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
static void ahrs_do_update_accel(void)
#define AHRS_UNINIT
Definition: ahrs.h:35
struct Int32Vect3 mag
magnetometer measurements
Definition: imu.h:42
#define FLOAT_QUAT_NORMALIZE(_q)
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:118
#define FLOAT_MAT33_INVERT(_mo, _mi)
float bafl_Pprio[BAFL_SSIZE][BAFL_SSIZE]
float bafl_K[6][3]
struct FloatRMat bafl_dcm
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:462
float bafl_R_accel
struct FloatVect3 bafl_accel_measure
void ahrs_align(void)
Aligns the AHRS.
struct FloatRates bafl_bias
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:294
float bafl_tempK[6][3]
struct FloatVect3 bafl_accel_last
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:641
#define FLOAT_QUAT_INVERT(_qo, _qi)
float bafl_sigma_mag
float bafl_theta_accel
float bafl_X[BAFL_SSIZE]
#define FLOAT_RMAT_OF_QUAT(_rm, _q)
#define BAFL_hz
struct FloatRates bafl_b_m_err
struct Int32Rates gyro
gyroscope measurements
Definition: imu.h:40
struct FloatQuat bafl_qtemp
struct FloatVect3 bafl_ya
float q
in rad/s^2
Linearized Kalman Filter for attitude estimation.
#define FLOAT_RMAT_VECT3_MUL(_vout, _rmat, _vin)
#define ahrs_float_lkf_SetRmag(_v)
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:167
#define FLOAT_QUAT_ZERO(_q)
#define AHRS_LTP_TO_BODY()
struct FloatQuat bafl_quat
struct FloatRates bafl_rates
float bafl_qom[4][4]
#define AHRS_RUNNING
Definition: ahrs.h:36
#define BAFL_SIGMA_MAG
struct Int32Rates lp_gyro
Definition: ahrs_aligner.h:40
float bafl_sigma_accel
float bafl_tempP[BAFL_SSIZE][BAFL_SSIZE]
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.