Skip to content

Commit

Permalink
[math][ins] rewrite/move some math to ins_float_invariant
Browse files Browse the repository at this point in the history
  • Loading branch information
@flixr
flixr committed Sep 11, 2014
1 parent 3f98492 commit 555d577
Show file tree
Hide file tree
Showing 2 changed files with 68 additions and 111 deletions.
101 changes: 6 additions & 95 deletions sw/airborne/math/pprz_algebra_float.h
View file
Expand Up @@ -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))

Expand All @@ -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)

Expand All @@ -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);

Expand Down
78 changes: 62 additions & 16 deletions sw/airborne/subsystems/ins/ins_float_invariant.c
View file
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);

//------------------------------------------------------------//

Expand Down Expand Up @@ -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) {
Expand All @@ -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);
Expand All @@ -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 */
Expand Down Expand Up @@ -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 ) ----------*/
Expand Down Expand Up @@ -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);
}

0 comments on commit 555d577

Please sign in to comment.