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

Merge pull request #339 from PX4/ekfPosCtrlLimits-wip

Browse Source 
Ekf pos ctrl limits wip
This commit is contained in:
Paul Riseborough 2017-10-23 06:57:23 +11:00 committed by GitHub
commit cd2ca57ec2
8 changed files with 124 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
EKF/airspeed_fusion.cpp
View File

@ -72,6 +72,9 @@ void Ekf::fuseAirspeed()
// Perform fusion of True Airspeed measurement
if (v_tas_pred > 1.0f) {
// determine if we need the sideslip fusion to correct states other than wind
bool update_wind_only = !_is_dead_reckoning;
// Calculate the observation jacobian
// intermediate variable from algebraic optimisation
SH_TAS[0] = 1.0f/v_tas_pred;
@ -95,14 +98,25 @@ void Ekf::fuseAirspeed()
} else { // Reset the estimator covarinace matrix
_fault_status.flags.bad_airspeed = true;
initialiseCovariance();
ECL_ERR("EKF airspeed fusion numerical error - covariance reset");
// if we are getting aiding from other sources, warn and reset the wind states and covariances only
if (update_wind_only) {
resetWindStates();
resetWindCovariance();
ECL_ERR("EKF airspeed fusion badly conditioned - wind covariance reset");
} else {
initialiseCovariance();
_state.wind_vel.setZero();
ECL_ERR("EKF airspeed fusion badly conditioned - full covariance reset");
}
return;
}
SK_TAS[1] = SH_TAS[1];
if (((_time_last_imu - _time_last_gps) < 1e6) || ((_time_last_imu - _time_last_ext_vision) < 1e6) || ((_time_last_imu - _time_last_optflow) < 1e6)) {
if (update_wind_only) {
// If we are getting aiding from other sources, then don't allow the airspeed measurements to affect the non-windspeed states
for (unsigned row = 0; row <= 21; row++) {
Kfusion[row] = 0.0f;

1
EKF/common.h
View File

@ -416,6 +416,7 @@ union filter_control_status_u {
uint32_t update_mag_states_only : 1; ///< 16 - true when only the magnetometer states are updated by the magnetometer
uint32_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
uint32_t mag_fault : 1; ///< 18 - true when the magnetomer has been declared faulty and is no longer being used
uint32_t fuse_aspd : 1; ///< 19 - true when airspedd measurements are being fused
} flags;
uint32_t value;
};

85
EKF/control.cpp
View File

@ -131,7 +131,7 @@ void Ekf::controlFusionModes()
// 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
// report dead reckoning if we are no longer fusing measurements that directly 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)
&& (_time_last_imu - _time_last_of_fuse > _params.no_aid_timeout_max);
@ -261,20 +261,11 @@ void Ekf::controlExternalVisionFusion()
fuseHeading();
}
}
} else if (_control_status.flags.ev_pos && (_time_last_imu - _time_last_ext_vision > 5e6)) {
// Turn off EV fusion mode if no data has been received
_control_status.flags.ev_pos = false;
ECL_INFO("EKF External Vision Data Stopped");
// handle the case when we are relying on ev data and lose it
if (_control_status.flags.ev_pos && !_control_status.flags.gps && !_control_status.flags.opt_flow) {
// We are relying on ev aiding to constrain drift so after 5s without aiding we need to do something
if ((_time_last_imu - _time_last_pos_fuse > 5e6)) {
// Switch to the non-aiding mode, zero the velocity states
// and set the synthetic position to the current estimate
_control_status.flags.ev_pos = false;
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
}
}
}
@ -381,20 +372,10 @@ void Ekf::controlOpticalFlowFusion()
_last_known_posNE(1) = _state.pos(1);
}
}
} else if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow > 5e6)) {
ECL_INFO("EKF Optical Flow Data Stopped");
_control_status.flags.opt_flow = false;
// handle the case when we are relying on optical flow fusion and lose it
if (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos) {
// 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();
ECL_WARN("EKF Stopping Optical Flow Use");
}
}
}
@ -490,19 +471,10 @@ void Ekf::controlGpsFusion()
}
} else {
// handle the case where we do not have GPS and have not been using it for an extended period, but are still relying on it
if ((_time_last_imu - _time_last_gps > 10e6) && (_time_last_imu - _time_last_airspeed > 1e6) && (_time_last_imu - _time_last_optflow > 1e6) && _control_status.flags.gps) {
// if we don't have a source of aiding to constrain attitude drift,
// then we need to switch to the non-aiding mode, zero the velocity states
// and set the synthetic GPS position to the current estimate
} else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > 10e6)) {
_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 measurement timeout - stopping navigation");
ECL_WARN("EKF GPS data stopped");
}
}
}
@ -1005,9 +977,18 @@ void Ekf::controlAirDataFusion()
}
// Always try to fuse airspeed data if available and we are in flight and the filter is operating in a normal aiding mode
bool is_aiding = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos;
if (_tas_data_ready && _control_status.flags.in_air && is_aiding) {
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
@ -1042,10 +1023,7 @@ void Ekf::controlBetaFusion()
// Suffient time has lapsed sice the last fusion
bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_us;
// The filter is operating in a mode where velocity states can be used
bool vel_states_active = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos;
if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air && vel_states_active) {
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
@ -1262,8 +1240,21 @@ 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)) {
if (!_control_status.flags.gps &&
!_control_status.flags.opt_flow &&
!_control_status.flags.ev_pos &&
!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta) &&
((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height))
{
// Reset position and velocity states if we re-commence this aiding method
if ((_time_last_imu - _time_last_fake_gps) > 4E5) {
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
ECL_WARN("EKF stopping navigation");
}
_fuse_pos = true;
_time_last_fake_gps = _time_last_imu;

7
EKF/ekf.h
View File

@ -138,6 +138,13 @@ public:
// get the 1-sigma horizontal and vertical velocity uncertainty
void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning);
/*
Returns the following vehicle control limits required by the estimator.
vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required.
tilt_rate_max : maximum allowed tilt rate against the direction of travel (rad/sec). NaN when no limiting required.
*/
void get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl);
void get_vel_var(Vector3f &vel_var);
void get_pos_var(Vector3f &pos_var);

60
EKF/ekf_helper.cpp
View File

@ -902,7 +902,10 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
// TODO - allow for baro drift in vertical position error
float hpos_err;
float vpos_err;
bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
bool vel_pos_aiding = (_control_status.flags.gps ||
_control_status.flags.opt_flow ||
_control_status.flags.ev_pos ||
(_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd));
if (vel_pos_aiding && _NED_origin_initialised) {
hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph));
@ -933,7 +936,10 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
// TODO - allow for baro drift in vertical position error
float hpos_err;
float vpos_err;
bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
bool vel_pos_aiding = (_control_status.flags.gps ||
_control_status.flags.opt_flow ||
_control_status.flags.ev_pos ||
(_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd));
if (vel_pos_aiding && _NED_origin_initialised) {
hpos_err = sqrtf(P[7][7] + P[8][8]);
@ -963,7 +969,10 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon
{
float hvel_err;
float vvel_err;
bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
bool vel_pos_aiding = (_control_status.flags.gps ||
_control_status.flags.opt_flow ||
_control_status.flags.ev_pos ||
(_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd));
if (vel_pos_aiding && _NED_origin_initialised) {
hvel_err = sqrtf(P[4][4] + P[5][5]);
@ -1000,6 +1009,36 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon
memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool));
}
/*
Returns the following vehicle control limits required by the estimator.
vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required.
limit_hagl : Boolean true when height above ground needs to be controlled to remain between optical flow focus and rang efinder max range limits.
*/
void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl)
{
float flow_gnd_spd_max;
bool flow_limit_hagl;
// If relying on optical flow for navigation we need to keep within flow and range sensor limits
bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos);
if (relying_on_optical_flow) {
// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
flow_gnd_spd_max = 0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2));
flow_gnd_spd_max = fmaxf(flow_gnd_spd_max , 0.0f);
flow_limit_hagl = true;
} else {
flow_gnd_spd_max = NAN;
flow_limit_hagl = false;
}
memcpy(vxy_max, &flow_gnd_spd_max, sizeof(float));
memcpy(limit_hagl, &flow_limit_hagl, sizeof(bool));
}
// get EKF innovation consistency check status information comprising of:
// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
@ -1028,9 +1067,9 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
{
ekf_solution_status soln_status{};
soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow ) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
soln_status.flags.pos_vert_agl = get_terrain_valid();
@ -1105,18 +1144,17 @@ void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first
bool Ekf::global_position_is_valid()
{
// return true if the position estimate is valid
// TODO implement proper check based on published GPS accuracy, innovation consistency checks and timeout status
return (_NED_origin_initialised && ((_time_last_imu - _time_last_gps) < 5e6) && _control_status.flags.gps);
// return true if we are not doing unconstrained free inertial navigation and the origin is set
return (_NED_origin_initialised && !inertial_dead_reckoning());
}
// return true if we are totally reliant on inertial dead-reckoning for position
bool Ekf::inertial_dead_reckoning()
{
bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos)
&& ((_time_last_imu - _time_last_pos_fuse <= 1E6) || (_time_last_imu - _time_last_vel_fuse <= 1E6));
bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= 1E6);
bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6);
&& ((_time_last_imu - _time_last_pos_fuse <= _params.no_aid_timeout_max) || (_time_last_imu - _time_last_vel_fuse <= _params.no_aid_timeout_max));
bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= _params.no_aid_timeout_max);
bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= _params.no_aid_timeout_max) && (_time_last_imu - _time_last_beta_fuse <= _params.no_aid_timeout_max);
return !velPosAiding && !optFlowAiding && !airDataAiding;
}

6
EKF/estimator_interface.cpp
View File

@ -464,8 +464,6 @@ void EstimatorInterface::unallocate_buffers()
bool EstimatorInterface::local_position_is_valid()
{
// return true if the position estimate is valid
return (((_time_last_imu - _time_last_optflow) < 5e6) && _control_status.flags.opt_flow) ||
(((_time_last_imu - _time_last_ext_vision) < 5e6) && _control_status.flags.ev_pos) ||
global_position_is_valid();
// return true if we are not doing unconstrained free inertial navigation
return !inertial_dead_reckoning();
}

9
EKF/estimator_interface.h
View File

@ -148,6 +148,13 @@ public:
// get the 1-sigma horizontal and vertical velocity uncertainty
virtual void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning) = 0;
/*
Returns the following vehicle control limits required by the estimator.
vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required.
limit_hagl : Boolean true when height above ground needs to be controlled to remain between optical flow focus and rang efinder max range limits.
*/
virtual void get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl) = 0;
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; }
@ -192,7 +199,7 @@ public:
void set_is_fixed_wing(bool is_fixed_wing) {_control_status.flags.fixed_wing = is_fixed_wing;}
// set flag if synthetic sideslip measurement should be fused
void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = fuse_beta;}
void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air);}
// set flag if only only mag states should be updated by the magnetometer
void set_update_mag_states_only_flag(bool update_mag_states_only) {_control_status.flags.update_mag_states_only = update_mag_states_only;}

4
EKF/sideslip_fusion.cpp
View File

File diff suppressed because one or more lines are too long