From 83cc9ef496d68ec0e3de0160c9532fb0690ae60b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 17 May 2016 13:15:57 +1000 Subject: [PATCH] ekf2: Enable tuning of initial tilt alignment uncertainty --- src/modules/ekf2/ekf2_main.cpp | 4 +++- src/modules/ekf2/ekf2_params.c | 11 +++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index f71f320230..45f77d4222 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -251,6 +251,7 @@ private: // IMU switch on bias paameters control::BlockParamFloat _gyr_bias_init; // 1-sigma gyro bias uncertainty at switch-on (rad/sec) control::BlockParamFloat _acc_bias_init; // 1-sigma accelerometer bias uncertainty at switch-on (m/s**2) + control::BlockParamFloat _ang_err_init; // 1-sigma uncertainty in tilt angle after gravity vector alignment (rad) int update_subscriptions(); @@ -338,7 +339,8 @@ Ekf2::Ekf2(): _tau_vel(this, "EKF2_TAU_VEL", false, &_params->vel_Tau), _tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau), _gyr_bias_init(this, "EKF2_GBIAS_INIT", false, &_params->switch_on_gyro_bias), - _acc_bias_init(this, "EKF2_ABIAS_INIT", false, &_params->switch_on_accel_bias) + _acc_bias_init(this, "EKF2_ABIAS_INIT", false, &_params->switch_on_accel_bias), + _ang_err_init(this, "EKF2_ANGERR_INIT", false, &_params->initial_tilt_err) { } diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 5e670c1450..4837d7df89 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -743,3 +743,14 @@ PARAM_DEFINE_FLOAT(EKF2_GBIAS_INIT, 0.1f); * @decimal 2 */ PARAM_DEFINE_FLOAT(EKF2_ABIAS_INIT, 0.2f); + +/** + * 1-sigma tilt angle uncertainty after gravity vector alignment + * + * @group EKF2 + * @min 0.0 + * @max 0.5 + * @unit rad + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_ANGERR_INIT, 0.1f);