From 88860d0307ffcf014a3adb1236dd7a7fe6df0558 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 17 May 2016 11:02:56 +1000 Subject: [PATCH] EKF: Enable tuning for IMU switch on bias errors --- EKF/common.h | 8 ++++++++ EKF/covariance.cpp | 4 ++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index f7ac319286..aabf413b82 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -197,6 +197,10 @@ struct parameters { float terrain_p_noise; // process noise for terrain offset (m/sec) float terrain_gradient; // gradient of terrain used to estimate process noise due to changing position (m/m) + // initial switch on bias uncertainty + float switch_on_gyro_bias; // 1-sigma gyro bias uncertainty at switch on (rad/sec) + float switch_on_accel_bias; // 1-sigma accelerometer bias uncertainty at switch on (m/s**2) + // position and velocity fusion float gps_vel_noise; // observation noise for gps velocity fusion (m/sec) float gps_pos_noise; // observation noise for gps position fusion (m) @@ -288,6 +292,10 @@ struct parameters { terrain_p_noise = 5.0f; terrain_gradient = 0.5f; + // initial switch on bias uncertainty + switch_on_gyro_bias = 0.1f; + switch_on_accel_bias = 0.2f; + // position and velocity fusion gps_vel_noise = 5.0e-1f; gps_pos_noise = 0.5f; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 1947505877..1d485963e4 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -80,12 +80,12 @@ void Ekf::initialiseCovariance() } // gyro bias - P[10][10] = sq(0.1f * dt); + P[10][10] = sq(_params.switch_on_gyro_bias * dt); P[11][11] = P[10][10]; P[12][12] = P[10][10]; // accel bias - P[13][13] = sq(0.2f * dt); + P[13][13] = sq(_params.switch_on_accel_bias * dt); P[14][14] = P[13][13]; P[15][15] = P[13][13];