void Ekf::fuseHagl()
{
// If the vehicle is excessively tilted, do not try to fuse range finder observations
if (_R_prev(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_prev(2, 2);
// predict the hagl from the vehicle position and terrain height
float pred_hagl = _terrain_vpos - _state.pos(2);
// calculate the innovation
_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[8][8], 0.0f) + sq(_params.range_noise / _R_prev(2, 2));
// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
// perform an innovation consistency check and only fuse data if it passes
float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
float test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
if (test_ratio <= 1.0f) {
// calculate the Kalman gain
float gain = _terrain_var / _hagl_innov_var;
// correct the state
_terrain_vpos -= gain * _hagl_innov;
// correct the variance
_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
// record last successful fusion time
_time_last_hagl_fuse = _time_last_imu;
}
} else {
return;
}
}
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;
}
}
