fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 07:28:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: optical flow don't compute innovation variance twice

Browse Source 
- collapse updateOptFlow() and startFlowFusion() to avoid recomputing H
 - this is a relatively expensive call we can easily avoid with the
   right structure
This commit is contained in:
Daniel Agar
2024-07-09 10:10:01 -04:00
parent f709ed409d
commit 8bf15b01c4
3 changed files with 64 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
+60 -39
View File
@@ -38,6 +38,8 @@
#include "ekf.h"
#include <ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h>
void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
{
if (!_flow_buffer || (_params.flow_ctrl != 1)) {
@@ -47,6 +49,8 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
bool flow_data_ready = false;
VectorState H;
// New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon
if (_flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed)) {
@@ -88,11 +92,35 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
flow_data_ready = true;
}
updateOptFlow(_aid_src_optical_flow, flow_sample);
// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
// correct for gyro bias errors in the data used to do the motion compensation
// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
const Vector3f flow_gyro_corrected = flow_sample.gyro_rate - _flow_gyro_bias;
const Vector2f flow_compensated = flow_sample.flow_rate - flow_gyro_corrected.xy();
// calculate the optical flow observation variance
const float R_LOS = calcOptFlowMeasVar(flow_sample);
Vector2f innov_var;
sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, R_LOS, FLT_EPSILON, &innov_var, &H);
// run the innovation consistency check and record result
updateAidSourceStatus(_aid_src_optical_flow,
flow_sample.time_us, // sample timestamp
flow_compensated, // observation
Vector2f{R_LOS, R_LOS}, // observation variance
predictFlow(flow_gyro_corrected) - flow_compensated, // innovation
innov_var, // innovation variance
math::max(_params.flow_innov_gate, 1.f)); // innovation gate
// logging
const Vector3f flow_gyro_corrected = flow_sample.gyro_rate - _flow_gyro_bias;
_flow_rate_compensated = flow_sample.flow_rate - flow_gyro_corrected.xy();
_flow_rate_compensated = flow_compensated;
// compute the velocities in body and local frames from corrected optical flow measurement for logging only
const float range = predictFlowRange();
_flow_vel_body(0) = -flow_compensated(1) * range;
_flow_vel_body(1) = flow_compensated(0) * range;
_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
}
if (flow_data_ready) {
@@ -116,7 +144,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
if (_control_status.flags.opt_flow) {
if (continuing_conditions_passing) {
fuseOptFlow(_hagl_sensor_status.flags.flow);
fuseOptFlow(H, _hagl_sensor_status.flags.flow);
// 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.no_aid_timeout_max)) {
@@ -134,7 +162,34 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
} else {
if (starting_conditions_passing) {
startFlowFusion();
// If the height is relative to the ground, terrain height cannot be observed.
_hagl_sensor_status.flags.flow = (_height_sensor_ref != HeightSensor::RANGE);
if (isHorizontalAidingActive()) {
if (fuseOptFlow(H, _hagl_sensor_status.flags.flow)) {
ECL_INFO("starting optical flow");
_control_status.flags.opt_flow = true;
} else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) {
ECL_INFO("starting optical flow, resetting terrain");
resetTerrainToFlow();
_control_status.flags.opt_flow = true;
}
} else {
if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) {
ECL_INFO("starting optical flow, resetting");
resetFlowFusion();
_control_status.flags.opt_flow = true;
} else if (_hagl_sensor_status.flags.flow) {
ECL_INFO("starting optical flow, resetting terrain");
resetTerrainToFlow();
_control_status.flags.opt_flow = true;
}
}
_hagl_sensor_status.flags.flow = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE);
}
}
@@ -143,40 +198,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
}
}
void Ekf::startFlowFusion()
{
if (_height_sensor_ref != HeightSensor::RANGE) {
// If the height is relative to the ground, terrain height cannot be observed.
_hagl_sensor_status.flags.flow = true;
}
if (isHorizontalAidingActive()) {
if (fuseOptFlow(_hagl_sensor_status.flags.flow)) {
ECL_INFO("starting optical flow");
_control_status.flags.opt_flow = true;
} else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) {
ECL_INFO("starting optical flow, resetting terrain");
resetTerrainToFlow();
_control_status.flags.opt_flow = true;
}
} else {
if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) {
ECL_INFO("starting optical flow, resetting");
resetFlowFusion();
_control_status.flags.opt_flow = true;
} else if (_hagl_sensor_status.flags.flow) {
ECL_INFO("starting optical flow, resetting terrain");
resetTerrainToFlow();
_control_status.flags.opt_flow = true;
}
}
_hagl_sensor_status.flags.flow = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE);
}
void Ekf::resetFlowFusion()
{
ECL_INFO("reset velocity to flow");
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
+3 -50
View File
@@ -42,57 +42,9 @@
#include <ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h>
#include <ekf_derivation/generated/compute_flow_y_innov_var_and_h.h>
void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src, const flowSample &flow_sample)
{
// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
// correct for gyro bias errors in the data used to do the motion compensation
// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
const Vector3f flow_gyro_corrected = flow_sample.gyro_rate - _flow_gyro_bias;
const Vector2f flow_compensated = flow_sample.flow_rate - flow_gyro_corrected.xy();
const Vector2f innovation = predictFlow(flow_gyro_corrected) - flow_compensated;
// calculate the optical flow observation variance
const float R_LOS = calcOptFlowMeasVar(flow_sample);
Vector2f innov_var;
VectorState H;
sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, R_LOS, FLT_EPSILON, &innov_var, &H);
// run the innovation consistency check and record result
updateAidSourceStatus(aid_src,
flow_sample.time_us, // sample timestamp
flow_compensated, // observation
Vector2f{R_LOS, R_LOS}, // observation variance
innovation, // innovation
innov_var, // innovation variance
math::max(_params.flow_innov_gate, 1.f)); // innovation gate
// compute the velocities in body and local frames from corrected optical flow measurement for logging only
const float range = predictFlowRange();
_flow_vel_body(0) = -flow_compensated(1) * range;
_flow_vel_body(1) = flow_compensated(0) * range;
_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
}
bool Ekf::fuseOptFlow(const bool update_terrain)
bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
{
const auto state_vector = _state.vector();
const float R_LOS = _aid_src_optical_flow.observation_variance[0];
Vector2f innov_var;
VectorState H;
sym::ComputeFlowXyInnovVarAndHx(state_vector, P, R_LOS, FLT_EPSILON, &innov_var, &H);
innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
if ((innov_var(0) < R_LOS) || (innov_var(1) < R_LOS)) {
// we need to reinitialise the covariance matrix and abort this fusion step
ECL_ERR("Opt flow error - covariance reset");
initialiseCovariance();
return false;
}
_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
@@ -107,7 +59,7 @@ bool Ekf::fuseOptFlow(const bool update_terrain)
// fuse observation axes sequentially
for (uint8_t index = 0; index <= 1; index++) {
if (_aid_src_optical_flow.innovation_variance[index] < R_LOS) {
if (_aid_src_optical_flow.innovation_variance[index] < _aid_src_optical_flow.observation_variance[index]) {
// we need to reinitialise the covariance matrix and abort this fusion step
ECL_ERR("Opt flow error - covariance reset");
initialiseCovariance();
@@ -119,6 +71,7 @@ bool Ekf::fuseOptFlow(const bool update_terrain)
} else if (index == 1) {
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
const float R_LOS = _aid_src_optical_flow.observation_variance[1];
sym::ComputeFlowYInnovVarAndH(state_vector, P, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
// recalculate the innovation using the updated state
src/modules/ekf2/EKF/ekf.h
+1 -3
View File
@@ -833,7 +833,6 @@ private:
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
// control fusion of optical flow observations
void controlOpticalFlowFusion(const imuSample &imu_delayed);
void startFlowFusion();
void resetFlowFusion();
void stopFlowFusion();
@@ -850,8 +849,7 @@ private:
Vector2f predictFlow(const Vector3f &flow_gyro) const;
// fuse optical flow line of sight rate measurements
void updateOptFlow(estimator_aid_source2d_s &aid_src, const flowSample &flow_sample);
bool fuseOptFlow(bool update_terrain);
bool fuseOptFlow(VectorState &H, bool update_terrain);
#endif // CONFIG_EKF2_OPTICAL_FLOW