EKF: rework initialisation of height state and offsets

EKF/ekf.cpp

@@ -74,7 +74,7 @@ Ekf::Ekf():
 	_last_gps_origin_time_us(0),
 	_gps_alt_ref(0.0f),
 	_hgt_counter(0),
-	_hgt_filt_state(0.0f),
+	_rng_filt_state(0.0f),
 	_mag_counter(0),
 	_time_last_mag(0),
 	_hgt_sensor_offset(0.0f),
@@ -388,12 +388,12 @@ bool Ekf::initialiseFilter(void)
 				_control_status.flags.baro_hgt = false;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = true;
-				_hgt_filt_state = _range_sample_delayed.rng;
+				_rng_filt_state = _range_sample_delayed.rng;
 				_hgt_counter = 1;
 			} else if (_hgt_counter != 0) {
 				// increment the sample count and apply a LPF to the measurement
 				_hgt_counter ++;
-				_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _range_sample_delayed.rng;
+				_rng_filt_state = 0.9f * _rng_filt_state + 0.1f * _range_sample_delayed.rng;
 			}
 		}
 
@@ -406,12 +406,12 @@ bool Ekf::initialiseFilter(void)
 				_control_status.flags.baro_hgt = true;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = false;
-				_hgt_filt_state = _baro_sample_delayed.hgt;
+				_baro_hgt_offset = _baro_sample_delayed.hgt;
 				_hgt_counter = 1;
 			} else if (_hgt_counter != 0) {
 				// increment the sample count and apply a LPF to the measurement
 				_hgt_counter ++;
-				_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _baro_sample_delayed.hgt;
+				_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
 			}
 		}
 
@@ -469,17 +469,12 @@ bool Ekf::initialiseFilter(void)
 		// calculate the initial magnetic field and yaw alignment
 		resetMagHeading(mag_init);
 
-		// calculate the averaged height reading to calulate the height of the origin
-		_hgt_sensor_offset = _hgt_filt_state;
-
-		// if we are not using the baro height as the primary source, then calculate an offset relative to the origin
-		// so it can be used as a backup
-		if (!_control_status.flags.baro_hgt) {
+		// if we are using the range finder as the primary source, then calculate the baro height at origin so  we can use baro as a backup
+		// so it can be used as a backup ad set the initial height using the range finder
+		if (_control_status.flags.rng_hgt) {
 			baroSample baro_newest = _baro_buffer.get_newest();
-			_baro_hgt_offset = baro_newest.hgt - _hgt_sensor_offset;
-
-		} else {
-			_baro_hgt_offset = 0.0f;
+			_baro_hgt_offset = baro_newest.hgt;
+			_state.pos(2) = -math::max(_rng_filt_state * _R_to_earth(2, 2),_params.rng_gnd_clearance);
 		}
 
 		// initialise the state covariance matrix

EKF/ekf.h

@@ -172,7 +172,7 @@ private:
 	float _mag_innov_var[3];	// earth magnetic field innovation variance
 
 	float _airspeed_innov;		// airspeed measurement innovation
-	float _airspeed_innov_var;  // airspeed measurement innovation variance
+	float _airspeed_innov_var;	// airspeed measurement innovation variance
 
 	float _heading_innov;		// heading measurement innovation
 	float _heading_innov_var;	// heading measurement innovation variance
@@ -211,12 +211,12 @@ private:
 
 	// Variables used to initialise the filter states
 	uint32_t _hgt_counter;		// number of height samples taken
-	float _hgt_filt_state;		// filtered height measurement
+	float _rng_filt_state;		// filtered height measurement
 	uint32_t _mag_counter;		// number of magnetometer samples taken
 	uint64_t _time_last_mag;	// measurement time of last magnetomter sample
 	Vector3f _mag_filt_state;	// filtered magnetometer measurement
 	Vector3f _delVel_sum;		// summed delta velocity
-	float _hgt_sensor_offset;	// height that needs to be subtracted from the primary height sensor so that it reads zero height at the origin (m)
+	float _hgt_sensor_offset;	// value subtracted from the height measurement to maintain consistency after switching height sensors (m)
 
 	gps_check_fail_status_u _gps_check_fail_status;
 
@@ -234,7 +234,7 @@ private:
 	bool _rng_hgt_faulty;		// true if valid rnage finder height data is unavailable for use
 	int _primary_hgt_source;	// priary source of height data set at initialisation
 
-	float _baro_hgt_offset;		// number of metres the baro height origin is above the local NED origin (m)
+	float _baro_hgt_offset;		// baro height reading at the local NED origin (m)
 	float _vert_pos_reset_delta;	// increase in vertical position state at the last reset(m)
 	uint64_t _time_vert_pos_reset;	// last system time in usec that the vertical position state was reset
 	float _vert_vel_reset_delta;	// increase in vertical position velocity at the last reset(m)