dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #160 from priseborough/pr-ekf2Improvements 

EKF: clean up fusion control logic
This commit is contained in:
Paul Riseborough 2016-06-30 17:18:14 +10:00 committed by GitHub
parent 9a95898414 f5e9ea1626
commit eaae95fdc4
5 changed files with 595 additions and 516 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

772
EKF/control.cpp
View File

@ -41,6 +41,7 @@
#include "../ecl.h" #include "../ecl.h"
#include "ekf.h" #include "ekf.h"
#include "mathlib.h"
void Ekf::controlFusionModes() void Ekf::controlFusionModes()
{ {
@ -60,228 +61,351 @@ void Ekf::controlFusionModes()
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) { if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) {
_control_status.flags.tilt_align = true; _control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
ECL_INFO("EKF alignment complete");
} }
} }
// control use of various external sources for position and velocity aiding // check for arrival of new sensor data at the fusion time horizon
controlExternalVisionAiding(); _gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
controlOpticalFlowAiding(); _mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
controlGpsAiding(); _baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
controlHeightAiding(); _range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
controlMagAiding(); && (_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::controlExternalVisionAiding() void Ekf::controlExternalVisionFusion()
{ {
// external vision position aiding selection logic // Check for new exernal vision data
if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) { if (_ev_data_ready) {
// check for a exernal vision measurement that has fallen behind the fusion time horizon
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // external vision position aiding selection logic
// turn on use of external vision measurements for position and height if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) {
_control_status.flags.ev_pos = true; // check for a exernal vision measurement that has fallen behind the fusion time horizon
ECL_INFO("EKF switching to external vision position fusion"); if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
// turn off other forms of height aiding // turn on use of external vision measurements for position and height
_control_status.flags.ev_pos = true;
ECL_INFO("EKF switching to external vision position fusion");
// turn off other forms of height aiding
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
// reset the position, height and velocity
resetPosition();
resetVelocity();
resetHeight();
}
}
// external vision yaw aiding selection logic
if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
// check for a exernal vision measurement that has fallen behind the fusion time horizon
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
// reset the yaw angle to the value from the observaton quaternion
// get the roll, pitch, yaw estimates from the quaternion states
matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
_state.quat_nominal(3));
matrix::Euler<float> euler_init(q_init);
// get initial yaw from the observation quaternion
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3));
matrix::Euler<float> 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
Quaternion quat_before_reset = _state.quat_nominal;
// calculate initial quaternion states for the ekf
_state.quat_nominal = Quaternion(euler_init);
// 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
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change;
_output_buffer.push_to_index(i,output_states);
}
// 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 magnetoemter fusion
_control_status.flags.ev_yaw = true;
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
_control_status.flags.mag_dec = false;
ECL_INFO("EKF switching to external vision yaw fusion");
}
}
// determine if we should use the height observation
if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
_control_status.flags.baro_hgt = false; _control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false; _control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false; _control_status.flags.rng_hgt = false;
// reset the position, height and velocity _control_status.flags.ev_hgt = true;
resetPosition(); _fuse_height = true;
resetVelocity();
resetHeight();
}
}
// external vision yaw aiding selection logic
if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
// check for a exernal vision measurement that has fallen behind the fusion time horizon
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
// reset the yaw angle to the value from the observaton quaternion
// get the roll, pitch, yaw estimates from the quaternion states
matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
_state.quat_nominal(3));
matrix::Euler<float> euler_init(q_init);
// get initial yaw from the observation quaternion
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3));
matrix::Euler<float> 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
Quaternion quat_before_reset = _state.quat_nominal;
// calculate initial quaternion states for the ekf
_state.quat_nominal = Quaternion(euler_init);
// 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
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change;
_output_buffer.push_to_index(i,output_states);
}
// 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 magnetoemter fusion
_control_status.flags.ev_yaw = true;
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
_control_status.flags.mag_dec = false;
ECL_INFO("EKF switching to external vision yaw fusion");
}
}
}
void Ekf::controlOpticalFlowAiding()
{
// 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 // determine if we should use the horizontal position observations
if (_control_status.flags.yaw_align) { if (_control_status.flags.ev_pos) {
// set the flag and reset the fusion timeout _fuse_pos = true;
_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 // correct position and height for offset relative to IMU
if (!_control_status.flags.gps) { Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
// constrain height above ground to be above minimum possible Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); _ev_sample_delayed.posNED(0) -= pos_offset_earth(0);
_ev_sample_delayed.posNED(1) -= pos_offset_earth(1);
// calculate absolute distance from focal point to centre of frame assuming a flat earth _ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
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)) { // determine if we should use the yaw observation
_control_status.flags.opt_flow = false; if (_control_status.flags.ev_yaw) {
} fuseHeading();
// 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();
} }
} }
} }
void Ekf::controlGpsAiding() void Ekf::controlOpticalFlowFusion()
{ {
// GPS fusion mode selection logic // Check for new optical flow data that has fallen behind the fusion time horizon
// To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data if (_flow_data_ready) {
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 // optical flow fusion mode selection logic
&& (_time_last_imu - _last_gps_fail_us > 5e6)) { 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 the heading is not aligned, reset the yaw and magnetic field states
if (!_control_status.flags.yaw_align) { if (!_control_status.flags.yaw_align) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
} }
// If the heading is valid start using gps aiding // If the heading is valid, start using optical flow aiding
if (_control_status.flags.yaw_align) { if (_control_status.flags.yaw_align) {
_control_status.flags.gps = true; // set the flag and reset the fusion timeout
_time_last_gps = _time_last_imu; _control_status.flags.opt_flow = true;
_time_last_of_fuse = _time_last_imu;
// if we are not already aiding with optical flow, then we need to reset the position and velocity // if we are not using GPS then the velocity and position states and covariances need to be set
if (!_control_status.flags.opt_flow) { if (!_control_status.flags.gps) {
_control_status.flags.gps = resetPosition(); // constrain height above ground to be above minimum possible
_control_status.flags.gps = resetVelocity(); 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;
} }
} else if (!(_params.fusion_mode & MASK_USE_GPS)) { // handle the case when we are relying on optical flow fusion and lose it
_control_status.flags.gps = false; 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)) {
// handle the case when we are relying on GPS fusion and lose it // Switch to the non-aiding mode, zero the velocity states
if (_control_status.flags.gps && !_control_status.flags.opt_flow) { // and set the synthetic position to the current estimate
// We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something _control_status.flags.opt_flow = false;
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(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1); _last_known_posNE(1) = _state.pos(1);
_state.vel.setZero(); _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;
} }
} }
// 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::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) || _control_status.flags.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;
}
} }
} }
@ -336,31 +460,39 @@ void Ekf::controlHeightSensorTimeouts()
// set height sensor health // set height sensor health
_baro_hgt_faulty = true; _baro_hgt_faulty = true;
_gps_hgt_faulty = false; _gps_hgt_faulty = false;
// declare the GPS height healthy // declare the GPS height healthy
_gps_hgt_faulty = false; _gps_hgt_faulty = false;
// reset the height mode // reset the height mode
_control_status.flags.baro_hgt = false; _control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true; _control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false; _control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false; _control_status.flags.ev_hgt = false;
// request a reset // request a reset
reset_height = true; reset_height = true;
ECL_INFO("EKF baro hgt timeout - reset to GPS"); ECL_INFO("EKF baro hgt timeout - reset to GPS");
} else if (reset_to_baro){ } else if (reset_to_baro){
// set height sensor health // set height sensor health
_baro_hgt_faulty = false; _baro_hgt_faulty = false;
// reset the height mode // reset the height mode
_control_status.flags.baro_hgt = true; _control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false; _control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false; _control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false; _control_status.flags.ev_hgt = false;
// request a reset // request a reset
reset_height = true; reset_height = true;
ECL_INFO("EKF baro hgt timeout - reset to baro"); ECL_INFO("EKF baro hgt timeout - reset to baro");
} else { } else {
// we have nothing we can reset to // we have nothing we can reset to
// deny a reset // deny a reset
reset_height = false; reset_height = false;
} }
} }
@ -370,6 +502,7 @@ void Ekf::controlHeightSensorTimeouts()
gpsSample gps_init = _gps_buffer.get_newest(); 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_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
// check the baro height source for consistency and freshness // check the baro height source for consistency and freshness
baroSample baro_init = _baro_buffer.get_newest(); baroSample baro_init = _baro_buffer.get_newest();
bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
@ -389,28 +522,35 @@ void Ekf::controlHeightSensorTimeouts()
// set height sensor health // set height sensor health
_gps_hgt_faulty = true; _gps_hgt_faulty = true;
_baro_hgt_faulty = false; _baro_hgt_faulty = false;
// reset the height mode // reset the height mode
_control_status.flags.baro_hgt = true; _control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false; _control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false; _control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false; _control_status.flags.ev_hgt = false;
// request a reset // request a reset
reset_height = true; reset_height = true;
ECL_INFO("EKF gps hgt timeout - reset to baro"); ECL_INFO("EKF gps hgt timeout - reset to baro");
} else if (reset_to_gps) { } else if (reset_to_gps) {
// set height sensor health // set height sensor health
_gps_hgt_faulty = false; _gps_hgt_faulty = false;
// reset the height mode // reset the height mode
_control_status.flags.baro_hgt = false; _control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true; _control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false; _control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false; _control_status.flags.ev_hgt = false;
// request a reset // request a reset
reset_height = true; reset_height = true;
ECL_INFO("EKF gps hgt timeout - reset to GPS"); ECL_INFO("EKF gps hgt timeout - reset to GPS");
} else { } else {
// we have nothing to reset to // we have nothing to reset to
reset_height = false; reset_height = false;
} }
} }
@ -419,6 +559,7 @@ void Ekf::controlHeightSensorTimeouts()
// check if range finder data is available // check if range finder data is available
rangeSample rng_init = _range_buffer.get_newest(); rangeSample rng_init = _range_buffer.get_newest();
bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL); bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
// check if baro data is available // check if baro data is available
baroSample baro_init = _baro_buffer.get_newest(); baroSample baro_init = _baro_buffer.get_newest();
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
@ -433,28 +574,35 @@ void Ekf::controlHeightSensorTimeouts()
// set height sensor health // set height sensor health
_rng_hgt_faulty = true; _rng_hgt_faulty = true;
_baro_hgt_faulty = false; _baro_hgt_faulty = false;
// reset the height mode // reset the height mode
_control_status.flags.baro_hgt = true; _control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false; _control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false; _control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false; _control_status.flags.ev_hgt = false;
// request a reset // request a reset
reset_height = true; reset_height = true;
ECL_INFO("EKF rng hgt timeout - reset to baro"); ECL_INFO("EKF rng hgt timeout - reset to baro");
} else if (reset_to_rng) { } else if (reset_to_rng) {
// set height sensor health // set height sensor health
_rng_hgt_faulty = false; _rng_hgt_faulty = false;
// reset the height mode // reset the height mode
_control_status.flags.baro_hgt = false; _control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false; _control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true; _control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false; _control_status.flags.ev_hgt = false;
// request a reset // request a reset
reset_height = true; reset_height = true;
ECL_INFO("EKF rng hgt timeout - reset to rng hgt"); ECL_INFO("EKF rng hgt timeout - reset to rng hgt");
} else { } else {
// we have nothing to reset to // we have nothing to reset to
reset_height = false; reset_height = false;
} }
} }
@ -463,6 +611,7 @@ void Ekf::controlHeightSensorTimeouts()
// check if vision data is available // check if vision data is available
extVisionSample ev_init = _ext_vision_buffer.get_newest(); extVisionSample ev_init = _ext_vision_buffer.get_newest();
bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL); bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL);
// check if baro data is available // check if baro data is available
baroSample baro_init = _baro_buffer.get_newest(); baroSample baro_init = _baro_buffer.get_newest();
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
@ -477,26 +626,32 @@ void Ekf::controlHeightSensorTimeouts()
// set height sensor health // set height sensor health
_rng_hgt_faulty = true; _rng_hgt_faulty = true;
_baro_hgt_faulty = false; _baro_hgt_faulty = false;
// reset the height mode // reset the height mode
_control_status.flags.baro_hgt = true; _control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false; _control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false; _control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false; _control_status.flags.ev_hgt = false;
// request a reset // request a reset
reset_height = true; reset_height = true;
ECL_INFO("EKF ev hgt timeout - reset to baro"); ECL_INFO("EKF ev hgt timeout - reset to baro");
} else if (reset_to_ev) { } else if (reset_to_ev) {
// reset the height mode // reset the height mode
_control_status.flags.baro_hgt = false; _control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false; _control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false; _control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = true; _control_status.flags.ev_hgt = true;
// request a reset // request a reset
reset_height = true; reset_height = true;
ECL_INFO("EKF ev hgt timeout - reset to ev hgt"); ECL_INFO("EKF ev hgt timeout - reset to ev hgt");
} else { } else {
// we have nothing to reset to // we have nothing to reset to
reset_height = false; reset_height = false;
} }
} }
@ -505,127 +660,206 @@ void Ekf::controlHeightSensorTimeouts()
resetHeight(); resetHeight();
// Reset the timout timer // Reset the timout timer
_time_last_hgt_fuse = _time_last_imu; _time_last_hgt_fuse = _time_last_imu;
} }
} }
} }
void Ekf::controlHeightAiding() void Ekf::controlBaroFusion()
{ {
// check for height sensor timeouts and reset and change sensor if necessary if (_baro_data_ready) {
controlHeightSensorTimeouts(); // determine if we should use the baro as our height source
uint64_t last_baro_time_us = _baro_sample_delayed.time_us;
if (((_params.vdist_sensor_type == VDIST_SENSOR_BARO) || _control_status.flags.baro_hgt) && !_baro_hgt_faulty) {
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
_fuse_height = true;
// Control the source of height measurements for the main filter }
// do not switch to a sensor if it is unhealthy or the data is stale
if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO) &&
!_baro_hgt_faulty &&
(((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) || _control_status.flags.baro_hgt)) {
_control_status.flags.baro_hgt = true; // calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
_control_status.flags.gps_hgt = false; if (!_control_status.flags.baro_hgt) {
_control_status.flags.rng_hgt = false; float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us);
_control_status.flags.ev_hgt = false; local_time_step = math::constrain(local_time_step,0.0f,1.0f);
last_baro_time_us = _baro_sample_delayed.time_us;
float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset;
_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS) && }
!_gps_hgt_faulty &&
(((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) || _control_status.flags.gps_hgt)) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) &&
!_rng_hgt_faulty &&
(((_imu_sample_delayed.time_us - _range_sample_delayed.time_us) < 2 * RNG_MAX_INTERVAL) || _control_status.flags.rng_hgt)) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_EV) &&
(((_imu_sample_delayed.time_us - _ev_sample_delayed.time_us) < 2 * EV_MAX_INTERVAL) || _control_status.flags.ev_hgt)) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = true;
}
// If we are on ground, store the local position and time to use as a reference for takeoff checks
if (!_control_status.flags.in_air) {
_last_on_ground_posD = _state.pos(2);
} }
} }
void Ekf::controlMagAiding() void Ekf::controlRangeFinderFusion()
{
// determine if we should use range finder data for height
if (_range_data_ready) {
// set the height data source to range if requested
if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false;
}
// correct the range data for position offset relative to the IMU
Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sample_delayed.rng += pos_offset_earth(2) / _R_to_earth(2, 2);
// always fuse available range finder data into a terrain height estimator if the estimator has been initialised
if (_terrain_initialised) {
fuseHagl();
}
// only use range finder as a height observation in the main filter if specifically enabled
if (_control_status.flags.rng_hgt) {
_fuse_height = true;
}
} else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) {
// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
if (!_control_status.flags.in_air) {
_range_sample_delayed.rng = _params.rng_gnd_clearance;
_range_sample_delayed.time_us = _imu_sample_delayed.time_us;
}
_fuse_height = true;
}
}
void Ekf::controlAirDataFusion()
{
// TODO This is just to get the logic inside but we will only start fusion once we tested this again
//if (_tas_data_ready) {
if (false) {
fuseAirspeed();
}
// control airspeed fusion - TODO move to a function
// 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;
}
}
void Ekf::controlMagFusion()
{ {
// If we are using external vision data for heading then no magnetometer fusion is used // If we are using external vision data for heading then no magnetometer fusion is used
if (_control_status.flags.ev_yaw) { if (_control_status.flags.ev_yaw) {
return; return;
} }
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances // If we are on ground, store the local position and time to use as a reference
// or the more accurate 3-axis fusion if (!_control_status.flags.in_air) {
if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) { _last_on_ground_posD = _state.pos(2);
// 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) { }
// 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 transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
if (!_control_status.flags.mag_3D) { if (!_control_status.flags.mag_3D) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
} }
// use 3D mag fusion when airborne // always use 3-axis mag fusion
_control_status.flags.mag_hdg = false; _control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = true; _control_status.flags.mag_3D = true;
} else { } else {
// use heading fusion when on the ground // do no magnetometer fusion at all
_control_status.flags.mag_hdg = true; _control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false; _control_status.flags.mag_3D = false;
} }
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) { // 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
// always use heading fusion // fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time
_control_status.flags.mag_hdg = true; if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) {
_control_status.flags.mag_3D = false; _control_status.flags.mag_dec = true;
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) { } else {
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states _control_status.flags.mag_dec = false;
if (!_control_status.flags.mag_3D) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
} }
// always use 3-axis mag fusion // fuse magnetometer data using the selected methods
_control_status.flags.mag_hdg = false; if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
_control_status.flags.mag_3D = true; fuseMag();
} else { if (_control_status.flags.mag_dec) {
// do no magnetometer fusion at all fuseDeclination();
_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 } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
// fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time // fusion of an Euler yaw angle from either a 321 or 312 rotation sequence
if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) { fuseHeading();
_control_status.flags.mag_dec = true;
} else {
// do no fusion at all
}
}
}
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 (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos
&& ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
_fuse_pos = true;
_time_last_fake_gps = _time_last_imu;
}
// Fuse available NED velocity and position data into the main filter
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight();
_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
} 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;
} }
} }

174
EKF/ekf.cpp
View File

@ -39,6 +39,7 @@
* @author Paul Riseborough <p_riseborough@live.com.au> * @author Paul Riseborough <p_riseborough@live.com.au>
*/ */
#include "../ecl.h"
#include "ekf.h" #include "ekf.h"
#include "mathlib.h" #include "mathlib.h"
@ -67,8 +68,13 @@ Ekf::Ekf():
_fuse_pos(false), _fuse_pos(false),
_fuse_hor_vel(false), _fuse_hor_vel(false),
_fuse_vert_vel(false), _fuse_vert_vel(false),
_fuse_flow(false), _gps_data_ready(false),
_fuse_hagl_data(false), _mag_data_ready(false),
_baro_data_ready(false),
_range_data_ready(false),
_flow_data_ready(false),
_ev_data_ready(false),
_tas_data_ready(false),
_time_last_fake_gps(0), _time_last_fake_gps(0),
_time_last_pos_fuse(0), _time_last_pos_fuse(0),
_time_last_vel_fuse(0), _time_last_vel_fuse(0),
@ -209,166 +215,9 @@ bool Ekf::update()
predictHagl(); predictHagl();
} }
// control logic // control fusion of observation data
controlFusionModes(); controlFusionModes();
// measurement updates
// Fuse magnetometer data using the selected fusion method and only if angular alignment is complete
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
fuseMag();
if (_control_status.flags.mag_dec) {
fuseDeclination();
}
} else if (_control_status.flags.mag_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
}
}
// determine if range finder data has fallen behind the fusion time horizon fuse it if we are
// not tilted too much to use it
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
&& (_R_to_earth(2, 2) > 0.7071f)) {
// correct the range data for position offset relative to the IMU
Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sample_delayed.rng += pos_offset_earth(2) / _R_to_earth(2, 2);
// if we have range data we always try to estimate terrain height
_fuse_hagl_data = true;
// only use range finder as a height observation in the main filter if specifically enabled
if (_control_status.flags.rng_hgt) {
_fuse_height = true;
}
} else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) {
// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
if (!_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;
}
// determine if baro data has fallen behind the fusion time horizon and fuse it in the main filter if enabled
uint64_t last_baro_time_us = _baro_sample_delayed.time_us;
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
if (_control_status.flags.baro_hgt) {
_fuse_height = true;
} else {
// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us);
local_time_step = math::constrain(local_time_step,0.0f,1.0f);
last_baro_time_us = _baro_sample_delayed.time_us;
float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset;
_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
}
}
// If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it
if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
// Only use GPS data for position and velocity aiding if enabled
if (_control_status.flags.gps) {
_fuse_pos = true;
_fuse_vert_vel = true;
_fuse_hor_vel = true;
// 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);
}
// only use gps height observation in the main filter if specifically enabled
if (_control_status.flags.gps_hgt) {
_fuse_height = true;
}
}
// If we are using external vision aiding and data has fallen behind the fusion time horizon then fuse it
if (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) {
// use external vision posiiton and height observations
if (_control_status.flags.ev_pos) {
_fuse_pos = true;
_fuse_height = 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.posNED(0) -= pos_offset_earth(0);
_ev_sample_delayed.posNED(1) -= pos_offset_earth(1);
_ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
}
// use external vision yaw observation
if (_control_status.flags.ev_yaw) {
fuseHeading();
}
}
// If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it
if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
&& _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5
&& (_R_to_earth(2, 2) > 0.7071f)) {
_fuse_flow = true;
}
// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
if (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos
&& ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
_fuse_pos = true;
_time_last_fake_gps = _time_last_imu;
}
// fuse available range finder data into a terrain height estimator if it has been initialised
if (_fuse_hagl_data && _terrain_initialised) {
fuseHagl();
_fuse_hagl_data = false;
}
// Fuse available NED velocity and position data into the main filter
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight();
_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
}
// Update optical flow bias estimates
calcOptFlowBias();
// Fuse optical flow LOS rate observations into the main filter
if (_fuse_flow) {
fuseOptFlow();
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_fuse_flow = false;
}
// If we are using airspeed measurements and data has fallen behind the fusion time horizon then fuse it
if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) {
fuseAirspeed();
}
} }
// the output observer always runs // the output observer always runs
@ -546,11 +395,16 @@ bool Ekf::initialiseFilter(void)
baroSample baro_newest = _baro_buffer.get_newest(); baroSample baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt; _baro_hgt_offset = baro_newest.hgt;
_state.pos(2) = -math::max(_rng_filt_state * _R_to_earth(2, 2),_params.rng_gnd_clearance); _state.pos(2) = -math::max(_rng_filt_state * _R_to_earth(2, 2),_params.rng_gnd_clearance);
ECL_INFO("EKF using range finder height - commencing alignment");
} else if (_control_status.flags.ev_hgt) { } else if (_control_status.flags.ev_hgt) {
// if we are using external vision data for height, then the vertical position state needs to be reset // if we are using external vision data for height, then the vertical position state needs to be reset
// because the initialisation position is not the zero datum // because the initialisation position is not the zero datum
resetHeight(); resetHeight();
ECL_INFO("EKF using vision height - commencing alignment");
} else if (_control_status.flags.baro_hgt){
ECL_INFO("EKF using pressure height - commencing alignment");
} }

34
EKF/ekf.h
View File

@ -187,8 +187,15 @@ private:
bool _fuse_pos; // gps position data should be fused bool _fuse_pos; // gps position data should be fused
bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused
bool _fuse_vert_vel; // gps vertical velocity measurement should be fused bool _fuse_vert_vel; // gps vertical velocity measurement should be fused
bool _fuse_flow; // flow measurement should be fused
bool _fuse_hagl_data; // if true then range data will be fused to estimate terrain height // booleans true when fresh sensor data is available at the fusion time horizon
bool _gps_data_ready;
bool _mag_data_ready;
bool _baro_data_ready;
bool _range_data_ready;
bool _flow_data_ready;
bool _ev_data_ready;
bool _tas_data_ready;
uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode
@ -370,19 +377,28 @@ private:
void controlFusionModes(); void controlFusionModes();
// control fusion of external vision observations // control fusion of external vision observations
void controlExternalVisionAiding(); void controlExternalVisionFusion();
// control fusion of optical flow observtions // control fusion of optical flow observtions
void controlOpticalFlowAiding(); void controlOpticalFlowFusion();
// control fusion of GPS observations // control fusion of GPS observations
void controlGpsAiding(); void controlGpsFusion();
// control fusion of height position observations
void controlHeightAiding();
// control fusion of magnetometer observations // control fusion of magnetometer observations
void controlMagAiding(); void controlMagFusion();
// control fusion of range finder observations
void controlRangeFinderFusion();
// control fusion of air data observations
void controlAirDataFusion();
// control fusion of pressure altitude observations
void controlBaroFusion();
// control fusion of velocity and position observations
void controlVelPosFusion();
// control for height sensor timeouts, sensor changes and state resets // control for height sensor timeouts, sensor changes and state resets
void controlHeightSensorTimeouts(); void controlHeightSensorTimeouts();

91
EKF/ekf_helper.cpp
View File

@ -56,21 +56,15 @@ bool Ekf::resetVelocity()
// reset EKF states // reset EKF states
if (_control_status.flags.gps) { if (_control_status.flags.gps) {
// if we have a valid GPS measurement use it to initialise velocity states // this reset is only called if we have new gps data at the fusion time horizon
gpsSample gps_newest = _gps_buffer.get_newest(); _state.vel = _gps_sample_delayed.vel;
if (_time_last_imu - gps_newest.time_us < 2*GPS_MAX_INTERVAL) {
_state.vel = gps_newest.vel;
} else {
// XXX use the value of the last known velocity
return false;
}
} else if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) { } else if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) {
_state.vel.setZero(); _state.vel.setZero();
} else { } else {
return false; return false;
} }
// calculate the change in velocity and apply to the output predictor state history // calculate the change in velocity and apply to the output predictor state history
@ -81,6 +75,7 @@ bool Ekf::resetVelocity()
output_states = _output_buffer.get_from_index(index); output_states = _output_buffer.get_from_index(index);
output_states.vel += velocity_change; output_states.vel += velocity_change;
_output_buffer.push_to_index(index,output_states); _output_buffer.push_to_index(index,output_states);
} }
// capture the reset event // capture the reset event
@ -103,45 +98,21 @@ bool Ekf::resetPosition()
posNE_before_reset(1) = _state.pos(1); posNE_before_reset(1) = _state.pos(1);
if (_control_status.flags.gps) { if (_control_status.flags.gps) {
// if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay // this reset is only called if we have new gps data at the fusion time horizon
gpsSample gps_newest = _gps_buffer.get_newest(); _state.pos(0) = _gps_sample_delayed.pos(0);
_state.pos(1) = _gps_sample_delayed.pos(1);
return true;
float time_delay = 1e-6f * (float)(_imu_sample_delayed.time_us - gps_newest.time_us);
float max_time_delay = 1e-6f * (float)GPS_MAX_INTERVAL;
if (time_delay < max_time_delay) {
_state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay;
_state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay;
return true;
} else {
// XXX use the value of the last known position
return false;
}
} else if (_control_status.flags.opt_flow) { } else if (_control_status.flags.opt_flow) {
_state.pos(0) = 0.0f; _state.pos(0) = 0.0f;
_state.pos(1) = 0.0f; _state.pos(1) = 0.0f;
return true; return true;
} else if (_control_status.flags.ev_pos) {
// if we have fresh data, reset the position to the measurement
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
if (_time_last_imu - ev_newest.time_us < 2*EV_MAX_INTERVAL) {
// use the most recent data if it's time offset from the fusion time horizon is smaller
int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
if (abs(dt_newest) < abs(dt_delayed)) {
_state.pos(0) = ev_newest.posNED(0);
_state.pos(1) = ev_newest.posNED(1);
} else {
_state.pos(0) = _ev_sample_delayed.posNED(0);
_state.pos(1) = _ev_sample_delayed.posNED(1);
}
return true;
} else { } else if (_control_status.flags.ev_pos) {
// XXX use the value of the last known position // this reset is only called if we have new ev data at the fusion time horizon
return false; _state.pos(0) = _ev_sample_delayed.posNED(0);
} _state.pos(1) = _ev_sample_delayed.posNED(1);
return true;
} else { } else {
return false; return false;
@ -844,21 +815,27 @@ Vector3f Ekf::calcRotVecVariances()
float t3 = acos(q0); float t3 = acos(q0);
float t4 = -t2+1.0f; float t4 = -t2+1.0f;
float t5 = t2-1.0f; float t5 = t2-1.0f;
float t6 = 1.0f/t5; if ((t4 > 1e-9f) && (t5 < -1e-9f)) {
float t7 = q1*t6*2.0f; float t6 = 1.0f/t5;
float t8 = 1.0f/powf(t4,1.5f); float t7 = q1*t6*2.0f;
float t9 = q0*q1*t3*t8*2.0f; float t8 = 1.0f/powf(t4,1.5f);
float t10 = t7+t9; float t9 = q0*q1*t3*t8*2.0f;
float t11 = 1.0f/sqrtf(t4); float t10 = t7+t9;
float t12 = q2*t6*2.0f; float t11 = 1.0f/sqrtf(t4);
float t13 = q0*q2*t3*t8*2.0f; float t12 = q2*t6*2.0f;
float t14 = t12+t13; float t13 = q0*q2*t3*t8*2.0f;
float t15 = q3*t6*2.0f; float t14 = t12+t13;
float t16 = q0*q3*t3*t8*2.0f; float t15 = q3*t6*2.0f;
float t17 = t15+t16; float t16 = q0*q3*t3*t8*2.0f;
rot_var_vec(0) = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f; float t17 = t15+t16;
rot_var_vec(1) = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f; rot_var_vec(0) = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f;
rot_var_vec(2) = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f; rot_var_vec(1) = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f;
rot_var_vec(2) = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f;
} else {
rot_var_vec(0) = 4.0f * P[1][1];
rot_var_vec(1) = 4.0f * P[2][2];
rot_var_vec(2) = 4.0f * P[3][3];
}
return rot_var_vec; return rot_var_vec;
} }

40
EKF/optflow_fusion.cpp
View File

@ -522,30 +522,28 @@ void Ekf::calcOptFlowBias()
// if accumulation time differences are not excessive and accumulation time is adequate // if accumulation time differences are not excessive and accumulation time is adequate
// compare the optical flow and and navigation rate data and calculate a bias error // compare the optical flow and and navigation rate data and calculate a bias error
if (_fuse_flow) { if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.05f) && (_delta_time_of > 0.01f)
if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.05f) && (_delta_time_of > 0.01f) && (_flow_sample_delayed.dt > 0.01f)) {
&& (_flow_sample_delayed.dt > 0.01f)) { // calculate a reference angular rate
// calculate a reference angular rate Vector3f reference_body_rate;
Vector3f reference_body_rate; reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);
reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);
// calculate the optical flow sensor measured body rate // calculate the optical flow sensor measured body rate
Vector3f of_body_rate; Vector3f of_body_rate;
of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt); of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt);
// calculate the bias estimate using a combined LPF and spike filter // calculate the bias estimate using a combined LPF and spike filter
_flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)), _flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)),
-0.1f, 0.1f); -0.1f, 0.1f);
_flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)), _flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)),
-0.1f, 0.1f); -0.1f, 0.1f);
_flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)), _flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)),
-0.1f, 0.1f); -0.1f, 0.1f);
}
// reset the accumulators
_imu_del_ang_of.setZero();
_delta_time_of = 0.0f;
} }
// reset the accumulators
_imu_del_ang_of.setZero();
_delta_time_of = 0.0f;
} }
// calculate the measurement variance for the optical flow sensor (rad/sec)^2 // calculate the measurement variance for the optical flow sensor (rad/sec)^2