fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 06:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: pos/vel reset helpers pass new variance

Browse Source 
- optical flow velocity reset use already computed _flow_vel_ne
This commit is contained in:
Daniel Agar
2022-10-24 10:59:58 -04:00
parent f9f466854b
commit 6f861ba889
6 changed files with 402 additions and 429 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+8 -9
View File
@@ -695,20 +695,20 @@ private:
// fuse single velocity and position measurement
bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
void resetVelocityTo(const Vector3f &vel);
void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);
void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var = NAN);
void resetVelocityTo(const Vector3f &vel, const Vector3f &new_vel_var);
void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var);
void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, float vel_var) { resetHorizontalVelocityTo(new_horz_vel, Vector2f(vel_var, vel_var)); }
void resetVelocityToGps(const gpsSample &gps_sample);
void resetHorizontalVelocityToOpticalFlow(const flowSample &flow_sample);
void resetVelocityToVision();
void resetHorizontalVelocityToZero();
void resetHorizontalPositionToGps(const gpsSample &gps_sample);
void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var);
void resetHorizontalPositionToVision();
void resetHorizontalPositionToOpticalFlow();
void resetHorizontalPositionToLastKnown();
void resetHorizontalPositionTo(const Vector2f &new_horz_pos);
void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var);
void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); }
bool isHeightResetRequired() const;
@@ -1025,7 +1025,6 @@ private:
void startAirspeedFusion();
void stopAirspeedFusion();
void startGpsFusion(const gpsSample &gps_sample);
void stopGpsFusion();
void stopGpsPosFusion();
void stopGpsVelFusion();
src/modules/ekf2/EKF/ekf_helper.cpp
+25 -94
View File
@@ -44,51 +44,11 @@
#include <mathlib/mathlib.h>
#include <cstdlib>
void Ekf::resetVelocityToGps(const gpsSample &gps_sample)
{
_information_events.flags.reset_vel_to_gps = true;
ECL_INFO("reset velocity to GPS");
resetVelocityTo(gps_sample.vel);
P.uncorrelateCovarianceSetVariance<3>(4, sq(gps_sample.sacc));
}
void Ekf::resetHorizontalVelocityToOpticalFlow(const flowSample &flow_sample)
{
_information_events.flags.reset_vel_to_flow = true;
ECL_INFO("reset velocity to flow");
// constrain height above ground to be above minimum possible
const 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
const float range = heightAboveGndEst / _range_sensor.getCosTilt();
if ((range - _params.rng_gnd_clearance) > 0.3f) {
// 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_compensated_XY_rad(1) / flow_sample.dt;
vel_optflow_body(1) = range * _flow_compensated_XY_rad(0) / flow_sample.dt;
vel_optflow_body(2) = 0.0f;
// rotate from body to earth frame
const Vector3f vel_optflow_earth = _R_to_earth * vel_optflow_body;
resetHorizontalVelocityTo(Vector2f(vel_optflow_earth));
} else {
resetHorizontalVelocityTo(Vector2f{0.f, 0.f});
}
// reset the horizontal velocity variance using the optical flow noise variance
P.uncorrelateCovarianceSetVariance<2>(4, sq(range) * calcOptFlowMeasVar(flow_sample));
}
void Ekf::resetVelocityToVision()
{
_information_events.flags.reset_vel_to_vision = true;
ECL_INFO("reset to vision velocity");
resetVelocityTo(getVisionVelocityInEkfFrame());
P.uncorrelateCovarianceSetVariance<3>(4, getVisionVelocityVarianceInEkfFrame());
resetVelocityTo(getVisionVelocityInEkfFrame(), getVisionVelocityVarianceInEkfFrame());
}
void Ekf::resetHorizontalVelocityToZero()
@@ -96,21 +56,28 @@ void Ekf::resetHorizontalVelocityToZero()
_information_events.flags.reset_vel_to_zero = true;
ECL_INFO("reset velocity to zero");
// Used when falling back to non-aiding mode of operation
resetHorizontalVelocityTo(Vector2f{0.f, 0.f});
P.uncorrelateCovarianceSetVariance<2>(4, 25.0f);
resetHorizontalVelocityTo(Vector2f{0.f, 0.f}, 25.f);
}
void Ekf::resetVelocityTo(const Vector3f &new_vel)
void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var)
{
resetHorizontalVelocityTo(Vector2f(new_vel));
resetVerticalVelocityTo(new_vel(2));
resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1)));
resetVerticalVelocityTo(new_vel(2), new_vel_var(2));
}
void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel)
void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var)
{
const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel);
_state.vel.xy() = new_horz_vel;
if (PX4_ISFINITE(new_horz_vel_var(0))) {
P.uncorrelateCovarianceSetVariance<1>(4, math::max(sq(0.01f), new_horz_vel_var(0)));
}
if (PX4_ISFINITE(new_horz_vel_var(1))) {
P.uncorrelateCovarianceSetVariance<1>(5, math::max(sq(0.01f), new_horz_vel_var(1)));
}
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
_output_buffer[index].vel.xy() += delta_horz_vel;
}
@@ -148,14 +115,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
_time_last_ver_vel_fuse = _imu_sample_delayed.time_us;
}
void Ekf::resetHorizontalPositionToGps(const gpsSample &gps_sample)
{
_information_events.flags.reset_pos_to_gps = true;
ECL_INFO("reset position to GPS");
resetHorizontalPositionTo(gps_sample.pos);
P.uncorrelateCovarianceSetVariance<2>(7, sq(gps_sample.hacc));
}
void Ekf::resetHorizontalPositionToVision()
{
_information_events.flags.reset_pos_to_vision = true;
@@ -166,45 +125,33 @@ void Ekf::resetHorizontalPositionToVision()
_ev_pos = _R_ev_to_ekf * _ev_sample_delayed.pos;
}
resetHorizontalPositionTo(Vector2f(_ev_pos));
P.uncorrelateCovarianceSetVariance<2>(7, _ev_sample_delayed.posVar.slice<2, 1>(0, 0));
resetHorizontalPositionTo(Vector2f(_ev_pos), _ev_sample_delayed.posVar.slice<2, 1>(0, 0));
// 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;
}
void Ekf::resetHorizontalPositionToOpticalFlow()
{
_information_events.flags.reset_pos_to_last_known = true;
if (!_control_status.flags.in_air) {
ECL_INFO("reset horizontal position to (0, 0)");
// we are likely starting OF for the first time so reset the horizontal position
resetHorizontalPositionTo(Vector2f(0.f, 0.f));
} else {
ECL_INFO("reset horizontal position to last known");
resetHorizontalPositionTo(_last_known_pos.xy());
}
// estimate is relative to initial position in this mode, so we start with zero error.
P.uncorrelateCovarianceSetVariance<2>(7, 0.0f);
}
void Ekf::resetHorizontalPositionToLastKnown()
{
_information_events.flags.reset_pos_to_last_known = true;
ECL_INFO("reset position to last known position");
// Used when falling back to non-aiding mode of operation
resetHorizontalPositionTo(_last_known_pos.xy());
P.uncorrelateCovarianceSetVariance<2>(7, sq(_params.pos_noaid_noise));
resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
}
void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos)
void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var)
{
const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}};
_state.pos.xy() = new_horz_pos;
if (PX4_ISFINITE(new_horz_pos_var(0))) {
P.uncorrelateCovarianceSetVariance<1>(7, math::max(sq(0.01f), new_horz_pos_var(0)));
}
if (PX4_ISFINITE(new_horz_pos_var(1))) {
P.uncorrelateCovarianceSetVariance<1>(8, math::max(sq(0.01f), new_horz_pos_var(1)));
}
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
_output_buffer[index].pos.xy() += delta_horz_pos;
}
@@ -1356,22 +1303,6 @@ void Ekf::stopAirspeedFusion()
_control_status.flags.fuse_aspd = false;
}
void Ekf::startGpsFusion(const gpsSample &gps_sample)
{
if (!_control_status.flags.gps) {
resetHorizontalPositionToGps(gps_sample);
// when already using another velocity source velocity reset is not necessary
if (!_control_status.flags.opt_flow && !_control_status.flags.ev_vel) {
resetVelocityToGps(gps_sample);
}
_information_events.flags.starting_gps_fusion = true;
ECL_INFO("starting GPS fusion");
_control_status.flags.gps = true;
}
}
void Ekf::stopGpsFusion()
{
if (_control_status.flags.gps) {
src/modules/ekf2/EKF/gps_control.cpp
+31 -10
View File
@@ -59,10 +59,11 @@ void Ekf::controlGpsFusion()
controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing);
// GNSS velocity
const float vel_obs_var = sq(math::max(gps_sample.sacc, _params.gps_vel_noise));
const float vel_var = sq(math::max(gps_sample.sacc, _params.gps_vel_noise));
const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
updateVelocityAidSrcStatus(gps_sample.time_us,
gps_sample.vel, // observation
Vector3f(vel_obs_var, vel_obs_var, vel_obs_var * sq(1.5f)), // observation variance
vel_obs_var, // observation variance
math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate
_aid_src_gnss_vel);
_aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL);
@@ -79,10 +80,11 @@ void Ekf::controlGpsFusion()
}
}
const float pos_obs_var = sq(pos_noise);
const float pos_var = sq(pos_noise);
const Vector2f pos_obs_var(pos_var, pos_var);
updateHorizontalPositionAidSrcStatus(gps_sample.time_us,
gps_sample.pos, // observation
Vector2f(pos_obs_var, pos_obs_var), // observation variance
pos_obs_var, // observation variance
math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
_aid_src_gnss_pos);
_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
@@ -129,9 +131,11 @@ void Ekf::controlGpsFusion()
}
}
ECL_WARN("GPS fusion timeout - resetting");
resetVelocityToGps(gps_sample);
resetHorizontalPositionToGps(gps_sample);
ECL_WARN("GPS fusion timeout, resetting velocity and position");
_information_events.flags.reset_vel_to_gps = true;
_information_events.flags.reset_pos_to_gps = true;
resetVelocityTo(gps_sample.vel, vel_obs_var);
resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
}
} else {
@@ -165,7 +169,20 @@ void Ekf::controlGpsFusion()
_inhibit_ev_yaw_use = true;
} else {
startGpsFusion(gps_sample);
ECL_INFO("starting GPS fusion");
_information_events.flags.starting_gps_fusion = true;
// reset position
_information_events.flags.reset_pos_to_gps = true;
resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
// when already using another velocity source velocity reset is not necessary
if (!isHorizontalAidingActive()) {
_information_events.flags.reset_vel_to_gps = true;
resetVelocityTo(gps_sample.vel, vel_obs_var);
}
_control_status.flags.gps = true;
}
} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) {
@@ -173,8 +190,12 @@ void Ekf::controlGpsFusion()
if (resetYawToEKFGSF()) {
_information_events.flags.yaw_aligned_to_imu_gps = true;
ECL_INFO("Yaw aligned using IMU and GPS");
resetVelocityToGps(gps_sample);
resetHorizontalPositionToGps(gps_sample);
ECL_INFO("reset velocity and position to GPS");
_information_events.flags.reset_vel_to_gps = true;
_information_events.flags.reset_pos_to_gps = true;
resetVelocityTo(gps_sample.vel, vel_obs_var);
resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
}
}
}
src/modules/ekf2/EKF/optical_flow_control.cpp
+30 -8
View File
@@ -77,11 +77,12 @@ void Ekf::controlOpticalFlowFusion()
_delta_time_of = _imu_sample_delayed.delta_ang_dt;
is_delta_time_good = true;
}
}
if (is_quality_good && !is_delta_time_good) {
ECL_DEBUG("Optical flow: bad delta time: OF dt %.6f s (min: %.3f, max: %.3f), IMU dt %.6f s",
(double)_flow_sample_delayed.dt, (double)delta_time_min, (double)delta_time_max, (double)_imu_sample_delayed.delta_ang_dt);
(double)_flow_sample_delayed.dt, (double)delta_time_min, (double)delta_time_max,
(double)_imu_sample_delayed.delta_ang_dt);
}
}
@@ -166,12 +167,25 @@ void Ekf::controlOpticalFlowFusion()
// if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set
if (!isHorizontalAidingActive()) {
resetHorizontalVelocityToOpticalFlow(_flow_sample_delayed);
resetHorizontalPositionToOpticalFlow();
ECL_INFO("reset velocity to flow");
_information_events.flags.reset_vel_to_flow = true;
resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
// reset position, estimate is relative to initial position in this mode, so we start with zero error
if (!_control_status.flags.in_air) {
ECL_INFO("reset position to zero");
resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f);
_last_known_pos.xy() = _state.pos.xy();
} else {
_information_events.flags.reset_pos_to_last_known = true;
ECL_INFO("reset position to last known position");
resetHorizontalPositionTo(_last_known_pos.xy(), 0.f);
}
}
_control_status.flags.opt_flow = true;
_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
_control_status.flags.opt_flow = true;
return;
}
@@ -190,11 +204,19 @@ void Ekf::controlOpticalFlowFusion()
}
// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)
if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)
&& !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) {
resetHorizontalVelocityToOpticalFlow(_flow_sample_delayed);
resetHorizontalPositionToOpticalFlow();
ECL_INFO("reset velocity to flow");
_information_events.flags.reset_vel_to_flow = true;
resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
// reset position, estimate is relative to initial position in this mode, so we start with zero error
ECL_INFO("reset position to last known");
_information_events.flags.reset_pos_to_last_known = true;
resetHorizontalPositionTo(_last_known_pos.xy(), 0.f);
_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
}
}