Log height innovation sensor specific

EKF/control.cpp

@@ -1163,73 +1163,83 @@ void Ekf::controlHeightFusion()
 
 	if (_fuse_height) {
 
-		Vector2f height_innov_gate{};
-		Vector2f height_test_ratio{};
-		Vector3f height_obs_var{};
-		Vector3f height_innov_var{};
-		Vector3f height_innov{};
+
 
 		if (_control_status.flags.baro_hgt) {
+			Vector2f baro_hgt_innov_gate{};
+			Vector3f baro_hgt_obs_var{};
+
 			// vertical position innovation - baro measurement has opposite sign to earth z axis
-			height_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
+			_baro_hgt_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
 			// observation variance - user parameter defined
-			height_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
-
+			baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
 			// innovation gate size
-			height_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
+			baro_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
 
 			// Compensate for positive static pressure transients (negative vertical position innovations)
 			// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
 			float deadzone_start = 0.0f;
 			float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
 
 			if (_control_status.flags.gnd_effect) {
-				if (height_innov(2) < -deadzone_start) {
-					if (height_innov(2) <= -deadzone_end) {
-						height_innov(2) += deadzone_end;
+				if (_baro_hgt_innov(2) < -deadzone_start) {
+					if (_baro_hgt_innov(2) <= -deadzone_end) {
+						_baro_hgt_innov(2) += deadzone_end;
 
 					} else {
-						height_innov(2) = -deadzone_start;
+						_baro_hgt_innov(2) = -deadzone_start;
 					}
 				}
 			}
+			// fuse height information
+			fuseVerticalPosition(_baro_hgt_innov,baro_hgt_innov_gate,
+				baro_hgt_obs_var, _baro_hgt_innov_var,_baro_hgt_test_ratio);
 
 		} else if (_control_status.flags.gps_hgt) {
+			Vector2f gps_hgt_innov_gate{};
+			Vector3f gps_hgt_obs_var{};
 			// vertical position innovation - gps measurement has opposite sign to earth z axis
-			height_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
+			_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
 			// observation variance - receiver defined and parameter limited
 			// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
 			float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
 			float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
-			height_obs_var(2) = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
+			gps_hgt_obs_var(2) = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
 			// innovation gate size
-			height_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
+			gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
+			// fuse height information
+			fuseVerticalPosition(_gps_pos_innov,gps_hgt_innov_gate,
+				gps_hgt_obs_var, _gps_pos_innov_var,_gps_pos_test_ratio);
 
 		} else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) {
+			// TODO: Tilt check does not belong here, should not set fuse height to true if tilted
+			Vector2f rng_hgt_innov_gate{};
+			Vector3f rng_hgt_obs_var{};
 			// use range finder with tilt correction
-			height_innov(2) = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
+			_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
 							 _params.rng_gnd_clearance)) - _hgt_sensor_offset;
 			// observation variance - user parameter defined
-			height_obs_var(2) = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f);
+			rng_hgt_obs_var(2) = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f);
 			// innovation gate size
-			height_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
+			rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
+			// fuse height information
+			fuseVerticalPosition(_rng_hgt_innov,rng_hgt_innov_gate,
+				rng_hgt_obs_var, _rng_hgt_innov_var,_rng_hgt_test_ratio);
 
 		} else if (_control_status.flags.ev_hgt) {
+			Vector2f ev_hgt_innov_gate{};
+			Vector3f ev_hgt_obs_var{};
 			// calculate the innovation assuming the external vision observation is in local NED frame
-			height_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
+			_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
 			// observation variance - defined externally
-			height_obs_var(2) = sq(fmaxf(_ev_sample_delayed.hgtErr, 0.01f));
+			ev_hgt_obs_var(2) = sq(fmaxf(_ev_sample_delayed.hgtErr, 0.01f));
 			// innovation gate size
-			height_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
+			ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
+			// fuse height information
+			fuseVerticalPosition(_ev_pos_innov,ev_hgt_innov_gate,
+				ev_hgt_obs_var, _ev_pos_innov_var,_ev_pos_test_ratio);
 		}
-		// fuse height inforamtion
-		fuseVerticalPosition(height_innov,height_innov_gate,
-				height_obs_var, height_innov_var,height_test_ratio);
-
-		// This is a temporary hack until we do proper height sensor fusion
-		_gps_pos_innov(2) = height_innov(2);
-		_gps_pos_innov_var(2) = height_innov_var(2);
-		_gps_pos_test_ratio(1) = height_test_ratio(1);
+
 	}
 
 }

EKF/ekf.h

@@ -75,6 +75,18 @@ class Ekf : public EstimatorInterface
 
 	void getEvVelPosInnovRatio(float& hvel, float &vvel, float& hpos, float &vpos) override;
 
+	void getBaroHgtInnov(float &baro_hgt_innov) override;
+
+	void getBaroHgtInnovVar(float &baro_hgt_innov_var) override;
+
+	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) override;
+
+	void getRngHgtInnov(float &rng_hgt_innov) override;
+
+	void getRngHgtInnovVar(float &rng_hgt_innov_var) override;
+
+	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) override;
+
 	void getAuxVelInnov(float aux_vel_innov[2]) override;
 
 	void getAuxVelInnovVar(float aux_vel_innov[2]) override;
@@ -387,6 +399,12 @@ class Ekf : public EstimatorInterface
 	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)
+
+	Vector3f _rng_hgt_innov {};	///< range hgt innovations (m)
+	Vector3f _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)
 

EKF/ekf_helper.cpp

@@ -889,6 +889,36 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp
 	vpos = _ev_pos_test_ratio(1);
 }
 
+void Ekf::getBaroHgtInnov(float &baro_hgt_innov)
+{
+	baro_hgt_innov = _baro_hgt_innov(2);
+}
+
+void Ekf::getBaroHgtInnovVar(float &baro_hgt_innov_var)
+{
+	baro_hgt_innov_var = _baro_hgt_innov_var(2);
+}
+
+void Ekf::getBaroHgtInnovRatio(float &baro_hgt_innov_ratio)
+{
+	baro_hgt_innov_ratio = _baro_hgt_test_ratio(1);
+}
+
+void Ekf::getRngHgtInnov(float &rng_hgt_innov)
+{
+	rng_hgt_innov = _rng_hgt_innov(2);
+}
+
+void Ekf::getRngHgtInnovVar(float &rng_hgt_innov_var)
+{
+	rng_hgt_innov_var = _rng_hgt_innov_var(2);
+}
+
+void Ekf::getRngHgtInnovRatio(float &rng_hgt_innov_ratio)
+{
+	rng_hgt_innov_ratio = _rng_hgt_test_ratio(1);
+}
+
 void Ekf::getAuxVelInnov(float aux_vel_innov[2])
 {
 	aux_vel_innov[0] = _aux_vel_innov(0);

EKF/estimator_interface.h

@@ -75,6 +75,14 @@ class EstimatorInterface
 	virtual void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0;
 	virtual void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) = 0;
 
+	virtual void getBaroHgtInnov(float &baro_hgt_innov) = 0;
+	virtual void getBaroHgtInnovVar(float &baro_hgt_innov_var) = 0;
+	virtual void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) = 0;
+
+	virtual void getRngHgtInnov(float &rng_hgt_innov) = 0;
+	virtual void getRngHgtInnovVar(float &rng_hgt_innov_var) = 0;
+	virtual void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) = 0;
+
 	virtual void getAuxVelInnov(float aux_vel_innov[2]) = 0;
 	virtual void getAuxVelInnovVar(float aux_vel_innov[2]) = 0;
 	virtual void getAuxVelInnovRatio(float &aux_vel_innov_ratio) = 0;
@@ -479,6 +487,8 @@ class EstimatorInterface
 	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{};		// Auxiliray 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 _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