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