39#include "generated/airframe.h"
43#ifndef AHRS_MAG_NOISE_X
44#define AHRS_MAG_NOISE_X 0.2
45#define AHRS_MAG_NOISE_Y 0.2
46#define AHRS_MAG_NOISE_Z 0.2
77 const float P0_a = 1.;
78 const float P0_b = 1e-4;
79 float P0[6][6] = {{ P0_a, 0., 0., 0., 0., 0. },
80 { 0., P0_a, 0., 0., 0., 0. },
81 { 0., 0., P0_a, 0., 0., 0. },
82 { 0., 0., 0., P0_b, 0., 0. },
83 { 0., 0., 0., 0., P0_b, 0. },
84 { 0., 0., 0., 0., 0., P0_b}
115 const float alpha = 0.92;
118 const struct FloatVect3 earth_g = {0., 0., -9.81 };
127#if AHRS_MAG_UPDATE_ALL_AXES
154#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
156 const float alpha = 0.1;
176 const float dp =
ahrs_mlkf.body_rate.p * dt;
177 const float dq =
ahrs_mlkf.body_rate.q * dt;
178 const float dr =
ahrs_mlkf.body_rate.r * dt;
180 float F[6][6] = {{ 1., dr, -dq, -dt, 0., 0. },
181 { -dr, 1., dp, 0., -dt, 0. },
182 { dq, -dp, 1., 0., 0., -dt },
183 { 0., 0., 0., 1., 0., 0. },
184 { 0., 0., 0., 0., 1., 0. },
185 { 0., 0., 0., 0., 0., 1. }
191 const float dt2 = dt * dt;
192 const float GQG[6] = {dt2 * 10e-3, dt2 * 10e-3, dt2 * 10e-3, dt2 * 9e-6, dt2 * 9e-6, dt2 * 9e-6 };
193 for (
int i = 0; i < 6; i++) {
215 float H[3][6] = {{ 0., -b_expected.
z, b_expected.
y, 0., 0., 0.},
216 { b_expected.
z, 0., -b_expected.
x, 0., 0., 0.},
217 { -b_expected.
y, b_expected.
x, 0., 0., 0., 0.}
241 float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. },
242 { 0., 1., 0., 0., 0., 0. },
243 { 0., 0., 1., 0., 0., 0. },
244 { 0., 0., 0., 1., 0., 0. },
245 { 0., 0., 0., 0., 1., 0. },
246 { 0., 0., 0., 0., 0., 1. }
257 ahrs_mlkf.gibbs_cor.qx +=
K[0][0] * e.
x +
K[0][1] * e.
y +
K[0][2] * e.
z;
258 ahrs_mlkf.gibbs_cor.qy +=
K[1][0] * e.
x +
K[1][1] * e.
y +
K[1][2] * e.
z;
259 ahrs_mlkf.gibbs_cor.qz +=
K[2][0] * e.
x +
K[2][1] * e.
y +
K[2][2] * e.
z;
260 ahrs_mlkf.gyro_bias.p +=
K[3][0] * e.
x +
K[3][1] * e.
y +
K[3][2] * e.
z;
261 ahrs_mlkf.gyro_bias.q +=
K[4][0] * e.
x +
K[4][1] * e.
y +
K[4][2] * e.
z;
262 ahrs_mlkf.gyro_bias.r +=
K[5][0] * e.
x +
K[5][1] * e.
y +
K[5][2] * e.
z;
284 struct FloatVect3 i_h_2d = {i_expected->
y, -i_expected->
x, 0.f};
288 float H[3][6] = {{ 0., 0., b_yaw.
x, 0., 0., 0.},
289 { 0., 0., b_yaw.
y, 0., 0., 0.},
290 { 0., 0., b_yaw.
z, 0., 0., 0.}
314 float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. },
315 { 0., 1., 0., 0., 0., 0. },
316 { 0., 0., 1., 0., 0., 0. },
317 { 0., 0., 0., 1., 0., 0. },
318 { 0., 0., 0., 0., 1., 0. },
319 { 0., 0., 0., 0., 0., 1. }
330 ahrs_mlkf.gibbs_cor.qx +=
K[0][0] * e.
x +
K[0][1] * e.
y +
K[0][2] * e.
z;
331 ahrs_mlkf.gibbs_cor.qy +=
K[1][0] * e.
x +
K[1][1] * e.
y +
K[1][2] * e.
z;
332 ahrs_mlkf.gibbs_cor.qz +=
K[2][0] * e.
x +
K[2][1] * e.
y +
K[2][2] * e.
z;
333 ahrs_mlkf.gyro_bias.p +=
K[3][0] * e.
x +
K[3][1] * e.
y +
K[3][2] * e.
z;
334 ahrs_mlkf.gyro_bias.q +=
K[4][0] * e.
x +
K[4][1] * e.
y +
K[4][2] * e.
z;
335 ahrs_mlkf.gyro_bias.r +=
K[5][0] * e.
x +
K[5][1] * e.
y +
K[5][2] * e.
z;
void ahrs_mlkf_update_mag(struct FloatVect3 *mag)
static void propagate_state(float dt)
Progagate filter's covariance We don't propagate state as we assume to have reseted.
void ahrs_mlkf_update_accel(struct FloatVect3 *accel)
void ahrs_mlkf_init(void)
bool ahrs_mlkf_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
static void update_state_heading(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured, struct FloatVect3 *noise)
Incorporate one 3D vector measurement, only correcting heading.
void ahrs_mlkf_update_mag_full(struct FloatVect3 *mag)
static void propagate_ref(struct FloatRates *gyro, float dt)
static void update_state(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured, struct FloatVect3 *noise)
Incorporate one 3D vector measurement.
static void reset_state(void)
Incorporate errors to reference and zeros state.
struct AhrsMlkf ahrs_mlkf
void ahrs_mlkf_propagate(struct FloatRates *gyro, float dt)
void ahrs_mlkf_update_mag_2d(struct FloatVect3 *mag)
Multiplicative linearized Kalman Filter in quaternion formulation.
Utility functions for floating point AHRS implementations.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
static void float_quat_normalize(struct FloatQuat *q)
static float float_vect3_norm(const struct FloatVect3 *v)
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
void float_quat_comp(struct FloatQuat *a2c, const struct FloatQuat *a2b, const struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2)
void float_quat_integrate(struct FloatQuat *q, const struct FloatRates *omega, const float dt)
in place quaternion integration with constant rotational velocity
#define FLOAT_RATES_ZERO(_r)
void float_quat_vmult(struct FloatVect3 *v_out, const struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
#define RATES_COPY(_a, _b)
#define RATES_SUB(_a, _b)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define VECT3_DIFF(_c, _a, _b)
static struct FloatVect3 H
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
Simple matrix helper macros.
#define MAT_MUL_T(_i, _k, _j, C, A, B)
#define MAT_MUL(_i, _k, _j, C, A, B)
#define MAT_SUB(_i, _j, C, A, B)
#define MAT_INV33(_invS, _S)