Rename IMU biases

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

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);
 		}
 

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) {

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

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

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++) {

EKF/terrain_estimator.cpp

@@ -307,5 +307,3 @@ void Ekf::getTerrainVertPos(float *ret)
 {
 	memcpy(ret, &_terrain_vpos, sizeof(float));
 }
-
-}