From 555d577bf83d82d6c94f9b789009f99bdce905af Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 11 Sep 2014 19:51:28 +0200 Subject: [PATCH] [math][ins] rewrite/move some math to ins_float_invariant --- sw/airborne/math/pprz_algebra_float.h | 101 ++---------------- .../subsystems/ins/ins_float_invariant.c | 78 +++++++++++--- 2 files changed, 68 insertions(+), 111 deletions(-) diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index a60fd230330..0d4bdc43ae8 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -278,6 +278,7 @@ extern float float_rmat_reorthogonalize(struct FloatRMat* rm); #define FLOAT_QUAT_ZERO(_q) QUAT_ASSIGN(_q, 1., 0., 0., 0.) +#define FLOAT_QUAT_NORM2(_q) (SQUARE((_q).qi) + SQUARE((_q).qx) + SQUARE((_q).qy) + SQUARE((_q).qz)) #define FLOAT_QUAT_NORM(_q) float_quat_norm(&(_q)) #define FLOAT_QUAT_NORMALIZE(_q) float_quat_normalize(&(_q)) @@ -297,6 +298,11 @@ static inline void float_quat_normalize(struct FloatQuat* q) } } +static inline void float_quat_invert(struct FloatQuat* qo, struct FloatQuat* qi) +{ + QUAT_INVERT(*qo, *qi); +} + /* */ #define FLOAT_QUAT_EXTRACT(_vo, _qi) QUAT_EXTRACT_Q(_vo, _qi) @@ -311,101 +317,6 @@ static inline void float_quat_wrap_shortest(struct FloatQuat* q) } } -/* - * - * Rotation Matrix using quaternions - * - */ - -/* - * The (non commutative) quaternion product * then reads - * - * [ p0.q0 - p.q ] - * p * q = [ ] - * [ p0.q + q0.p + p x q ] - * - */ - -/* (qi)-1 * vi * qi represents R_q of n->b on vectors vi - * - * "FLOAT_QUAT_EXTRACT : Extracted of the vector part" - */ - -#define FLOAT_QUAT_RMAT_N2B(_n2b, _qi, _vi){ \ - \ - struct FloatQuat quatinv; \ - struct FloatVect3 quat3, v1, v2; \ - float qi; \ - \ - FLOAT_QUAT_INVERT(quatinv, _qi); \ - FLOAT_QUAT_NORMALIZE(quatinv); \ - \ - FLOAT_QUAT_EXTRACT(quat3, quatinv); \ - qi = - VECT3_DOT_PRODUCT(quat3, _vi); \ - VECT3_CROSS_PRODUCT(v1, quat3, _vi); \ - VECT3_SMUL(v2, _vi, (quatinv.qi)) ; \ - VECT3_ADD(v2, v1); \ - \ - FLOAT_QUAT_EXTRACT(quat3, _qi); \ - VECT3_CROSS_PRODUCT(_n2b, v2, quat3); \ - VECT3_SMUL(v1, v2, (_qi).qi); \ - VECT3_ADD(_n2b,v1); \ - VECT3_SMUL(v1, quat3, qi); \ - VECT3_ADD(_n2b,v1); \ - } - -/* - * qi * vi * (qi)-1 represents R_q of b->n on vectors vi - */ -#define FLOAT_QUAT_RMAT_B2N(_b2n,_qi,_vi){ \ - \ - struct FloatQuat _quatinv; \ - \ - \ - FLOAT_QUAT_INVERT(_quatinv, _qi); \ - FLOAT_QUAT_NORMALIZE(_quatinv); \ - \ - FLOAT_QUAT_RMAT_N2B(_b2n, _quatinv, _vi); \ - } - -/* Right multiplication by a quaternion - * - * vi * qi - * - */ -#define FLOAT_QUAT_VMUL_RIGHT(_mright,_qi,_vi){ \ - \ - struct FloatVect3 quat3, v1, v2; \ - float qi; \ - \ - FLOAT_QUAT_EXTRACT(quat3, _qi); \ - qi = - VECT3_DOT_PRODUCT(_vi, quat3); \ - VECT3_CROSS_PRODUCT(v1, _vi, quat3); \ - VECT3_SMUL(v2, _vi, (_qi.qi)); \ - VECT3_ADD(v2, v1); \ - QUAT_ASSIGN(_mright, qi, v2.x, v2.y, v2.z);\ - } - - -/* Left multiplication by a quaternion -* -* qi * vi -* -*/ -#define FLOAT_QUAT_VMUL_LEFT(_mleft,_qi,_vi){ \ - \ - struct FloatVect3 quat3, v1, v2; \ - float qi; \ - \ - FLOAT_QUAT_EXTRACT(quat3, _qi); \ - qi = - VECT3_DOT_PRODUCT(quat3, _vi); \ - VECT3_CROSS_PRODUCT(v1, quat3, _vi); \ - VECT3_SMUL(v2, _vi, (_qi.qi)); \ - VECT3_ADD(v2, v1); \ - QUAT_ASSIGN(_mleft, qi, v2.x, v2.y, v2.z);\ - } - - /* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */ extern void float_quat_comp(struct FloatQuat* a2c, struct FloatQuat* a2b, struct FloatQuat* b2c); diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index b98712a8f64..24164fa5e49 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -205,6 +205,20 @@ static inline void error_output(struct InsFloatInv * _ins); /* propagation model (called by runge-kutta library) */ static inline void invariant_model(float * o, const float * x, const int n, const float * u, const int m); + +/** Right multiplication by a quaternion. + * vi * q + */ +void float_quat_vmul_right(struct FloatQuat* mright, const struct FloatQuat* q, + struct FloatVect3* vi); + +/** Left multiplication by a quaternion. + * q * vi + */ +void float_quat_vmul_left(struct FloatQuat* mleft, const struct FloatQuat* q, + struct FloatVect3* vi); + + /* init state and measurements */ static inline void init_invariant_state(void) { // init state @@ -344,7 +358,7 @@ void ahrs_align(void) } void ahrs_propagate(void) { - struct NedCoor_f accel; + struct FloatVect3 accel; struct FloatRates body_rates; // realign all the filter if needed @@ -397,10 +411,12 @@ void ahrs_propagate(void) { stateSetPositionNed_f(&ins_impl.state.pos); stateSetSpeedNed_f(&ins_impl.state.speed); // untilt accel and remove gravity - FLOAT_QUAT_RMAT_B2N(accel, ins_impl.state.quat, ins_impl.cmd.accel); + struct FloatQuat q_b2n; + float_quat_invert(&q_b2n, &ins_impl.state.quat); + float_quat_vmult(&accel, &q_b2n, &ins_impl.cmd.accel); VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as)); VECT3_ADD(accel, A); - stateSetAccelNed_f(&accel); + stateSetAccelNed_f((struct NedCoor_f*)&accel); //------------------------------------------------------------// @@ -571,15 +587,14 @@ void ahrs_update_mag(void) { * * x_dot = evolution_model + (gain_matrix * error) */ -static inline void invariant_model(float * o, const float * x, const int n, const float * u, const int m __attribute__((unused))) { +static inline void invariant_model(float* o, const float* x, const int n, const float* u, const int m __attribute__((unused))) { - const struct inv_state * s = (const struct inv_state *)x; - const struct inv_command * c = (const struct inv_command *)u; + struct inv_state* s = (struct inv_state*)x; + struct inv_command* c = (struct inv_command*)u; struct inv_state s_dot; struct FloatRates rates; struct FloatVect3 tmp_vect; struct FloatQuat tmp_quat; - float norm; // test accel sensitivity if (fabs(s->as) < 0.1) { @@ -592,19 +607,20 @@ static inline void invariant_model(float * o, const float * x, const int n, cons /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */ RATES_DIFF(rates, c->rates, s->bias); VECT3_ASSIGN(tmp_vect, rates.p, rates.q, rates.r); - FLOAT_QUAT_VMUL_LEFT(s_dot.quat, s->quat, tmp_vect); + float_quat_vmul_left(&s_dot.quat, &(s->quat), &tmp_vect); QUAT_SMUL(s_dot.quat, s_dot.quat, 0.5); - FLOAT_QUAT_VMUL_RIGHT(tmp_quat, s->quat, ins_impl.corr.LE); + float_quat_vmul_right(&tmp_quat, &(s->quat), &ins_impl.corr.LE); QUAT_ADD(s_dot.quat, tmp_quat); - norm = FLOAT_QUAT_NORM(s->quat); - norm = 1. - (norm*norm); - QUAT_SMUL(tmp_quat, s->quat, norm); + float norm2_r = 1. - FLOAT_QUAT_NORM2(s->quat); + QUAT_SMUL(tmp_quat, s->quat, norm2_r); QUAT_ADD(s_dot.quat, tmp_quat); /* dot_V = A + (1/as) * (q * am * q-1) + ME */ - FLOAT_QUAT_RMAT_B2N(s_dot.speed, s->quat, c->accel); + struct FloatQuat q_b2n; + float_quat_invert(&q_b2n, &(s->quat)); + float_quat_vmult((struct FloatVect3*)&s_dot.speed, &q_b2n, &(c->accel)); VECT3_SMUL(s_dot.speed, s_dot.speed, 1. / (s->as)); VECT3_ADD(s_dot.speed, A); VECT3_ADD(s_dot.speed, ins_impl.corr.ME); @@ -613,7 +629,7 @@ static inline void invariant_model(float * o, const float * x, const int n, cons VECT3_SUM(s_dot.pos, s->speed, ins_impl.corr.NE); /* bias_dot = q-1 * (OE) * q */ - FLOAT_QUAT_RMAT_N2B(tmp_vect, s->quat, ins_impl.corr.OE); + float_quat_vmult(&tmp_vect, &(s->quat), &ins_impl.corr.OE); RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z); /* as_dot = as * RE */ @@ -643,9 +659,11 @@ static inline void error_output(struct InsFloatInv * _ins) { } /* YBt = q * yB * q-1 */ - FLOAT_QUAT_RMAT_B2N(YBt, _ins->state.quat, _ins->meas.mag); + struct FloatQuat q_b2n; + float_quat_invert(&q_b2n, &(_ins->state.quat)); + float_quat_vmult(&YBt, &q_b2n, &(_ins->meas.mag)); - FLOAT_QUAT_RMAT_B2N(I, _ins->state.quat, _ins->cmd.accel); + float_quat_vmult(&I, &q_b2n, &(_ins->cmd.accel)); VECT3_SMUL(I, I, 1. / (_ins->state.as)); /*--------- E = ( ลท - y ) ----------*/ @@ -718,3 +736,31 @@ static inline void error_output(struct InsFloatInv * _ins) { } + +void float_quat_vmul_right(struct FloatQuat* mright, const struct FloatQuat* q, + struct FloatVect3* vi) +{ + struct FloatVect3 qvec, v1, v2; + float qi; + + FLOAT_QUAT_EXTRACT(qvec, *q); + qi = - VECT3_DOT_PRODUCT(*vi, qvec); + VECT3_CROSS_PRODUCT(v1, *vi, qvec); + VECT3_SMUL(v2, *vi, q->qi); + VECT3_ADD(v2, v1); + QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z); +} + +void float_quat_vmul_left(struct FloatQuat* mleft, const struct FloatQuat* q, + struct FloatVect3* vi) +{ + struct FloatVect3 qvec, v1, v2; + float qi; + + FLOAT_QUAT_EXTRACT(qvec, *q); + qi = - VECT3_DOT_PRODUCT(qvec, *vi); + VECT3_CROSS_PRODUCT(v1, qvec, *vi); + VECT3_SMUL(v2, *vi, q->qi); + VECT3_ADD(v2, v1); + QUAT_ASSIGN(*mleft, qi, v2.x, v2.y, v2.z); +}