forked from Archive/PX4-Autopilot
ekf2: add OF estimator aid src status
This commit is contained in:
Daniel Agar
parent
75c63aee2a
commit
535415a537
|
|
@ -20,4 +20,4 @@ bool fused # true if the sample was successfully fused
|
|||
|
||||
# TOPICS estimator_aid_source_2d
|
||||
# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
|
||||
# TOPICS estimator_aid_src_aux_vel
|
||||
# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow
|
||||
|
|
|
|||
|
|
@ -104,6 +104,7 @@ px4_add_module(
|
|||
EKF/imu_down_sampler.cpp
|
||||
EKF/mag_control.cpp
|
||||
EKF/mag_fusion.cpp
|
||||
EKF/optical_flow_control.cpp
|
||||
EKF/optflow_fusion.cpp
|
||||
EKF/range_finder_consistency_check.cpp
|
||||
EKF/range_height_control.cpp
|
||||
|
|
|
|||
|
|
@ -55,6 +55,7 @@ add_library(ecl_EKF
|
|||
imu_down_sampler.cpp
|
||||
mag_control.cpp
|
||||
mag_fusion.cpp
|
||||
optical_flow_control.cpp
|
||||
optflow_fusion.cpp
|
||||
range_finder_consistency_check.cpp
|
||||
range_height_control.cpp
|
||||
|
|
|
|||
|
|
@ -317,7 +317,7 @@ void Ekf::controlExternalVisionFusion()
|
|||
// check if we have been deadreckoning too long
|
||||
if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) {
|
||||
// only reset velocity if we have no another source of aiding constraining it
|
||||
if (isTimedOut(_time_last_of_fuse, (uint64_t)1E6) &&
|
||||
if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) &&
|
||||
isTimedOut(_time_last_hor_vel_fuse, (uint64_t)1E6)) {
|
||||
|
||||
if (_control_status.flags.ev_vel) {
|
||||
|
|
@ -348,7 +348,7 @@ void Ekf::controlExternalVisionFusion()
|
|||
// check if we have been deadreckoning too long
|
||||
if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) {
|
||||
// only reset velocity if we have no another source of aiding constraining it
|
||||
if (isTimedOut(_time_last_of_fuse, (uint64_t)1E6) &&
|
||||
if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) &&
|
||||
isTimedOut(_time_last_hor_pos_fuse, (uint64_t)1E6)) {
|
||||
resetVelocityToVision();
|
||||
}
|
||||
|
|
@ -414,171 +414,6 @@ void Ekf::controlExternalVisionFusion()
|
|||
}
|
||||
}
|
||||
|
||||
void Ekf::controlOpticalFlowFusion()
|
||||
{
|
||||
// Check if on ground motion is un-suitable for use of optical flow
|
||||
if (!_control_status.flags.in_air) {
|
||||
updateOnGroundMotionForOpticalFlowChecks();
|
||||
|
||||
} else {
|
||||
resetOnGroundMotionForOpticalFlowChecks();
|
||||
}
|
||||
|
||||
// Accumulate autopilot gyro data across the same time interval as the flow sensor
|
||||
_imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.delta_ang_bias;
|
||||
_delta_time_of += _imu_sample_delayed.delta_ang_dt;
|
||||
|
||||
if (_flow_data_ready) {
|
||||
const bool is_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min);
|
||||
const bool is_magnitude_good = !_flow_sample_delayed.flow_xy_rad.longerThan(_flow_sample_delayed.dt * _flow_max_rate);
|
||||
const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
|
||||
|
||||
const float delta_time_min = fmaxf(0.7f * _delta_time_of, 0.001f);
|
||||
const float delta_time_max = fminf(1.3f * _delta_time_of, 0.2f);
|
||||
const bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min && _flow_sample_delayed.dt <= delta_time_max;
|
||||
const bool is_body_rate_comp_available = calcOptFlowBodyRateComp();
|
||||
|
||||
if (is_quality_good
|
||||
&& is_magnitude_good
|
||||
&& is_tilt_good
|
||||
&& is_body_rate_comp_available
|
||||
&& is_delta_time_good) {
|
||||
// compensate for body motion to give a LOS rate
|
||||
_flow_compensated_XY_rad = _flow_sample_delayed.flow_xy_rad - _flow_sample_delayed.gyro_xyz.xy();
|
||||
|
||||
} else if (!_control_status.flags.in_air) {
|
||||
|
||||
if (!is_delta_time_good) {
|
||||
// handle special case of SITL and PX4Flow where dt is forced to
|
||||
// zero when the quaity is 0
|
||||
_flow_sample_delayed.dt = delta_time_min;
|
||||
}
|
||||
|
||||
// don't allow invalid flow gyro_xyz to propagate
|
||||
if (!is_body_rate_comp_available) {
|
||||
if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2))) {
|
||||
_flow_sample_delayed.gyro_xyz.zero();
|
||||
}
|
||||
}
|
||||
|
||||
// when on the ground with poor flow quality,
|
||||
// assume zero ground relative velocity and LOS rate
|
||||
_flow_compensated_XY_rad.setZero();
|
||||
|
||||
} else {
|
||||
// don't use this flow data and wait for the next data to arrive
|
||||
_flow_data_ready = false;
|
||||
_flow_compensated_XY_rad.setZero();
|
||||
}
|
||||
} else {
|
||||
_flow_compensated_XY_rad.setZero();
|
||||
}
|
||||
|
||||
// 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_data_ready) {
|
||||
// Inhibit flow use if motion is un-suitable or we have good quality GPS
|
||||
// Apply hysteresis to prevent rapid mode switching
|
||||
const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f;
|
||||
|
||||
// Check if we are in-air and require optical flow to control position drift
|
||||
const bool is_flow_required = _control_status.flags.in_air
|
||||
&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|
||||
|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)
|
||||
|| (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
|
||||
|
||||
|
||||
// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
|
||||
const bool preflight_motion_not_ok = !_control_status.flags.in_air
|
||||
&& ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5))
|
||||
|| (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6)));
|
||||
const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid();
|
||||
|
||||
_inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required)
|
||||
|| !_control_status.flags.tilt_align;
|
||||
|
||||
// Handle cases where we are using optical flow but we should not use it anymore
|
||||
if (_control_status.flags.opt_flow) {
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW)
|
||||
|| _inhibit_flow_use) {
|
||||
|
||||
stopFlowFusion();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// optical flow fusion mode selection logic
|
||||
if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user
|
||||
&& !_control_status.flags.opt_flow // we are not yet using flow data
|
||||
&& !_inhibit_flow_use) {
|
||||
// If the heading is valid and use is not inhibited , start using optical flow aiding
|
||||
if (_control_status.flags.yaw_align || _params.mag_fusion_type == MagFuseType::NONE) {
|
||||
// set the flag and reset the fusion timeout
|
||||
ECL_INFO("starting optical flow fusion");
|
||||
_control_status.flags.opt_flow = true;
|
||||
_time_last_of_fuse = _imu_sample_delayed.time_us;
|
||||
|
||||
// if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set
|
||||
const bool flow_aid_only = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow);
|
||||
|
||||
if (flow_aid_only) {
|
||||
resetHorizontalVelocityToOpticalFlow();
|
||||
resetHorizontalPositionToOpticalFlow();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_control_status.flags.opt_flow) {
|
||||
// Wait until the midpoint of the flow sample has fallen behind the fusion time horizon
|
||||
if (_imu_sample_delayed.time_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
|
||||
// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
|
||||
// but use a relaxed time criteria to enable it to coast through bad range finder data
|
||||
if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) {
|
||||
fuseOptFlow();
|
||||
_last_known_pos.xy() = _state.pos.xy();
|
||||
}
|
||||
|
||||
_flow_data_ready = false;
|
||||
}
|
||||
|
||||
// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
|
||||
if (isTimedOut(_time_last_of_fuse, _params.reset_timeout_max)
|
||||
&& !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) {
|
||||
|
||||
resetHorizontalVelocityToOpticalFlow();
|
||||
resetHorizontalPositionToOpticalFlow();
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.opt_flow && !isRecent(_flow_sample_delayed.time_us, (uint64_t)10e6)) {
|
||||
|
||||
stopFlowFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::updateOnGroundMotionForOpticalFlowChecks()
|
||||
{
|
||||
// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation
|
||||
const float accel_norm = _accel_vec_filt.norm();
|
||||
|
||||
const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit
|
||||
|| (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
|
||||
|| (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
|
||||
|| (_R_to_earth(2, 2) < cosf(math::radians(30.0f)))); // tilted excessively
|
||||
|
||||
if (motion_is_excessive) {
|
||||
_time_bad_motion_us = _imu_sample_delayed.time_us;
|
||||
|
||||
} else {
|
||||
_time_good_motion_us = _imu_sample_delayed.time_us;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::resetOnGroundMotionForOpticalFlowChecks()
|
||||
{
|
||||
_time_bad_motion_us = 0;
|
||||
_time_good_motion_us = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
|
||||
{
|
||||
if (!(_params.gnss_ctrl & GnssCtrl::YAW)
|
||||
|
|
@ -792,5 +627,5 @@ bool Ekf::hasHorizontalAidingTimedOut() const
|
|||
{
|
||||
return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
|
||||
&& isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -206,7 +206,6 @@ bool Ekf::initialiseFilter()
|
|||
_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
|
||||
_time_last_hagl_fuse = _imu_sample_delayed.time_us;
|
||||
_time_last_flow_terrain_fuse = _imu_sample_delayed.time_us;
|
||||
_time_last_of_fuse = _imu_sample_delayed.time_us;
|
||||
|
||||
// reset the output predictor state history to match the EKF initial values
|
||||
alignOutputFilter();
|
||||
|
|
@ -239,7 +238,7 @@ bool Ekf::initialiseTilt()
|
|||
void Ekf::predictState()
|
||||
{
|
||||
// apply imu bias corrections
|
||||
const Vector3f delta_ang_bias_scaled = (_state.delta_ang_bias / _dt_ekf_avg) * _imu_sample_delayed.delta_ang_dt;
|
||||
const Vector3f delta_ang_bias_scaled = getGyroBias() * _imu_sample_delayed.delta_ang_dt;
|
||||
Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - delta_ang_bias_scaled;
|
||||
|
||||
// subtract component of angular rate due to earth rotation
|
||||
|
|
@ -252,7 +251,7 @@ void Ekf::predictState()
|
|||
_R_to_earth = Dcmf(_state.quat_nominal);
|
||||
|
||||
// Calculate an earth frame delta velocity
|
||||
const Vector3f delta_vel_bias_scaled = (_state.delta_vel_bias / _dt_ekf_avg) * _imu_sample_delayed.delta_vel_dt;
|
||||
const Vector3f delta_vel_bias_scaled = getAccelBias() * _imu_sample_delayed.delta_vel_dt;
|
||||
const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - delta_vel_bias_scaled;
|
||||
const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel;
|
||||
|
||||
|
|
@ -306,7 +305,7 @@ void Ekf::calculateOutputStates(const imuSample &imu)
|
|||
// Use full rate IMU data at the current time horizon
|
||||
|
||||
// correct delta angles for bias offsets
|
||||
const Vector3f delta_ang_bias_scaled = (_state.delta_ang_bias / _dt_ekf_avg) * imu.delta_ang_dt;
|
||||
const Vector3f delta_ang_bias_scaled = getGyroBias() * imu.delta_ang_dt;
|
||||
|
||||
// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
|
||||
const Vector3f delta_angle(imu.delta_ang - delta_ang_bias_scaled + _delta_angle_corr);
|
||||
|
|
@ -517,7 +516,7 @@ Quatf Ekf::calculate_quaternion() const
|
|||
{
|
||||
// Correct delta angle data for bias errors using bias state estimates from the EKF and also apply
|
||||
// corrections required to track the EKF quaternion states
|
||||
const Vector3f delta_ang_bias_scaled = (_state.delta_ang_bias / _dt_ekf_avg) * _newest_high_rate_imu_sample.delta_ang_dt;
|
||||
const Vector3f delta_ang_bias_scaled = getGyroBias() * _newest_high_rate_imu_sample.delta_ang_dt;
|
||||
|
||||
const Vector3f delta_angle{_newest_high_rate_imu_sample.delta_ang - delta_ang_bias_scaled + _delta_angle_corr};
|
||||
|
||||
|
|
|
|||
|
|
@ -101,9 +101,9 @@ public:
|
|||
void getAuxVelInnovVar(float aux_vel_innov[2]) const;
|
||||
void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); }
|
||||
|
||||
void getFlowInnov(float flow_innov[2]) const { _flow_innov.copyTo(flow_innov); }
|
||||
void getFlowInnovVar(float flow_innov_var[2]) const { _flow_innov_var.copyTo(flow_innov_var); }
|
||||
void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = _optflow_test_ratio; }
|
||||
void getFlowInnov(float flow_innov[2]) const;
|
||||
void getFlowInnovVar(float flow_innov_var[2]) const;
|
||||
void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); }
|
||||
|
||||
const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
|
||||
const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
|
||||
|
|
@ -437,6 +437,8 @@ public:
|
|||
|
||||
const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
|
||||
|
||||
const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
|
||||
|
||||
private:
|
||||
|
||||
// set the internal states and status to their default value
|
||||
|
|
@ -497,7 +499,6 @@ private:
|
|||
uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec)
|
||||
uint64_t _time_last_heading_fuse{0};
|
||||
|
||||
uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec)
|
||||
uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
|
||||
uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
|
||||
uint64_t _time_last_healthy_rng_data{0};
|
||||
|
|
@ -540,8 +541,6 @@ private:
|
|||
float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2)
|
||||
|
||||
// optical flow processing
|
||||
Vector2f _flow_innov{}; ///< flow measurement innovation (rad/sec)
|
||||
Vector2f _flow_innov_var{}; ///< flow innovation variance ((rad/sec)**2)
|
||||
Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
|
||||
Vector2f _flow_vel_body{}; ///< velocity from corrected flow measurement (body frame)(m/s)
|
||||
Vector2f _flow_vel_ne{}; ///< velocity from corrected flow measurement (local frame) (m/s)
|
||||
|
|
@ -550,7 +549,6 @@ private:
|
|||
float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec)
|
||||
uint64_t _time_bad_motion_us{0}; ///< last system time that on-ground motion exceeded limits (uSec)
|
||||
uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec)
|
||||
bool _inhibit_flow_use{false}; ///< true when use of optical flow and range finder is being inhibited
|
||||
Vector2f _flow_compensated_XY_rad{}; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
|
||||
|
||||
estimator_aid_source_1d_s _aid_src_baro_hgt{};
|
||||
|
|
@ -576,6 +574,8 @@ private:
|
|||
|
||||
estimator_aid_source_2d_s _aid_src_aux_vel{};
|
||||
|
||||
estimator_aid_source_2d_s _aid_src_optical_flow{};
|
||||
|
||||
// output predictor states
|
||||
Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad)
|
||||
Vector3f _vel_err_integ{}; ///< integral of velocity tracking error (m)
|
||||
|
|
@ -708,7 +708,7 @@ private:
|
|||
void resetVerticalVelocityTo(float new_vert_vel);
|
||||
|
||||
void resetVelocityToGps(const gpsSample &gps_sample);
|
||||
void resetHorizontalVelocityToOpticalFlow();
|
||||
void resetHorizontalVelocityToOpticalFlow(const flowSample &flow_sample);
|
||||
void resetVelocityToVision();
|
||||
void resetHorizontalVelocityToZero();
|
||||
|
||||
|
|
@ -732,6 +732,7 @@ private:
|
|||
void resetVerticalVelocityToZero();
|
||||
|
||||
// fuse optical flow line of sight rate measurements
|
||||
void updateOptFlow(estimator_aid_source_2d_s &aid_src);
|
||||
void fuseOptFlow();
|
||||
|
||||
// horizontal and vertical position aid source
|
||||
|
|
@ -992,7 +993,7 @@ private:
|
|||
float getRngHeightVariance() const;
|
||||
|
||||
// calculate the measurement variance for the optical flow sensor
|
||||
float calcOptFlowMeasVar();
|
||||
float calcOptFlowMeasVar(const flowSample &flow_sample);
|
||||
|
||||
// rotate quaternion covariances into variances for an equivalent rotation vector
|
||||
Vector3f calcRotVecVariances();
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ void Ekf::resetVelocityToGps(const gpsSample &gps_sample)
|
|||
P.uncorrelateCovarianceSetVariance<3>(4, sq(gps_sample.sacc));
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalVelocityToOpticalFlow()
|
||||
void Ekf::resetHorizontalVelocityToOpticalFlow(const flowSample &flow_sample)
|
||||
{
|
||||
_information_events.flags.reset_vel_to_flow = true;
|
||||
ECL_INFO("reset velocity to flow");
|
||||
|
|
@ -66,8 +66,8 @@ void Ekf::resetHorizontalVelocityToOpticalFlow()
|
|||
// 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_delayed.dt;
|
||||
vel_optflow_body(1) = range * _flow_compensated_XY_rad(0) / _flow_sample_delayed.dt;
|
||||
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
|
||||
|
|
@ -80,7 +80,7 @@ void Ekf::resetHorizontalVelocityToOpticalFlow()
|
|||
}
|
||||
|
||||
// reset the horizontal velocity variance using the optical flow noise variance
|
||||
P.uncorrelateCovarianceSetVariance<2>(4, sq(range) * calcOptFlowMeasVar());
|
||||
P.uncorrelateCovarianceSetVariance<2>(4, sq(range) * calcOptFlowMeasVar(flow_sample));
|
||||
}
|
||||
|
||||
void Ekf::resetVelocityToVision()
|
||||
|
|
@ -172,13 +172,14 @@ void Ekf::resetHorizontalPositionToVision()
|
|||
void Ekf::resetHorizontalPositionToOpticalFlow()
|
||||
{
|
||||
_information_events.flags.reset_pos_to_last_known = true;
|
||||
ECL_INFO("reset position to last known position");
|
||||
|
||||
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());
|
||||
}
|
||||
|
||||
|
|
@ -623,6 +624,18 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
|
|||
aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1];
|
||||
}
|
||||
|
||||
void Ekf::getFlowInnov(float flow_innov[2]) const
|
||||
{
|
||||
flow_innov[0] = _aid_src_optical_flow.innovation[0];
|
||||
flow_innov[1] = _aid_src_optical_flow.innovation[1];
|
||||
}
|
||||
|
||||
void Ekf::getFlowInnovVar(float flow_innov_var[2]) const
|
||||
{
|
||||
flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0];
|
||||
flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1];
|
||||
}
|
||||
|
||||
// get the state vector at the delayed time horizon
|
||||
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
|
||||
{
|
||||
|
|
@ -761,7 +774,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
|
|||
|
||||
if (_control_status.flags.opt_flow) {
|
||||
float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
|
||||
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * _flow_innov.norm();
|
||||
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
|
||||
}
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
|
|
@ -927,7 +940,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
|||
}
|
||||
|
||||
if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
|
||||
float of_vel = sqrtf(_optflow_test_ratio);
|
||||
float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max());
|
||||
vel = math::max(of_vel, FLT_MIN);
|
||||
}
|
||||
|
||||
|
|
@ -1040,7 +1053,7 @@ void Ekf::updateHorizontalDeadReckoningstatus()
|
|||
&& (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
|
||||
|| isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));
|
||||
|
||||
const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max);
|
||||
const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
|
||||
|
||||
const bool airDataAiding = _control_status.flags.wind &&
|
||||
isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) &&
|
||||
|
|
@ -1618,9 +1631,8 @@ void Ekf::stopFlowFusion()
|
|||
if (_control_status.flags.opt_flow) {
|
||||
ECL_INFO("stopping optical flow fusion");
|
||||
_control_status.flags.opt_flow = false;
|
||||
_flow_innov.setZero();
|
||||
_flow_innov_var.setZero();
|
||||
_optflow_test_ratio = 0.0f;
|
||||
|
||||
resetEstimatorAidStatus(_aid_src_optical_flow);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -342,7 +342,6 @@ protected:
|
|||
// innovation consistency check monitoring ratios
|
||||
AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
|
||||
|
||||
float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio
|
||||
float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio
|
||||
Vector2f _drag_test_ratio{}; // drag innovation consistency check ratio
|
||||
innovation_fault_status_u _innov_check_fail_status{};
|
||||
|
|
|
|||
|
|
@ -79,6 +79,9 @@ void Ekf::controlFakePosFusion()
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.fake_pos && isHorizontalAidingActive()) {
|
||||
stopFakePosFusion();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -47,23 +47,10 @@
|
|||
#include <float.h>
|
||||
#include "utils.hpp"
|
||||
|
||||
void Ekf::fuseOptFlow()
|
||||
void Ekf::updateOptFlow(estimator_aid_source_2d_s &aid_src)
|
||||
{
|
||||
float gndclearance = fmaxf(_params.rng_gnd_clearance, 0.1f);
|
||||
|
||||
// get latest estimated orientation
|
||||
const float q0 = _state.quat_nominal(0);
|
||||
const float q1 = _state.quat_nominal(1);
|
||||
const float q2 = _state.quat_nominal(2);
|
||||
const float q3 = _state.quat_nominal(3);
|
||||
|
||||
// get latest velocity in earth frame
|
||||
const float vn = _state.vel(0);
|
||||
const float ve = _state.vel(1);
|
||||
const float vd = _state.vel(2);
|
||||
|
||||
// calculate the optical flow observation variance
|
||||
const float R_LOS = calcOptFlowMeasVar();
|
||||
resetEstimatorAidStatus(aid_src);
|
||||
aid_src.timestamp_sample = _flow_sample_delayed.time_us;
|
||||
|
||||
// get rotation matrix from earth to body
|
||||
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
|
||||
|
|
@ -81,35 +68,64 @@ void Ekf::fuseOptFlow()
|
|||
// rotate into body frame
|
||||
const Vector3f vel_body = earth_to_body * vel_rel_earth;
|
||||
|
||||
// height above ground of the IMU
|
||||
float heightAboveGndEst = _terrain_vpos - _state.pos(2);
|
||||
|
||||
// calculate the sensor position relative to the IMU in earth frame
|
||||
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
||||
|
||||
// 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.
|
||||
heightAboveGndEst -= pos_offset_earth(2);
|
||||
|
||||
// constrain minimum height above ground
|
||||
heightAboveGndEst = math::max(heightAboveGndEst, gndclearance);
|
||||
const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
|
||||
|
||||
// calculate range from focal point to centre of image
|
||||
const float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view
|
||||
const float range = height_above_gnd_est / earth_to_body(2, 2); // absolute distance to the frame region in view
|
||||
|
||||
// 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_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias);
|
||||
const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt;
|
||||
|
||||
// compute the velocities in body and local frames from corrected optical flow measurement
|
||||
// for logging only
|
||||
// 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_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));
|
||||
|
||||
_flow_innov(0) = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
|
||||
_flow_innov(1) = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis
|
||||
aid_src.observation[0] = opt_flow_rate(0); // flow around the X axis
|
||||
aid_src.observation[1] = opt_flow_rate(1); // flow around the Y axis
|
||||
|
||||
aid_src.innovation[0] = (vel_body(1) / range) - aid_src.observation[0];
|
||||
aid_src.innovation[1] = (-vel_body(0) / range) - aid_src.observation[1];
|
||||
|
||||
// calculate the optical flow observation variance
|
||||
const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
|
||||
aid_src.observation_variance[0] = R_LOS;
|
||||
aid_src.observation_variance[1] = R_LOS;
|
||||
}
|
||||
|
||||
void Ekf::fuseOptFlow()
|
||||
{
|
||||
_aid_src_optical_flow.fusion_enabled = true;
|
||||
|
||||
const float R_LOS = _aid_src_optical_flow.observation_variance[0];
|
||||
|
||||
// get latest estimated orientation
|
||||
const float q0 = _state.quat_nominal(0);
|
||||
const float q1 = _state.quat_nominal(1);
|
||||
const float q2 = _state.quat_nominal(2);
|
||||
const float q3 = _state.quat_nominal(3);
|
||||
|
||||
// get latest velocity in earth frame
|
||||
const float vn = _state.vel(0);
|
||||
const float ve = _state.vel(1);
|
||||
const float vd = _state.vel(2);
|
||||
|
||||
// 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.
|
||||
const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
|
||||
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
||||
const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
|
||||
|
||||
// calculate range from focal point to centre of image
|
||||
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
|
||||
const float range = height_above_gnd_est / earth_to_body(2, 2); // absolute distance to the frame region in view
|
||||
|
||||
// The derivation allows for an arbitrary body to flow sensor frame rotation which is
|
||||
// currently not supported by the EKF, so assume sensor frame is aligned with the
|
||||
|
|
@ -170,14 +186,14 @@ void Ekf::fuseOptFlow()
|
|||
// const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
|
||||
|
||||
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
||||
_flow_innov_var(0) = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
|
||||
_aid_src_optical_flow.innovation_variance[0] = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
|
||||
|
||||
if (_flow_innov_var(0) < R_LOS) {
|
||||
if (_aid_src_optical_flow.innovation_variance[0] < R_LOS) {
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
return;
|
||||
}
|
||||
const float HK50 = HK4/_flow_innov_var(0);
|
||||
const float HK50 = HK4 / _aid_src_optical_flow.innovation_variance[0];
|
||||
|
||||
const float HK51 = Tbs(0,1)*q1;
|
||||
const float HK52 = Tbs(0,2)*q0;
|
||||
|
|
@ -226,110 +242,102 @@ void Ekf::fuseOptFlow()
|
|||
// const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
|
||||
|
||||
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
|
||||
_flow_innov_var(1) = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
|
||||
if (_flow_innov_var(1) < R_LOS) {
|
||||
_aid_src_optical_flow.innovation_variance[1] = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
|
||||
if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
return;
|
||||
}
|
||||
const float HK95 = HK4/_flow_innov_var(1);
|
||||
|
||||
const float HK95 = HK4 / _aid_src_optical_flow.innovation_variance[1];
|
||||
|
||||
// run the innovation consistency check and record result
|
||||
bool all_innovation_checks_passed = true;
|
||||
float test_ratio[2];
|
||||
test_ratio[0] = sq(_flow_innov(0)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(0));
|
||||
test_ratio[1] = sq(_flow_innov(1)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(1));
|
||||
_optflow_test_ratio = math::max(test_ratio[0], test_ratio[1]);
|
||||
setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
|
||||
|
||||
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
|
||||
const bool innov_check_fail = (test_ratio[obs_index] > 1.0f);
|
||||
|
||||
if (innov_check_fail) {
|
||||
all_innovation_checks_passed = false;
|
||||
}
|
||||
|
||||
if (obs_index == 0) {
|
||||
_innov_check_fail_status.flags.reject_optflow_X = innov_check_fail;
|
||||
|
||||
} else {
|
||||
_innov_check_fail_status.flags.reject_optflow_Y = innov_check_fail;
|
||||
}
|
||||
}
|
||||
_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);
|
||||
|
||||
// if either axis fails we abort the fusion
|
||||
if (!all_innovation_checks_passed) {
|
||||
if (_aid_src_optical_flow.innovation_rejected) {
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
bool fused[2] {false, false};
|
||||
|
||||
// fuse observation axes sequentially
|
||||
SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
|
||||
Vector24f Kfusion; // Optical flow Kalman gains
|
||||
{
|
||||
// Optical flow observation Jacobians - axis 0
|
||||
SparseVector24f<0,1,2,3,4,5,6> Hfusion;
|
||||
Hfusion.at<0>() = HK3*HK5;
|
||||
Hfusion.at<1>() = HK5*HK7;
|
||||
Hfusion.at<2>() = HK5*HK8;
|
||||
Hfusion.at<3>() = HK5*HK9;
|
||||
Hfusion.at<4>() = HK25*HK4;
|
||||
Hfusion.at<5>() = HK33*HK4;
|
||||
Hfusion.at<6>() = HK37*HK4;
|
||||
|
||||
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
|
||||
// Optical flow Kalman gains - axis 0
|
||||
Vector24f Kfusion;
|
||||
Kfusion(0) = HK42*HK50;
|
||||
Kfusion(1) = HK49*HK50;
|
||||
Kfusion(2) = HK47*HK50;
|
||||
Kfusion(3) = HK48*HK50;
|
||||
Kfusion(4) = HK46*HK50;
|
||||
Kfusion(5) = HK45*HK50;
|
||||
Kfusion(6) = HK44*HK50;
|
||||
|
||||
// calculate observation Jocobians and Kalman gains
|
||||
if (obs_index == 0) {
|
||||
// Observation Jacobians - axis 0
|
||||
Hfusion.at<0>() = HK3*HK5;
|
||||
Hfusion.at<1>() = HK5*HK7;
|
||||
Hfusion.at<2>() = HK5*HK8;
|
||||
Hfusion.at<3>() = HK5*HK9;
|
||||
Hfusion.at<4>() = HK25*HK4;
|
||||
Hfusion.at<5>() = HK33*HK4;
|
||||
Hfusion.at<6>() = HK37*HK4;
|
||||
for (unsigned row = 7; row <= 23; row++) {
|
||||
Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row));
|
||||
}
|
||||
|
||||
// Kalman gains - axis 0
|
||||
Kfusion(0) = HK42*HK50;
|
||||
Kfusion(1) = HK49*HK50;
|
||||
Kfusion(2) = HK47*HK50;
|
||||
Kfusion(3) = HK48*HK50;
|
||||
Kfusion(4) = HK46*HK50;
|
||||
Kfusion(5) = HK45*HK50;
|
||||
Kfusion(6) = HK44*HK50;
|
||||
|
||||
for (unsigned row = 7; row <= 23; row++) {
|
||||
Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row));
|
||||
}
|
||||
if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[0])) {
|
||||
fused[0] = true;
|
||||
_fault_status.flags.bad_optflow_X = false;
|
||||
|
||||
} else {
|
||||
// Observation Jacobians - axis 1
|
||||
Hfusion.at<0>() = -HK5*HK63;
|
||||
Hfusion.at<1>() = -HK5*HK66;
|
||||
Hfusion.at<2>() = -HK5*HK68;
|
||||
Hfusion.at<3>() = -HK5*HK70;
|
||||
Hfusion.at<4>() = -HK4*HK74;
|
||||
Hfusion.at<5>() = -HK4*HK77;
|
||||
Hfusion.at<6>() = -HK4*HK79;
|
||||
_fault_status.flags.bad_optflow_X = true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Kalman gains - axis 1
|
||||
Kfusion(0) = -HK87*HK95;
|
||||
Kfusion(1) = -HK94*HK95;
|
||||
Kfusion(2) = -HK91*HK95;
|
||||
Kfusion(3) = -HK93*HK95;
|
||||
Kfusion(4) = -HK90*HK95;
|
||||
Kfusion(5) = -HK89*HK95;
|
||||
Kfusion(6) = -HK88*HK95;
|
||||
{
|
||||
// Optical flow observation Jacobians - axis 1
|
||||
SparseVector24f<0,1,2,3,4,5,6> Hfusion;
|
||||
Hfusion.at<0>() = -HK5*HK63;
|
||||
Hfusion.at<1>() = -HK5*HK66;
|
||||
Hfusion.at<2>() = -HK5*HK68;
|
||||
Hfusion.at<3>() = -HK5*HK70;
|
||||
Hfusion.at<4>() = -HK4*HK74;
|
||||
Hfusion.at<5>() = -HK4*HK77;
|
||||
Hfusion.at<6>() = -HK4*HK79;
|
||||
|
||||
for (unsigned row = 7; row <= 23; row++) {
|
||||
Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row));
|
||||
}
|
||||
// Optical flow Kalman gains - axis 1
|
||||
Vector24f Kfusion;
|
||||
Kfusion(0) = -HK87*HK95;
|
||||
Kfusion(1) = -HK94*HK95;
|
||||
Kfusion(2) = -HK91*HK95;
|
||||
Kfusion(3) = -HK93*HK95;
|
||||
Kfusion(4) = -HK90*HK95;
|
||||
Kfusion(5) = -HK89*HK95;
|
||||
Kfusion(6) = -HK88*HK95;
|
||||
|
||||
for (unsigned row = 7; row <= 23; row++) {
|
||||
Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row));
|
||||
}
|
||||
|
||||
const bool is_fused = measurementUpdate(Kfusion, Hfusion, _flow_innov(obs_index));
|
||||
if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[1])) {
|
||||
fused[1] = true;
|
||||
_fault_status.flags.bad_optflow_Y = false;
|
||||
|
||||
if (obs_index == 0) {
|
||||
_fault_status.flags.bad_optflow_X = !is_fused;
|
||||
|
||||
} else if (obs_index == 1) {
|
||||
_fault_status.flags.bad_optflow_Y = !is_fused;
|
||||
} else {
|
||||
_fault_status.flags.bad_optflow_Y = true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (is_fused) {
|
||||
_time_last_of_fuse = _imu_sample_delayed.time_us;
|
||||
}
|
||||
if (fused[0] && fused[1]) {
|
||||
_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
_aid_src_optical_flow.fused = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -337,13 +345,6 @@ void Ekf::fuseOptFlow()
|
|||
// returns false if bias corrected body rate data is unavailable
|
||||
bool Ekf::calcOptFlowBodyRateComp()
|
||||
{
|
||||
// reset the accumulators if the time interval is too large
|
||||
if (_delta_time_of > 1.0f) {
|
||||
_imu_del_ang_of.setZero();
|
||||
_delta_time_of = 0.0f;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool is_body_rate_comp_available = false;
|
||||
const bool use_flow_sensor_gyro = PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2));
|
||||
|
||||
|
|
@ -362,6 +363,9 @@ bool Ekf::calcOptFlowBodyRateComp()
|
|||
// calculate the bias estimate using a combined LPF and spike filter
|
||||
_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f;
|
||||
|
||||
// apply gyro bias
|
||||
_flow_sample_delayed.gyro_xyz -= (_flow_gyro_bias * _flow_sample_delayed.dt);
|
||||
|
||||
is_body_rate_comp_available = true;
|
||||
}
|
||||
|
||||
|
|
@ -370,6 +374,7 @@ bool Ekf::calcOptFlowBodyRateComp()
|
|||
// for clarification of the sign see definition of flowSample and imuSample in common.h
|
||||
if ((_delta_time_of > FLT_EPSILON)
|
||||
&& (_flow_sample_delayed.dt > FLT_EPSILON)) {
|
||||
|
||||
_flow_sample_delayed.gyro_xyz = -_imu_del_ang_of / _delta_time_of * _flow_sample_delayed.dt;
|
||||
_flow_gyro_bias.zero();
|
||||
|
||||
|
|
@ -384,25 +389,24 @@ bool Ekf::calcOptFlowBodyRateComp()
|
|||
}
|
||||
|
||||
// calculate the measurement variance for the optical flow sensor (rad/sec)^2
|
||||
float Ekf::calcOptFlowMeasVar()
|
||||
float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample)
|
||||
{
|
||||
// calculate the observation noise variance - scaling noise linearly across flow quality range
|
||||
const float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
|
||||
const float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
|
||||
|
||||
// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
|
||||
float weighting = (255.0f - (float)_params.flow_qual_min);
|
||||
float weighting = (255.f - (float)_params.flow_qual_min);
|
||||
|
||||
if (weighting >= 1.0f) {
|
||||
weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f,
|
||||
1.0f);
|
||||
if (weighting >= 1.f) {
|
||||
weighting = math::constrain((float)(flow_sample.quality - _params.flow_qual_min) / weighting, 0.f, 1.f);
|
||||
|
||||
} else {
|
||||
weighting = 0.0f;
|
||||
}
|
||||
|
||||
// take the weighted average of the observation noise for the best and wort flow quality
|
||||
const float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
|
||||
const float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.f - weighting));
|
||||
|
||||
return R_LOS;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,230 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file optical_flow_control.cpp
|
||||
* Control functions for optical flow fusion
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
|
||||
void Ekf::controlOpticalFlowFusion()
|
||||
{
|
||||
// Check if on ground motion is un-suitable for use of optical flow
|
||||
if (!_control_status.flags.in_air) {
|
||||
updateOnGroundMotionForOpticalFlowChecks();
|
||||
|
||||
} else {
|
||||
resetOnGroundMotionForOpticalFlowChecks();
|
||||
}
|
||||
|
||||
// Accumulate autopilot gyro data across the same time interval as the flow sensor
|
||||
const Vector3f delta_angle(_imu_sample_delayed.delta_ang - (getGyroBias() * _imu_sample_delayed.delta_ang_dt));
|
||||
if (_delta_time_of < 0.1f) {
|
||||
_imu_del_ang_of += delta_angle;
|
||||
_delta_time_of += _imu_sample_delayed.delta_ang_dt;
|
||||
|
||||
} else {
|
||||
// reset the accumulators if the time interval is too large
|
||||
_imu_del_ang_of = delta_angle;
|
||||
_delta_time_of = _imu_sample_delayed.delta_ang_dt;
|
||||
}
|
||||
|
||||
if (_flow_data_ready) {
|
||||
const bool is_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min);
|
||||
const bool is_magnitude_good = !_flow_sample_delayed.flow_xy_rad.longerThan(_flow_sample_delayed.dt * _flow_max_rate);
|
||||
const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
|
||||
|
||||
const float delta_time_min = fmaxf(0.7f * _delta_time_of, 0.001f);
|
||||
const float delta_time_max = fminf(1.3f * _delta_time_of, 0.2f);
|
||||
bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min && _flow_sample_delayed.dt <= delta_time_max;
|
||||
|
||||
if (!is_delta_time_good && (_flow_sample_delayed.dt > FLT_EPSILON)) {
|
||||
|
||||
if (fabsf(_imu_sample_delayed.delta_ang_dt - _flow_sample_delayed.dt) < 0.1f) {
|
||||
// reset accumulators to current IMU
|
||||
_imu_del_ang_of = delta_angle;
|
||||
_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);
|
||||
}
|
||||
}
|
||||
|
||||
const bool is_body_rate_comp_available = calcOptFlowBodyRateComp();
|
||||
|
||||
// don't allow invalid flow gyro_xyz to propagate
|
||||
if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2))) {
|
||||
|
||||
_flow_sample_delayed.gyro_xyz.zero();
|
||||
}
|
||||
|
||||
if (is_quality_good
|
||||
&& is_magnitude_good
|
||||
&& is_tilt_good
|
||||
&& is_body_rate_comp_available
|
||||
&& is_delta_time_good) {
|
||||
// compensate for body motion to give a LOS rate
|
||||
_flow_compensated_XY_rad = _flow_sample_delayed.flow_xy_rad - _flow_sample_delayed.gyro_xyz.xy();
|
||||
|
||||
} else if (!_control_status.flags.in_air) {
|
||||
|
||||
if (!is_delta_time_good) {
|
||||
// handle special case of SITL and PX4Flow where dt is forced to
|
||||
// zero when the quaity is 0
|
||||
_flow_sample_delayed.dt = delta_time_min;
|
||||
}
|
||||
|
||||
// when on the ground with poor flow quality,
|
||||
// assume zero ground relative velocity and LOS rate
|
||||
_flow_compensated_XY_rad.setZero();
|
||||
|
||||
} else {
|
||||
// don't use this flow data and wait for the next data to arrive
|
||||
_flow_data_ready = false;
|
||||
_flow_compensated_XY_rad.setZero();
|
||||
}
|
||||
|
||||
updateOptFlow(_aid_src_optical_flow);
|
||||
|
||||
} else {
|
||||
_flow_compensated_XY_rad.setZero();
|
||||
}
|
||||
|
||||
// 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_data_ready) {
|
||||
|
||||
// Inhibit flow use if motion is un-suitable or we have good quality GPS
|
||||
// Apply hysteresis to prevent rapid mode switching
|
||||
const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f;
|
||||
|
||||
// Check if we are in-air and require optical flow to control position drift
|
||||
const bool is_flow_required = _control_status.flags.in_air
|
||||
&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|
||||
|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)
|
||||
|| (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
|
||||
|
||||
// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
|
||||
const bool preflight_motion_not_ok = !_control_status.flags.in_air
|
||||
&& ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5))
|
||||
|| (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6)));
|
||||
const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid();
|
||||
|
||||
const bool inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required)
|
||||
|| !_control_status.flags.tilt_align;
|
||||
|
||||
// Handle cases where we are using optical flow but we should not use it anymore
|
||||
if (_control_status.flags.opt_flow) {
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW)
|
||||
|| inhibit_flow_use) {
|
||||
|
||||
stopFlowFusion();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// optical flow fusion mode selection logic
|
||||
if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user
|
||||
&& !_control_status.flags.opt_flow // we are not yet using flow data
|
||||
&& !inhibit_flow_use) {
|
||||
|
||||
// set the flag and reset the fusion timeout
|
||||
ECL_INFO("starting optical flow fusion");
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
||||
_control_status.flags.opt_flow = true;
|
||||
_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (_control_status.flags.opt_flow) {
|
||||
// Wait until the midpoint of the flow sample has fallen behind the fusion time horizon
|
||||
if (_imu_sample_delayed.time_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
|
||||
// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
|
||||
// but use a relaxed time criteria to enable it to coast through bad range finder data
|
||||
if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) {
|
||||
fuseOptFlow();
|
||||
_last_known_pos.xy() = _state.pos.xy();
|
||||
}
|
||||
|
||||
_flow_data_ready = false;
|
||||
}
|
||||
|
||||
// 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)
|
||||
&& !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) {
|
||||
|
||||
resetHorizontalVelocityToOpticalFlow(_flow_sample_delayed);
|
||||
resetHorizontalPositionToOpticalFlow();
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.opt_flow && !isRecent(_flow_sample_delayed.time_us, (uint64_t)10e6)) {
|
||||
|
||||
stopFlowFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::updateOnGroundMotionForOpticalFlowChecks()
|
||||
{
|
||||
// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation
|
||||
const float accel_norm = _accel_vec_filt.norm();
|
||||
|
||||
const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit
|
||||
|| (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
|
||||
|| (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
|
||||
|| (_R_to_earth(2, 2) < cosf(math::radians(30.0f)))); // tilted excessively
|
||||
|
||||
if (motion_is_excessive) {
|
||||
_time_bad_motion_us = _imu_sample_delayed.time_us;
|
||||
|
||||
} else {
|
||||
_time_good_motion_us = _imu_sample_delayed.time_us;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::resetOnGroundMotionForOpticalFlowChecks()
|
||||
{
|
||||
_time_bad_motion_us = 0;
|
||||
_time_good_motion_us = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
|
@ -193,11 +193,15 @@ void Ekf::resetHaglRng()
|
|||
|
||||
void Ekf::stopHaglRngFusion()
|
||||
{
|
||||
_hagl_sensor_status.flags.range_finder = false;
|
||||
_hagl_innov = 0.f;
|
||||
_hagl_innov_var = 0.f;
|
||||
_hagl_test_ratio = 0.f;
|
||||
_innov_check_fail_status.flags.reject_hagl = false;
|
||||
if (_hagl_sensor_status.flags.range_finder) {
|
||||
|
||||
_hagl_innov = 0.f;
|
||||
_hagl_innov_var = 0.f;
|
||||
_hagl_test_ratio = 0.f;
|
||||
_innov_check_fail_status.flags.reject_hagl = false;
|
||||
|
||||
_hagl_sensor_status.flags.range_finder = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseHaglRng()
|
||||
|
|
@ -277,7 +281,7 @@ void Ekf::controlHaglFlowFusion()
|
|||
}
|
||||
|
||||
} else if (_hagl_sensor_status.flags.flow
|
||||
&& (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) {
|
||||
&& (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) {
|
||||
// No data anymore. Stop until it comes back.
|
||||
stopHaglFlowFusion();
|
||||
}
|
||||
|
|
@ -293,11 +297,10 @@ void Ekf::startHaglFlowFusion()
|
|||
|
||||
void Ekf::stopHaglFlowFusion()
|
||||
{
|
||||
_hagl_sensor_status.flags.flow = false;
|
||||
_hagl_innov = 0.f;
|
||||
_hagl_innov_var = 0.f;
|
||||
_hagl_test_ratio = 0.f;
|
||||
_innov_check_fail_status.flags.reject_hagl = false;
|
||||
if (_hagl_sensor_status.flags.flow) {
|
||||
|
||||
_hagl_sensor_status.flags.flow = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::resetHaglFlow()
|
||||
|
|
@ -310,10 +313,12 @@ void Ekf::resetHaglFlow()
|
|||
|
||||
void Ekf::fuseFlowForTerrain()
|
||||
{
|
||||
_aid_src_optical_flow.fusion_enabled = true;
|
||||
|
||||
// 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_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias);
|
||||
const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt;
|
||||
|
||||
// get latest estimated orientation
|
||||
const float q0 = _state.quat_nominal(0);
|
||||
|
|
@ -322,7 +327,7 @@ void Ekf::fuseFlowForTerrain()
|
|||
const float q3 = _state.quat_nominal(3);
|
||||
|
||||
// calculate the optical flow observation variance
|
||||
const float R_LOS = calcOptFlowMeasVar();
|
||||
const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
|
||||
|
||||
// get rotation matrix from earth to body
|
||||
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
|
||||
|
|
@ -340,72 +345,64 @@ void Ekf::fuseFlowForTerrain()
|
|||
// rotate into body frame
|
||||
const Vector3f vel_body = earth_to_body * vel_rel_earth;
|
||||
|
||||
const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
|
||||
|
||||
// constrain terrain to minimum allowed value and predict height above ground
|
||||
_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
|
||||
const float pred_hagl_inv = 1.f / (_terrain_vpos - _state.pos(2));
|
||||
|
||||
// Calculate observation matrix for flow around the vehicle x axis
|
||||
const float Hx = vel_body(1) * t0 * pred_hagl_inv * pred_hagl_inv;
|
||||
// calculate prediced optical flow
|
||||
const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv;
|
||||
const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv;
|
||||
|
||||
// calculate flow innovation
|
||||
_aid_src_optical_flow.innovation[0] = pred_flow_x - opt_flow_rate(0);
|
||||
_aid_src_optical_flow.innovation[1] = pred_flow_y - opt_flow_rate(1);
|
||||
|
||||
const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
|
||||
|
||||
// Calculate observation matrix for flow around
|
||||
const float Hx = vel_body(1) * t0 * pred_hagl_inv * pred_hagl_inv;
|
||||
const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv;
|
||||
|
||||
// Constrain terrain variance to be non-negative
|
||||
_terrain_var = fmaxf(_terrain_var, 0.0f);
|
||||
_terrain_var = fmaxf(_terrain_var, sq(0.01f));
|
||||
|
||||
// Cacluate innovation variance
|
||||
_flow_innov_var(0) = Hx * Hx * _terrain_var + R_LOS;
|
||||
_aid_src_optical_flow.innovation_variance[0] = Hx * Hx * _terrain_var + R_LOS;
|
||||
_aid_src_optical_flow.innovation_variance[1] = Hy * Hy * _terrain_var + R_LOS;
|
||||
|
||||
// calculate the kalman gain for the flow x measurement
|
||||
const float Kx = _terrain_var * Hx / _flow_innov_var(0);
|
||||
// run the innovation consistency check and record result
|
||||
setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
|
||||
|
||||
// calculate prediced optical flow about x axis
|
||||
const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv;
|
||||
// do not perform measurement update if badly conditioned
|
||||
if (_aid_src_optical_flow.innovation_rejected) {
|
||||
return;
|
||||
}
|
||||
|
||||
// calculate flow innovation (x axis)
|
||||
_flow_innov(0) = pred_flow_x - opt_flow_rate(0);
|
||||
// calculate the kalman gain for the flow measurement
|
||||
const float Kx = _terrain_var * Hx / _aid_src_optical_flow.innovation_variance[0];
|
||||
|
||||
// calculate correction term for terrain variance
|
||||
const float KxHxP = Kx * Hx * _terrain_var;
|
||||
|
||||
// innovation consistency check
|
||||
const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
|
||||
float flow_test_ratio = sq(_flow_innov(0)) / (sq(gate_size) * _flow_innov_var(0));
|
||||
_terrain_vpos += Kx * _aid_src_optical_flow.innovation[0];
|
||||
// guard against negative variance
|
||||
_terrain_var = fmaxf(_terrain_var - KxHxP, sq(0.01f));
|
||||
|
||||
// do not perform measurement update if badly conditioned
|
||||
if (flow_test_ratio <= 1.0f) {
|
||||
_terrain_vpos += Kx * _flow_innov(0);
|
||||
// guard against negative variance
|
||||
_terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
|
||||
_time_last_flow_terrain_fuse = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
// Calculate observation matrix for flow around the vehicle y axis
|
||||
const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv;
|
||||
|
||||
// Calculuate innovation variance
|
||||
_flow_innov_var(1) = Hy * Hy * _terrain_var + R_LOS;
|
||||
|
||||
// calculate the kalman gain for the flow y measurement
|
||||
const float Ky = _terrain_var * Hy / _flow_innov_var(1);
|
||||
|
||||
// calculate prediced optical flow about y axis
|
||||
const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv;
|
||||
|
||||
// calculate flow innovation (y axis)
|
||||
_flow_innov(1) = pred_flow_y - opt_flow_rate(1);
|
||||
// calculate the kalman gain for the flow measurement
|
||||
const float Ky = _terrain_var * Hy / _aid_src_optical_flow.innovation_variance[1];
|
||||
|
||||
// calculate correction term for terrain variance
|
||||
const float KyHyP = Ky * Hy * _terrain_var;
|
||||
|
||||
// innovation consistency check
|
||||
flow_test_ratio = sq(_flow_innov(1)) / (sq(gate_size) * _flow_innov_var(1));
|
||||
_terrain_vpos += Ky * _aid_src_optical_flow.innovation_variance[1];
|
||||
// guard against negative variance
|
||||
_terrain_var = fmaxf(_terrain_var - KyHyP, sq(0.01f));
|
||||
|
||||
if (flow_test_ratio <= 1.0f) {
|
||||
_terrain_vpos += Ky * _flow_innov(1);
|
||||
// guard against negative variance
|
||||
_terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
|
||||
_time_last_flow_terrain_fuse = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
_time_last_flow_terrain_fuse = _imu_sample_delayed.time_us;
|
||||
//_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us; // TODO: separate aid source status for OF terrain?
|
||||
_aid_src_optical_flow.fused = true;
|
||||
}
|
||||
|
||||
void Ekf::controlHaglFakeFusion()
|
||||
|
|
|
|||
|
|
@ -747,6 +747,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp)
|
|||
|
||||
// aux velocity
|
||||
PublishAidSourceStatus(_ekf.aid_src_aux_vel(), _status_aux_vel_pub_last, _estimator_aid_src_aux_vel_pub);
|
||||
|
||||
// optical flow
|
||||
PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub);
|
||||
}
|
||||
|
||||
void EKF2::PublishAttitude(const hrt_abstime ×tamp)
|
||||
|
|
@ -1531,9 +1534,12 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp)
|
|||
|
||||
void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp)
|
||||
{
|
||||
if (_ekf.getFlowCompensated().longerThan(0.f)) {
|
||||
const hrt_abstime timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample;
|
||||
|
||||
if ((timestamp_sample != 0) && (timestamp_sample > _status_optical_flow_pub_last)) {
|
||||
|
||||
vehicle_optical_flow_vel_s flow_vel{};
|
||||
flow_vel.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
|
||||
flow_vel.timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample;
|
||||
|
||||
_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
|
||||
_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
|
||||
|
|
|
|||
|
|
@ -285,6 +285,8 @@ private:
|
|||
|
||||
hrt_abstime _status_aux_vel_pub_last{0};
|
||||
|
||||
hrt_abstime _status_optical_flow_pub_last{0};
|
||||
|
||||
float _last_baro_bias_published{};
|
||||
float _last_gnss_hgt_bias_published{};
|
||||
float _last_rng_hgt_bias_published{};
|
||||
|
|
@ -374,6 +376,8 @@ private:
|
|||
|
||||
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
|
||||
|
||||
// publications with topic dependent on multi-mode
|
||||
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;
|
||||
uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub;
|
||||
|
|
|
|||
|
|
@ -191,6 +191,7 @@ void LoggedTopics::add_default_topics()
|
|||
// add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
// add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
|
||||
// log all raw sensors at minimal rate (at least 1 Hz)
|
||||
|
|
@ -281,6 +282,7 @@ void LoggedTopics::add_default_topics()
|
|||
add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_aid_src_sideslip", 0, MAX_ESTIMATOR_INSTANCES);
|
||||
|
||||
#endif /* CONFIG_ARCH_BOARD_PX4_SITL */
|
||||
|
|
|
|||
Loading…
Reference in New Issue