fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 06:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: update default tuning parameters

Browse Source 
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
This commit is contained in:
Paul Riseborough
2016-04-05 22:23:37 +02:00
committed by Roman
parent eaf94935f0
commit 470098e182
2 changed files with 20 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+15 -13
View File
@@ -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;
EKF/covariance.cpp
+5 -5
View File
@@ -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;