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

ekf2: add AUX velocity aid src status

Browse Source 
- also includes velocity and position helpers for using estimator aid
   source status messages that will later be used for GPS, EV, etc
This commit is contained in:
Daniel Agar 2022-07-26 11:14:00 -04:00
parent a397c09e59
commit 41d9c3dd2a
8 changed files with 146 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/estimator_aid_source_3d.msg
View File

@ -19,4 +19,5 @@ bool[3] innovation_rejected # true if the observation has been rejected
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 estimator_aid_src_mag
# TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel
# TOPICS estimator_aid_src_mag estimator_aid_src_aux_vel

17
src/modules/ekf2/EKF/control.cpp
View File

@ -1131,18 +1131,13 @@ void Ekf::controlAuxVelFusion()
if (_auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &auxvel_sample_delayed)) {
updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
if (isHorizontalAidingActive()) {
const float aux_vel_innov_gate = fmaxf(_params.auxvel_gate, 1.f);
_aux_vel_innov = _state.vel - auxvel_sample_delayed.vel;
fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar,
_aux_vel_innov_var, _aux_vel_test_ratio);
// Can be enabled after bit for this is added to EKF_AID_MASK
// fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar,
// _aux_vel_innov_var, _aux_vel_test_ratio);
_aid_src_aux_vel.fusion_enabled[0] = PX4_ISFINITE(auxvel_sample_delayed.vel(0));
_aid_src_aux_vel.fusion_enabled[1] = PX4_ISFINITE(auxvel_sample_delayed.vel(1));
_aid_src_aux_vel.fusion_enabled[2] = PX4_ISFINITE(auxvel_sample_delayed.vel(2));
fuseVelocity(_aid_src_aux_vel);
}
}
}

35
src/modules/ekf2/EKF/ekf.h
View File

@ -96,7 +96,7 @@ public:
void getAuxVelInnov(float aux_vel_innov[2]) const;
void getAuxVelInnovVar(float aux_vel_innov[2]) const;
void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = _aux_vel_test_ratio(0); }
void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = Vector3f(_aid_src_aux_vel.test_ratio).max(); }
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); }
@ -404,6 +404,8 @@ public:
const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
const auto &aid_src_mag() const { return _aid_src_mag; }
const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
private:
// set the internal states and status to their default value
@ -512,9 +514,6 @@ private:
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)
Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2)
Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2)
@ -553,6 +552,8 @@ private:
estimator_aid_source_1d_s _aid_src_mag_heading{};
estimator_aid_source_3d_s _aid_src_mag{};
estimator_aid_source_3d_s _aid_src_aux_vel{};
// output predictor states
Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad)
Vector3f _vel_err_integ{}; ///< integral of velocity tracking error (m)
@ -719,6 +720,12 @@ private:
// fuse optical flow line of sight rate measurements
void fuseOptFlow();
void updateVelocityAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& velocity, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& vel_aid_src) const;
void updatePositionAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& position, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& pos_aid_src) const;
void fuseVelocity(estimator_aid_source_3d_s& vel_aid_src);
void fusePosition(estimator_aid_source_3d_s& pos_aid_src);
bool fuseHorizontalVelocity(const Vector3f &innov, float innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio);
@ -1157,16 +1164,28 @@ private:
void setEstimatorAidStatusTestRatio(estimator_aid_source_1d_s &status, float innovation_gate) const
{
status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
status.innovation_rejected = (status.test_ratio > 1.f);
if (PX4_ISFINITE(status.innovation) && PX4_ISFINITE(status.innovation_variance)) {
status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
status.innovation_rejected = (status.test_ratio > 1.f);
} else {
status.test_ratio = INFINITY;
status.innovation_rejected = true;
}
}
template <typename T>
void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const
{
for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) {
status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]);
status.innovation_rejected[i] = (status.test_ratio[i] > 1.f);
if (PX4_ISFINITE(status.innovation[i]) && PX4_ISFINITE(status.innovation_variance[i])) {
status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]);
status.innovation_rejected[i] = (status.test_ratio[i] > 1.f);
} else {
status.test_ratio[i] = INFINITY;
status.innovation_rejected[i] = true;
}
}
}
};

14
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -657,14 +657,14 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp
void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
{
aux_vel_innov[0] = _aux_vel_innov(0);
aux_vel_innov[1] = _aux_vel_innov(1);
aux_vel_innov[0] = _aid_src_aux_vel.innovation[0];
aux_vel_innov[1] = _aid_src_aux_vel.innovation[1];
}
void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
{
aux_vel_innov_var[0] = _aux_vel_innov_var(0);
aux_vel_innov_var[1] = _aux_vel_innov_var(1);
aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0];
aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1];
}
// get the state vector at the delayed time horizon
@ -1761,9 +1761,9 @@ void Ekf::stopEvYawFusion()
void Ekf::stopAuxVelFusion()
{
_aux_vel_innov.setZero();
_aux_vel_innov_var.setZero();
_aux_vel_test_ratio.setZero();
ECL_INFO("stopping aux vel fusion");
//_control_status.flags.aux_vel = false;
resetEstimatorAidStatus(_aid_src_aux_vel);
}
void Ekf::stopFlowFusion()

1
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -331,7 +331,6 @@ protected:
AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
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 _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio
float _beta_test_ratio{}; // sideslip innovation consistency check ratio

96
src/modules/ekf2/EKF/vel_pos_fusion.cpp
View File

@ -159,6 +159,102 @@ bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const
}
}
void Ekf::updateVelocityAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& velocity, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& vel_aid_src) const
{
resetEstimatorAidStatus(vel_aid_src);
for (int i = 0; i < 3; i++) {
vel_aid_src.observation[i] = velocity(i);
vel_aid_src.observation_variance[i] = obs_var(i);
vel_aid_src.innovation[i] = _state.vel(i) - velocity(i);
vel_aid_src.innovation_variance[i] = P(4 + i, 4 + i) + obs_var(i);
}
setEstimatorAidStatusTestRatio(vel_aid_src, innov_gate);
// 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 && vel_aid_src.innovation_rejected[2]) {
const float innov_limit = innov_gate * sqrtf(vel_aid_src.innovation_variance[2]);
vel_aid_src.innovation[2] = math::constrain(vel_aid_src.innovation[2], -innov_limit, innov_limit);
vel_aid_src.innovation_rejected[2] = false;
}
vel_aid_src.timestamp_sample = sample_time_us;
}
void Ekf::updatePositionAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& position, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& pos_aid_src) const
{
resetEstimatorAidStatus(pos_aid_src);
for (int i = 0; i < 3; i++) {
pos_aid_src.observation[i] = position(i);
pos_aid_src.observation_variance[i] = obs_var(i);
pos_aid_src.innovation[i] = _state.pos(i) - position(i);
pos_aid_src.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
}
setEstimatorAidStatusTestRatio(pos_aid_src, innov_gate);
// 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 && pos_aid_src.innovation_rejected[2]) {
const float innov_limit = innov_gate * sqrtf(pos_aid_src.innovation_variance[2]);
pos_aid_src.innovation[2] = math::constrain(pos_aid_src.innovation[2], -innov_limit, innov_limit);
pos_aid_src.innovation_rejected[2] = false;
}
pos_aid_src.timestamp_sample = sample_time_us;
}
void Ekf::fuseVelocity(estimator_aid_source_3d_s& vel_aid_src)
{
// vx & vy
if (vel_aid_src.fusion_enabled[0] && !vel_aid_src.innovation_rejected[0]
&& vel_aid_src.fusion_enabled[1] && !vel_aid_src.innovation_rejected[1]
) {
for (int i = 0; i < 2; i++) {
if (fuseVelPosHeight(vel_aid_src.innovation[i], vel_aid_src.innovation_variance[i], i)) {
vel_aid_src.fused[i] = true;
vel_aid_src.time_last_fuse[i] = _time_last_imu;
}
}
}
// vz
if (vel_aid_src.fusion_enabled[2] && !vel_aid_src.innovation_rejected[2]) {
if (fuseVelPosHeight(vel_aid_src.innovation[2], vel_aid_src.innovation_variance[2], 2)) {
vel_aid_src.fused[2] = true;
vel_aid_src.time_last_fuse[2] = _time_last_imu;
}
}
}
void Ekf::fusePosition(estimator_aid_source_3d_s& pos_aid_src)
{
// x & y
if (pos_aid_src.fusion_enabled[0] && !pos_aid_src.innovation_rejected[0]
&& pos_aid_src.fusion_enabled[1] && !pos_aid_src.innovation_rejected[1]
) {
for (int i = 0; i < 2; i++) {
if (fuseVelPosHeight(pos_aid_src.innovation[i], pos_aid_src.innovation_variance[i], 3 + i)) {
pos_aid_src.fused[i] = true;
pos_aid_src.time_last_fuse[i] = _time_last_imu;
}
}
}
// z
if (pos_aid_src.fusion_enabled[2] && !pos_aid_src.innovation_rejected[2]) {
if (fuseVelPosHeight(pos_aid_src.innovation[2], pos_aid_src.innovation_variance[2], 5)) {
pos_aid_src.fused[2] = true;
pos_aid_src.time_last_fuse[2] = _time_last_imu;
}
}
}
// Helper function that fuses a single velocity or position measurement
bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index)
{

7
src/modules/ekf2/EKF2.cpp
View File

@ -663,6 +663,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
// mag 3d
PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub);
// aux velocity
PublishAidSourceStatus(_ekf.aid_src_aux_vel(), _status_aux_vel_pub_last, _estimator_aid_src_aux_vel_pub);
}
void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@ -1563,8 +1566,8 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
// velocity of vehicle relative to target has opposite sign to target relative to vehicle
auxVelSample auxvel_sample{
.time_us = landing_target_pose.timestamp,
.vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f},
.velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f},
.vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, NAN},
.velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, NAN},
};
_ekf.setAuxVelData(auxvel_sample);
}

3
src/modules/ekf2/EKF2.hpp
View File

@ -266,6 +266,8 @@ private:
hrt_abstime _status_mag_pub_last{0};
hrt_abstime _status_mag_heading_pub_last{0};
hrt_abstime _status_aux_vel_pub_last{0};
float _last_baro_bias_published{};
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
@ -339,6 +341,7 @@ private:
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)};
uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
// publications with topic dependent on multi-mode
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;