mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
ekf2: add estimator aid source status (EV pos, EV vel)
This commit is contained in:
Daniel Agar
parent
002579b3e0
commit
f3be32e02f
@ -20,3 +20,4 @@ bool fused # true if the sample was successfully fused
|
||||
|
||||
# TOPICS estimator_aid_source_1d
|
||||
# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt
|
||||
# TOPICS estimator_aid_src_ev_yaw
|
||||
|
||||
@ -20,3 +20,4 @@ bool[3] fused # true if the sample was successfully fused
|
||||
|
||||
# TOPICS estimator_aid_source_3d
|
||||
# TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel
|
||||
# TOPICS estimator_aid_src_ev_pos estimator_aid_src_ev_vel
|
||||
|
||||
@ -56,6 +56,7 @@ px4_add_module(
|
||||
EKF/ekf_helper.cpp
|
||||
EKF/EKFGSF_yaw.cpp
|
||||
EKF/estimator_interface.cpp
|
||||
EKF/ev_fusion.cpp
|
||||
EKF/fake_pos_control.cpp
|
||||
EKF/gps_checks.cpp
|
||||
EKF/gps_control.cpp
|
||||
|
||||
@ -41,6 +41,7 @@ add_library(ecl_EKF
|
||||
ekf_helper.cpp
|
||||
EKFGSF_yaw.cpp
|
||||
estimator_interface.cpp
|
||||
ev_fusion.cpp
|
||||
fake_pos_control.cpp
|
||||
gps_checks.cpp
|
||||
gps_control.cpp
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
|
||||
* Copyright (c) 2015-2022 Estimation and Control Library (ECL). All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015-2020 Estimation and Control Library (ECL). All rights reserved.
|
||||
* Copyright (c) 2015-2022 Estimation and Control Library (ECL). All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -162,6 +162,63 @@ void Ekf::controlFusionModes()
|
||||
|
||||
if (_ext_vision_buffer) {
|
||||
_ev_data_ready = _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
|
||||
|
||||
if (_ev_data_ready) {
|
||||
|
||||
// Use an incremental position fusion method for EV position data if GPS is also used
|
||||
_fuse_hpos_as_odom = (_params.fusion_mode & SensorFusionMask::USE_GPS);
|
||||
|
||||
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
|
||||
// needs to be calculated and the observations rotated into the EKF frame of reference
|
||||
if ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW) && !_control_status.flags.ev_yaw) {
|
||||
|
||||
// Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
|
||||
const Quatf q_error((_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized());
|
||||
_R_ev_to_ekf = Dcmf(q_error);
|
||||
|
||||
} else {
|
||||
_R_ev_to_ekf.setIdentity();
|
||||
}
|
||||
|
||||
// correct position and height for offset relative to IMU
|
||||
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
|
||||
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
||||
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
|
||||
|
||||
if ((_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) || _fuse_hpos_as_odom) {
|
||||
// rotate measurement into body frame is required when fusing with GPS
|
||||
_ev_sample_delayed.pos = _R_ev_to_ekf * (_ev_sample_delayed.pos - pos_offset_earth);
|
||||
|
||||
Matrix3f obs_var = matrix::diag(_ev_sample_delayed.posVar);
|
||||
obs_var = _R_ev_to_ekf * obs_var * _R_ev_to_ekf.transpose();
|
||||
_ev_sample_delayed.posVar = obs_var.diag();
|
||||
|
||||
} else {
|
||||
_ev_sample_delayed.pos = (_ev_sample_delayed.pos - pos_offset_earth);
|
||||
}
|
||||
|
||||
// rotate measurement into correct earth frame if required
|
||||
switch (_ev_sample_delayed.vel_frame) {
|
||||
case VelocityFrame::BODY_FRAME_FRD:
|
||||
_ev_sample_delayed.vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
|
||||
_ev_sample_delayed.velCov = _R_to_earth * _ev_sample_delayed.velCov * _R_to_earth.transpose();
|
||||
break;
|
||||
|
||||
case VelocityFrame::LOCAL_FRAME_FRD:
|
||||
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
|
||||
|
||||
if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
|
||||
_ev_sample_delayed.vel = _R_ev_to_ekf * (_ev_sample_delayed.vel - vel_offset_earth);
|
||||
_ev_sample_delayed.velCov = _R_ev_to_ekf * _ev_sample_delayed.velCov * _R_ev_to_ekf.transpose();
|
||||
|
||||
} else {
|
||||
_ev_sample_delayed.vel = _ev_sample_delayed.vel - vel_offset_earth;
|
||||
// _ev_sample_delayed.velCov no change
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_airspeed_buffer) {
|
||||
@ -200,9 +257,21 @@ void Ekf::controlFusionModes()
|
||||
|
||||
void Ekf::controlExternalVisionFusion()
|
||||
{
|
||||
if (!_control_status_prev.flags.gps && _control_status.flags.gps) {
|
||||
// GPS now enabled
|
||||
stopEvYawFusion();
|
||||
|
||||
_fuse_hpos_as_odom = true;
|
||||
_hpos_prev_available = false;
|
||||
}
|
||||
|
||||
// Check for new external vision data
|
||||
if (_ev_data_ready) {
|
||||
|
||||
// reset flags
|
||||
resetEstimatorAidStatusFlags(_aid_src_ev_vel);
|
||||
resetEstimatorAidStatusFlags(_aid_src_ev_pos);
|
||||
|
||||
bool reset = false;
|
||||
|
||||
if (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) {
|
||||
@ -213,15 +282,6 @@ void Ekf::controlExternalVisionFusion()
|
||||
stopEvYawFusion();
|
||||
}
|
||||
|
||||
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
|
||||
// needs to be calculated and the observations rotated into the EKF frame of reference
|
||||
if ((_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) && ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)
|
||||
|| (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL)) && !_control_status.flags.ev_yaw) {
|
||||
|
||||
// rotate EV measurements into the EKF Navigation frame
|
||||
calcExtVisRotMat();
|
||||
}
|
||||
|
||||
// external vision aiding selection logic
|
||||
if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
|
||||
|
||||
@ -241,8 +301,10 @@ void Ekf::controlExternalVisionFusion()
|
||||
|
||||
// external vision yaw aiding selection logic
|
||||
if (!_inhibit_ev_yaw_use && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW) && !_control_status.flags.ev_yaw
|
||||
&& _control_status.flags.tilt_align) {
|
||||
|
||||
&& _control_status.flags.tilt_align
|
||||
&& !_control_status.flags.ev_yaw
|
||||
&& !_control_status.flags.gps
|
||||
) {
|
||||
// don't start using EV data unless data is arriving frequently
|
||||
if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
|
||||
if (resetYawToEv()) {
|
||||
@ -260,57 +322,7 @@ void Ekf::controlExternalVisionFusion()
|
||||
}
|
||||
}
|
||||
|
||||
Vector3f ev_pos_obs_var;
|
||||
|
||||
// correct position and height for offset relative to IMU
|
||||
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
|
||||
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
||||
_ev_sample_delayed.pos -= pos_offset_earth;
|
||||
|
||||
// Use an incremental position fusion method for EV position data if GPS is also used
|
||||
if (_params.fusion_mode & SensorFusionMask::USE_GPS) {
|
||||
_fuse_hpos_as_odom = true;
|
||||
|
||||
} else {
|
||||
_fuse_hpos_as_odom = false;
|
||||
}
|
||||
|
||||
if (_fuse_hpos_as_odom) {
|
||||
if (!_hpos_prev_available) {
|
||||
// no previous observation available to calculate position change
|
||||
_hpos_prev_available = true;
|
||||
|
||||
} else {
|
||||
// calculate the change in position since the last measurement
|
||||
// rotate measurement into body frame is required when fusing with GPS
|
||||
Vector3f ev_delta_pos = _R_ev_to_ekf * Vector3f(_ev_sample_delayed.pos - _ev_sample_delayed_prev.pos);
|
||||
|
||||
// use the change in position since the last measurement
|
||||
_ev_pos_innov(0) = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
|
||||
_ev_pos_innov(1) = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
|
||||
|
||||
// observation 1-STD error, incremental pos observation is expected to have more uncertainty
|
||||
Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
|
||||
ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
|
||||
ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.5f));
|
||||
ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.5f));
|
||||
}
|
||||
} else {
|
||||
// use the absolute position
|
||||
Vector3f ev_pos_meas = _ev_sample_delayed.pos;
|
||||
Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
|
||||
|
||||
if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
|
||||
ev_pos_meas = _R_ev_to_ekf * ev_pos_meas;
|
||||
ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
|
||||
}
|
||||
|
||||
_ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0);
|
||||
_ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1);
|
||||
|
||||
ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.01f));
|
||||
ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.01f));
|
||||
|
||||
if (!_fuse_hpos_as_odom) {
|
||||
// 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
|
||||
@ -325,11 +337,6 @@ void Ekf::controlExternalVisionFusion()
|
||||
resetHorizontalPositionToVision();
|
||||
}
|
||||
}
|
||||
|
||||
// innovation gate size
|
||||
const float ev_pos_innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.0f);
|
||||
|
||||
fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gate, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
|
||||
}
|
||||
|
||||
// determine if we should use the velocity observations
|
||||
@ -338,8 +345,6 @@ void Ekf::controlExternalVisionFusion()
|
||||
resetVelocityToVision();
|
||||
}
|
||||
|
||||
_ev_vel_innov = _state.vel - getVisionVelocityInEkfFrame();
|
||||
|
||||
// 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
|
||||
@ -348,15 +353,20 @@ void Ekf::controlExternalVisionFusion()
|
||||
resetVelocityToVision();
|
||||
}
|
||||
}
|
||||
|
||||
const Vector3f obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
|
||||
|
||||
const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f);
|
||||
|
||||
fuseHorizontalVelocity(_ev_vel_innov, innov_gate, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
|
||||
fuseVerticalVelocity(_ev_vel_innov, innov_gate, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
|
||||
}
|
||||
|
||||
updateEvVel(_ev_sample_delayed);
|
||||
updateEvPos(_ev_sample_delayed);
|
||||
|
||||
if (_control_status.flags.ev_vel) {
|
||||
fuseEvVel();
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_pos) {
|
||||
fuseEvPos();
|
||||
}
|
||||
|
||||
// External Vision Yaw
|
||||
// determine if we should use the yaw observation
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
if (reset && _control_status_prev.flags.ev_yaw) {
|
||||
@ -375,7 +385,8 @@ void Ekf::controlExternalVisionFusion()
|
||||
|
||||
// record observation and estimate for use next time
|
||||
_ev_sample_delayed_prev = _ev_sample_delayed;
|
||||
_hpos_pred_prev = _state.pos.xy();
|
||||
_hpos_pred_prev = _state.pos;
|
||||
_hpos_prev_available = true;
|
||||
|
||||
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw)
|
||||
&& isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) {
|
||||
@ -966,13 +977,6 @@ void Ekf::controlHeightFusion()
|
||||
fuseRngHgt(_aid_src_rng_hgt);
|
||||
}
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
|
||||
if (_control_status.flags.ev_hgt && _ev_data_ready) {
|
||||
fuseEvHgt();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::checkRangeAidSuitability()
|
||||
|
||||
@ -988,13 +988,13 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
|
||||
if (fabsf(down_dvel_bias) > dVel_bias_lim) {
|
||||
|
||||
bool bad_vz_gps = _control_status.flags.gps && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f);
|
||||
bool bad_vz_ev = _control_status.flags.ev_vel && (down_dvel_bias * _ev_vel_innov(2) < 0.0f);
|
||||
bool bad_vz_ev = _control_status.flags.ev_vel && (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.0f);
|
||||
|
||||
if (bad_vz_gps || bad_vz_ev) {
|
||||
bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f);
|
||||
bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_pos.innovation[2] < 0.0f);
|
||||
bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.0f);
|
||||
bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _ev_pos_innov(2) < 0.0f);
|
||||
bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _aid_src_ev_pos.innovation[2] < 0.0f);
|
||||
|
||||
|
||||
if (bad_z_baro || bad_z_gps || bad_z_rng || bad_z_ev) {
|
||||
|
||||
@ -348,6 +348,9 @@ public:
|
||||
const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
|
||||
const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
|
||||
|
||||
const auto &aid_src_ev_vel() const { return _aid_src_gnss_vel; }
|
||||
const auto &aid_src_ev_pos() const { return _aid_src_gnss_pos; }
|
||||
|
||||
private:
|
||||
|
||||
// set the internal states and status to their default value
|
||||
@ -381,7 +384,7 @@ private:
|
||||
|
||||
// variables used when position data is being fused using a relative position odometry model
|
||||
bool _fuse_hpos_as_odom{false}; ///< true when the NE position data is being fused using an odometry assumption
|
||||
Vector2f _hpos_pred_prev{}; ///< previous value of NE position state used by odometry fusion (m)
|
||||
Vector3f _hpos_pred_prev{}; ///< previous value of NE position state used by odometry fusion (m)
|
||||
bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use
|
||||
Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
|
||||
bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
|
||||
@ -450,12 +453,6 @@ private:
|
||||
float _vert_vel_innov_ratio{0.f}; ///< standard deviation of vertical velocity innovation
|
||||
uint64_t _vert_vel_fuse_time_us{0}; ///< last system time in usec time vertical velocity measurement fuson was attempted
|
||||
|
||||
Vector3f _ev_vel_innov{}; ///< external vision velocity innovations (m/sec)
|
||||
Vector3f _ev_vel_innov_var{}; ///< external vision velocity innovation variances ((m/sec)**2)
|
||||
|
||||
Vector3f _ev_pos_innov{}; ///< external vision position innovations (m)
|
||||
Vector3f _ev_pos_innov_var{}; ///< external vision position innovation variances (m**2)
|
||||
|
||||
Vector3f _aux_vel_innov{}; ///< horizontal auxiliary velocity innovations: (m/sec)
|
||||
Vector3f _aux_vel_innov_var{}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
|
||||
|
||||
@ -499,6 +496,9 @@ private:
|
||||
estimator_aid_source_3d_s _aid_src_gnss_vel{};
|
||||
estimator_aid_source_3d_s _aid_src_gnss_pos{};
|
||||
|
||||
estimator_aid_source_3d_s _aid_src_ev_vel{};
|
||||
estimator_aid_source_3d_s _aid_src_ev_pos{};
|
||||
|
||||
// output predictor states
|
||||
Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad)
|
||||
Vector3f _vel_err_integ{}; ///< integral of velocity tracking error (m)
|
||||
@ -647,7 +647,6 @@ private:
|
||||
|
||||
void fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt);
|
||||
void fuseRngHgt(estimator_aid_source_1d_s &range_hgt);
|
||||
void fuseEvHgt();
|
||||
|
||||
void updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt);
|
||||
void updateRngHgt(estimator_aid_source_1d_s &rng_hgt);
|
||||
@ -701,6 +700,12 @@ private:
|
||||
void updateGpsPos(const gpsSample &gps_sample);
|
||||
void fuseGpsPos();
|
||||
|
||||
void updateEvVel(const extVisionSample &ev_sample);
|
||||
void fuseEvVel();
|
||||
|
||||
void updateEvPos(const extVisionSample &ev_sample);
|
||||
void fuseEvPos();
|
||||
|
||||
// calculate optical flow body angular rate compensation
|
||||
// returns false if bias corrected body rate data is unavailable
|
||||
bool calcOptFlowBodyRateComp();
|
||||
@ -748,13 +753,6 @@ private:
|
||||
// modify output filter to match the the EKF state at the fusion time horizon
|
||||
void alignOutputFilter();
|
||||
|
||||
// update the rotation matrix which transforms EV navigation frame measurements into NED
|
||||
void calcExtVisRotMat();
|
||||
|
||||
Vector3f getVisionVelocityInEkfFrame() const;
|
||||
|
||||
Vector3f getVisionVelocityVarianceInEkfFrame() const;
|
||||
|
||||
// matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24>
|
||||
// that is optimized by exploring the sparsity in H
|
||||
template <size_t ...Idxs>
|
||||
|
||||
@ -87,8 +87,8 @@ 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(_ev_sample_delayed.vel);
|
||||
P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velCov.diag());
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalVelocityToZero()
|
||||
@ -156,13 +156,8 @@ void Ekf::resetHorizontalPositionToVision()
|
||||
{
|
||||
_information_events.flags.reset_pos_to_vision = true;
|
||||
ECL_INFO("reset position to ev position");
|
||||
Vector3f _ev_pos = _ev_sample_delayed.pos;
|
||||
|
||||
if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
|
||||
_ev_pos = _R_ev_to_ekf * _ev_sample_delayed.pos;
|
||||
}
|
||||
|
||||
resetHorizontalPositionTo(Vector2f(_ev_pos));
|
||||
resetHorizontalPositionTo(Vector2f(_ev_sample_delayed.pos));
|
||||
P.uncorrelateCovarianceSetVariance<2>(7, _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
|
||||
@ -637,30 +632,33 @@ void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &v
|
||||
|
||||
void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
|
||||
{
|
||||
hvel[0] = _ev_vel_innov(0);
|
||||
hvel[1] = _ev_vel_innov(1);
|
||||
vvel = _ev_vel_innov(2);
|
||||
hpos[0] = _ev_pos_innov(0);
|
||||
hpos[1] = _ev_pos_innov(1);
|
||||
vpos = _ev_pos_innov(2);
|
||||
hvel[0] = _aid_src_ev_vel.innovation[0];
|
||||
hvel[1] = _aid_src_ev_vel.innovation[1];
|
||||
vvel = _aid_src_ev_vel.innovation[2];
|
||||
|
||||
hpos[0] = _aid_src_ev_pos.innovation[0];
|
||||
hpos[1] = _aid_src_ev_pos.innovation[1];
|
||||
vpos = _aid_src_ev_pos.innovation[2];
|
||||
}
|
||||
|
||||
void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const
|
||||
{
|
||||
hvel[0] = _ev_vel_innov_var(0);
|
||||
hvel[1] = _ev_vel_innov_var(1);
|
||||
vvel = _ev_vel_innov_var(2);
|
||||
hpos[0] = _ev_pos_innov_var(0);
|
||||
hpos[1] = _ev_pos_innov_var(1);
|
||||
vpos = _ev_pos_innov_var(2);
|
||||
hvel[0] = _aid_src_ev_vel.innovation_variance[0];
|
||||
hvel[1] = _aid_src_ev_vel.innovation_variance[1];
|
||||
vvel = _aid_src_ev_vel.innovation_variance[2];
|
||||
|
||||
hpos[0] = _aid_src_ev_pos.innovation_variance[0];
|
||||
hpos[1] = _aid_src_ev_pos.innovation_variance[1];
|
||||
vpos = _aid_src_ev_pos.innovation_variance[2];
|
||||
}
|
||||
|
||||
void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
|
||||
{
|
||||
hvel = _ev_vel_test_ratio(0);
|
||||
vvel = _ev_vel_test_ratio(1);
|
||||
hpos = _ev_pos_test_ratio(0);
|
||||
vpos = _ev_pos_test_ratio(1);
|
||||
hvel = fmaxf(_aid_src_ev_vel.test_ratio[0], _aid_src_ev_vel.test_ratio[1]);
|
||||
vvel = _aid_src_ev_vel.test_ratio[2];
|
||||
|
||||
hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]);
|
||||
vpos = _aid_src_ev_pos.test_ratio[2];
|
||||
}
|
||||
|
||||
void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
|
||||
@ -748,7 +746,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_pos) {
|
||||
hpos_err = math::max(hpos_err, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1))));
|
||||
hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm());
|
||||
}
|
||||
}
|
||||
|
||||
@ -793,11 +791,11 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
|
||||
vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
|
||||
|
||||
} else if (_control_status.flags.ev_pos) {
|
||||
vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1))));
|
||||
vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_pos.innovation).norm());
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_vel) {
|
||||
vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_vel_innov(0)) + sq(_ev_vel_innov(1))));
|
||||
vel_err_conservative = math::max(vel_err_conservative, Vector3f(_aid_src_ev_vel.innovation).norm());
|
||||
}
|
||||
|
||||
hvel_err = math::max(hvel_err, vel_err_conservative);
|
||||
@ -925,12 +923,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_vel) {
|
||||
float ev_vel = sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1)));
|
||||
float ev_vel = sqrtf(Vector3f(_aid_src_ev_vel.test_ratio).max());
|
||||
vel = math::max(vel, ev_vel, FLT_MIN);
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_pos) {
|
||||
float ev_pos = sqrtf(_ev_pos_test_ratio(0));
|
||||
float ev_pos = sqrtf(Vector2f(_aid_src_ev_pos.test_ratio).max());
|
||||
pos = math::max(pos, ev_pos, FLT_MIN);
|
||||
}
|
||||
|
||||
@ -950,7 +948,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
||||
hgt = math::max(sqrtf(_aid_src_rng_hgt.test_ratio), FLT_MIN);
|
||||
|
||||
} else if (_control_status.flags.ev_hgt) {
|
||||
hgt = math::max(sqrtf(_ev_pos_test_ratio(1)), FLT_MIN);
|
||||
hgt = math::max(sqrtf(_aid_src_ev_pos.test_ratio[2]), FLT_MIN);
|
||||
|
||||
} else {
|
||||
hgt = NAN;
|
||||
@ -1415,64 +1413,6 @@ void Ekf::updateGroundEffect()
|
||||
}
|
||||
}
|
||||
|
||||
Vector3f Ekf::getVisionVelocityInEkfFrame() const
|
||||
{
|
||||
Vector3f vel;
|
||||
// correct velocity for offset relative to IMU
|
||||
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
|
||||
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
|
||||
|
||||
// rotate measurement into correct earth frame if required
|
||||
switch (_ev_sample_delayed.vel_frame) {
|
||||
case VelocityFrame::BODY_FRAME_FRD:
|
||||
vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
|
||||
break;
|
||||
|
||||
case VelocityFrame::LOCAL_FRAME_FRD:
|
||||
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
|
||||
|
||||
if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
|
||||
vel = _R_ev_to_ekf * _ev_sample_delayed.vel - vel_offset_earth;
|
||||
|
||||
} else {
|
||||
vel = _ev_sample_delayed.vel - vel_offset_earth;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
return vel;
|
||||
}
|
||||
|
||||
Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
|
||||
{
|
||||
Matrix3f ev_vel_cov = _ev_sample_delayed.velCov;
|
||||
|
||||
// rotate measurement into correct earth frame if required
|
||||
switch (_ev_sample_delayed.vel_frame) {
|
||||
case VelocityFrame::BODY_FRAME_FRD:
|
||||
ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
|
||||
break;
|
||||
|
||||
case VelocityFrame::LOCAL_FRAME_FRD:
|
||||
if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
|
||||
ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
return ev_vel_cov.diag();
|
||||
}
|
||||
|
||||
// update the rotation matrix which rotates EV measurements into the EKF's navigation frame
|
||||
void Ekf::calcExtVisRotMat()
|
||||
{
|
||||
// Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
|
||||
const Quatf q_error((_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized());
|
||||
_R_ev_to_ekf = Dcmf(q_error);
|
||||
}
|
||||
|
||||
// Increase the yaw error variance of the quaternions
|
||||
// Argument is additional yaw variance in rad**2
|
||||
void Ekf::increaseQuatYawErrVariance(float yaw_variance)
|
||||
@ -1638,31 +1578,45 @@ void Ekf::stopGpsYawFusion()
|
||||
|
||||
void Ekf::startEvPosFusion()
|
||||
{
|
||||
_control_status.flags.ev_pos = true;
|
||||
resetHorizontalPositionToVision();
|
||||
_information_events.flags.starting_vision_pos_fusion = true;
|
||||
ECL_INFO("starting vision pos fusion");
|
||||
if (!_control_status.flags.ev_pos) {
|
||||
ECL_INFO("starting vision pos fusion");
|
||||
_information_events.flags.starting_vision_pos_fusion = true;
|
||||
|
||||
if (!_control_status.flags.gps) {
|
||||
resetHorizontalPositionToVision();
|
||||
}
|
||||
|
||||
_control_status.flags.ev_pos = true;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::startEvVelFusion()
|
||||
{
|
||||
_control_status.flags.ev_vel = true;
|
||||
resetVelocityToVision();
|
||||
_information_events.flags.starting_vision_vel_fusion = true;
|
||||
ECL_INFO("starting vision vel fusion");
|
||||
if (!_control_status.flags.ev_vel) {
|
||||
ECL_INFO("starting vision vel fusion");
|
||||
_information_events.flags.starting_vision_vel_fusion = true;
|
||||
|
||||
if (!_control_status.flags.gps) {
|
||||
resetVelocityToVision();
|
||||
}
|
||||
|
||||
_control_status.flags.ev_vel = true;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::startEvYawFusion()
|
||||
{
|
||||
// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
|
||||
_control_status.flags.ev_yaw = true;
|
||||
_control_status.flags.mag_dec = false;
|
||||
if (!_control_status.flags.ev_yaw) {
|
||||
// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
|
||||
ECL_INFO("starting vision yaw fusion");
|
||||
_information_events.flags.starting_vision_yaw_fusion = true;
|
||||
|
||||
stopMagHdgFusion();
|
||||
stopMag3DFusion();
|
||||
_control_status.flags.ev_yaw = true;
|
||||
_control_status.flags.mag_dec = false;
|
||||
|
||||
_information_events.flags.starting_vision_yaw_fusion = true;
|
||||
ECL_INFO("starting vision yaw fusion");
|
||||
stopMagHdgFusion();
|
||||
stopMag3DFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopEvFusion()
|
||||
@ -1674,23 +1628,28 @@ void Ekf::stopEvFusion()
|
||||
|
||||
void Ekf::stopEvPosFusion()
|
||||
{
|
||||
_control_status.flags.ev_pos = false;
|
||||
_ev_pos_innov.setZero();
|
||||
_ev_pos_innov_var.setZero();
|
||||
_ev_pos_test_ratio.setZero();
|
||||
if (_control_status.flags.ev_pos) {
|
||||
ECL_INFO("stopping EV position fusion");
|
||||
resetEstimatorAidStatus(_aid_src_ev_pos);
|
||||
_control_status.flags.ev_pos = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopEvVelFusion()
|
||||
{
|
||||
_control_status.flags.ev_vel = false;
|
||||
_ev_vel_innov.setZero();
|
||||
_ev_vel_innov_var.setZero();
|
||||
_ev_vel_test_ratio.setZero();
|
||||
if (_control_status.flags.ev_vel) {
|
||||
ECL_INFO("stopping EV velocity fusion");
|
||||
resetEstimatorAidStatus(_aid_src_ev_vel);
|
||||
_control_status.flags.ev_vel = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopEvYawFusion()
|
||||
{
|
||||
_control_status.flags.ev_yaw = false;
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
ECL_INFO("stopping EV yaw fusion");
|
||||
_control_status.flags.ev_yaw = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopAuxVelFusion()
|
||||
|
||||
@ -331,8 +331,6 @@ protected:
|
||||
float _yaw_test_ratio{}; // yaw innovation consistency check ratio
|
||||
AlphaFilter<float> _yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
|
||||
Vector3f _mag_test_ratio{}; // magnetometer XYZ innovation consistency check ratios
|
||||
Vector2f _ev_vel_test_ratio{}; // EV velocity innovation consistency check ratios
|
||||
Vector2f _ev_pos_test_ratio{}; // EV position innovation consistency check ratios
|
||||
Vector2f _aux_vel_test_ratio{}; // Auxiliary horizontal velocity innovation consistency check ratio
|
||||
float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio
|
||||
float _tas_test_ratio{}; // tas innovation consistency check ratio
|
||||
|
||||
182
src/modules/ekf2/EKF/ev_fusion.cpp
Normal file
182
src/modules/ekf2/EKF/ev_fusion.cpp
Normal file
@ -0,0 +1,182 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4. 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 ev_fusion.cpp
|
||||
* Function for fusing external vision (EV) velocity and position measurements
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
|
||||
void Ekf::updateEvVel(const extVisionSample &ev_sample)
|
||||
{
|
||||
// innovation gate size
|
||||
const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f);
|
||||
|
||||
auto &ev_vel = _aid_src_ev_vel;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
ev_vel.observation[i] = ev_sample.vel(i);
|
||||
ev_vel.observation_variance[i] = fmaxf(ev_sample.velCov(i, i), sq(0.05f));
|
||||
|
||||
ev_vel.innovation[i] = _state.vel(i) - ev_sample.vel(i);
|
||||
ev_vel.innovation_variance[i] = P(4 + i, 4 + i) + ev_vel.observation_variance[i];
|
||||
ev_vel.test_ratio[i] = sq(ev_vel.innovation[i]) / (sq(innov_gate) * ev_vel.innovation_variance[i]);
|
||||
|
||||
ev_vel.innovation_rejected[i] = (ev_vel.test_ratio[i] > 1.f);
|
||||
}
|
||||
|
||||
// vz special case if there is bad vertical acceleration data, then don't reject measurement,
|
||||
// but limit innovation to prevent spikes that could destabilise the filter
|
||||
if (_fault_status.flags.bad_acc_vertical && ev_vel.innovation_rejected[2]) {
|
||||
const float innov_limit = innov_gate * sqrtf(ev_vel.innovation_variance[2]);
|
||||
ev_vel.innovation[2] = math::constrain(ev_vel.innovation[2], -innov_limit, innov_limit);
|
||||
ev_vel.innovation_rejected[2] = false;
|
||||
}
|
||||
|
||||
ev_vel.timestamp_sample = ev_sample.time_us;
|
||||
}
|
||||
|
||||
void Ekf::updateEvPos(const extVisionSample &ev_sample)
|
||||
{
|
||||
bool updated = false;
|
||||
|
||||
// innovation gate size
|
||||
float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f);
|
||||
|
||||
auto &ev_pos = _aid_src_ev_pos;
|
||||
|
||||
if (!_fuse_hpos_as_odom) {
|
||||
// use the absolute position
|
||||
for (int i = 0; i < 3; i++) {
|
||||
ev_pos.observation[i] = _ev_sample_delayed.pos(i);
|
||||
ev_pos.observation_variance[i] = fmaxf(_ev_sample_delayed.posVar(i), sq(0.01f));
|
||||
|
||||
ev_pos.innovation[i] = _state.pos(i) - _ev_sample_delayed.pos(i);
|
||||
}
|
||||
|
||||
updated = true;
|
||||
|
||||
} else if (_hpos_prev_available) {
|
||||
// calculate the change in position since the last measurement
|
||||
Vector3f delta_pos = _ev_sample_delayed.pos - _ev_sample_delayed_prev.pos;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
ev_pos.observation[i] = delta_pos(i);
|
||||
|
||||
// observation 1-STD error, incremental pos observation is expected to have more uncertainty
|
||||
ev_pos.observation_variance[i] = fmaxf(_ev_sample_delayed.posVar(i), sq(0.5f));
|
||||
|
||||
ev_pos.innovation[i] = _state.pos(i) - _hpos_pred_prev(i) - delta_pos(i);
|
||||
}
|
||||
|
||||
updated = true;
|
||||
}
|
||||
|
||||
if (updated) {
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
ev_pos.innovation_variance[i] = P(7 + i, 7 + i) + ev_pos.observation_variance[i];
|
||||
ev_pos.test_ratio[i] = sq(ev_pos.innovation[i]) / (sq(innov_gate) * ev_pos.innovation_variance[i]);
|
||||
|
||||
ev_pos.innovation_rejected[i] = (ev_pos.test_ratio[i] > 1.f);
|
||||
}
|
||||
|
||||
// z special case if there is bad vertical acceleration data, then don't reject measurement,
|
||||
// but limit innovation to prevent spikes that could destabilise the filter
|
||||
if (_fault_status.flags.bad_acc_vertical && ev_pos.innovation_rejected[2]) {
|
||||
const float innov_limit = innov_gate * sqrtf(ev_pos.innovation_variance[2]);
|
||||
ev_pos.innovation[2] = math::constrain(ev_pos.innovation[2], -innov_limit, innov_limit);
|
||||
ev_pos.innovation_rejected[2] = false;
|
||||
}
|
||||
|
||||
ev_pos.timestamp_sample = ev_sample.time_us;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseEvVel()
|
||||
{
|
||||
// velocity
|
||||
auto &ev_vel = _aid_src_ev_vel;
|
||||
|
||||
// vx & vy
|
||||
ev_vel.fusion_enabled[0] = true;
|
||||
ev_vel.fusion_enabled[1] = true;
|
||||
|
||||
if (!ev_vel.innovation_rejected[0] && !ev_vel.innovation_rejected[1]) {
|
||||
for (int i = 0; i < 2; i++) {
|
||||
if (fuseVelPosHeight(ev_vel.innovation[i], ev_vel.innovation_variance[i], i)) {
|
||||
ev_vel.fused[i] = true;
|
||||
ev_vel.time_last_fuse[i] = _time_last_imu;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// vz
|
||||
ev_vel.fusion_enabled[2] = true;
|
||||
|
||||
if (ev_vel.fusion_enabled[2] && !ev_vel.innovation_rejected[2]) {
|
||||
if (fuseVelPosHeight(ev_vel.innovation[2], ev_vel.innovation_variance[2], 2)) {
|
||||
ev_vel.fused[2] = true;
|
||||
ev_vel.time_last_fuse[2] = _time_last_imu;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseEvPos()
|
||||
{
|
||||
auto &ev_pos = _aid_src_ev_pos;
|
||||
|
||||
// x & y
|
||||
ev_pos.fusion_enabled[0] = true;
|
||||
ev_pos.fusion_enabled[1] = true;
|
||||
|
||||
if (!ev_pos.innovation_rejected[0] && !ev_pos.innovation_rejected[1]) {
|
||||
for (int i = 0; i < 2; i++) {
|
||||
if (fuseVelPosHeight(ev_pos.innovation[i], ev_pos.innovation_variance[i], 3 + i)) {
|
||||
ev_pos.fused[i] = true;
|
||||
ev_pos.time_last_fuse[i] = _time_last_imu;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// z
|
||||
ev_pos.fusion_enabled[2] = _control_status.flags.ev_hgt;
|
||||
|
||||
if (ev_pos.fusion_enabled[2] && !ev_pos.innovation_rejected[2]) {
|
||||
if (fuseVelPosHeight(ev_pos.innovation[2], ev_pos.innovation_variance[2], 2)) {
|
||||
ev_pos.fused[2] = true;
|
||||
ev_pos.time_last_fuse[2] = _time_last_imu;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4. All rights reserved.
|
||||
* Copyright (c) 2021-2022 PX4. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
||||
@ -142,19 +142,3 @@ void Ekf::fuseRngHgt(estimator_aid_source_1d_s &rng_hgt)
|
||||
rng_hgt.time_last_fuse = _time_last_imu;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseEvHgt()
|
||||
{
|
||||
// calculate the innovation assuming the external vision observation is in local NED frame
|
||||
_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
|
||||
|
||||
// innovation gate size
|
||||
float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f);
|
||||
|
||||
// observation variance - defined externally
|
||||
float obs_var = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
|
||||
|
||||
// _ev_pos_test_ratio(1) is the vertical test ratio
|
||||
fuseVerticalPosition(_ev_pos_innov(2), innov_gate, obs_var,
|
||||
_ev_pos_innov_var(2), _ev_pos_test_ratio(1));
|
||||
}
|
||||
|
||||
@ -649,6 +649,10 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp)
|
||||
PublishAidSourceStatus(_ekf.aid_src_gnss_vel(), _status_gnss_vel_pub_last, _estimator_aid_src_gnss_vel_pub);
|
||||
PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub);
|
||||
|
||||
// EV velocity & position
|
||||
PublishAidSourceStatus(_ekf.aid_src_ev_vel(), _status_ev_vel_pub_last, _estimator_aid_src_ev_vel_pub);
|
||||
PublishAidSourceStatus(_ekf.aid_src_ev_pos(), _status_ev_pos_pub_last, _estimator_aid_src_ev_pos_pub);
|
||||
|
||||
}
|
||||
|
||||
void EKF2::PublishAttitude(const hrt_abstime ×tamp)
|
||||
|
||||
@ -263,6 +263,9 @@ private:
|
||||
hrt_abstime _status_gnss_vel_pub_last{0};
|
||||
hrt_abstime _status_gnss_pos_pub_last{0};
|
||||
|
||||
hrt_abstime _status_ev_vel_pub_last{0};
|
||||
hrt_abstime _status_ev_pos_pub_last{0};
|
||||
|
||||
float _last_baro_bias_published{};
|
||||
|
||||
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
|
||||
@ -330,6 +333,9 @@ private:
|
||||
uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
|
||||
uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)};
|
||||
uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)};
|
||||
|
||||
// publications with topic dependent on multi-mode
|
||||
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;
|
||||
uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user