fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 02:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #311 from PX4/pr-ekfExtVis

Browse Source 
EKF: Add ability to use EV and GPS data together
This commit is contained in:
Paul Riseborough
2017-07-29 16:29:53 +10:00
committed by GitHub
parent 74d1955dfa 3ee6898710
commit ba5de96fcb
5 changed files with 80 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+43 -15
View File
@@ -115,7 +115,6 @@ void Ekf::controlFusionModes()
// control use of observations for aiding
controlMagFusion();
controlExternalVisionFusion();
controlOpticalFlowFusion();
controlGpsFusion();
controlAirDataFusion();
@@ -123,10 +122,13 @@ void Ekf::controlFusionModes()
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, external vision, baro, range finder, etc)
// 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 sensor can also be fused.
controlExternalVisionFusion();
// report dead reckoning if we are no longer fusing measurements that constrain velocity drift
_is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > _params.no_aid_timeout_max)
&& (_time_last_imu - _time_last_vel_fuse > _params.no_aid_timeout_max)
@@ -143,20 +145,27 @@ void Ekf::controlExternalVisionFusion()
if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_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) {
// turn on use of external vision measurements for position and height
setControlEVHeight();
// turn on use of external vision measurements for position
_control_status.flags.ev_pos = true;
ECL_INFO("EKF commencing external vision position fusion");
// reset the position, height and velocity
resetPosition();
resetVelocity();
resetHeight();
_control_status.flags.ev_pos=true;
// reset the position if we are not already aiding using GPS, else use a relative position
// method for fusing the position data
if (_control_status.flags.gps) {
_hpos_odometry = true;
} else {
resetPosition();
resetVelocity();
_hpos_odometry = false;
}
}
}
// 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
// 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 observaton quaternion
// get the roll, pitch, yaw estimates from the quaternion states
@@ -207,11 +216,18 @@ void Ekf::controlExternalVisionFusion()
}
}
// determine if we should use the height observation
// determine if we should start using the height observations
if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
setControlEVHeight();
_fuse_height = true;
// don't start using EV data unless daa 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
@@ -224,11 +240,24 @@ void Ekf::controlExternalVisionFusion()
_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);
// if GPS data is being used, then use an incremental position fusion method for EV data
if (_control_status.flags.gps) {
_hpos_odometry = true;
}
}
// Fuse available NED position data into the main filter
if (_fuse_height || _fuse_pos) {
fuseVelPosHeight();
_fuse_pos = _fuse_height = false;
}
// determine if we should use the yaw observation
if (_control_status.flags.ev_yaw) {
fuseHeading();
}
}
@@ -245,7 +274,6 @@ void Ekf::controlExternalVisionFusion()
}
}
}
void Ekf::controlOpticalFlowFusion()
EKF/ekf.h
+6
View File
@@ -238,6 +238,12 @@ private:
bool _fuse_hor_vel{false}; ///< true when gps horizontal velocity measurement should be fused
bool _fuse_vert_vel{false}; ///< true when gps vertical velocity measurement should be fused
// variables used when position data is being fused using a relative position odometry model
bool _hpos_odometry{false}; ///< true when the NE position data is being fused using an odometry assumption
Vector2f _hpos_meas_prev; ///< previous value of NE position measurement fused using odometry assumption (m)
Vector2f _hpos_pred_prev; ///< previous value of NE position state used by odometry fusion (m)
bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use
// booleans true when fresh sensor data is available at the fusion time horizon
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
bool _mag_data_ready{false}; ///< true when new magnetometer data has fallen behind the fusion time horizon and is available to be fused
EKF/ekf_helper.cpp
+3
View File
@@ -98,6 +98,9 @@ bool Ekf::resetPosition()
posNE_before_reset(0) = _state.pos(0);
posNE_before_reset(1) = _state.pos(1);
// let the next odometry update know that the previous value of states cannot be used to calculate the change in position
_hpos_prev_available = false;
if (_control_status.flags.gps) {
// this reset is only called if we have new gps data at the fusion time horizon
_state.pos(0) = _gps_sample_delayed.pos(0);
EKF/gps_checks.cpp
+1 -1
View File
@@ -68,7 +68,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
// if we are already doing aiding, corect for the change in posiiton since the EKF started navigating
if (_control_status.flags.opt_flow || _control_status.flags.gps) {
if (_control_status.flags.opt_flow || _control_status.flags.gps || _control_status.flags.ev_pos) {
double est_lat, est_lon;
map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon);
map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);
EKF/vel_pos_fusion.cpp
+27 -3
View File
@@ -99,10 +99,34 @@ void Ekf::fuseVelPosHeight()
} else if (_control_status.flags.ev_pos) {
// we are using external vision measurements
// calculate innovations
if(_hpos_odometry) {
if(!_hpos_prev_available) {
// no previous observation available to calculate position change
fuse_map[3] = fuse_map[4] = false;
_hpos_prev_available = true;
} else {
// use the change in position since the last measurement
_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - _ev_sample_delayed.posNED(0) + _hpos_meas_prev(0);
_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - _ev_sample_delayed.posNED(1) + _hpos_meas_prev(1);
}
// record observation and estimate for use next time
_hpos_meas_prev(0) = _ev_sample_delayed.posNED(0);
_hpos_meas_prev(1) = _ev_sample_delayed.posNED(1);
_hpos_pred_prev(0) = _state.pos(0);
_hpos_pred_prev(1) = _state.pos(1);
} else {
// use the absolute position
_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
}
// observation 1-STD error
R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
// innovation gate size
gate_size[3] = fmaxf(_params.ev_innov_gate, 1.0f);