diff --git a/EKF/common.h b/EKF/common.h index 41e8372bbf..8a8dea0436 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -50,7 +50,7 @@ struct gps_message { float eph; // GPS horizontal position accuracy in m float epv; // GPS vertical position accuracy in m float sacc; // GPS speed accuracy in m/s - uint64_t time_usec_vel; // Timestamp for velocity informations + uint64_t time_usec_vel; // Timestamp for velocity informations float vel_m_s; // GPS ground speed (m/s) float vel_ned[3]; // GPS ground speed NED bool vel_ned_valid; // GPS ground speed is valid @@ -112,33 +112,34 @@ struct flowSample { }; struct parameters { - float mag_delay_ms = 0.0f; - float baro_delay_ms = 0.0f; - float gps_delay_ms = 200.0f; - float airspeed_delay_ms = 200.0f; + float mag_delay_ms = 0.0f; // magnetometer measurement delay relative to the IMU + float baro_delay_ms = 0.0f; // barometer height measurement delay relative to the IMU + float gps_delay_ms = 200.0f; // GPS measurement delay relative to the IMU + float airspeed_delay_ms = 200.0f; // airspeed measurement delay relative to the IMU // input noise - float gyro_noise = 0.001f; - float accel_noise = 0.1f; + float gyro_noise = 1.0e-3f; // IMU angular rate noise used for covariance prediction + float accel_noise = 2.5e-1f; // IMU acceleration noise use for covariance prediction // process noise - float gyro_bias_p_noise = 1e-5f; - float accel_bias_p_noise = 1e-3f; - float gyro_scale_p_noise = 1e-4f; - float mag_p_noise = 1e-2f; - float wind_vel_p_noise = 0.05f; - - float gps_vel_noise = 0.05f; - float gps_pos_noise = 1.0f; - float baro_noise = 0.1f; - float baro_innov_gate = 5.0f; // barometric height innovation consistency gate size in standard deviations - float posNE_innov_gate = 5.0f; // GPS horizontal position innovation consistency gate size in standard deviations + float gyro_bias_p_noise = 7.0e-5f; // process noise for IMU delta angle bias prediction + float accel_bias_p_noise = 1.0e-4f; // process noise for IMU delta velocity bias prediction + float gyro_scale_p_noise = 3.0e-3f; // process noise for gyro scale factor prediction + float mag_p_noise = 2.5e-2f; // process noise for magnetic field prediction + float wind_vel_p_noise = 1.0e-1f; // process noise for wind velocity prediction + + float gps_vel_noise = 5.0e-1f; // observation noise for gps velocity fusion + float gps_pos_noise = 1.0f; // observation noise for gps position fusion + float baro_noise = 3.0f; // observation noise for barometric height fusion + float baro_innov_gate = 3.0f; // barometric height innovation consistency gate size in standard deviations + float posNE_innov_gate = 3.0f; // GPS horizontal position innovation consistency gate size in standard deviations float vel_innov_gate = 3.0f; // GPS velocity innovation consistency gate size in standard deviations - float mag_heading_noise = 3e-2f; // measurement noise used for simple heading fusion - float mag_declination_deg = 0.0f; // magnetic declination in degrees - float heading_innov_gate = 3.0f; // heading fusion innovation consistency gate size in standard deviations - float mag_innov_gate = 3.0f; // magnetometer fusion innovation consistency gate size in standard deviations + float mag_heading_noise = 1.7e-1f; // measurement noise used for simple heading fusion + float mag_noise = 5.0e-2f; // measurement noise used for 3-axis magnetoemeter fusion + float mag_declination_deg = 0.0f; // magnetic declination in degrees + float heading_innov_gate = 3.0f; // heading fusion innovation consistency gate size in standard deviations + float mag_innov_gate = 3.0f; // magnetometer fusion innovation consistency gate size in standard deviations // these parameters control the strictness of GPS quality checks used to determine uf the GPS is // good enough to set a local origin and commence aiding diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 01b4c2f225..bc7af4d44d 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -130,12 +130,12 @@ void Ekf::predictCovariance() // compute process noise float process_noise[_k_num_states] = {}; - float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1e-4f); - float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 1e-6f, 1e-2f); - float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 1e-6f, 1e-2f); - float mag_I_sig = dt * math::constrain(_params.mag_p_noise, 1e-4f, 1e-1f); - float mag_B_sig = dt * math::constrain(_params.mag_p_noise, 1e-4f, 1e-1f); - float wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.01f, 1.0f); + float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1e-4f); + float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1e-2f); + float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 0.0f, 1e-2f); + float mag_I_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f); + float mag_B_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f); + float wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f); for (unsigned i = 0; i < 9; i++) { process_noise[i] = 0.0f; diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 924495f4a7..3b19d44ed3 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -84,7 +84,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) * Return true if the GPS solution quality is adequate to set an origin for the EKF * and start GPS aiding. * All activated checks must pass for 10 seconds. - * Checks are activated using the EKF2_GPS_CHECKS bitmask parameter + * Checks are activated using the EKF2_GPS_CHECK bitmask parameter * Checks are adjusted using the EKF2_REQ_* parameters */ bool Ekf::gps_is_good(struct gps_message *gps) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index fd7590827f..fccd6bf3a3 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -55,7 +55,8 @@ void Ekf::fuseMag() float magD = _state.mag_I(2); // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations - float R_MAG = 1e-3f; + float R_MAG = fmaxf(_params.mag_noise, 1.0e-3f); + R_MAG = R_MAG*R_MAG; // intermediate variables from algebraic optimisation float SH_MAG[9]; @@ -471,7 +472,8 @@ void Ekf::fuseHeading() float magY = _mag_sample_delayed.mag(1); float magZ = _mag_sample_delayed.mag(2); - float R_mag = _params.mag_heading_noise; + float R_mag = fmaxf(_params.mag_heading_noise, 1.0e-2f); + R_mag = R_mag*R_mag; float t2 = q0 * q0; float t3 = q1 * q1; diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 5bbc889fff..2d0a2ac241 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -54,8 +54,8 @@ void Ekf::fuseVelPosHeight() fuse_map[0] = fuse_map[1] = true; _vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0); _vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1); - R[0] = _params.gps_vel_noise; - R[1] = _params.gps_vel_noise; + R[0] = fmaxf(_params.gps_vel_noise, 0.01f); + R[1] = R[0]; gate_size[0] = fmaxf(_params.vel_innov_gate, 1.0f); gate_size[1] = gate_size[0]; } @@ -63,7 +63,7 @@ void Ekf::fuseVelPosHeight() if (_fuse_vert_vel) { fuse_map[2] = true; _vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2); - R[2] = _params.gps_vel_noise; + R[2] = 1.5f * fmaxf(_params.gps_vel_noise, 0.01f); gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f); } @@ -71,8 +71,8 @@ void Ekf::fuseVelPosHeight() fuse_map[3] = fuse_map[4] = true; _vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0); _vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); - R[3] = _params.gps_pos_noise; - R[4] = _params.gps_pos_noise; + R[3] = fmaxf(_params.gps_pos_noise, 0.01f); + R[4] = R[3]; gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f); gate_size[4] = gate_size[3]; } @@ -80,7 +80,7 @@ void Ekf::fuseVelPosHeight() if (_fuse_height) { fuse_map[5] = true; _vel_pos_innov[5] = _state.pos(2) - (-_baro_sample_delayed.hgt); // baro measurement has inversed z axis - R[5] = _params.baro_noise; + R[5] = fmaxf(_params.baro_noise, 0.01f); gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f); }