From 470098e182523919c4f9674cb7a861ea17d57904 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 3 Apr 2016 15:24:24 -0700 Subject: [PATCH] EKF: update default tuning parameters Parameters needed to be changed when covariance prediction error was corrected. The magnetic earth and body field process noise now use separate parameters to help with tuning --- EKF/common.h | 28 +++++++++++++++------------- EKF/covariance.cpp | 10 +++++----- 2 files changed, 20 insertions(+), 18 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 1be9f5f9b3..1e874fe792 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -171,7 +171,8 @@ struct parameters { float gyro_bias_p_noise; // process noise for IMU delta angle bias prediction (rad/sec) float accel_bias_p_noise; // process noise for IMU delta velocity bias prediction (m/sec/sec) float gyro_scale_p_noise; // process noise for gyro scale factor prediction (N/A) - float mag_p_noise; // process noise for magnetic field prediction (Guass/sec) + float mage_p_noise; // process noise for earth magnetic field prediction (Guass/sec) + float magb_p_noise; // process noise for body magnetic field prediction (Guass/sec) float wind_vel_p_noise; // process noise for wind velocity prediction (m/sec/sec) 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) @@ -238,33 +239,34 @@ struct parameters { range_delay_ms = 200.0f; // input noise - gyro_noise = 1.0e-3f; + gyro_noise = 6.0e-2f; accel_noise = 2.5e-1f; // process noise - gyro_bias_p_noise = 7.0e-5f; - accel_bias_p_noise = 1.0e-4f; - gyro_scale_p_noise = 3.0e-3f; - mag_p_noise = 2.5e-2f; + gyro_bias_p_noise = 2.5e-6f; + accel_bias_p_noise = 3.0e-5f; + gyro_scale_p_noise = 3.0e-4f; + mage_p_noise = 2.5e-3f; + magb_p_noise = 5.0e-4f; wind_vel_p_noise = 1.0e-1f; terrain_p_noise = 5.0f; terrain_gradient = 0.5f; // position and velocity fusion gps_vel_noise = 5.0e-1f; - gps_pos_noise = 1.0f; + gps_pos_noise = 0.5f; pos_noaid_noise = 10.0f; - baro_noise = 3.0f; - baro_innov_gate = 3.0f; - posNE_innov_gate = 3.0f; - vel_innov_gate = 3.0f; + baro_noise = 2.0f; + baro_innov_gate = 5.0f; + posNE_innov_gate = 5.0f; + vel_innov_gate = 5.0f; hgt_reset_lim = 5.0f; // magnetometer fusion - mag_heading_noise = 1.7e-1f; + mag_heading_noise = 3.0e-1f; mag_noise = 5.0e-2f; mag_declination_deg = 0.0f; - heading_innov_gate = 3.0f; + heading_innov_gate = 2.6f; mag_innov_gate = 3.0f; mag_declination_source = 3; mag_fusion_type = 0; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 527de849f3..8869f7d213 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -153,17 +153,17 @@ void Ekf::predictCovariance() float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 0.0f, 1e-2f); float mag_I_sig, mag_B_sig; - // Don't continue to grow the body field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned + // Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned if (_control_status.flags.mag_3D && (P[16][16] + P[17][17] + P[18][8]) < 0.1f) { - mag_I_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f); + mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1e-1f); } else { mag_I_sig = 0.0f; } - // Don't continue to grow the earth field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned + // Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned if (_control_status.flags.mag_3D && (P[19][19] + P[20][20] + P[21][21]) < 0.1f) { - mag_B_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f); + mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1e-1f); } else { mag_B_sig = 0.0f; @@ -210,7 +210,7 @@ void Ekf::predictCovariance() // inputs to the system are 3 delta angles and 3 delta velocities float daxNoise, dayNoise, dazNoise; float dvxNoise, dvyNoise, dvzNoise; - float gyro_noise = math::constrain(_params.gyro_noise, 1e-4f, 1e-2f); + float gyro_noise = math::constrain(_params.gyro_noise, 1e-4f, 1e-1f); daxNoise = dayNoise = dazNoise = dt * gyro_noise; float accel_noise = math::constrain(_params.accel_noise, 1e-2f, 1.0f); dvxNoise = dvyNoise = dvzNoise = dt * accel_noise;