EKF: Improvements to covariance reset

EKF/control.cpp

@@ -291,18 +291,12 @@ void Ekf::controlFusionModes()
 			// check if baro data is available
 			baroSample baro_init = _baro_buffer.get_newest();
 			bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
-			// check if baro data is consistent
-			float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
-			bool baro_data_consistent = sq(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate);
 
-			// reset to baro if data is available and we have no range data
+			// reset to baro if we have no range data and baro data is available
 			bool reset_to_baro = !rng_data_available && baro_data_available;
 
-			// reset to baro if data is acceptable
-			reset_to_baro = reset_to_baro || (baro_data_consistent && baro_data_available && !_baro_hgt_faulty);
-
-			// reset to range data if it is available and we cannot switch to baro
-			bool reset_to_rng = !reset_to_baro && rng_data_available;
+			// reset to range data if it is available
+			bool reset_to_rng = rng_data_available;
 
 			if (reset_to_baro) {
 				// set height sensor health

EKF/covariance.cpp

@@ -732,42 +732,42 @@ void Ekf::limitCov()
 
 	for (int i = 0; i <= 3; i++) {
 
-		math::constrain(P[i][i], 0.0f, P_lim[0]);
+		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[0]);
 	}
 
 	for (int i = 4; i <= 6; i++) {
 
-		math::constrain(P[i][i], 0.0f, P_lim[1]);
+		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[1]);
 	}
 
 	for (int i = 7; i <= 9; i++) {
 
 
-		math::constrain(P[i][i], 0.0f, P_lim[2]);
+		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[2]);
 	}
 
 	for (int i = 10; i <= 12; i++) {
 
 
-		math::constrain(P[i][i], 0.0f, P_lim[3]);
+		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[3]);
 	}
 
 	for (int i = 13; i <= 15; i++) {
 
 
-		math::constrain(P[i][i], 0.0f, P_lim[4]);
+		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[4]);
 	}
 
 
 	for (int i = 16; i <= 18; i++) {
-		math::constrain(P[i][i], 0.0f, P_lim[5]);
+		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[5]);
 	}
 
 	for (int i = 19; i <= 21; i++) {
-		math::constrain(P[i][i], 0.0f, P_lim[6]);
+		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[6]);
 	}
 
 	for (int i = 22; i <= 23; i++) {
-		math::constrain(P[i][i], 0.0f, P_lim[7]);
+		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[7]);
 	}
 }

EKF/ekf.cpp

@@ -263,14 +263,17 @@ bool Ekf::update()
 		}
 
 		// determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled
+		uint64_t last_baro_time_us = _baro_sample_delayed.time_us;
 		if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
 			if (_control_status.flags.baro_hgt) {
 				_fuse_height = true;
 
 			} else {
 				// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro  as a height reference
-				float offset_error =  _state.pos(2) + _baro_sample_delayed.hgt - _hgt_sensor_offset - _baro_hgt_offset;
-				_baro_hgt_offset += 0.02f * math::constrain(offset_error, -5.0f, 5.0f);
+				float dt = math::constrain(1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us),0.0f,1.0f);
+				last_baro_time_us = _baro_sample_delayed.time_us;
+				float raw_offset_error =  (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset;
+				_baro_hgt_offset += dt * math::constrain(0.1f * raw_offset_error, -0.1f, 0.1f);
 			}
 		}
 

EKF/ekf_helper.cpp

@@ -135,7 +135,7 @@ void Ekf::resetHeight()
 			zeroRows(P, 9, 9);
 			zeroCols(P, 9, 9);
 
-			// the state variance is th esame as the observation
+			// the state variance is the same as the observation
 			P[9][9] = sq(_params.baro_noise);
 
 			vert_pos_reset = true;
@@ -185,7 +185,7 @@ void Ekf::resetHeight()
 
 		// Set the variance to a value large enough to allow the state to converge quickly
 		// that does not destabilise the filter
-		P[6][6] = fminf(sq(_state.vel(2)),100.0f);
+		P[6][6] = 10.0f;
 
 	}
 	vert_vel_reset = true;

EKF/optflow_fusion.cpp

@@ -438,7 +438,7 @@ void Ekf::fuseOptFlow()
 		// first calculate expression for KHP
 		// then calculate P - KHP
 		for (unsigned row = 0; row < _k_num_states; row++) {
-			for (unsigned column = 0; column <= 5; column++) {
+			for (unsigned column = 0; column <= 6; column++) {
 				KH[row][column] = gain[row] * H_LOS[obs_index][column];
 			}
 		}