mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-01 18:04:08 +08:00
c7bdf25663
* ekf_control: Inhibit mag fusion when field magnitude is large Move mag inhibition check in separate function * ekf_control: pull out of functionalities out of controlMagFusion - yaw abd mag bias observability checks - mag 3D conditions - load mag covariances - set and clear mag control modes * ekf_control: refactor mag heading/3D start/stop. Move mag declination, mag 3d and mag heading fusion out of the main function * ekf_control: extract mag yaw reset and mag declination fusion requirements * ekf_control: use WMM in isStronMagneticField for mag fusion inhibition - Correct units of WMM strength table * ekf_control: extract mag_state_only functionality of AUTOFW (VTOL custom) Also split inAirYawReset from onGroundYawReset * ekf_control: extract mag automatic selection - transform if-else into switch-case for parameter fusion type selection * ekf_control: extract run3DMagAndDeclFusion, reorganize functions, fix flag naming in Test script * ekf_control: do not run mag fusion if tilt is not aligned. Reset some variables on ground even if mag fusion is not running yet. It could be that it runs later so we need to make sure that those variables are properly set. * ekf_control: move controlMagFusion and related functions to mag_control.cpp * ekf control: check for validity of mag strength from WMM and falls back to average earth mag field with larger gate if not valid * ekf control: remove evyaw check for mag inhibition * ekf control: change nested ternary operator into if-else if * Ekf: create AlphaFilter template class for simple low-pass filtering 0.1/0.9 type low-pass filters are commonly used to smooth data, this class is meant to abstract the computation of this filter * ekf control: reset heading using mag_lpf data to avoid resetting on an outlier fixes ecl issue #525 * ekf control: replace mag_states_only flag with mag_field_disturbed and add parameter to enable or disable mag field strength check * ekf control: remove AUTOFW mag fusion type as not needed This was implemented for VTOL but did not solve the problem and should not be used anymore * ekf control: use start/stop mag functions everywhere instead of setting the flag * ekf control: Run mag fusion depending on yaw_align instead of tilt_align as there is no reason to fuse mag when the ekf isn't aligned * AlphaFilter: add test for float and Vector3f
1380 lines
54 KiB
C++
1380 lines
54 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in
|
|
* the documentation and/or other materials provided with the
|
|
* distribution.
|
|
* 3. Neither the name ECL nor the names of its contributors may be
|
|
* used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
/**
|
|
* @file control.cpp
|
|
* Control functions for ekf attitude and position estimator.
|
|
*
|
|
* @author Paul Riseborough <p_riseborough@live.com.au>
|
|
*
|
|
*/
|
|
|
|
#include "../ecl.h"
|
|
#include "ekf.h"
|
|
#include <mathlib/mathlib.h>
|
|
|
|
void Ekf::controlFusionModes()
|
|
{
|
|
// Store the status to enable change detection
|
|
_control_status_prev.value = _control_status.value;
|
|
|
|
// 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(math::radians(3.0f))) {
|
|
_control_status.flags.tilt_align = true;
|
|
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState());
|
|
|
|
// send alignment status message to the console
|
|
if (_control_status.flags.baro_hgt) {
|
|
ECL_INFO("EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
|
|
|
|
} else if (_control_status.flags.ev_hgt) {
|
|
ECL_INFO("EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
|
|
|
|
} else if (_control_status.flags.gps_hgt) {
|
|
ECL_INFO("EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
|
|
|
|
} else if (_control_status.flags.rng_hgt) {
|
|
ECL_INFO("EKF aligned, (range height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
|
|
} else {
|
|
ECL_ERR("EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
// check for intermittent data (before pop_first_older_than)
|
|
const baroSample &baro_init = _baro_buffer.get_newest();
|
|
_baro_hgt_faulty = !((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
|
|
|
|
const gpsSample &gps_init = _gps_buffer.get_newest();
|
|
_gps_hgt_intermittent = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
|
|
|
|
// 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);
|
|
|
|
if (_mag_data_ready) {
|
|
// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
|
|
// this is useful if there is a lot of interference on the sensor measurement.
|
|
if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) &&_NED_origin_initialised) {
|
|
Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
|
|
_mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred);
|
|
_control_status.flags.synthetic_mag_z = true;
|
|
} else {
|
|
_control_status.flags.synthetic_mag_z = false;
|
|
}
|
|
}
|
|
|
|
_delta_time_baro_us = _baro_sample_delayed.time_us;
|
|
_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
|
|
|
|
// if we have a new baro sample save the delta time between this sample and the last sample which is
|
|
// used below for baro offset calculations
|
|
if (_baro_data_ready) {
|
|
_delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us;
|
|
}
|
|
|
|
// calculate 2,2 element of rotation matrix from sensor frame to earth frame
|
|
// this is required for use of range finder and flow data
|
|
_R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng;
|
|
|
|
// Get range data from buffer and check validity
|
|
_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed);
|
|
|
|
updateRangeDataValidity();
|
|
|
|
if (_range_data_ready && _rng_hgt_valid) {
|
|
// 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_rng_to_earth_2_2;
|
|
}
|
|
|
|
// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
|
|
// This means we stop looking for new data until the old data has been fused.
|
|
if (!_flow_data_ready) {
|
|
_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
|
|
&& (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
|
|
}
|
|
|
|
// check if we should fuse flow data for terrain estimation
|
|
if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) {
|
|
// only fuse flow for terrain if range data hasn't been fused for 5 seconds
|
|
_flow_for_terrain_data_ready = (_time_last_imu - _time_last_hagl_fuse) > 5 * 1000 * 1000;
|
|
// only fuse flow for terrain if the main filter is not fusing flow and we are using gps
|
|
_flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps);
|
|
}
|
|
|
|
_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();
|
|
controlOpticalFlowFusion();
|
|
controlGpsFusion();
|
|
controlAirDataFusion();
|
|
controlBetaFusion();
|
|
controlDragFusion();
|
|
controlHeightFusion();
|
|
|
|
// For efficiency, fusion of direct state observations for position and velocity is performed sequentially
|
|
// in a single function using sensor data from multiple sources (GPS, baro, range finder, etc)
|
|
controlVelPosFusion();
|
|
|
|
// Additional data from an external vision pose estimator can be fused.
|
|
controlExternalVisionFusion();
|
|
|
|
// Additional NE velocity data from an auxiliary sensor can be fused
|
|
controlAuxVelFusion();
|
|
|
|
// check if we are no longer fusing measurements that directly constrain velocity drift
|
|
update_deadreckoning_status();
|
|
}
|
|
|
|
void Ekf::controlExternalVisionFusion()
|
|
{
|
|
// Check for new external vision data
|
|
if (_ev_data_ready) {
|
|
|
|
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
|
|
// needs to be calculated and the observations rotated into the EKF frame of reference
|
|
if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) {
|
|
// rotate EV measurements into the EKF Navigation frame
|
|
calcExtVisRotMat();
|
|
}
|
|
|
|
// external vision aiding selection logic
|
|
if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
|
|
|
|
// check for a external vision measurement that has fallen behind the fusion time horizon
|
|
if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL)) {
|
|
// turn on use of external vision measurements for position
|
|
if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
|
|
_control_status.flags.ev_pos = true;
|
|
resetPosition();
|
|
ECL_INFO_TIMESTAMPED("EKF commencing external vision position fusion");
|
|
}
|
|
|
|
// turn on use of external vision measurements for velocity
|
|
if (_params.fusion_mode & MASK_USE_EVVEL && !_control_status.flags.ev_vel) {
|
|
_control_status.flags.ev_vel = true;
|
|
resetVelocity();
|
|
ECL_INFO_TIMESTAMPED("EKF commencing external vision velocity fusion");
|
|
}
|
|
|
|
if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)
|
|
&& !_ev_rot_mat_initialised) {
|
|
// Reset transformation between EV and EKF navigation frames when starting fusion
|
|
resetExtVisRotMat();
|
|
_ev_rot_mat_initialised = true;
|
|
ECL_INFO_TIMESTAMPED("EKF external vision aligned");
|
|
}
|
|
}
|
|
}
|
|
|
|
// external vision yaw aiding selection logic
|
|
if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
|
|
// don't start using EV data unless daa is arriving frequently
|
|
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
|
|
// reset the yaw angle to the value from the observation quaternion
|
|
// get the roll, pitch, yaw estimates from the quaternion states
|
|
Quatf q_init(_state.quat_nominal);
|
|
Eulerf euler_init(q_init);
|
|
|
|
// get initial yaw from the observation quaternion
|
|
const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();
|
|
Quatf q_obs(ev_newest.quat);
|
|
Eulerf euler_obs(q_obs);
|
|
euler_init(2) = euler_obs(2);
|
|
|
|
// save a copy of the quaternion state for later use in calculating the amount of reset change
|
|
Quatf quat_before_reset = _state.quat_nominal;
|
|
|
|
// calculate initial quaternion states for the ekf
|
|
_state.quat_nominal = Quatf(euler_init);
|
|
uncorrelateQuatStates();
|
|
|
|
// adjust the quaternion covariances estimated yaw error
|
|
increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
|
|
|
|
// calculate the amount that the quaternion has changed by
|
|
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
|
|
|
|
// add the reset amount to the output observer buffered data
|
|
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
|
|
_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
|
|
}
|
|
|
|
// apply the change in attitude quaternion to our newest quaternion estimate
|
|
// which was already taken out from the output buffer
|
|
_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
|
|
|
|
// capture the reset event
|
|
_state_reset_status.quat_counter++;
|
|
|
|
// flag the yaw as aligned
|
|
_control_status.flags.yaw_align = true;
|
|
|
|
// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
|
|
_control_status.flags.ev_yaw = true;
|
|
_control_status.flags.mag_dec = false;
|
|
|
|
stopMagHdgFusion();
|
|
stopMag3DFusion();
|
|
|
|
ECL_INFO_TIMESTAMPED("EKF commencing external vision yaw fusion");
|
|
}
|
|
}
|
|
|
|
// determine if we should start using the height observations
|
|
if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
|
|
// don't start using EV data unless data is arriving frequently
|
|
if (!_control_status.flags.ev_hgt && ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL))) {
|
|
setControlEVHeight();
|
|
resetHeight();
|
|
}
|
|
}
|
|
|
|
// determine if we should use the vertical position observation
|
|
if (_control_status.flags.ev_hgt) {
|
|
_fuse_height = true;
|
|
}
|
|
|
|
// determine if we should use the horizontal position observations
|
|
if (_control_status.flags.ev_pos) {
|
|
_fuse_pos = true;
|
|
|
|
// correct position and height for offset relative to IMU
|
|
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
|
|
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
|
_ev_sample_delayed.pos(0) -= pos_offset_earth(0);
|
|
_ev_sample_delayed.pos(1) -= pos_offset_earth(1);
|
|
_ev_sample_delayed.pos(2) -= pos_offset_earth(2);
|
|
|
|
// Use an incremental position fusion method for EV position data if GPS is also used
|
|
if (_params.fusion_mode & MASK_USE_GPS) {
|
|
_fuse_hpos_as_odom = true;
|
|
} else {
|
|
_fuse_hpos_as_odom = false;
|
|
}
|
|
|
|
if (_fuse_hpos_as_odom) {
|
|
if (!_hpos_prev_available) {
|
|
// no previous observation available to calculate position change
|
|
_fuse_pos = false;
|
|
_hpos_prev_available = true;
|
|
|
|
} else {
|
|
// calculate the change in position since the last measurement
|
|
Vector3f ev_delta_pos = _ev_sample_delayed.pos - _pos_meas_prev;
|
|
|
|
// rotate measurement into body frame is required when fusing with GPS
|
|
ev_delta_pos = _ev_rot_mat * ev_delta_pos;
|
|
|
|
// use the change in position since the last measurement
|
|
_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
|
|
_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
|
|
|
|
// observation 1-STD error, incremental pos observation is expected to have more uncertainty
|
|
_posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.5f);
|
|
}
|
|
|
|
// record observation and estimate for use next time
|
|
_pos_meas_prev = _ev_sample_delayed.pos;
|
|
_hpos_pred_prev(0) = _state.pos(0);
|
|
_hpos_pred_prev(1) = _state.pos(1);
|
|
|
|
} else {
|
|
// use the absolute position
|
|
Vector3f ev_pos_meas = _ev_sample_delayed.pos;
|
|
if (_params.fusion_mode & MASK_ROTATE_EV) {
|
|
ev_pos_meas = _ev_rot_mat * ev_pos_meas;
|
|
}
|
|
_vel_pos_innov[3] = _state.pos(0) - ev_pos_meas(0);
|
|
_vel_pos_innov[4] = _state.pos(1) - ev_pos_meas(1);
|
|
// observation 1-STD error
|
|
_posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.01f);
|
|
|
|
// check if we have been deadreckoning too long
|
|
if ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max) {
|
|
// don't reset velocity if we have another source of aiding constraining it
|
|
if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_vel_fuse) > (uint64_t)1E6)) {
|
|
resetVelocity();
|
|
}
|
|
|
|
resetPosition();
|
|
}
|
|
}
|
|
|
|
// innovation gate size
|
|
_posInnovGateNE = fmaxf(_params.ev_pos_innov_gate, 1.0f);
|
|
}else{
|
|
_vel_pos_innov[3] = 0.0f;
|
|
_vel_pos_innov[4] = 0.0f;
|
|
}
|
|
|
|
// determine if we should use the velocity observations
|
|
if (_control_status.flags.ev_vel) {
|
|
_fuse_hor_vel = true;
|
|
_fuse_vert_vel = true;
|
|
|
|
Vector3f vel_aligned{_ev_sample_delayed.vel};
|
|
|
|
// rotate measurement into correct earth frame if required
|
|
if (_params.fusion_mode & MASK_ROTATE_EV) {
|
|
vel_aligned = _ev_rot_mat * _ev_sample_delayed.vel;
|
|
}
|
|
|
|
// 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.ev_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;
|
|
vel_aligned -= vel_offset_earth;
|
|
|
|
_vel_pos_innov[0] = _state.vel(0) - vel_aligned(0);
|
|
_vel_pos_innov[1] = _state.vel(1) - vel_aligned(1);
|
|
_vel_pos_innov[2] = _state.vel(2) - vel_aligned(2);
|
|
|
|
// check if we have been deadreckoning too long
|
|
if ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max) {
|
|
// don't reset velocity if we have another source of aiding constraining it
|
|
if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_pos_fuse) > (uint64_t)1E6)) {
|
|
resetVelocity();
|
|
}
|
|
}
|
|
|
|
// observation 1-STD error
|
|
_velObsVarNED(2) = _velObsVarNED(1) = _velObsVarNED(0) = fmaxf(_ev_sample_delayed.velErr, 0.01f);
|
|
|
|
// innovation gate size
|
|
_vvelInnovGate = _hvelInnovGate = fmaxf(_params.ev_vel_innov_gate, 1.0f);
|
|
}
|
|
|
|
// Fuse available NED position data into the main filter
|
|
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
|
|
fuseVelPosHeight();
|
|
_fuse_vert_vel = _fuse_hor_vel = false;
|
|
_fuse_pos = _fuse_height = false;
|
|
_fuse_hpos_as_odom = false;
|
|
|
|
}
|
|
|
|
// determine if we should use the yaw observation
|
|
if (_control_status.flags.ev_yaw) {
|
|
fuseHeading();
|
|
|
|
}
|
|
|
|
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel)
|
|
&& (_time_last_imu >= _time_last_ext_vision)
|
|
&& ((_time_last_imu - _time_last_ext_vision) > (uint64_t)_params.reset_timeout_max)) {
|
|
|
|
// Turn off EV fusion mode if no data has been received
|
|
_control_status.flags.ev_pos = false;
|
|
_control_status.flags.ev_vel = false;
|
|
_control_status.flags.ev_yaw = false;
|
|
ECL_INFO_TIMESTAMPED("EKF External Vision Data Stopped");
|
|
|
|
}
|
|
}
|
|
|
|
void Ekf::controlOpticalFlowFusion()
|
|
{
|
|
// Check if on ground motion is un-suitable for use of optical flow
|
|
if (!_control_status.flags.in_air) {
|
|
// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation
|
|
const float accel_norm = _accel_vec_filt.norm();
|
|
|
|
const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit
|
|
|| (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
|
|
|| (_ang_rate_mag_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
|
|
|| (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively
|
|
|
|
if (motion_is_excessive) {
|
|
_time_bad_motion_us = _imu_sample_delayed.time_us;
|
|
|
|
} else {
|
|
_time_good_motion_us = _imu_sample_delayed.time_us;
|
|
}
|
|
|
|
} else {
|
|
_time_bad_motion_us = 0;
|
|
_time_good_motion_us = _imu_sample_delayed.time_us;
|
|
}
|
|
|
|
// Accumulate autopilot gyro data across the same time interval as the flow sensor
|
|
_imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias;
|
|
_delta_time_of += _imu_sample_delayed.delta_ang_dt;
|
|
|
|
// New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon
|
|
if (_flow_data_ready) {
|
|
// Inhibit flow use if motion is un-suitable or we have good quality GPS
|
|
// Apply hysteresis to prevent rapid mode switching
|
|
float gps_err_norm_lim;
|
|
if (_control_status.flags.opt_flow) {
|
|
gps_err_norm_lim = 0.7f;
|
|
} else {
|
|
gps_err_norm_lim = 1.0f;
|
|
}
|
|
|
|
// Check if we are in-air and require optical flow to control position drift
|
|
bool flow_required = _control_status.flags.in_air &&
|
|
(_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|
|
|| (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel) // is completely reliant on optical flow
|
|
|| (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
|
|
|
|
if (!_inhibit_flow_use && _control_status.flags.opt_flow) {
|
|
// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
|
|
bool preflight_motion_not_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5);
|
|
bool flight_motion_not_ok = _control_status.flags.in_air && !isRangeAidSuitable();
|
|
if ((preflight_motion_not_ok || flight_motion_not_ok) && !flow_required) {
|
|
_inhibit_flow_use = true;
|
|
}
|
|
} else if (_inhibit_flow_use && !_control_status.flags.opt_flow){
|
|
// allow use of optical flow if motion is suitable or we are reliant on it for flight navigation
|
|
bool preflight_motion_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6);
|
|
bool flight_motion_ok = _control_status.flags.in_air && isRangeAidSuitable();
|
|
if (preflight_motion_ok || flight_motion_ok || flow_required) {
|
|
_inhibit_flow_use = false;
|
|
}
|
|
}
|
|
|
|
// Handle cases where we are using optical flow but are no longer able to because data is old
|
|
// or its use has been inhibited.
|
|
if (_control_status.flags.opt_flow) {
|
|
if (_inhibit_flow_use) {
|
|
_control_status.flags.opt_flow = false;
|
|
_time_last_of_fuse = 0;
|
|
|
|
} else if ((_time_last_imu - _time_last_of_fuse) > (uint64_t)_params.reset_timeout_max) {
|
|
_control_status.flags.opt_flow = false;
|
|
|
|
}
|
|
}
|
|
|
|
// 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
|
|
&& !_inhibit_flow_use
|
|
&& isTerrainEstimateValid())
|
|
{
|
|
// 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_lpf.getState());
|
|
}
|
|
|
|
// If the heading is valid and use is not inhibited , 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 or external vision aiding, then the velocity and position states and covariances need to be set
|
|
const bool flow_aid_only = !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel);
|
|
if (flow_aid_only) {
|
|
resetVelocity();
|
|
resetPosition();
|
|
|
|
// 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 have optical flow, are reliant on it, but have not been using it for an extended period
|
|
if (_control_status.flags.opt_flow
|
|
&& !_control_status.flags.gps
|
|
&& !_control_status.flags.ev_pos
|
|
&& !_control_status.flags.ev_vel) {
|
|
|
|
bool do_reset = ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max);
|
|
|
|
if (do_reset) {
|
|
resetVelocity();
|
|
resetPosition();
|
|
}
|
|
}
|
|
|
|
// Only fuse optical flow if valid body rate compensation data is available
|
|
if (calcOptFlowBodyRateComp()) {
|
|
|
|
bool flow_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min);
|
|
|
|
if (!flow_quality_good && !_control_status.flags.in_air) {
|
|
// when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate
|
|
_flowRadXYcomp.zero();
|
|
} else {
|
|
// compensate for body motion to give a LOS rate
|
|
_flowRadXYcomp(0) = _flow_sample_delayed.flowRadXY(0) - _flow_sample_delayed.gyroXYZ(0);
|
|
_flowRadXYcomp(1) = _flow_sample_delayed.flowRadXY(1) - _flow_sample_delayed.gyroXYZ(1);
|
|
}
|
|
} else {
|
|
// don't use this flow data and wait for the next data to arrive
|
|
_flow_data_ready = false;
|
|
}
|
|
}
|
|
|
|
// Wait until the midpoint of the flow sample has fallen behind the fusion time horizon
|
|
if (_flow_data_ready && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
|
|
// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
|
|
// but use a relaxed time criteria to enable it to coast through bad range finder data
|
|
if (_control_status.flags.opt_flow && ((_time_last_imu - _time_last_hagl_fuse) < (uint64_t)10e6)) {
|
|
fuseOptFlow();
|
|
_last_known_posNE(0) = _state.pos(0);
|
|
_last_known_posNE(1) = _state.pos(1);
|
|
}
|
|
|
|
_flow_data_ready = false;
|
|
}
|
|
}
|
|
|
|
void Ekf::controlGpsFusion()
|
|
{
|
|
// Check for new GPS data that has fallen behind the fusion time horizon
|
|
if (_gps_data_ready) {
|
|
|
|
// GPS yaw aiding selection logic
|
|
if ((_params.fusion_mode & MASK_USE_GPSYAW)
|
|
&& ISFINITE(_gps_sample_delayed.yaw)
|
|
&& _control_status.flags.tilt_align
|
|
&& (!_control_status.flags.gps_yaw || !_control_status.flags.yaw_align)
|
|
&& ((_time_last_imu - _time_last_gps) < (2 * GPS_MAX_INTERVAL))) {
|
|
|
|
if (resetGpsAntYaw()) {
|
|
// flag the yaw as aligned
|
|
_control_status.flags.yaw_align = true;
|
|
|
|
// turn on fusion of external vision yaw measurements and disable all other yaw fusion
|
|
_control_status.flags.gps_yaw = true;
|
|
_control_status.flags.ev_yaw = false;
|
|
_control_status.flags.mag_dec = false;
|
|
|
|
stopMagHdgFusion();
|
|
stopMag3DFusion();
|
|
|
|
ECL_INFO_TIMESTAMPED("EKF commencing GPS yaw fusion");
|
|
}
|
|
}
|
|
|
|
// fuse the yaw observation
|
|
if (_control_status.flags.gps_yaw) {
|
|
fuseGpsAntYaw();
|
|
}
|
|
|
|
// 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
|
|
bool gps_checks_passing = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6);
|
|
bool gps_checks_failing = (_time_last_imu - _last_gps_pass_us > (uint64_t)5e6);
|
|
if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
|
|
if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) {
|
|
// If the heading is not aligned, reset the yaw and magnetic field states
|
|
// Do not use external vision for yaw if using GPS because yaw needs to be
|
|
// defined relative to an NED reference frame
|
|
const bool want_to_reset_mag_heading = !_control_status.flags.yaw_align ||
|
|
_control_status.flags.ev_yaw ||
|
|
_mag_inhibit_yaw_reset_req;
|
|
if (want_to_reset_mag_heading && canResetMagHeading()) {
|
|
_control_status.flags.ev_yaw = false;
|
|
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState());
|
|
// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
|
|
// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
|
|
if (_mag_inhibit_yaw_reset_req) {
|
|
_mag_inhibit_yaw_reset_req = false;
|
|
// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
|
|
setDiag(P, 12, 12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
|
|
}
|
|
}
|
|
|
|
// If the heading is valid start using gps aiding
|
|
if (_control_status.flags.yaw_align) {
|
|
// if we are not already aiding with optical flow, then we need to reset the position and velocity
|
|
// otherwise we only need to reset the position
|
|
_control_status.flags.gps = true;
|
|
|
|
if (!_control_status.flags.opt_flow) {
|
|
if (!resetPosition() || !resetVelocity()) {
|
|
_control_status.flags.gps = false;
|
|
|
|
}
|
|
|
|
} else if (!resetPosition()) {
|
|
_control_status.flags.gps = false;
|
|
|
|
}
|
|
|
|
if (_control_status.flags.gps) {
|
|
ECL_INFO_TIMESTAMPED("EKF commencing GPS fusion");
|
|
_time_last_gps = _time_last_imu;
|
|
}
|
|
}
|
|
}
|
|
|
|
} else if (!(_params.fusion_mode & MASK_USE_GPS)) {
|
|
_control_status.flags.gps = false;
|
|
|
|
}
|
|
|
|
// Handle the case where we are using GPS and another source of aiding and GPS is failing checks
|
|
if (_control_status.flags.gps && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) {
|
|
_control_status.flags.gps = false;
|
|
// Reset position state to external vision if we are going to use absolute values
|
|
if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
|
|
resetPosition();
|
|
}
|
|
ECL_WARN_TIMESTAMPED("EKF GPS data quality poor - stopping use");
|
|
}
|
|
|
|
// handle the case when we now have GPS, but have not been using it for an extended period
|
|
if (_control_status.flags.gps) {
|
|
// We are relying on aiding to constrain drift so after a specified time
|
|
// with no aiding we need to do something
|
|
bool do_reset = ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max)
|
|
&& ((_time_last_imu - _time_last_delpos_fuse) > _params.reset_timeout_max)
|
|
&& ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max)
|
|
&& ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max);
|
|
|
|
// We haven't had an absolute position fix for a longer time so need to do something
|
|
do_reset = do_reset || ((_time_last_imu - _time_last_pos_fuse) > (2 * _params.reset_timeout_max));
|
|
|
|
if (do_reset) {
|
|
// use GPS velocity data to check and correct yaw angle if a FW vehicle
|
|
if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
|
|
// if flying a fixed wing aircraft, do a complete reset that includes yaw
|
|
_control_status.flags.mag_aligned_in_flight = realignYawGPS();
|
|
}
|
|
|
|
resetVelocity();
|
|
resetPosition();
|
|
_velpos_reset_request = false;
|
|
ECL_WARN_TIMESTAMPED("EKF GPS fusion timeout - reset 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);
|
|
|
|
// calculate observation process noise
|
|
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
|
|
|
|
if (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel) {
|
|
// if we are using other sources of aiding, then relax the upper observation
|
|
// noise limit which prevents bad GPS perturbing the position estimate
|
|
_posObsNoiseNE = fmaxf(_gps_sample_delayed.hacc, lower_limit);
|
|
|
|
} else {
|
|
// if we are not using another source of aiding, then we are reliant on the GPS
|
|
// observations to constrain attitude errors and must limit the observation noise value.
|
|
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
|
|
_posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
|
|
}
|
|
|
|
_velObsVarNED(2) = _velObsVarNED(1) = _velObsVarNED(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise));
|
|
|
|
// calculate innovations
|
|
_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
|
|
_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
|
|
_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
|
|
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
|
|
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
|
|
|
|
// set innovation gate size
|
|
_posInnovGateNE = fmaxf(_params.gps_pos_innov_gate, 1.0f);
|
|
_hvelInnovGate = _vvelInnovGate = fmaxf(_params.gps_vel_innov_gate, 1.0f);
|
|
}
|
|
|
|
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
|
|
_control_status.flags.gps = false;
|
|
ECL_WARN_TIMESTAMPED("EKF GPS data stopped");
|
|
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) {
|
|
// Handle the case where we are fusing another position source along GPS,
|
|
// stop waiting for GPS after 1 s of lost signal
|
|
_control_status.flags.gps = false;
|
|
ECL_WARN_TIMESTAMPED("EKF GPS data stopped, using only EV or OF");
|
|
}
|
|
}
|
|
|
|
void Ekf::controlHeightSensorTimeouts()
|
|
{
|
|
/*
|
|
* 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.
|
|
*/
|
|
|
|
// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical innovations being positive and not stale.
|
|
// Clipping causes the average accel reading to move towards zero which makes the INS think it is falling and produces positive vertical innovations
|
|
float var_product_lim = sq(_params.vert_innov_test_lim) * sq(_params.vert_innov_test_lim);
|
|
bool bad_vert_accel = (_control_status.flags.baro_hgt && // we can only run this check if vertical position and velocity observations are independent
|
|
(sq(_vel_pos_innov[5] * _vel_pos_innov[2]) > var_product_lim * (_vel_pos_innov_var[5] * _vel_pos_innov_var[2])) && // vertical position and velocity sensors are in agreement that we have a significant error
|
|
(_vel_pos_innov[2] > 0.0f) && // positive innovation indicates that the inertial nav thinks it is falling
|
|
((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) && // vertical position data is fresh
|
|
((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL)); // vertical velocity data is fresh
|
|
|
|
// record time of last bad vert accel
|
|
if (bad_vert_accel) {
|
|
_time_bad_vert_accel = _time_last_imu;
|
|
|
|
} else {
|
|
_time_good_vert_accel = _time_last_imu;
|
|
}
|
|
|
|
// declare a bad vertical acceleration measurement and make the declaration persist
|
|
// for a minimum of 10 seconds
|
|
if (_bad_vert_accel_detected) {
|
|
_bad_vert_accel_detected = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION);
|
|
|
|
} else {
|
|
_bad_vert_accel_detected = bad_vert_accel;
|
|
}
|
|
|
|
// check if height is continuously failing because of accel errors
|
|
bool continuous_bad_accel_hgt = ((_time_last_imu - _time_good_vert_accel) > (unsigned)_params.bad_acc_reset_delay_us);
|
|
|
|
// check if height has been inertial deadreckoning for too long
|
|
bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > (uint64_t)5e6);
|
|
|
|
// reset the vertical position and velocity states
|
|
if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
|
|
// boolean that indicates we will do a height reset
|
|
bool reset_height = false;
|
|
|
|
// handle the case where we are using baro for height
|
|
if (_control_status.flags.baro_hgt) {
|
|
// check if GPS height is available
|
|
const gpsSample &gps_init = _gps_buffer.get_newest();
|
|
bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
|
|
|
|
const baroSample &baro_init = _baro_buffer.get_newest();
|
|
bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
|
|
|
|
// check for inertial sensing errors in the last 10 seconds
|
|
bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION);
|
|
|
|
// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
|
|
bool reset_to_gps = !_gps_hgt_intermittent && gps_hgt_accurate && !prev_bad_vert_accel;
|
|
|
|
// reset to GPS if GPS data is available and there is no Baro data
|
|
reset_to_gps = reset_to_gps || (!_gps_hgt_intermittent && !baro_hgt_available);
|
|
|
|
// reset to Baro if we are not doing a GPS reset and baro data is available
|
|
bool reset_to_baro = !reset_to_gps && baro_hgt_available;
|
|
|
|
if (reset_to_gps) {
|
|
// set height sensor health
|
|
_baro_hgt_faulty = true;
|
|
|
|
// reset the height mode
|
|
setControlGPSHeight();
|
|
|
|
// request a reset
|
|
reset_height = true;
|
|
ECL_WARN_TIMESTAMPED("EKF baro hgt timeout - reset to GPS");
|
|
|
|
} else if (reset_to_baro) {
|
|
// set height sensor health
|
|
_baro_hgt_faulty = false;
|
|
|
|
// reset the height mode
|
|
setControlBaroHeight();
|
|
|
|
// request a reset
|
|
reset_height = true;
|
|
ECL_WARN_TIMESTAMPED("EKF baro hgt timeout - reset to baro");
|
|
|
|
} else {
|
|
// we have nothing we can reset to
|
|
// deny a reset
|
|
reset_height = false;
|
|
|
|
}
|
|
}
|
|
|
|
// handle the case we are using GPS for height
|
|
if (_control_status.flags.gps_hgt) {
|
|
// check if GPS height is available
|
|
const gpsSample &gps_init = _gps_buffer.get_newest();
|
|
bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
|
|
|
|
// check the baro height source for consistency and freshness
|
|
const 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[9][9]) * sq(_params.baro_innov_gate);
|
|
|
|
// if baro data is acceptable and GPS data is inaccurate, reset height to baro
|
|
bool reset_to_baro = baro_data_consistent && baro_data_fresh && !_baro_hgt_faulty && !gps_hgt_accurate;
|
|
|
|
// if GPS height is unavailable and baro data is available, reset height to baro
|
|
reset_to_baro = reset_to_baro || (_gps_hgt_intermittent && baro_data_fresh);
|
|
|
|
// if we cannot switch to baro and GPS data is available, reset height to GPS
|
|
bool reset_to_gps = !reset_to_baro && !_gps_hgt_intermittent;
|
|
|
|
if (reset_to_baro) {
|
|
// set height sensor health
|
|
_baro_hgt_faulty = false;
|
|
|
|
// reset the height mode
|
|
setControlBaroHeight();
|
|
|
|
// request a reset
|
|
reset_height = true;
|
|
ECL_WARN_TIMESTAMPED("EKF gps hgt timeout - reset to baro");
|
|
|
|
} else if (reset_to_gps) {
|
|
// reset the height mode
|
|
setControlGPSHeight();
|
|
|
|
// request a reset
|
|
reset_height = true;
|
|
ECL_WARN_TIMESTAMPED("EKF gps hgt timeout - reset to GPS");
|
|
|
|
} else {
|
|
// we have nothing to reset to
|
|
reset_height = false;
|
|
|
|
}
|
|
}
|
|
|
|
// handle the case we are using range finder for height
|
|
if (_control_status.flags.rng_hgt) {
|
|
|
|
// check if baro data is available
|
|
const baroSample &baro_init = _baro_buffer.get_newest();
|
|
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
|
|
|
|
// reset to baro if we have no range data and baro data is available
|
|
bool reset_to_baro = !_rng_hgt_valid && baro_data_available;
|
|
|
|
if (_rng_hgt_valid) {
|
|
|
|
// reset the height mode
|
|
setControlRangeHeight();
|
|
|
|
// request a reset
|
|
reset_height = true;
|
|
ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to rng hgt");
|
|
|
|
} else if (reset_to_baro) {
|
|
// set height sensor health
|
|
_baro_hgt_faulty = false;
|
|
|
|
// reset the height mode
|
|
setControlBaroHeight();
|
|
|
|
// request a reset
|
|
reset_height = true;
|
|
ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to baro");
|
|
|
|
} else {
|
|
// we have nothing to reset to
|
|
reset_height = false;
|
|
|
|
}
|
|
}
|
|
|
|
// handle the case where we are using external vision data for height
|
|
if (_control_status.flags.ev_hgt) {
|
|
// check if vision data is available
|
|
const extVisionSample &ev_init = _ext_vision_buffer.get_newest();
|
|
bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL);
|
|
|
|
// check if baro data is available
|
|
const baroSample &baro_init = _baro_buffer.get_newest();
|
|
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
|
|
|
|
// reset to baro if we have no vision data and baro data is available
|
|
bool reset_to_baro = !ev_data_available && baro_data_available;
|
|
|
|
// reset to ev data if it is available
|
|
bool reset_to_ev = ev_data_available;
|
|
|
|
if (reset_to_baro) {
|
|
// set height sensor health
|
|
_baro_hgt_faulty = false;
|
|
|
|
// reset the height mode
|
|
setControlBaroHeight();
|
|
|
|
// request a reset
|
|
reset_height = true;
|
|
ECL_WARN_TIMESTAMPED("EKF ev hgt timeout - reset to baro");
|
|
|
|
} else if (reset_to_ev) {
|
|
// reset the height mode
|
|
setControlEVHeight();
|
|
|
|
// request a reset
|
|
reset_height = true;
|
|
ECL_WARN_TIMESTAMPED("EKF ev hgt timeout - reset to ev hgt");
|
|
|
|
} else {
|
|
// we have nothing to reset to
|
|
reset_height = false;
|
|
|
|
}
|
|
}
|
|
|
|
// Reset vertical position and velocity states to the last measurement
|
|
if (reset_height) {
|
|
resetHeight();
|
|
// Reset the timout timer
|
|
_time_last_hgt_fuse = _time_last_imu;
|
|
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
void Ekf::controlHeightFusion()
|
|
{
|
|
// set control flags for the desired primary height source
|
|
|
|
checkRangeAidSuitability();
|
|
_range_aid_mode_selected = (_params.range_aid == 1) && isRangeAidSuitable();
|
|
|
|
if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
|
|
|
|
if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
|
|
setControlRangeHeight();
|
|
_fuse_height = true;
|
|
|
|
// we have just switched to using range finder, calculate height sensor offset such that current
|
|
// measurement matches our current height estimate
|
|
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
|
|
if (isTerrainEstimateValid()) {
|
|
_hgt_sensor_offset = _terrain_vpos;
|
|
|
|
} else {
|
|
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
|
|
}
|
|
}
|
|
|
|
} else if (!_range_aid_mode_selected && _baro_data_ready && !_baro_hgt_faulty) {
|
|
setControlBaroHeight();
|
|
_fuse_height = true;
|
|
|
|
// we have just switched to using baro height, we don't need to set a height sensor offset
|
|
// since we track a separate _baro_hgt_offset
|
|
if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
|
|
_hgt_sensor_offset = 0.0f;
|
|
}
|
|
|
|
// Turn off ground effect compensation if it times out
|
|
if (_control_status.flags.gnd_effect) {
|
|
if ((_time_last_imu - _time_last_gnd_effect_on > GNDEFFECT_TIMEOUT)) {
|
|
|
|
_control_status.flags.gnd_effect = false;
|
|
}
|
|
}
|
|
|
|
} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) {
|
|
// switch to gps if there was a reset to gps
|
|
_fuse_height = true;
|
|
|
|
// we have just switched to using gps height, calculate height sensor offset such that current
|
|
// measurement matches our current height estimate
|
|
if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
|
|
_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
|
|
}
|
|
}
|
|
}
|
|
|
|
// set the height data source to range if requested
|
|
if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _rng_hgt_valid) {
|
|
setControlRangeHeight();
|
|
_fuse_height = _range_data_ready;
|
|
|
|
// we have just switched to using range finder, calculate height sensor offset such that current
|
|
// measurement matches our current height estimate
|
|
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
|
|
// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
|
|
if (_control_status.flags.in_air && isTerrainEstimateValid()) {
|
|
|
|
_hgt_sensor_offset = _terrain_vpos;
|
|
|
|
} else if (_control_status.flags.in_air) {
|
|
|
|
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
|
|
|
|
} else {
|
|
|
|
_hgt_sensor_offset = _params.rng_gnd_clearance;
|
|
}
|
|
}
|
|
|
|
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _baro_data_ready && !_baro_hgt_faulty) {
|
|
setControlBaroHeight();
|
|
_fuse_height = true;
|
|
|
|
// we have just switched to using baro height, we don't need to set a height sensor offset
|
|
// since we track a separate _baro_hgt_offset
|
|
if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
|
|
_hgt_sensor_offset = 0.0f;
|
|
}
|
|
}
|
|
|
|
// Determine if GPS should be used as the height source
|
|
if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
|
|
|
|
if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
|
|
setControlRangeHeight();
|
|
_fuse_height = true;
|
|
|
|
// we have just switched to using range finder, calculate height sensor offset such that current
|
|
// measurement matches our current height estimate
|
|
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
|
|
if (isTerrainEstimateValid()) {
|
|
_hgt_sensor_offset = _terrain_vpos;
|
|
|
|
} else {
|
|
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
|
|
}
|
|
}
|
|
|
|
} else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) {
|
|
setControlGPSHeight();
|
|
_fuse_height = true;
|
|
|
|
// we have just switched to using gps height, calculate height sensor offset such that current
|
|
// measurement matches our current height estimate
|
|
if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
|
|
_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
|
|
}
|
|
|
|
} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
|
|
// switch to baro if there was a reset to baro
|
|
_fuse_height = true;
|
|
|
|
// we have just switched to using baro height, we don't need to set a height sensor offset
|
|
// since we track a separate _baro_hgt_offset
|
|
if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
|
|
_hgt_sensor_offset = 0.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Determine if we rely on EV height but switched to baro
|
|
if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
|
|
if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
|
|
// switch to baro if there was a reset to baro
|
|
_fuse_height = true;
|
|
|
|
// we have just switched to using baro height, we don't need to set a height sensor offset
|
|
// since we track a separate _baro_hgt_offset
|
|
if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
|
|
_hgt_sensor_offset = 0.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
|
|
if (!_control_status.flags.baro_hgt && _baro_data_ready) {
|
|
float local_time_step = 1e-6f * _delta_time_baro_us;
|
|
local_time_step = math::constrain(local_time_step, 0.0f, 1.0f);
|
|
|
|
// apply a 10 second first order low pass filter to baro offset
|
|
float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos(
|
|
2) - _baro_hgt_offset);
|
|
_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
|
|
}
|
|
|
|
if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt
|
|
&& (!_range_data_ready || !_rng_hgt_valid)) {
|
|
|
|
// 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;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
void Ekf::checkRangeAidSuitability()
|
|
{
|
|
const bool horz_vel_valid = _control_status.flags.gps
|
|
|| _control_status.flags.ev_pos
|
|
|| _control_status.flags.ev_vel
|
|
|| _control_status.flags.opt_flow;
|
|
|
|
if (_control_status.flags.in_air
|
|
&& _rng_hgt_valid
|
|
&& isTerrainEstimateValid()
|
|
&& horz_vel_valid) {
|
|
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
|
|
// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
|
|
const bool is_in_range = _is_range_aid_suitable
|
|
? (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid)
|
|
: (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid * 0.7f);
|
|
|
|
const float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1));
|
|
const bool is_below_max_speed = _is_range_aid_suitable
|
|
? ground_vel < _params.max_vel_for_range_aid
|
|
: ground_vel < _params.max_vel_for_range_aid * 0.7f;
|
|
|
|
const bool is_hagl_stable = _is_range_aid_suitable
|
|
? ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f)
|
|
: ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 0.01f);
|
|
|
|
_is_range_aid_suitable = is_in_range && is_below_max_speed && is_hagl_stable;
|
|
|
|
} else {
|
|
_is_range_aid_suitable = false;
|
|
}
|
|
}
|
|
|
|
void Ekf::controlAirDataFusion()
|
|
{
|
|
// control activation and initialisation/reset of wind states required for airspeed fusion
|
|
|
|
// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
|
|
bool airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6);
|
|
bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6);
|
|
|
|
if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
|
|
_control_status.flags.wind = false;
|
|
|
|
}
|
|
|
|
if (_control_status.flags.fuse_aspd && airspeed_timed_out) {
|
|
_control_status.flags.fuse_aspd = false;
|
|
|
|
}
|
|
|
|
// Always try to fuse airspeed data if available and we are in flight
|
|
if (_tas_data_ready && _control_status.flags.in_air) {
|
|
// always fuse airsped data if we are flying and data is present
|
|
if (!_control_status.flags.fuse_aspd) {
|
|
_control_status.flags.fuse_aspd = true;
|
|
}
|
|
|
|
// If starting wind state estimation, reset the wind states and covariances before fusing any data
|
|
if (!_control_status.flags.wind) {
|
|
// activate the wind states
|
|
_control_status.flags.wind = true;
|
|
// reset the timout timer to prevent repeated resets
|
|
_time_last_arsp_fuse = _time_last_imu;
|
|
_time_last_beta_fuse = _time_last_imu;
|
|
// reset the wind speed states and corresponding covariances
|
|
resetWindStates();
|
|
resetWindCovariance();
|
|
|
|
}
|
|
|
|
fuseAirspeed();
|
|
|
|
}
|
|
}
|
|
|
|
void Ekf::controlBetaFusion()
|
|
{
|
|
// control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion
|
|
|
|
// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
|
|
bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6);
|
|
bool airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6);
|
|
|
|
if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
|
|
_control_status.flags.wind = false;
|
|
}
|
|
|
|
// Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria:
|
|
|
|
// Sufficient time has lapsed sice the last fusion
|
|
bool beta_fusion_time_triggered = ((_time_last_imu - _time_last_beta_fuse) > _params.beta_avg_ft_us);
|
|
|
|
if (beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) {
|
|
// If starting wind state estimation, reset the wind states and covariances before fusing any data
|
|
if (!_control_status.flags.wind) {
|
|
// activate the wind states
|
|
_control_status.flags.wind = true;
|
|
// reset the timeout timers to prevent repeated resets
|
|
_time_last_beta_fuse = _time_last_imu;
|
|
_time_last_arsp_fuse = _time_last_imu;
|
|
// reset the wind speed states and corresponding covariances
|
|
resetWindStates();
|
|
resetWindCovariance();
|
|
}
|
|
|
|
fuseSideslip();
|
|
}
|
|
}
|
|
|
|
void Ekf::controlDragFusion()
|
|
{
|
|
if (_params.fusion_mode & MASK_USE_DRAG) {
|
|
if (_control_status.flags.in_air
|
|
&& !_mag_inhibit_yaw_reset_req) {
|
|
if (!_control_status.flags.wind) {
|
|
// reset the wind states and covariances when starting drag accel fusion
|
|
_control_status.flags.wind = true;
|
|
resetWindStates();
|
|
resetWindCovariance();
|
|
|
|
} else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) {
|
|
fuseDrag();
|
|
|
|
}
|
|
|
|
} else {
|
|
_control_status.flags.wind = false;
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
void Ekf::controlVelPosFusion()
|
|
{
|
|
// 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 (!(_params.fusion_mode & MASK_USE_GPS)) {
|
|
_control_status.flags.gps = false;
|
|
}
|
|
|
|
if (!_control_status.flags.gps &&
|
|
!_control_status.flags.opt_flow &&
|
|
!_control_status.flags.ev_pos &&
|
|
!_control_status.flags.ev_vel &&
|
|
!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) {
|
|
|
|
// We now need to use a synthetic position observation to prevent unconstrained drift of the INS states.
|
|
_using_synthetic_position = true;
|
|
|
|
// Fuse synthetic position observations every 200msec
|
|
if (((_time_last_imu - _time_last_fake_gps) > (uint64_t)2e5) || _fuse_height) {
|
|
// Reset position and velocity states if we re-commence this aiding method
|
|
if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) {
|
|
resetPosition();
|
|
resetVelocity();
|
|
_fuse_hpos_as_odom = false;
|
|
|
|
if (_time_last_fake_gps != 0) {
|
|
ECL_WARN_TIMESTAMPED("EKF stopping navigation");
|
|
}
|
|
|
|
}
|
|
|
|
_fuse_pos = true;
|
|
_fuse_hor_vel = false;
|
|
_fuse_vert_vel = false;
|
|
_time_last_fake_gps = _time_last_imu;
|
|
|
|
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
|
|
_posObsNoiseNE = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
|
|
|
|
} else {
|
|
_posObsNoiseNE = 0.5f;
|
|
}
|
|
|
|
_vel_pos_innov[0] = 0.0f;
|
|
_vel_pos_innov[1] = 0.0f;
|
|
_vel_pos_innov[2] = 0.0f;
|
|
_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
|
|
_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
|
|
|
|
// glitch protection is not required so set gate to a large value
|
|
_posInnovGateNE = 100.0f;
|
|
|
|
}
|
|
|
|
} else {
|
|
_using_synthetic_position = false;
|
|
}
|
|
|
|
// Fuse available NED velocity and position data into the main filter
|
|
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
|
|
fuseVelPosHeight();
|
|
|
|
}
|
|
}
|
|
|
|
void Ekf::controlAuxVelFusion()
|
|
{
|
|
bool data_ready = _auxvel_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_auxvel_sample_delayed);
|
|
bool primary_aiding = _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.opt_flow;
|
|
|
|
if (data_ready && primary_aiding) {
|
|
_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
|
|
_fuse_hor_vel_aux = true;
|
|
_aux_vel_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
|
|
_aux_vel_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1);
|
|
_velObsVarNED(0) = _auxvel_sample_delayed.velVarNE(0);
|
|
_velObsVarNED(1) = _auxvel_sample_delayed.velVarNE(1);
|
|
_hvelInnovGate = _params.auxvel_gate;
|
|
fuseVelPosHeight();
|
|
}
|
|
}
|