fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 09:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: update flow aid src status every sample

Browse Source
This commit is contained in:
Daniel Agar
2024-07-09 10:10:01 -04:00
parent bcd666b3f8
commit 4d324da9f8
3 changed files with 55 additions and 53 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
+7 -4
View File
@@ -64,19 +64,22 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
if (is_quality_good
&& is_magnitude_good
&& is_tilt_good) {
// compensate for body motion to give a LOS rate
calcOptFlowBodyRateComp(imu_delayed);
_flow_rate_compensated = _flow_sample_delayed.flow_rate - _flow_sample_delayed.gyro_rate.xy();
} else {
// don't use this flow data and wait for the next data to arrive
_flow_data_ready = false;
_flow_rate_compensated.setZero();
}
updateOptFlow(_aid_src_optical_flow, _flow_sample_delayed);
// logging
const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias;
_flow_rate_compensated = _flow_sample_delayed.flow_rate - flow_gyro_corrected.xy();
}
if (_flow_data_ready) {
updateOptFlow(_aid_src_optical_flow);
// Check if we are in-air and require optical flow to control position drift
bool is_flow_required = _control_status.flags.in_air
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
+41 -44
View File
@@ -42,28 +42,18 @@
#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)
void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src, const flowSample &flow_sample)
{
const Vector2f vel_body = predictFlowVelBody();
const float range = predictFlowRange();
// 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 Vector2f opt_flow_rate = _flow_rate_compensated;
const Vector3f flow_gyro_corrected = flow_sample.gyro_rate - _flow_gyro_bias;
const Vector2f flow_compensated = flow_sample.flow_rate - flow_gyro_corrected.xy();
// compute the velocities in body and local frames from corrected optical flow measurement for logging only
_flow_vel_body(0) = -opt_flow_rate(1) * range;
_flow_vel_body(1) = opt_flow_rate(0) * range;
_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
Vector2f innovation{
(vel_body(1) / range) - opt_flow_rate(0),
(-vel_body(0) / range) - opt_flow_rate(1)
};
const Vector2f innovation = predictFlow(flow_gyro_corrected) - flow_compensated;
// calculate the optical flow observation variance
const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
const float R_LOS = calcOptFlowMeasVar(flow_sample);
Vector2f innov_var;
VectorState H;
@@ -71,23 +61,26 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
// run the innovation consistency check and record result
updateAidSourceStatus(aid_src,
_flow_sample_delayed.time_us, // sample timestamp
opt_flow_rate, // observation
Vector2f{R_LOS, R_LOS}, // observation variance
innovation, // innovation
innov_var, // innovation variance
math::max(_params.flow_innov_gate, 1.f)); // innovation gate
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)
{
const float R_LOS = _aid_src_optical_flow.observation_variance[0];
// calculate the height above the ground of the optical flow camera. Since earth frame is NED
// a positive offset in earth frame leads to a smaller height above the ground.
float range = predictFlowRange();
const auto state_vector = _state.vector();
const float R_LOS = _aid_src_optical_flow.observation_variance[0];
Vector2f innov_var;
VectorState H;
@@ -113,6 +106,14 @@ 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) {
// we need to reinitialise the covariance matrix and abort this fusion step
ECL_ERR("Opt flow error - covariance reset");
initialiseCovariance();
return false;
}
if (index == 0) {
// everything was already computed above
@@ -121,16 +122,8 @@ bool Ekf::fuseOptFlow(const bool update_terrain)
sym::ComputeFlowYInnovVarAndH(state_vector, P, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
// recalculate the innovation using the updated state
const Vector2f vel_body = predictFlowVelBody();
range = predictFlowRange();
_aid_src_optical_flow.innovation[1] = (-vel_body(0) / range) - _aid_src_optical_flow.observation[1];
if (_aid_src_optical_flow.innovation_variance[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;
}
const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias;
_aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - _aid_src_optical_flow.observation[1];
}
VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index];
@@ -157,7 +150,7 @@ bool Ekf::fuseOptFlow(const bool update_terrain)
return false;
}
float Ekf::predictFlowRange()
float Ekf::predictFlowRange() const
{
// calculate the sensor position relative to the IMU
const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
@@ -180,24 +173,28 @@ float Ekf::predictFlowRange()
return flow_range;
}
Vector2f Ekf::predictFlowVelBody()
Vector2f Ekf::predictFlow(const Vector3f &flow_gyro) const
{
// calculate the sensor position relative to the IMU
const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
// calculate the velocity of the sensor relative to the imu in body frame
// Note: _flow_sample_delayed.gyro_rate is the negative of the body angular velocity, thus use minus sign
const Vector3f vel_rel_imu_body = Vector3f(-(_flow_sample_delayed.gyro_rate - _flow_gyro_bias)) % pos_offset_body;
// Note: flow gyro is the negative of the body angular velocity, thus use minus sign
const Vector3f vel_rel_imu_body = -flow_gyro % pos_offset_body;
// calculate the velocity of the sensor in the earth frame
const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
// rotate into body frame
return _state.quat_nominal.rotateVectorInverse(vel_rel_earth).xy();
const Vector2f vel_body = _state.quat_nominal.rotateVectorInverse(vel_rel_earth).xy();
// calculate range from focal point to centre of image
const float range = predictFlowRange();
return Vector2f(vel_body(1) / range, -vel_body(0) / range);
}
// calculate the measurement variance for the optical flow sensor (rad/sec)^2
float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample)
float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample) const
{
// calculate the observation noise variance - scaling noise linearly across flow quality range
const float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
src/modules/ekf2/EKF/ekf.h
+7 -5
View File
@@ -842,17 +842,19 @@ private:
void updateOnGroundMotionForOpticalFlowChecks();
void resetOnGroundMotionForOpticalFlowChecks();
// calculate the measurement variance for the optical flow sensor
float calcOptFlowMeasVar(const flowSample &flow_sample);
// calculate the measurement variance for the optical flow sensor (rad/sec)^2
float calcOptFlowMeasVar(const flowSample &flow_sample) const;
// calculate optical flow body angular rate compensation
void calcOptFlowBodyRateComp(const imuSample &imu_delayed);
float predictFlowRange() const;
Vector2f predictFlow(const Vector3f &flow_gyro) const;
// fuse optical flow line of sight rate measurements
void updateOptFlow(estimator_aid_source2d_s &aid_src);
void updateOptFlow(estimator_aid_source2d_s &aid_src, const flowSample &flow_sample);
bool fuseOptFlow(bool update_terrain);
float predictFlowRange();
Vector2f predictFlowVelBody();
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_MAGNETOMETER)