diff --git a/EKF/common.h b/EKF/common.h index 78e2b1e0c6..acfd491a64 100644 --- a/EKF/common.h +++ b/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; diff --git a/EKF/control.cpp b/EKF/control.cpp index 28d8cc9465..c306aea692 100644 --- a/EKF/control.cpp +++ b/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) { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 5aa691f325..0b9aeea5fe 100644 --- a/EKF/ekf.cpp +++ b/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 diff --git a/EKF/ekf.h b/EKF/ekf.h index 8b53c20a04..1f59b1d525 100644 --- a/EKF/ekf.h +++ b/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 diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 5be79c3b3f..286475f04b 100644 --- a/EKF/terrain_estimator.cpp +++ b/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); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index c537ba9ea2..7c3459de86 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/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);