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 File Reference

Linearized Kalman Filter for attitude estimation. More...

#include "ahrs_float_lkf.h"
#include "subsystems/imu.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "generated/airframe.h"
#include "math/pprz_algebra_float.h"
#include <stdio.h>
+ Include dependency graph for ahrs_float_lkf.c:

Go to the source code of this file.

Macros

#define BAFL_DEBUG
 
#define BAFL_g   9.81
 
#define BAFL_hx   1.0
 
#define BAFL_hy   0.0
 
#define BAFL_hz   1.0
 
#define BAFL_Q_GYRO   1e-04
 
#define BAFL_Q_ATT   0
 
#define BAFL_SIGMA_ACCEL   1000.0
 
#define BAFL_SIGMA_MAG   20.
 
#define BAFL_DT   (1./512.)
 
#define FLOAT_MAT33_INVERT(_mo, _mi)
 
#define AHRS_TO_BFP()
 
#define AHRS_LTP_TO_BODY()
 

Functions

static void ahrs_do_update_accel (void)
 
static void ahrs_do_update_mag (void)
 
void ahrs_init (void)
 AHRS initialization. More...
 
void ahrs_align (void)
 Aligns the AHRS. More...
 
static void ahrs_lowpass_accel (void)
 
void ahrs_propagate (void)
 Propagation. More...
 
void ahrs_update_accel (void)
 Update AHRS state with accerleration measurements. More...
 
void ahrs_update_mag (void)
 Update AHRS state with magnetometer measurements. More...
 
void ahrs_update (void)
 

Variables

struct FloatQuat bafl_quat
 
struct FloatRates bafl_bias
 
struct FloatRates bafl_rates
 
struct FloatEulers bafl_eulers
 
struct FloatRMat bafl_dcm
 
struct FloatQuat bafl_q_a_err
 
struct FloatQuat bafl_q_m_err
 
struct FloatRates bafl_b_a_err
 
struct FloatRates bafl_b_m_err
 
struct FloatQuat bafl_qtemp
 
struct FloatQuat bafl_qr
 
float bafl_qnorm
 
float bafl_phi_accel
 
float bafl_theta_accel
 
float bafl_P [BAFL_SSIZE][BAFL_SSIZE]
 
float bafl_X [BAFL_SSIZE]
 
float bafl_F [3][3]
 
float bafl_T [6][6]
 
float bafl_Pprio [BAFL_SSIZE][BAFL_SSIZE]
 
float bafl_tempP [BAFL_SSIZE][BAFL_SSIZE]
 
float bafl_K [6][3]
 
float bafl_tempK [6][3]
 
float bafl_S [3][3]
 
float bafl_tempS [3][6]
 
float bafl_invS [3][3]
 
struct FloatVect3 bafl_ya
 
struct FloatVect3 bafl_ym
 
float bafl_H [3][3]
 
struct FloatVect3 bafl_accel_measure
 
struct FloatVect3 bafl_accel_last
 
struct FloatVect3 bafl_mag
 
float bafl_qom [4][4]
 
struct FloatVect3 bafl_h
 
float bafl_Q_gyro
 
float bafl_Q_att
 
float bafl_sigma_accel
 
float bafl_sigma_mag
 
float bafl_R_accel
 
float bafl_R_mag
 

Detailed Description

Linearized Kalman Filter for attitude estimation.

Definition in file ahrs_float_lkf.c.

Macro Definition Documentation

#define AHRS_LTP_TO_BODY ( )
Value:
{ \
/* Compute LTP to BODY quaternion */ \
INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); \
/* Compute LTP to BODY rotation matrix */ \
INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); \
/* compute LTP to BODY eulers */ \
INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat); \
/* compute body rates */ \
}
#define INT32_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
struct Int32RMat body_to_imu_rmat
rotation from body to imu frame as a rotation matrix
Definition: imu.h:52
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
#define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c)
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
#define INT32_EULERS_OF_RMAT(_e, _rm)
#define INT32_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)
struct Int32Quat body_to_imu_quat
rotation from body to imu frame as a unit quaternion
Definition: imu.h:51

Definition at line 206 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), and ahrs_propagate().

#define AHRS_TO_BFP ( )
Value:
{ \
/* IMU rate */ \
/* LTP to IMU eulers */ \
EULERS_BFP_OF_REAL(ahrs.ltp_to_imu_euler, bafl_eulers); \
/* LTP to IMU quaternion */ \
QUAT_BFP_OF_REAL(ahrs.ltp_to_imu_quat, bafl_quat); \
/* LTP to IMU rotation matrix */ \
RMAT_BFP_OF_REAL(ahrs.ltp_to_imu_rmat, bafl_dcm); \
}
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:610
#define RMAT_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:579
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
struct FloatEulers bafl_eulers
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:623
struct FloatRMat bafl_dcm
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:573
struct FloatQuat bafl_quat
struct FloatRates bafl_rates

Definition at line 195 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), and ahrs_propagate().

#define BAFL_DEBUG

Definition at line 41 of file ahrs_float_lkf.c.

#define BAFL_DT   (1./512.)

Definition at line 164 of file ahrs_float_lkf.c.

Referenced by ahrs_propagate().

#define BAFL_g   9.81

Definition at line 129 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel().

#define BAFL_hx   1.0

Definition at line 131 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define BAFL_hy   0.0

Definition at line 132 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define BAFL_hz   1.0

Definition at line 133 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define BAFL_Q_ATT   0

Definition at line 145 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define BAFL_Q_GYRO   1e-04

Definition at line 144 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define BAFL_SIGMA_ACCEL   1000.0

Definition at line 156 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define BAFL_SIGMA_MAG   20.

Definition at line 157 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define FLOAT_MAT33_INVERT (   _mo,
  _mi 
)
Value:
{ \
float _det = 0.; \
_det = _mi[0][0] * (_mi[2][2] * _mi[1][1] - _mi[2][1] * _mi[1][2]) \
- _mi[1][0] * (_mi[2][2] * _mi[0][1] - _mi[2][1] * _mi[0][2]) \
+ _mi[2][0] * (_mi[1][2] * _mi[0][1] - _mi[1][1] * _mi[0][2]); \
if (_det != 0.) { /* ? test for zero? */ \
_mo[0][0] = (_mi[1][1] * _mi[2][2] - _mi[1][2] * _mi[2][1]) / _det; \
_mo[0][1] = (_mi[0][2] * _mi[2][1] - _mi[0][1] * _mi[2][2]) / _det; \
_mo[0][2] = (_mi[0][1] * _mi[1][2] - _mi[0][2] * _mi[1][1]) / _det; \
\
_mo[1][0] = (_mi[1][2] * _mi[2][0] - _mi[1][0] * _mi[2][2]) / _det; \
_mo[1][1] = (_mi[0][0] * _mi[2][2] - _mi[0][2] * _mi[2][0]) / _det; \
_mo[1][2] = (_mi[0][2] * _mi[1][0] - _mi[0][0] * _mi[1][2]) / _det; \
\
_mo[2][0] = (_mi[1][0] * _mi[2][1] - _mi[1][1] * _mi[2][0]) / _det; \
_mo[2][1] = (_mi[0][1] * _mi[2][0] - _mi[0][0] * _mi[2][1]) / _det; \
_mo[2][2] = (_mi[0][0] * _mi[1][1] - _mi[0][1] * _mi[1][0]) / _det; \
} else { \
printf("ERROR! Not invertible!\n"); \
for (int _i=0; _i<3; _i++) { \
for (int _j=0; _j<3; _j++) { \
_mo[_i][_j] = 0.0; \
} \
} \
} \
}
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer

Definition at line 168 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

Function Documentation

void ahrs_align ( void  )

Aligns the AHRS.

Called after ahrs_aligner has run to set initial attitude and biases. Must set the ahrs status to AHRS_RUNNING. Needs to be implemented by each AHRS algorithm.

Definition at line 256 of file ahrs_float_lkf.c.

References ACCELS_FLOAT_OF_BFP, ahrs, ahrs_aligner, AHRS_RUNNING, bafl_accel_measure, bafl_bias, AhrsAligner::lp_accel, AhrsAligner::lp_gyro, RATES_FLOAT_OF_BFP, and Ahrs::status.

void ahrs_init ( void  )
static void ahrs_lowpass_accel ( void  )
inlinestatic

Definition at line 262 of file ahrs_float_lkf.c.

References Imu::accel, ACCELS_FLOAT_OF_BFP, bafl_accel_last, bafl_accel_measure, bafl_phi_accel, bafl_theta_accel, imu, VECT3_ADD, VECT3_SDIV, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_propagate().

+ Here is the caller graph for this function:

void ahrs_propagate ( void  )
void ahrs_update ( void  )

Definition at line 854 of file ahrs_float_lkf.c.

References ahrs_update_accel(), and ahrs_update_mag().

+ Here is the call graph for this function:

void ahrs_update_accel ( void  )

Update AHRS state with accerleration measurements.

Reads the global imu data struct. Needs to be implemented by each AHRS algorithm.

Definition at line 413 of file ahrs_float_lkf.c.

References ahrs_do_update_accel().

Referenced by ahrs_update().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ahrs_update_mag ( void  )

Update AHRS state with magnetometer measurements.

Reads the global imu data struct. Needs to be implemented by each AHRS algorithm.

Definition at line 644 of file ahrs_float_lkf.c.

References ahrs_do_update_mag().

Referenced by ahrs_update().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Variable Documentation

struct FloatVect3 bafl_accel_last

Definition at line 122 of file ahrs_float_lkf.c.

Referenced by ahrs_lowpass_accel().

struct FloatVect3 bafl_accel_measure

Definition at line 121 of file ahrs_float_lkf.c.

Referenced by ahrs_align(), ahrs_do_update_accel(), and ahrs_lowpass_accel().

struct FloatRates bafl_b_a_err

Definition at line 63 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel().

struct FloatRates bafl_b_m_err

Definition at line 65 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_mag().

struct FloatRates bafl_bias
struct FloatRMat bafl_dcm

Definition at line 56 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), and ahrs_propagate().

struct FloatEulers bafl_eulers

Definition at line 54 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), and ahrs_propagate().

float bafl_F[3][3]

Definition at line 90 of file ahrs_float_lkf.c.

float bafl_H[3][3]

Definition at line 118 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

struct FloatVect3 bafl_h

Definition at line 134 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_mag(), and ahrs_init().

float bafl_invS[3][3]

Definition at line 107 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

float bafl_K[6][3]

Definition at line 103 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

struct FloatVect3 bafl_mag

Definition at line 124 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_mag().

float bafl_P[BAFL_SSIZE][BAFL_SSIZE]
float bafl_phi_accel

Definition at line 74 of file ahrs_float_lkf.c.

Referenced by ahrs_lowpass_accel().

float bafl_Pprio[BAFL_SSIZE][BAFL_SSIZE]

Definition at line 100 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

struct FloatQuat bafl_q_a_err

Definition at line 59 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel().

float bafl_Q_att

Definition at line 147 of file ahrs_float_lkf.c.

Referenced by ahrs_init(), and ahrs_propagate().

float bafl_Q_gyro

Definition at line 146 of file ahrs_float_lkf.c.

Referenced by ahrs_init(), and ahrs_propagate().

struct FloatQuat bafl_q_m_err

Definition at line 61 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_mag().

float bafl_qnorm

Definition at line 71 of file ahrs_float_lkf.c.

Referenced by ahrs_propagate().

float bafl_qom[4][4]

Definition at line 127 of file ahrs_float_lkf.c.

struct FloatQuat bafl_qr

Definition at line 69 of file ahrs_float_lkf.c.

Referenced by ahrs_propagate().

struct FloatQuat bafl_qtemp

Definition at line 67 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), and ahrs_propagate().

struct FloatQuat bafl_quat
float bafl_R_accel

Definition at line 160 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel().

float bafl_R_mag

Definition at line 161 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_mag().

struct FloatRates bafl_rates

Definition at line 52 of file ahrs_float_lkf.c.

Referenced by ahrs_propagate().

float bafl_S[3][3]

Definition at line 105 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

float bafl_sigma_accel

Definition at line 158 of file ahrs_float_lkf.c.

float bafl_sigma_mag

Definition at line 159 of file ahrs_float_lkf.c.

float bafl_T[6][6]

Definition at line 95 of file ahrs_float_lkf.c.

Referenced by ahrs_init(), and ahrs_propagate().

float bafl_tempK[6][3]

Definition at line 104 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

float bafl_tempP[BAFL_SSIZE][BAFL_SSIZE]

Definition at line 102 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), and ahrs_propagate().

float bafl_tempS[3][6]

Definition at line 106 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

float bafl_theta_accel

Definition at line 75 of file ahrs_float_lkf.c.

Referenced by ahrs_lowpass_accel().

float bafl_X[BAFL_SSIZE]

Definition at line 83 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

struct FloatVect3 bafl_ya

Definition at line 108 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel().

struct FloatVect3 bafl_ym

Definition at line 109 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_mag().