ekf2: don't use Vectors for height innovations (baro, rng, etc)

src/modules/ekf2/EKF/covariance.cpp

@@ -993,8 +993,8 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 				     && ((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 && _control_status.flags.gps_hgt)
-					 || (down_dvel_bias * _baro_hgt_innov(2) < 0.0f && _control_status.flags.baro_hgt)
-					 || (down_dvel_bias * _rng_hgt_innov(2) < 0.0f && _control_status.flags.rng_hgt)
+					 || (down_dvel_bias * _baro_hgt_innov < 0.0f && _control_status.flags.baro_hgt)
+					 || (down_dvel_bias * _rng_hgt_innov < 0.0f && _control_status.flags.rng_hgt)
 					 || (down_dvel_bias * _ev_pos_innov(2) < 0.0f && _control_status.flags.ev_hgt)));
 
 		// record the pass/fail

src/modules/ekf2/EKF/ekf.h

@@ -78,13 +78,13 @@ public:
 	void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
 	void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
 
-	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov(2); }
-	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var(2); }
-	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio(1); }
+	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov; }
+	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var; }
+	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio; }
 
-	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov(2); }
-	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var(2); }
-	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); }
+	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov; }
+	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var; }
+	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio; }
 
 	void getAuxVelInnov(float aux_vel_innov[2]) const;
 	void getAuxVelInnovVar(float aux_vel_innov[2]) const;
@@ -440,11 +440,11 @@ private:
 	Vector3f _ev_pos_innov{};	///< external vision position innovations (m)
 	Vector3f _ev_pos_innov_var{};	///< external vision position innovation variances (m**2)
 
-	Vector3f _baro_hgt_innov{};		///< baro hgt innovations (m)
-	Vector3f _baro_hgt_innov_var{};	///< baro hgt innovation variances (m**2)
+	float _baro_hgt_innov{};		///< baro hgt innovations (m)
+	float _baro_hgt_innov_var{};	///< baro hgt innovation variances (m**2)
 
-	Vector3f _rng_hgt_innov{};	///< range hgt innovations (m)
-	Vector3f _rng_hgt_innov_var{};	///< range hgt innovation variances (m**2)
+	float _rng_hgt_innov{};	///< range hgt innovations (m)
+	float _rng_hgt_innov_var{};	///< range hgt innovation variances (m**2)
 
 	Vector3f _aux_vel_innov{};	///< horizontal auxiliary velocity innovations: (m/sec)
 	Vector3f _aux_vel_innov_var{};	///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
@@ -672,8 +672,8 @@ private:
 	bool fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
 				    Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate = false);
 
-	bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
-				  Vector3f &innov_var, Vector2f &test_ratio);
+	bool fuseVerticalPosition(const float innov, const float innov_gate, const float obs_var,
+				  float &innov_var, float &test_ratio);
 
 	void fuseGpsVelPos();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -965,13 +965,13 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 
 	// return the vertical position innovation test ratio
 	if (_control_status.flags.baro_hgt) {
-		hgt = math::max(sqrtf(_baro_hgt_test_ratio(1)), FLT_MIN);
+		hgt = math::max(sqrtf(_baro_hgt_test_ratio), FLT_MIN);
 
 	} else if (_control_status.flags.gps_hgt) {
 		hgt = math::max(sqrtf(_gps_pos_test_ratio(1)), FLT_MIN);
 
 	} else if (_control_status.flags.rng_hgt) {
-		hgt = math::max(sqrtf(_rng_hgt_test_ratio(1)), FLT_MIN);
+		hgt = math::max(sqrtf(_rng_hgt_test_ratio), FLT_MIN);
 
 	} else if (_control_status.flags.ev_hgt) {
 		hgt = math::max(sqrtf(_ev_pos_test_ratio(1)), FLT_MIN);

src/modules/ekf2/EKF/estimator_interface.h

@@ -338,8 +338,8 @@ protected:
 	Vector2f _ev_vel_test_ratio{};		// EV velocity innovation consistency check ratios
 	Vector2f _ev_pos_test_ratio{};		// EV position innovation consistency check ratios
 	Vector2f _aux_vel_test_ratio{};		// Auxiliary horizontal velocity innovation consistency check ratio
-	Vector2f _baro_hgt_test_ratio{};	// baro height innovation consistency check ratios
-	Vector2f _rng_hgt_test_ratio{};		// range finder height innovation consistency check ratios
+	float _baro_hgt_test_ratio{};	// baro height innovation consistency check ratios
+	float _rng_hgt_test_ratio{};		// range finder height innovation consistency check ratios
 	float _optflow_test_ratio{};		// Optical flow innovation consistency check ratio
 	float _tas_test_ratio{};		// tas innovation consistency check ratio
 	float _hagl_test_ratio{};		// height above terrain measurement innovation consistency check ratio

src/modules/ekf2/EKF/height_fusion.cpp

@@ -40,16 +40,10 @@
 
 void Ekf::fuseBaroHgt()
 {
-	Vector2f baro_hgt_innov_gate;
-	Vector3f baro_hgt_obs_var;
-
 	// vertical position innovation - baro measurement has opposite sign to earth z axis
 	const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
-	_baro_hgt_innov(2) = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
-	// observation variance - user parameter defined
-	baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
-	// innovation gate size
-	baro_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
+
+	_baro_hgt_innov = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
 
 	// Compensate for positive static pressure transients (negative vertical position innovations)
 	// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
@@ -57,62 +51,69 @@ void Ekf::fuseBaroHgt()
 	const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
 
 	if (_control_status.flags.gnd_effect) {
-		if (_baro_hgt_innov(2) < -deadzone_start) {
-			if (_baro_hgt_innov(2) <= -deadzone_end) {
-				_baro_hgt_innov(2) += deadzone_end;
+		if (_baro_hgt_innov < -deadzone_start) {
+			if (_baro_hgt_innov <= -deadzone_end) {
+				_baro_hgt_innov += deadzone_end;
 
 			} else {
-				_baro_hgt_innov(2) = -deadzone_start;
+				_baro_hgt_innov = -deadzone_start;
 			}
 		}
 	}
 
-	fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate,
-			     baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio);
+	// innovation gate size
+	float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
+
+	// observation variance - user parameter defined
+	float obs_var = sq(fmaxf(_params.baro_noise, 0.01f));
+
+	fuseVerticalPosition(_baro_hgt_innov, innov_gate, obs_var,
+			     _baro_hgt_innov_var, _baro_hgt_test_ratio);
 }
 
 void Ekf::fuseGpsHgt()
 {
-	Vector2f gps_hgt_innov_gate;
-	Vector3f gps_hgt_obs_var;
 	// vertical position innovation - gps measurement has opposite sign to earth z axis
 	_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
-	gps_hgt_obs_var(2) = getGpsHeightVariance();
+
 	// innovation gate size
-	gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
+	float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
 
-	fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate,
-			     gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
+	float obs_var = getGpsHeightVariance();
+
+	// _gps_pos_test_ratio(1) is the vertical test ratio
+	fuseVerticalPosition(_gps_pos_innov(2), innov_gate, obs_var,
+			     _gps_pos_innov_var(2), _gps_pos_test_ratio(1));
 }
 
 void Ekf::fuseRngHgt()
 {
-	Vector2f rng_hgt_innov_gate;
-	Vector3f rng_hgt_obs_var;
 	// use range finder with tilt correction
-	_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
-					     _params.rng_gnd_clearance)) - _hgt_sensor_offset;
-	// observation variance - user parameter defined
-	rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
-				   + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
+	_rng_hgt_innov = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
+					  _params.rng_gnd_clearance)) - _hgt_sensor_offset;
+
 	// innovation gate size
-	rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
+	float innov_gate = fmaxf(_params.range_innov_gate, 1.f);
+
+	// observation variance - user parameter defined
+	float obs_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
 
-	fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate,
-			     rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio);
+	fuseVerticalPosition(_rng_hgt_innov, innov_gate, obs_var,
+			     _rng_hgt_innov_var, _rng_hgt_test_ratio);
 }
 
 void Ekf::fuseEvHgt()
 {
-	Vector2f ev_hgt_innov_gate;
-	Vector3f ev_hgt_obs_var;
 	// calculate the innovation assuming the external vision observation is in local NED frame
 	_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
-	// observation variance - defined externally
-	ev_hgt_obs_var(2) = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
+
 	// innovation gate size
-	ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
+	float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f);
+
+	// observation variance - defined externally
+	float obs_var = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
 
-	fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate,
-			     ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
+	// _ev_pos_test_ratio(1) is the vertical test ratio
+	fuseVerticalPosition(_ev_pos_innov(2), innov_gate, obs_var,
+			     _ev_pos_innov_var(2), _ev_pos_test_ratio(1));
 }

src/modules/ekf2/EKF/vel_pos_fusion.cpp

@@ -146,33 +146,32 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
 	}
 }
 
-bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
-			       Vector3f &innov_var, Vector2f &test_ratio)
+bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const float obs_var,
+			       float &innov_var, float &test_ratio)
 {
-
-	innov_var(2) = P(9, 9) + obs_var(2);
-	test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
-	_vert_pos_innov_ratio = innov(2) / sqrtf(innov_var(2));
+	innov_var = P(9, 9) + obs_var;
+	test_ratio = sq(innov) / (sq(innov_gate) * innov_var);
+	_vert_pos_innov_ratio = innov / sqrtf(innov_var);
 	_vert_pos_fuse_attempt_time_us = _time_last_imu;
-	bool innov_check_pass = test_ratio(1) <= 1.0f;
+	bool innov_check_pass = test_ratio <= 1.0f;
 
 	// if there is bad vertical acceleration data, then don't reject measurement,
 	// but limit innovation to prevent spikes that could destabilise the filter
 	float innovation;
 
 	if (_fault_status.flags.bad_acc_vertical && !innov_check_pass) {
-		const float innov_limit = innov_gate(1) * sqrtf(innov_var(2));
-		innovation = math::constrain(innov(2), -innov_limit, innov_limit);
+		const float innov_limit = innov_gate * sqrtf(innov_var);
+		innovation = math::constrain(innov, -innov_limit, innov_limit);
 		innov_check_pass = true;
 
 	} else {
-		innovation = innov(2);
+		innovation = innov;
 	}
 
 	if (innov_check_pass) {
 		_time_last_hgt_fuse = _time_last_imu;
 		_innov_check_fail_status.flags.reject_ver_pos = false;
-		fuseVelPosHeight(innovation, innov_var(2), 5);
+		fuseVelPosHeight(innovation, innov_var, 5);
 
 		return true;