From 07e804676c06df5e8d0738aa783e92f8408c478c Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 29 Oct 2019 10:58:51 +0100 Subject: [PATCH] Rename IMU biases --- EKF/common.h | 4 ++-- EKF/control.cpp | 14 +++++++------- EKF/covariance.cpp | 21 +++++++++++---------- EKF/drag_fusion.cpp | 4 ++-- EKF/ekf.cpp | 18 +++++++++--------- EKF/ekf_helper.cpp | 30 +++++++++++++++--------------- EKF/terrain_estimator.cpp | 2 -- 7 files changed, 46 insertions(+), 47 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 5737f1d840..76d3a5069e 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -368,8 +368,8 @@ struct stateSample { Quatf quat_nominal; ///< quaternion defining the rotation from body to earth frame Vector3f vel; ///< NED velocity in earth frame in m/s Vector3f pos; ///< NED position in earth frame in m - Vector3f gyro_bias; ///< delta angle bias estimate in rad - Vector3f accel_bias; ///< delta velocity bias estimate in m/s + Vector3f delta_ang_bias; ///< delta angle bias estimate in rad + Vector3f delta_vel_bias; ///< delta velocity bias estimate in m/s Vector3f mag_I; ///< NED earth magnetic field in gauss Vector3f mag_B; ///< magnetometer bias estimate in body frame in gauss Vector2f wind_vel; ///< wind velocity in m/s diff --git a/EKF/control.cpp b/EKF/control.cpp index bc77d664c3..61ba2450ff 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -424,7 +424,7 @@ void Ekf::controlOpticalFlowFusion() } // Accumulate autopilot gyro data across the same time interval as the flow sensor - _imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias; + _imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.delta_ang_bias; _delta_time_of += _imu_sample_delayed.delta_ang_dt; // New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon @@ -1383,8 +1383,8 @@ void Ekf::controlFakePosFusion() // Fuse synthetic position observations every 200msec if (((_time_last_imu - _time_last_fake_pos) > (uint64_t)2e5) || _fuse_height) { - Vector3f fake_gps_pos_obs_var{}; - Vector2f fake_gps_pos_innov_gate{}; + Vector3f fake_pos_obs_var{}; + Vector2f fake_pos_innov_gate{}; // Reset position and velocity states if we re-commence this aiding method @@ -1401,19 +1401,19 @@ void Ekf::controlFakePosFusion() _time_last_fake_pos = _time_last_imu; if (_control_status.flags.in_air && _control_status.flags.tilt_align) { - fake_gps_pos_obs_var(0) = fake_gps_pos_obs_var(1) = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise); + fake_pos_obs_var(0) = fake_pos_obs_var(1) = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise); } else { - fake_gps_pos_obs_var(0) = fake_gps_pos_obs_var(1) = 0.5f; + fake_pos_obs_var(0) = fake_pos_obs_var(1) = 0.5f; } _gps_pos_innov(0) = _state.pos(0) - _last_known_posNE(0); _gps_pos_innov(1) = _state.pos(1) - _last_known_posNE(1); // glitch protection is not required so set gate to a large value - fake_gps_pos_innov_gate(0) = 100.0f; + fake_pos_innov_gate(0) = 100.0f; - fuseHorizontalPosition(_gps_pos_innov, fake_gps_pos_innov_gate, fake_gps_pos_obs_var, + fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio); } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index e5f317936b..a478de1313 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -148,13 +148,13 @@ void Ekf::predictCovariance() float dvy = _imu_sample_delayed.delta_vel(1); float dvz = _imu_sample_delayed.delta_vel(2); - float dax_b = _state.gyro_bias(0); - float day_b = _state.gyro_bias(1); - float daz_b = _state.gyro_bias(2); + float dax_b = _state.delta_ang_bias(0); + float day_b = _state.delta_ang_bias(1); + float daz_b = _state.delta_ang_bias(2); - float dvx_b = _state.accel_bias(0); - float dvy_b = _state.accel_bias(1); - float dvz_b = _state.accel_bias(2); + float dvx_b = _state.delta_vel_bias(0); + float dvy_b = _state.delta_vel_bias(1); + float dvz_b = _state.delta_vel_bias(2); float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S); float dt_inv = 1.0f / dt; @@ -337,7 +337,7 @@ void Ekf::predictCovariance() SPP[10] = SF[16]; // covariance update - float nextP[24][24]; + float nextP[24][24] = {}; // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states nextP[0][0] = P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10] + (daxVar*SQ[10])/4 + SF[9]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[11]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[10]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) + SF[15]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) + SPP[10]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + (dayVar*sq(q2))/4 + (dazVar*sq(q3))/4; @@ -822,13 +822,14 @@ void Ekf::fixCovarianceErrors() float down_dvel_bias = 0.0f; for (uint8_t axis_index = 0; axis_index < 3; axis_index++) { - down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2, axis_index); + down_dvel_bias += _state.delta_vel_bias(axis_index) * _R_to_earth(2, axis_index); } // check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim - && down_dvel_bias * _vel_pos_innov[2] < 0.0f - && down_dvel_bias * _vel_pos_innov[5] < 0.0f); + && ( (down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps) + || (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel) ) + && down_dvel_bias * _gps_pos_innov(2) < 0.0f); // record the pass/fail if (!bad_acc_bias) { diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index 876518aaac..fb9e8ee48d 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -90,7 +90,7 @@ void Ekf::fuseDrag() // calculate observation jacobiam and Kalman gain vectors if (axis_index == 0) { // Estimate the airspeed from the measured drag force and ballistic coefficient - float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.accel_bias(axis_index) / _dt_ekf_avg; + float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg; float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_x * rho)); // Estimate the derivative of specific force wrt airspeed along the X axis @@ -163,7 +163,7 @@ void Ekf::fuseDrag() } else if (axis_index == 1) { // Estimate the airspeed from the measured drag force and ballistic coefficient - float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.accel_bias(axis_index) / _dt_ekf_avg; + float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg; float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_y * rho)); // Estimate the derivative of specific force wrt airspeed along the X axis diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index cc749e7b01..2f5352ed70 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -100,8 +100,8 @@ void Ekf::resetStates() { _state.vel.setZero(); _state.pos.setZero(); - _state.gyro_bias.setZero(); - _state.accel_bias.setZero(); + _state.delta_ang_bias.setZero(); + _state.delta_vel_bias.setZero(); _state.mag_I.setZero(); _state.mag_B.setZero(); _state.wind_vel.setZero(); @@ -241,8 +241,8 @@ bool Ekf::initialiseFilter() // Zero all of the states _state.vel.setZero(); _state.pos.setZero(); - _state.gyro_bias.setZero(); - _state.accel_bias.setZero(); + _state.delta_ang_bias.setZero(); + _state.delta_vel_bias.setZero(); _state.mag_I.setZero(); _state.mag_B.setZero(); _state.wind_vel.setZero(); @@ -325,8 +325,8 @@ void Ekf::predictState() } // apply imu bias corrections - Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.gyro_bias; - Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.accel_bias; + Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.delta_ang_bias; + Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias; // correct delta angles for earth rotation rate corrected_delta_ang -= -_R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; @@ -464,7 +464,7 @@ void Ekf::calculateOutputStates() const float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg; // Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon - const Vector3f delta_angle{imu.delta_ang - _state.gyro_bias * dt_scale_correction + _delta_angle_corr}; + const Vector3f delta_angle{imu.delta_ang - _state.delta_ang_bias * dt_scale_correction + _delta_angle_corr}; // calculate a yaw change about the earth frame vertical const float spin_del_ang_D = _R_to_earth_now(2, 0) * delta_angle(0) + @@ -491,7 +491,7 @@ void Ekf::calculateOutputStates() _R_to_earth_now = Dcmf(_output_new.quat_nominal); // correct delta velocity for bias offsets - const Vector3f delta_vel{imu.delta_vel - _state.accel_bias * dt_scale_correction}; + const Vector3f delta_vel{imu.delta_vel - _state.delta_vel_bias * dt_scale_correction}; // rotate the delta velocity to earth frame Vector3f delta_vel_NED{_R_to_earth_now * delta_vel}; @@ -681,7 +681,7 @@ Quatf Ekf::calculate_quaternion() const { // Correct delta angle data for bias errors using bias state estimates from the EKF and also apply // corrections required to track the EKF quaternion states - const Vector3f delta_angle{_imu_sample_new.delta_ang - _state.gyro_bias * (_dt_imu_avg / _dt_ekf_avg) + _delta_angle_corr}; + const Vector3f delta_angle{_imu_sample_new.delta_ang - _state.delta_ang_bias * (_dt_imu_avg / _dt_ekf_avg) + _delta_angle_corr}; // increment the quaternions using the corrected delta angle vector // the quaternions must always be normalised after modification diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 0a086dfacc..e7361e934c 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -805,11 +805,11 @@ void Ekf::constrainStates() } for (int i = 0; i < 3; i++) { - _state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -math::radians(20.f) * _dt_ekf_avg, math::radians(20.f) * _dt_ekf_avg); + _state.delta_ang_bias(i) = math::constrain(_state.delta_ang_bias(i), -math::radians(20.f) * _dt_ekf_avg, math::radians(20.f) * _dt_ekf_avg); } for (int i = 0; i < 3; i++) { - _state.accel_bias(i) = math::constrain(_state.accel_bias(i), -_params.acc_bias_lim * _dt_ekf_avg, _params.acc_bias_lim * _dt_ekf_avg); + _state.delta_vel_bias(i) = math::constrain(_state.delta_vel_bias(i), -_params.acc_bias_lim * _dt_ekf_avg, _params.acc_bias_lim * _dt_ekf_avg); } for (int i = 0; i < 3; i++) { @@ -953,7 +953,7 @@ void Ekf::getMagInnovRatio(float &mag_innov_ratio) void Ekf::getDragInnov(float drag_innov[2]) { - memcpy(&drag_innov, _drag_innov, sizeof(_drag_innov)); + memcpy(drag_innov, _drag_innov, sizeof(_drag_innov)); } void Ekf::getDragInnovVar(float drag_innov_var[2]) @@ -1033,11 +1033,11 @@ void Ekf::get_state_delayed(float *state) } for (int i = 0; i < 3; i++) { - state[i + 10] = _state.gyro_bias(i); + state[i + 10] = _state.delta_ang_bias(i); } for (int i = 0; i < 3; i++) { - state[i + 13] = _state.accel_bias(i); + state[i + 13] = _state.delta_vel_bias(i); } for (int i = 0; i < 3; i++) { @@ -1057,9 +1057,9 @@ void Ekf::get_state_delayed(float *state) void Ekf::get_accel_bias(float bias[3]) { float temp[3]; - temp[0] = _state.accel_bias(0) / _dt_ekf_avg; - temp[1] = _state.accel_bias(1) / _dt_ekf_avg; - temp[2] = _state.accel_bias(2) / _dt_ekf_avg; + temp[0] = _state.delta_vel_bias(0) / _dt_ekf_avg; + temp[1] = _state.delta_vel_bias(1) / _dt_ekf_avg; + temp[2] = _state.delta_vel_bias(2) / _dt_ekf_avg; memcpy(bias, temp, 3 * sizeof(float)); } @@ -1067,9 +1067,9 @@ void Ekf::get_accel_bias(float bias[3]) void Ekf::get_gyro_bias(float bias[3]) { float temp[3]; - temp[0] = _state.gyro_bias(0) / _dt_ekf_avg; - temp[1] = _state.gyro_bias(1) / _dt_ekf_avg; - temp[2] = _state.gyro_bias(2) / _dt_ekf_avg; + temp[0] = _state.delta_ang_bias(0) / _dt_ekf_avg; + temp[1] = _state.delta_ang_bias(1) / _dt_ekf_avg; + temp[2] = _state.delta_ang_bias(2) / _dt_ekf_avg; memcpy(bias, temp, 3 * sizeof(float)); } @@ -1250,8 +1250,8 @@ bool Ekf::reset_imu_bias() } // Zero the delta angle and delta velocity bias states - _state.gyro_bias.zero(); - _state.accel_bias.zero(); + _state.delta_ang_bias.zero(); + _state.delta_vel_bias.zero(); // Zero the corresponding covariances zeroCols(P, 10, 15); @@ -1342,11 +1342,11 @@ void Ekf::fuse(float *K, float innovation) } for (unsigned i = 0; i < 3; i++) { - _state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 10] * innovation; + _state.delta_ang_bias(i) = _state.delta_ang_bias(i) - K[i + 10] * innovation; } for (unsigned i = 0; i < 3; i++) { - _state.accel_bias(i) = _state.accel_bias(i) - K[i + 13] * innovation; + _state.delta_vel_bias(i) = _state.delta_vel_bias(i) - K[i + 13] * innovation; } for (unsigned i = 0; i < 3; i++) { diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index a3430ac70e..2150cb3257 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -307,5 +307,3 @@ void Ekf::getTerrainVertPos(float *ret) { memcpy(ret, &_terrain_vpos, sizeof(float)); } - -}