void Ekf::controlRangeFinderFusion()
{
// determine if we should use range finder data for height
if (_range_data_ready) {
// set the height data source to range if requested
if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false;
}
// 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);
// always fuse available range finder data into a terrain height estimator if the estimator has been initialised
if (_terrain_initialised) {
fuseHagl();
}
// only use range finder as a height observation in the main filter if specifically enabled
if (_control_status.flags.rng_hgt) {
_fuse_height = true;
}
} else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) {
// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
if (!_control_status.flags.in_air) {
_range_sample_delayed.rng = _params.rng_gnd_clearance;
_range_sample_delayed.time_us = _imu_sample_delayed.time_us;
}
_fuse_height = true;
}
}
bool Ekf::update()
{
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
if (!_filter_initialised) {
return false;
}
}
// Only run the filter if IMU data in the buffer has been updated
if (_imu_updated) {
// perform state and covariance prediction for the main filter
predictState();
predictCovariance();
// perform state and variance prediction for the terrain estimator
if (!_terrain_initialised) {
_terrain_initialised = initHagl();
} else {
predictHagl();
}
// control logic
controlFusionModes();
// measurement updates
// Fuse magnetometer data using the selected fuson method and only if angular alignment is complete
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
fuseMag();
if (_control_status.flags.mag_dec) {
fuseDeclination();
}
} else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) {
fuseMag2D();
} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
// fusion of a Euler yaw angle from either a 321 or 312 rotation sequence
fuseHeading();
} else {
// do no fusion at all
}
}
// determine if range finder data has fallen behind the fusin time horizon fuse it if we are
// not tilted too much to use it
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
&& (_R_prev(2, 2) > 0.7071f)) {
// if we have range data we always try to estimate terrain height
_fuse_hagl_data = true;
// only use range finder as a height observation in the main filter if specifically enabled
if (_control_status.flags.rng_hgt) {
_fuse_height = true;
}
} else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) {
// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
if (!_in_air) {
_range_sample_delayed.rng = _params.rng_gnd_clearance;
_range_sample_delayed.time_us = _imu_sample_delayed.time_us;
}
_fuse_height = true;
}
// determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
if (_control_status.flags.baro_hgt) {
_fuse_height = true;
} else {
// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
float offset_error = _state.pos(2) + _baro_sample_delayed.hgt - _hgt_sensor_offset - _baro_hgt_offset;
_baro_hgt_offset += 0.02f * math::constrain(offset_error, -5.0f, 5.0f);
}
}
// If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it
if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
// Only use GPS data for position and velocity aiding if enabled
if (_control_status.flags.gps) {
_fuse_pos = true;
_fuse_vert_vel = true;
_fuse_hor_vel = true;
}
// only use gps height observation in the main filter if specifically enabled
if (_control_status.flags.gps_hgt) {
_fuse_height = true;
}
}
// If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it
if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
&& _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5
&& (_R_prev(2, 2) > 0.7071f)) {
_fuse_flow = true;
}
// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
if (!_control_status.flags.gps && !_control_status.flags.opt_flow
&& ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
_fuse_pos = true;
_gps_sample_delayed.pos(0) = _last_known_posNE(0);
_gps_sample_delayed.pos(1) = _last_known_posNE(1);
_time_last_fake_gps = _time_last_imu;
}
// fuse available range finder data into a terrain height estimator if it has been initialised
if (_fuse_hagl_data && _terrain_initialised) {
fuseHagl();
_fuse_hagl_data = false;
}
// Fuse available NED velocity and position data into the main filter
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight();
_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
}
// Update optical flow bias estimates
calcOptFlowBias();
// Fuse optical flow LOS rate observations into the main filter
if (_fuse_flow) {
fuseOptFlow();
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_fuse_flow = false;
}
// TODO This is just to get the logic inside but we will only start fusion once we tested this again
if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed) && false) {
fuseAirspeed();
}
}
// the output observer always runs
calculateOutputStates();
// check for NaN on attitude states
if (isnan(_state.quat_nominal(0)) || isnan(_output_new.quat_nominal(0))) {
return false;
}
// We don't have valid data to output until tilt and yaw alignment is complete
if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
return true;
} else {
return false;
}
}