EKF: Add arbitrary pitch offset for range sensor

EKF/common.h

@@ -234,6 +234,7 @@ struct parameters {
 	float range_noise;		// observation noise for range finder measurements (m)
 	float range_innov_gate;		// range finder fusion innovation consistency gate size (STD)
 	float rng_gnd_clearance;	// minimum valid value for range when on ground (m)
+	float rng_sens_pitch;		// Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
 
 	// vision position fusion
 	float ev_innov_gate;		// vision estimator fusion innovation consistency gate size (STD)
@@ -337,6 +338,7 @@ struct parameters {
 		range_noise = 0.1f;
 		range_innov_gate = 5.0f;
 		rng_gnd_clearance = 0.1f;
+		rng_sens_pitch = 0.0f;
 
 		// vision position fusion
 		ev_innov_gate = 5.0f;

EKF/control.cpp

@@ -83,8 +83,12 @@ void Ekf::controlFusionModes()
 	_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
 	_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
 	_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
+
+	// calculate 2,2 element of rotation matrix from sensor frame to earth frame
+	_R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng;
 	_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
-			&& (_R_to_earth(2, 2) > 0.7071f);
+			&& (_R_rng_to_earth_2_2 > 0.7071f);
+
 	_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
 			&&  (_R_to_earth(2, 2) > 0.7071f);
 	_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
@@ -244,7 +248,7 @@ void Ekf::controlOpticalFlowFusion()
 					float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
 
 					// calculate absolute distance from focal point to centre of frame assuming a flat earth
-					float range = heightAboveGndEst / _R_to_earth(2, 2);
+					float range = heightAboveGndEst / _R_rng_to_earth_2_2;
 
 					if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
 						// we should have reliable OF measurements so
@@ -738,7 +742,7 @@ void Ekf::controlRangeFinderFusion()
 		// correct the range data for position offset relative to the IMU
 		Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
 		Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-		_range_sample_delayed.rng += pos_offset_earth(2) / _R_to_earth(2, 2);
+		_range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
 
 		// always fuse available range finder data into a terrain height estimator if the estimator has been initialised
 		if (_terrain_initialised) {

EKF/ekf.cpp

@@ -181,6 +181,8 @@ bool Ekf::init(uint64_t timestamp)
 
 	_filter_initialised = false;
 	_terrain_initialised = false;
+	_sin_tilt_rng = sinf(_params.rng_sens_pitch);
+	_cos_tilt_rng = cosf(_params.rng_sens_pitch);
 
 	_control_status.value = 0;
 	_control_status_prev.value = 0;
@@ -393,8 +395,8 @@ bool Ekf::initialiseFilter()
 			// so it can be used as a backup ad set the initial height using the range finder
 			baroSample baro_newest = _baro_buffer.get_newest();
 			_baro_hgt_offset = baro_newest.hgt;
-			_state.pos(2) = -math::max(_rng_filt_state * _R_to_earth(2, 2),_params.rng_gnd_clearance);
-
+			_state.pos(2) = -math::max(_rng_filt_state * _R_rng_to_earth_2_2,_params.rng_gnd_clearance);
+			ECL_INFO("EKF using range finder height - commencing alignment");
 		} else if (_control_status.flags.ev_hgt) {
 			// if we are using external vision data for height, then the vertical position state needs to be reset
 			// because the initialisation position is not the zero datum

EKF/ekf.h

@@ -315,6 +315,9 @@ private:
 	float _hagl_innov_var;		// innovation variance for the last height above terrain measurement (m^2)
 	uint64_t _time_last_hagl_fuse;	// last system time in usec that the hagl measurement failed it's checks
 	bool _terrain_initialised;	// true when the terrain estimator has been intialised
+	float _sin_tilt_rng;		// sine of the range finder tilt rotation about the Y body axis
+	float _cos_tilt_rng;		// cosine of the range finder tilt rotation about the Y body axis
+	float _R_rng_to_earth_2_2;	// 2,2 element of the rotation matrix from sensor frame to earth frame
 
 	// height sensor fault status
 	bool _baro_hgt_faulty;		// true if valid baro data is unavailable for use

EKF/terrain_estimator.cpp

@@ -47,9 +47,9 @@ bool Ekf::initHagl()
 	// get most recent range measurement from buffer
 	rangeSample latest_measurement = _range_buffer.get_newest();
 
-	if ((_time_last_imu - latest_measurement.time_us) < 2e5 && _R_to_earth(2,2) > 0.7071f) {
+	if ((_time_last_imu - latest_measurement.time_us) < 2e5 && _R_rng_to_earth_2_2 > 0.7071f) {
 		// if we have a fresh measurement, use it to initialise the terrain estimator
-		_terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_to_earth(2, 2);
+		_terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2;
 		// initialise state variance to variance of measurement
 		_terrain_var = sq(_params.range_noise);
 		// success
@@ -88,9 +88,9 @@ void Ekf::predictHagl()
 void Ekf::fuseHagl()
 {
 	// If the vehicle is excessively tilted, do not try to fuse range finder observations
-	if (_R_to_earth(2, 2) > 0.7071f) {
+	if (_R_rng_to_earth_2_2 > 0.7071f) {
 		// get a height above ground measurement from the range finder assuming a flat earth
-		float meas_hagl = _range_sample_delayed.rng * _R_to_earth(2, 2);
+		float meas_hagl = _range_sample_delayed.rng * _R_rng_to_earth_2_2;
 
 		// predict the hagl from the vehicle position and terrain height
 		float pred_hagl = _terrain_vpos - _state.pos(2);
@@ -99,7 +99,7 @@ void Ekf::fuseHagl()
 		_hagl_innov = pred_hagl - meas_hagl;
 
 		// calculate the observation variance adding the variance of the vehicles own height uncertainty and factoring in the effect of tilt on measurement error
-		float obs_variance = fmaxf(P[9][9], 0.0f) + sq(_params.range_noise / _R_to_earth(2, 2));
+		float obs_variance = fmaxf(P[9][9], 0.0f) + sq(_params.range_noise / _R_rng_to_earth_2_2);
 
 		// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
 		_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);

EKF/vel_pos_fusion.cpp

@@ -155,10 +155,10 @@ void Ekf::fuseVelPosHeight()
 			// innovation gate size
 			gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
 
-		} else if (_control_status.flags.rng_hgt && (_R_to_earth(2, 2) > 0.7071f)) {
+		} else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > 0.7071f)) {
 			fuse_map[5] = true;
 			// use range finder with tilt correction
-			_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_to_earth(2, 2),
+			_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
 							     _params.rng_gnd_clearance));
 			// observation variance - user parameter defined
 			R[5] = fmaxf(_params.range_noise, 0.01f);