void Ekf::controlFusionModes()
{
// Store the status to enable change detection
_control_status_prev.value = _control_status.value;
// Get the magnetic declination
calcMagDeclination();
// monitor the tilt alignment
if (!_control_status.flags.tilt_align) {
// whilst we are aligning the tilt, monitor the variances
Vector3f angle_err_var_vec = calcRotVecVariances();
// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
// and declare the tilt alignment complete
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
}
// control use of various external sources for position and velocity aiding
controlExternalVisionAiding();
controlOpticalFlowAiding();
controlGpsAiding();
controlHeightAiding();
controlMagAiding();
}
void Ekf::controlGpsAiding()
{
// GPS fusion mode selection logic
// To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data
if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
if (_control_status.flags.tilt_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised
&& (_time_last_imu - _last_gps_fail_us > 5e6)) {
// If the heading is not aligned, reset the yaw and magnetic field states
if (!_control_status.flags.yaw_align) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// If the heading is valid start using gps aiding
if (_control_status.flags.yaw_align) {
_control_status.flags.gps = true;
_time_last_gps = _time_last_imu;
// if we are not already aiding with optical flow, then we need to reset the position and velocity
if (!_control_status.flags.opt_flow) {
_control_status.flags.gps = resetPosition();
_control_status.flags.gps = resetVelocity();
}
}
}
} else if (!(_params.fusion_mode & MASK_USE_GPS)) {
_control_status.flags.gps = false;
}
// handle the case when we are relying on GPS fusion and lose it
if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
// We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something
if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) {
if (_time_last_imu - _time_last_gps > 5e5) {
// if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states
// and set the synthetic GPS position to the current estimate
_control_status.flags.gps = false;
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
} else {
// Reset states to the last GPS measurement
resetPosition();
resetVelocity();
// Reset the timeout counters
_time_last_pos_fuse = _time_last_imu;
_time_last_vel_fuse = _time_last_imu;
}
}
}
}
void Ekf::controlFusionModes()
{
// Store the status to enable change detection
_control_status_prev.value = _control_status.value;
// Get the magnetic declination
calcMagDeclination();
// monitor the tilt alignment
if (!_control_status.flags.tilt_align) {
// whilst we are aligning the tilt, monitor the variances
Vector3f angle_err_var_vec = calcRotVecVariances();
// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
// and declare the tilt alignment complete
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
ECL_INFO("EKF alignment complete");
}
}
// check for arrival of new sensor data at the fusion time horizon
_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);
_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
&& (_R_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);
_tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
// check for height sensor timeouts and reset and change sensor if necessary
controlHeightSensorTimeouts();
// control use of observations for aiding
controlMagFusion();
controlExternalVisionFusion();
controlOpticalFlowFusion();
controlGpsFusion();
controlBaroFusion();
controlRangeFinderFusion();
controlAirDataFusion();
// for efficiency, fusion of direct state observations for position ad velocity is performed sequentially
// in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc)
controlVelPosFusion();
}
void Ekf::controlMagFusion()
{
// If we are using external vision data for heading then no magnetometer fusion is used
if (_control_status.flags.ev_yaw) {
return;
}
// If we are on ground, store the local position and time to use as a reference
if (!_control_status.flags.in_air) {
_last_on_ground_posD = _state.pos(2);
}
// checs for new magnetometer data tath has fallen beind the fusion time horizon
if (_mag_data_ready) {
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
// or the more accurate 3-axis fusion
if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
// start 3D fusion if in-flight and height has increased sufficiently
// to be away from ground magnetic anomalies
// don't switch back to heading fusion until we are back on the ground
bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f;
bool use_3D_fusion = _control_status.flags.in_air && (_control_status.flags.mag_3D || height_achieved);
if (use_3D_fusion && _control_status.flags.tilt_align) {
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
if (!_control_status.flags.mag_3D) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// use 3D mag fusion when airborne
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = true;
} else {
// use heading fusion when on the ground
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_3D = false;
}
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
// always use heading fusion
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_3D = false;
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
if (!_control_status.flags.mag_3D) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// always use 3-axis mag fusion
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = true;
} else {
// do no magnetometer fusion at all
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
}
// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift
// fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time
if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) {
_control_status.flags.mag_dec = true;
} else {
_control_status.flags.mag_dec = false;
}
// fuse magnetometer data using the selected methods
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_hdg && _control_status.flags.yaw_align) {
// fusion of an Euler yaw angle from either a 321 or 312 rotation sequence
fuseHeading();
} else {
// do no fusion at all
}
}
}
void Ekf::controlGpsFusion()
{
// Check for new GPS data that has fallen behind the fusion time horizon
if (_gps_data_ready) {
// Determine if we should use GPS aiding for velocity and horizontal position
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
if (_control_status.flags.tilt_align && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) {
// If the heading is not aligned, reset the yaw and magnetic field states
if (!_control_status.flags.yaw_align) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// If the heading is valid start using gps aiding
if (_control_status.flags.yaw_align) {
_control_status.flags.gps = true;
_time_last_gps = _time_last_imu;
// if we are not already aiding with optical flow, then we need to reset the position and velocity
if (!_control_status.flags.opt_flow) {
if (resetPosition() && resetVelocity()) {
_control_status.flags.gps = true;
} else {
_control_status.flags.gps = false;
}
}
if (_control_status.flags.gps) {
ECL_INFO("EKF commencing GPS aiding");
}
}
}
} else if (!(_params.fusion_mode & MASK_USE_GPS)) {
_control_status.flags.gps = false;
}
// handle the case when we are relying on GPS fusion and lose it
if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
// We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something
if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) {
if (_time_last_imu - _time_last_gps > 5e5) {
// if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states
// and set the synthetic GPS position to the current estimate
_control_status.flags.gps = false;
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
ECL_WARN("EKF GPS fusion timout - stopping GPS aiding");
} else {
// Reset states to the last GPS measurement
resetPosition();
resetVelocity();
ECL_WARN("EKF GPS fusion timout - resetting to GPS");
// Reset the timeout counters
_time_last_pos_fuse = _time_last_imu;
_time_last_vel_fuse = _time_last_imu;
}
}
}
// 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;
// correct velocity for offset relative to IMU
Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt);
Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body);
Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
_gps_sample_delayed.vel -= vel_offset_earth;
// correct position and height for offset relative to IMU
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_gps_sample_delayed.pos(0) -= pos_offset_earth(0);
_gps_sample_delayed.pos(1) -= pos_offset_earth(1);
_gps_sample_delayed.hgt += pos_offset_earth(2);
}
// Determine if GPS should be used as the height source
if (((_params.vdist_sensor_type == VDIST_SENSOR_GPS)) && !_gps_hgt_faulty) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
_fuse_height = true;
}
}
}
void Ekf::controlOpticalFlowFusion()
{
// Check for new optical flow data that has fallen behind the fusion time horizon
if (_flow_data_ready) {
// optical flow fusion mode selection logic
if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
&& !_control_status.flags.opt_flow // we are not yet using flow data
&& _control_status.flags.tilt_align // we know our tilt attitude
&& (_time_last_imu - _time_last_hagl_fuse) < 5e5) // we have a valid distance to ground estimate
{
// If the heading is not aligned, reset the yaw and magnetic field states
if (!_control_status.flags.yaw_align) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// If the heading is valid, start using optical flow aiding
if (_control_status.flags.yaw_align) {
// set the flag and reset the fusion timeout
_control_status.flags.opt_flow = true;
_time_last_of_fuse = _time_last_imu;
// if we are not using GPS then the velocity and position states and covariances need to be set
if (!_control_status.flags.gps) {
// constrain height above ground to be above minimum possible
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);
if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
// we should have reliable OF measurements so
// calculate X and Y body relative velocities from OF measurements
Vector3f vel_optflow_body;
vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
vel_optflow_body(1) = range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt;
vel_optflow_body(2) = 0.0f;
// rotate from body to earth frame
Vector3f vel_optflow_earth;
vel_optflow_earth = _R_to_earth * vel_optflow_body;
// take x and Y components
_state.vel(0) = vel_optflow_earth(0);
_state.vel(1) = vel_optflow_earth(1);
} else {
_state.vel(0) = 0.0f;
_state.vel(1) = 0.0f;
}
// reset the velocity covariance terms
zeroRows(P,4,5);
zeroCols(P,4,5);
// reset the horizontal velocity variance using the optical flow noise variance
P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
if (!_control_status.flags.in_air) {
// we are likely starting OF for the first time so reset the horizontal position and vertical velocity states
_state.pos(0) = 0.0f;
_state.pos(1) = 0.0f;
// reset the corresponding covariances
// we are by definition at the origin at commencement so variances are also zeroed
zeroRows(P,7,8);
zeroCols(P,7,8);
// align the output observer to the EKF states
alignOutputFilter();
}
}
}
} else if (!(_params.fusion_mode & MASK_USE_OF)) {
_control_status.flags.opt_flow = false;
}
// handle the case when we are relying on optical flow fusion and lose it
if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
// Switch to the non-aiding mode, zero the velocity states
// and set the synthetic position to the current estimate
_control_status.flags.opt_flow = false;
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
}
}
// fuse the data
if (_control_status.flags.opt_flow) {
// Update optical flow bias estimates
calcOptFlowBias();
// Fuse optical flow LOS rate observations into the main filter
fuseOptFlow();
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
}
}
}
void Ekf::controlMagAiding()
{
// If we are using external vision data for heading then no magnetometer fusion is used
if (_control_status.flags.ev_yaw) {
return;
}
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
// or the more accurate 3-axis fusion
if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
if (!_control_status.flags.mag_3D) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// use 3D mag fusion when airborne
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = true;
} else {
// use heading fusion when on the ground
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_3D = false;
}
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
// always use heading fusion
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_3D = false;
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
if (!_control_status.flags.mag_3D) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// always use 3-axis mag fusion
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = true;
} else {
// do no magnetometer fusion at all
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
}
// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift
// fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time
if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) {
_control_status.flags.mag_dec = true;
} else {
_control_status.flags.mag_dec = false;
}
// if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid
if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) {
_control_status.flags.wind = false;
} else {
_control_status.flags.wind = true;
}
}
bool Ekf::initialiseFilter(void)
{
// Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter
// Sum the IMU delta angle measurements
imuSample imu_init = _imu_buffer.get_newest();
_delVel_sum += imu_init.delta_vel;
// Sum the magnetometer measurements
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
if (_mag_counter == 0) {
_mag_filt_state = _mag_sample_delayed.mag;
}
_mag_counter ++;
_mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f;
}
// set the default height source from the adjustable parameter
if (_hgt_counter == 0) {
_primary_hgt_source = _params.vdist_sensor_type;
}
if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
if (_hgt_counter == 0) {
_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;
}
_hgt_counter ++;
_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _range_sample_delayed.rng;
}
} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) {
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
if (_hgt_counter == 0) {
_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;
}
_hgt_counter ++;
_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _baro_sample_delayed.hgt;
}
} else {
return false;
}
// check to see if we have enough measurements and return false if not
if (_hgt_counter <= 10 || _mag_counter <= 10) {
return false;
} else {
// reset variables that are shared with post alignment GPS checks
_gps_drift_velD = 0.0f;
_gps_alt_ref = 0.0f;
// Zero all of the states
_state.ang_error.setZero();
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
_state.accel_z_bias = 0.0f;
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
float pitch = 0.0f;
float roll = 0.0f;
if (_delVel_sum.norm() > 0.001f) {
_delVel_sum.normalize();
pitch = asinf(_delVel_sum(0));
roll = atan2f(-_delVel_sum(1), -_delVel_sum(2));
} else {
return false;
}
// calculate initial tilt alignment
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
// initialise the filtered alignment error estimate to a larger starting value
_tilt_err_length_filt = 1.0f;
// calculate the averaged magnetometer reading
Vector3f mag_init = _mag_filt_state;
// 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) {
baroSample baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt - _hgt_sensor_offset;
} else {
_baro_hgt_offset = 0.0f;
}
// initialise the state covariance matrix
initialiseCovariance();
// initialise the terrain estimator
initHagl();
return true;
}
}
void Ekf::controlFusionModes()
{
// Determine the vehicle status
calculateVehicleStatus();
// Get the magnetic declination
calcMagDeclination();
// Check for tilt convergence during initial alignment
// filter the tilt error vector using a 1 sec time constant LPF
float filt_coef = 1.0f * _imu_sample_delayed.delta_ang_dt;
_tilt_err_length_filt = filt_coef * _tilt_err_vec.norm() + (1.0f - filt_coef) * _tilt_err_length_filt;
// Once the tilt error has reduced sufficiently, initialise the yaw and magnetic field states
if (_tilt_err_length_filt < 0.005f && !_control_status.flags.tilt_align) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// optical flow fusion mode selection logic
// to start using optical flow data we need angular alignment complete, and fresh optical flow and height above terrain data
if ((_params.fusion_mode & MASK_USE_OF) && !_control_status.flags.opt_flow && _control_status.flags.tilt_align
&& (_time_last_imu - _time_last_optflow) < 5e5 && (_time_last_imu - _time_last_hagl_fuse) < 5e5) {
// If the heading is not aligned, reset the yaw and magnetic field states
if (!_control_status.flags.yaw_align) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// If the heading is valid, start using optical flow aiding
if (_control_status.flags.yaw_align) {
// set the flag and reset the fusion timeout
_control_status.flags.opt_flow = true;
_time_last_of_fuse = _time_last_imu;
// if we are not using GPS and are in air, then we need to reset the velocity to be consistent with the optical flow reading
if (!_control_status.flags.gps) {
// calculate the rotation matrix from body to earth frame
matrix::Dcm<float> body_to_earth(_state.quat_nominal);
// constrain height above ground to be above minimum possible
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 / body_to_earth(2, 2);
if (_in_air && (range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
// calculate X and Y body relative velocities from OF measurements
Vector3f vel_optflow_body;
vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
vel_optflow_body(1) = range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt;
vel_optflow_body(2) = 0.0f;
// rotate from body to earth frame
Vector3f vel_optflow_earth;
vel_optflow_earth = body_to_earth * vel_optflow_body;
// take x and Y components
_state.vel(0) = vel_optflow_earth(0);
_state.vel(1) = vel_optflow_earth(1);
} else {
_state.vel.setZero();
}
}
}
} else if (!(_params.fusion_mode & MASK_USE_OF)) {
_control_status.flags.opt_flow = false;
}
// GPS fusion mode selection logic
// To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data
if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
if (_control_status.flags.tilt_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised
&& (_time_last_imu - _last_gps_fail_us > 5e6)) {
// If the heading is not aligned, reset the yaw and magnetic field states
if (!_control_status.flags.yaw_align) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// If the heading is valid start using gps aiding
if (_control_status.flags.yaw_align) {
_control_status.flags.gps = true;
_time_last_gps = _time_last_imu;
// if we are not already aiding with optical flow, then we need to reset the position and velocity
if (!_control_status.flags.opt_flow) {
_control_status.flags.gps = resetPosition();
_control_status.flags.gps = resetVelocity();
}
}
}
} else if (!(_params.fusion_mode & MASK_USE_GPS)) {
_control_status.flags.gps = false;
}
// handle the case when we are relying on GPS fusion and lose it
if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
// We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something
if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) {
if (_time_last_imu - _time_last_gps > 5e5) {
// if we don't have gps then we need to switch to the non-aiding mode, zero the veloity states
// and set the synthetic GPS position to the current estimate
_control_status.flags.gps = false;
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
} else {
// Reset states to the last GPS measurement
resetPosition();
resetVelocity();
}
}
}
/*
* Handle the case where we have not fused height measurements recently and
* uncertainty exceeds the max allowable. Reset using the best available height
* measurement source, continue using it after the reset and declare the current
* source failed if we have switched.
*/
if ((P[8][8] > sq(_params.hgt_reset_lim)) && ((_time_last_imu - _time_last_hgt_fuse) > 5e6)) {
// handle the case where we are using baro for height
if (_control_status.flags.baro_hgt) {
// check if GPS height is available
gpsSample gps_init = _gps_buffer.get_newest();
bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
baroSample baro_init = _baro_buffer.get_newest();
bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
// use the gps if it is accurate or there is no baro data available
if (gps_hgt_available && (gps_hgt_accurate || !baro_hgt_available)) {
// declare the baro as unhealthy
_baro_hgt_faulty = true;
// set the height mode to the GPS
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
// adjust the height offset so we can use the GPS
_hgt_sensor_offset = _state.pos(2) + gps_init.hgt - _gps_alt_ref;
if (!baro_hgt_available) {
printf("EKF baro hgt timeout - switching to gps\n");
}
}
}
// handle the case we are using GPS for height
if (_control_status.flags.gps_hgt) {
// check if GPS height is available
gpsSample gps_init = _gps_buffer.get_newest();
bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
// check the baro height source for consistency and freshness
baroSample baro_init = _baro_buffer.get_newest();
bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate);
// if baro data is consistent and fresh or GPS height is unavailable or inaccurate, we switch to baro for height
if ((baro_data_consistent && baro_data_fresh) || !gps_hgt_available || !gps_hgt_accurate) {
// declare the GPS height unhealthy
_gps_hgt_faulty = true;
// set the height mode to the baro
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
printf("EKF gps hgt timeout - switching to baro\n");
}
}
// handle the case we are using range finder for height
if (_control_status.flags.rng_hgt) {
// check if range finder data is available
rangeSample rng_init = _range_buffer.get_newest();
bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
// check if baro data is available
baroSample baro_init = _baro_buffer.get_newest();
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
// check if baro data is consistent
float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
bool baro_data_consistent = sq(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate);
// switch to baro if necessary or preferable
bool switch_to_baro = (!rng_data_available && baro_data_available) || (baro_data_consistent && baro_data_available);
if (switch_to_baro) {
// declare the range finder height unhealthy
_rng_hgt_faulty = true;
// set the height mode to the baro
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
printf("EKF rng hgt timeout - switching to baro\n");
}
}
// Reset vertical position and velocity states to the last measurement
resetHeight();
}
// handle the case when we are relying on optical flow fusion and lose it
if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
// Switch to the non-aiding mode, zero the veloity states
// and set the synthetic position to the current estimate
_control_status.flags.opt_flow = false;
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
}
}
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
// or the more accurate 3-axis fusion
if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
if (!_control_status.flags.armed) {
// use heading fusion for initial startup
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = false;
} else {
if (_control_status.flags.in_air) {
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
if (!_control_status.flags.mag_3D) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// use 3D mag fusion when airborne
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = true;
} else {
// use heading fusion when on the ground
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = false;
}
}
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
// always use heading fusion
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = false;
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_2D) {
// always use 2D mag fusion
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = true;
_control_status.flags.mag_3D = false;
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
if (!_control_status.flags.mag_3D) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// always use 3-axis mag fusion
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = true;
} else {
// do no magnetometer fusion at all
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = false;
}
// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift
// fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time
if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) {
_control_status.flags.mag_dec = true;
} else {
_control_status.flags.mag_dec = false;
}
// Control the soure of height measurements for the main filter
if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) {
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
} else 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;
}
// Placeholder for control of wind velocity states estimation
// TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement
if (false) {
_control_status.flags.wind = false;
}
// Store the status to enable change detection
_control_status_prev.value = _control_status.value;
}
