Skip to content

Commit

Permalink
[ahrs] int_cmpl_quat: precompute some gains for efficiency
Browse files Browse the repository at this point in the history
  • Loading branch information
@flixr
flixr committed Sep 4, 2013
1 parent 5665b37 commit 61b4507
Show file tree
Hide file tree
Showing 3 changed files with 112 additions and 64 deletions.
8 changes: 4 additions & 4 deletions conf/settings/estimation/ahrs_int_cmpl_quat.xml
View file
Expand Up @@ -5,10 +5,10 @@

<dl_settings NAME="AHRS">
<dl_setting var="ahrs_impl.use_gravity_heuristic" min="0" step="1" max="1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="gravity_heuristic" values="OFF|ON" param="AHRS_GRAVITY_UPDATE_NORM_HEURISTIC"/>
<dl_setting var="ahrs_impl.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s"/>
<dl_setting var="ahrs_impl.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA"/>
<dl_setting var="ahrs_impl.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s"/>
<dl_setting var="ahrs_impl.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_zeta" param="AHRS_MAG_ZETA"/>
<dl_setting var="ahrs_impl.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s" handler="SetAccelOmega"/>
<dl_setting var="ahrs_impl.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA" handler="SetAccelZeta"/>
<dl_setting var="ahrs_impl.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s" handler="SetMagOmega"/>
<dl_setting var="ahrs_impl.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_zeta" param="AHRS_MAG_ZETA" handler="SetMagZeta"/>
</dl_settings>

</dl_settings>
Expand Down
118 changes: 66 additions & 52 deletions sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
View file
Expand Up @@ -135,8 +135,10 @@ void ahrs_init(void) {
/* set default filter cut-off frequency and damping */
ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA;
ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA;
ahrs_set_accel_gains();
ahrs_impl.mag_omega = AHRS_MAG_OMEGA;
ahrs_impl.mag_zeta = AHRS_MAG_ZETA;
ahrs_set_mag_gains();

/* set default gravity heuristic */
ahrs_impl.weight = 1.0;
Expand All @@ -147,11 +149,7 @@ void ahrs_init(void) {
ahrs_impl.correct_gravity = FALSE;
#endif

#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
ahrs_impl.use_gravity_heuristic = TRUE;
#else
ahrs_impl.use_gravity_heuristic = FALSE;
#endif
ahrs_impl.use_gravity_heuristic = AHRS_GRAVITY_UPDATE_NORM_HEURISTIC;

VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z));
Expand All @@ -175,8 +173,8 @@ void ahrs_align(void) {
set_body_state_from_quat();

/* Use low passed gyro value as initial bias */
RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro);
RATES_COPY(ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
RATES_COPY(ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro);
INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28);

ahrs.status = AHRS_RUNNING;
Expand Down Expand Up @@ -213,6 +211,28 @@ void ahrs_propagate(void) {
}


void ahrs_set_accel_gains(void) {
/* Complementary filter proportionnal gain
* Kp = 2 * omega * zeta * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
*
* accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain)
* accel_inv_kp = 4096 * 9.81 / Kp
*/
ahrs_impl.accel_inv_kp = 4096 * 9.81 /
(2 * ahrs_impl.accel_omega * ahrs_impl.accel_zeta *
AHRS_PROPAGATE_FREQUENCY / AHRS_PROPAGATE_FREQUENCY);

/* Complementary filter integral gain
* Ki = omega^2 / AHRS_CORRECT_FREQUENCY
*
* accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain)
* accel_inv_ki = 2^5 / 2^16 * 9.81 / Ki
* accel_inv_ki = 9.81 / 2048 / Ki
*/
ahrs_impl.accel_inv_ki = AHRS_CORRECT_FREQUENCY * 9.81 / 2048 /
(ahrs_impl.accel_omega * ahrs_impl.accel_omega);
}

void ahrs_update_accel(void) {

// c2 = ltp z-axis in imu-frame
Expand Down Expand Up @@ -283,44 +303,45 @@ void ahrs_update_accel(void) {
}

/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
* Kp = 2 * zeta * omega * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
*
* residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
* rate_correction FRAC: RATE_FRAC = 12
* FRAC_conversion: 2^12 / 2^24 = 1 / 4096
* cross_product_gain : 9.81 m/s2
*
* Kp = 1 / inv_rate_scale / FRAC_conversion * cross_product_gain
* inv_rate_scale= 4096 * 9.81 / (2 * zeta * omega * weight *
* AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY )
* inv_rate_scale= 2048 * 9.81 * AHRS_CORRECT_FREQUENCY /
* (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY )
* accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain)
* accel_inv_kp = 4096 * 9.81 / Kp
*
* inv_rate_scale = 1 / (weight * Kp * FRAC_conversion / cross_product_gain)
* inv_rate_scale = inv_kp / weight
*/

int32_t inv_rate_scale = 2048 * 9.81 * AHRS_CORRECT_FREQUENCY /
(ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY);
int32_t inv_rate_scale = (int32_t)(ahrs_impl.accel_inv_kp / ahrs_impl.weight);
Bound(inv_rate_scale, 8192, 4194304);

ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale;
ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale;
ahrs_impl.rate_correction.r -= residual.z / inv_rate_scale;


/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = omega^2 / AHRS_CORRECT_FREQUENCY
*
* Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
* residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
* high_rez_bias = RATE_FRAC+28 = 40
* FRAC_conversion: 2^40 / 2^24 = 2^16
* cross_product_gain : 9.81 m/s2
*
* Ki = 1 / FRAC_conversion / inv_bias_gain * cross_product_gain * 2^5
* inv_bias_gain = 9.81 / 2^16 / (omega * omega * weight * weight / AHRS_CORRECT_FREQUENCY) * 2^5
* inv_bias_gain = 9.81 * AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 2048)
* accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain)
* accel_inv_ki = 2^5 / 2^16 * 9.81 * Ki = 9.81 / 2^11 * Ki
*
* inv_bias_gain = 2^5 / (weight^2 * Ki * FRAC_conversion / cross_product_gain)
* inv_bias_gain = accel_inv_ki / weight^2
*/

int32_t inv_bias_gain = 9.81 * AHRS_CORRECT_FREQUENCY /
(ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 2048);
int32_t inv_bias_gain = (int32_t)(ahrs_impl.accel_inv_ki /
(ahrs_impl.weight * ahrs_impl.weight));
Bound(inv_bias_gain, 8, 65536)
ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5;
ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5;
Expand All @@ -339,6 +360,15 @@ void ahrs_update_mag(void) {
#endif
}

void ahrs_set_mag_gains(void) {
/* Complementary filter proportionnal gain = 2*omega*zeta */
ahrs_impl.mag_kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega *
AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY;
/* Complementary filter integral gain = omega^2 */
ahrs_impl.mag_ki = ahrs_impl.mag_omega * ahrs_impl.mag_omega /
AHRS_MAG_CORRECT_FREQUENCY;
}


static inline void ahrs_update_mag_full(void) {

Expand All @@ -350,7 +380,6 @@ static inline void ahrs_update_mag_full(void) {

struct Int32Vect3 residual;
INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
//INT32_VECT3_RSHIFT(residual,residual,5);

/* Complementary filter proportionnal gain.
* Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
Expand All @@ -359,15 +388,11 @@ static inline void ahrs_update_mag_full(void) {
* rate_correction FRAC: RATE_FRAC = 12
* FRAC conversion: 2^12 / 2^22 = 1/1024
*
* Kp = 1/ inv_rate_gain / FRAC_conversion
* inv_rate_gain = 1 / (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) *
* AHRS_MAG_CORRECT_FREQUENCY / FRAC_conversion
* inv_rate_gain = 1024 * AHRS_MAG_CORRECT_FREQUENCY /
* (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY)
* inv_rate_gain = 1 / Kp / FRAC_conversion
* inv_rate_gain = 1024 / Kp
*/

const int32_t inv_rate_gain = 512 * AHRS_MAG_CORRECT_FREQUENCY /
(ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY);
const int32_t inv_rate_gain = (int32_t)(1024.0 / ahrs_impl.mag_kp);

ahrs_impl.rate_correction.p += residual.x / inv_rate_gain;
ahrs_impl.rate_correction.q += residual.y / inv_rate_gain;
Expand All @@ -376,19 +401,15 @@ static inline void ahrs_update_mag_full(void) {
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY
*
* residual FRAC: 2* MAG_FRAC = 22
* high_rez_bias FRAC: RATE_FRAC+28 = 40
* FRAC conversion: 2^40 / 2^22 = 2^18
*
* Ki = bias_gain / FRAC_conversion = ahrs_impl.omega * ahrs_impl.omega /
* AHRS_MAG_CORRECT_FREQUENCY
* bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega /
* AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion
* bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega * 2^18 /
* AHRS_MAG_CORRECT_FREQUENCY
* bias_gain = Ki * FRAC_conversion = Ki * 2^18
*/
const int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega * (1<<18) /
AHRS_MAG_CORRECT_FREQUENCY;
const int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * (1<<18));

ahrs_impl.high_rez_bias.p -= residual.x * bias_gain;
ahrs_impl.high_rez_bias.q -= residual.y * bias_gain;
ahrs_impl.high_rez_bias.r -= residual.z * bias_gain;
Expand Down Expand Up @@ -427,39 +448,32 @@ static inline void ahrs_update_mag_2d(void) {

/* Complementary filter proportionnal gain.
* Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
*
* residual_imu FRAC = residual_ltp FRAC = 17
* rate_correction FRAC: RATE_FRAC = 12
* FRAC conversion: 2^12 / 2^17 = 1/32
*
* Kp = 1/ inv_rate_gain / FRAC_conversion
* inv_rate_gain = 1 / (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) *
* AHRS_MAG_CORRECT_FREQUENCY / FRAC_conversion
* inv_rate_gain = 32 * AHRS_MAG_CORRECT_FREQUENCY /
* (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY)
* inv_rate_gain = 16 * AHRS_MAG_CORRECT_FREQUENCY /
* (mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY)
* inv_rate_gain = 1 / Kp / FRAC_conversion
* inv_rate_gain = 32 / Kp
*/
int32_t inv_rate_gain = (int32_t)(32.0 / ahrs_impl.mag_kp);

int32_t inv_rate_gain = 16 * AHRS_MAG_CORRECT_FREQUENCY /
(ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY);
ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain);
ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain);
ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain);

/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY
*
* residual_imu FRAC = residual_ltp FRAC = 17
* high_rez_bias FRAC: RATE_FRAC+28 = 40
* FRAC conversion: 2^40 / 2^17 = 2^23
*
* Ki = bias_gain / FRAC_conversion = omega * omega / AHRS_MAG_CORRECT_FREQUENCY
* bias_gain = mag_omega * mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion
* bias_gain = mag_omega * mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^23
* bias_gain = Ki * FRAC_conversion = Ki * 2^23
*/
int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * (1 << 23));

int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega * (1 << 23) /
AHRS_MAG_CORRECT_FREQUENCY;
ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain);
ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain);
ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain);
Expand Down
50 changes: 42 additions & 8 deletions sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h
View file
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2011 The Paparazzi Team
* Copyright (C) 2008-2013 The Paparazzi Team
*
* This file is part of paparazzi.
*
Expand Down Expand Up @@ -46,16 +46,23 @@ struct AhrsIntCmplQuat {
struct Int64Rates high_rez_bias;
struct Int32Quat ltp_to_imu_quat;
struct Int32Vect3 mag_h;
float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement)
float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement)
float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer
float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer
float weight;

int32_t ltp_vel_norm;
bool_t ltp_vel_norm_valid;
bool_t heading_aligned;
float weight;
float accel_inv_kp;
float accel_inv_ki;
float mag_kp;
float mag_ki;

/* parameters/options that can be changed */
bool_t correct_gravity;
bool_t use_gravity_heuristic;
bool_t heading_aligned;
float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement)
float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement)
float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer
float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer
};

extern struct AhrsIntCmplQuat ahrs_impl;
Expand All @@ -74,10 +81,37 @@ void ahrs_update_heading(int32_t heading);
*/
void ahrs_realign_heading(int32_t heading);


/// update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta
extern void ahrs_set_accel_gains(void);

static inline void ahrs_int_cmpl_quat_SetAccelOmega(float omega) {
ahrs_impl.accel_omega = omega;
ahrs_set_accel_gains();
}

static inline void ahrs_int_cmpl_quat_SetAccelZeta(float zeta) {
ahrs_impl.accel_zeta = zeta;
ahrs_set_accel_gains();
}

/// update pre-computed kp and ki gains from mag_omega and mag_zeta
extern void ahrs_set_mag_gains(void);

static inline void ahrs_int_cmpl_quat_SetMagOmega(float omega) {
ahrs_impl.mag_omega = omega;
ahrs_set_mag_gains();
}

static inline void ahrs_int_cmpl_quat_SetMagZeta(float zeta) {
ahrs_impl.mag_zeta = zeta;
ahrs_set_mag_gains();
}


#ifdef AHRS_UPDATE_FW_ESTIMATOR
extern float ins_roll_neutral;
extern float ins_pitch_neutral;
#endif


#endif /* AHRS_INT_CMPL_QUAT_H */

0 comments on commit 61b4507

Please sign in to comment.