mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 13:23:34 +08:00
estimator_aid_source split GNSS pos (3d) -> pos (2d) + hgt
- per estimator air source status only keep a single set of flags and timestamp that applies to the entire source
This commit is contained in:
Daniel Agar
@@ -52,7 +52,7 @@
|
||||
void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const
|
||||
{
|
||||
// reset flags
|
||||
resetEstimatorAidStatusFlags(airspeed);
|
||||
resetEstimatorAidStatus(airspeed);
|
||||
|
||||
// Variance for true airspeed measurement - (m/sec)^2
|
||||
const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
|
||||
|
||||
@@ -240,8 +240,8 @@ struct dragSample {
|
||||
|
||||
struct auxVelSample {
|
||||
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
|
||||
Vector3f vel{}; ///< measured NE velocity relative to the local origin (m/sec)
|
||||
Vector3f velVar{}; ///< estimated error variance of the NE velocity (m/sec)**2
|
||||
Vector2f vel{}; ///< measured NE velocity relative to the local origin (m/sec)
|
||||
Vector2f velVar{}; ///< estimated error variance of the NE velocity (m/sec)**2
|
||||
};
|
||||
|
||||
struct stateSample {
|
||||
|
||||
@@ -240,6 +240,8 @@ void Ekf::controlExternalVisionFusion()
|
||||
|
||||
// determine if we should use the horizontal position observations
|
||||
if (_control_status.flags.ev_pos) {
|
||||
resetEstimatorAidStatus(_aid_src_ev_pos);
|
||||
|
||||
if (reset && _control_status_prev.flags.ev_pos) {
|
||||
if (!_fuse_hpos_as_odom) {
|
||||
resetHorizontalPositionToVision();
|
||||
@@ -272,14 +274,23 @@ void Ekf::controlExternalVisionFusion()
|
||||
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);
|
||||
_aid_src_ev_pos.observation[0] = ev_delta_pos(0);
|
||||
_aid_src_ev_pos.observation[1] = ev_delta_pos(1);
|
||||
|
||||
_aid_src_ev_pos.innovation[0] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
|
||||
_aid_src_ev_pos.innovation[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));
|
||||
|
||||
_aid_src_ev_pos.observation_variance[0] = ev_pos_obs_var(0);
|
||||
_aid_src_ev_pos.observation_variance[1] = ev_pos_obs_var(1);
|
||||
|
||||
_aid_src_ev_pos.innovation_variance[0] = P(7, 7) + _aid_src_ev_pos.observation_variance[0];
|
||||
_aid_src_ev_pos.innovation_variance[1] = P(8, 8) + _aid_src_ev_pos.observation_variance[1];
|
||||
}
|
||||
} else {
|
||||
// use the absolute position
|
||||
@@ -291,11 +302,17 @@ void Ekf::controlExternalVisionFusion()
|
||||
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);
|
||||
_aid_src_ev_pos.observation[0] = ev_pos_meas(0);
|
||||
_aid_src_ev_pos.observation[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));
|
||||
_aid_src_ev_pos.observation_variance[0] = fmaxf(ev_pos_var(0, 0), sq(0.01f));
|
||||
_aid_src_ev_pos.observation_variance[1] = fmaxf(ev_pos_var(1, 1), sq(0.01f));
|
||||
|
||||
_aid_src_ev_pos.innovation[0] = _state.pos(0) - _aid_src_ev_pos.observation[0];
|
||||
_aid_src_ev_pos.innovation[1] = _state.pos(1) - _aid_src_ev_pos.observation[1];
|
||||
|
||||
_aid_src_ev_pos.innovation_variance[0] = P(7, 7) + _aid_src_ev_pos.observation_variance[0];
|
||||
_aid_src_ev_pos.innovation_variance[1] = P(8, 8) + _aid_src_ev_pos.observation_variance[1];
|
||||
|
||||
// check if we have been deadreckoning too long
|
||||
if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) {
|
||||
@@ -314,8 +331,12 @@ void Ekf::controlExternalVisionFusion()
|
||||
|
||||
// innovation gate size
|
||||
const float ev_pos_innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.0f);
|
||||
setEstimatorAidStatusTestRatio(_aid_src_ev_pos, ev_pos_innov_gate);
|
||||
|
||||
fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gate, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
|
||||
_aid_src_ev_pos.timestamp_sample = _ev_sample_delayed.time_us;
|
||||
_aid_src_ev_pos.fusion_enabled = true;
|
||||
|
||||
fuseHorizontalPosition(_aid_src_ev_pos);
|
||||
}
|
||||
|
||||
// determine if we should use the velocity observations
|
||||
@@ -324,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
|
||||
@@ -335,12 +354,17 @@ void Ekf::controlExternalVisionFusion()
|
||||
}
|
||||
}
|
||||
|
||||
resetEstimatorAidStatus(_aid_src_ev_vel);
|
||||
|
||||
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);
|
||||
updateVelocityAidSrcStatus(_ev_sample_delayed.time_us, getVisionVelocityInEkfFrame(), obs_var, innov_gate, _aid_src_ev_vel);
|
||||
|
||||
_aid_src_ev_vel.fusion_enabled = true;
|
||||
|
||||
fuseVelocity(_aid_src_ev_vel);
|
||||
}
|
||||
|
||||
// determine if we should use the yaw observation
|
||||
@@ -752,12 +776,12 @@ void Ekf::controlAuxVelFusion()
|
||||
|
||||
if (_auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &auxvel_sample_delayed)) {
|
||||
|
||||
resetEstimatorAidStatus(_aid_src_aux_vel);
|
||||
|
||||
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()) {
|
||||
_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));
|
||||
_aid_src_aux_vel.fusion_enabled = true;
|
||||
fuseVelocity(_aid_src_aux_vel);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1001,13 +1001,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_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_hgt.innovation < 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 * _aid_src_ev_hgt.innovation < 0.0f);
|
||||
|
||||
|
||||
if (bad_z_baro || bad_z_gps || bad_z_rng || bad_z_ev) {
|
||||
|
||||
+62
-80
@@ -99,7 +99,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 = Vector3f(_aid_src_aux_vel.test_ratio).max(); }
|
||||
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); }
|
||||
@@ -422,11 +422,15 @@ public:
|
||||
const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; }
|
||||
const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
|
||||
|
||||
const auto &aid_src_ev_hgt() const { return _aid_src_ev_hgt; }
|
||||
const auto &aid_src_ev_pos() const { return _aid_src_ev_pos; }
|
||||
const auto &aid_src_ev_vel() const { return _aid_src_ev_vel; }
|
||||
const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; }
|
||||
|
||||
const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
|
||||
const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
|
||||
const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; }
|
||||
const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
|
||||
const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
|
||||
const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
|
||||
|
||||
const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
|
||||
const auto &aid_src_mag() const { return _aid_src_mag; }
|
||||
@@ -529,17 +533,6 @@ private:
|
||||
Vector3f _delta_vel_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta velocity bias variance
|
||||
Vector3f _delta_angle_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta angle bias variance
|
||||
|
||||
float _vert_pos_innov_ratio{0.f}; ///< vertical position innovation divided by estimated standard deviation of innovation
|
||||
uint64_t _vert_pos_fuse_attempt_time_us{0}; ///< last system time in usec vertical position measurement fuson was attempted
|
||||
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)
|
||||
|
||||
Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2)
|
||||
Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2)
|
||||
|
||||
@@ -568,16 +561,20 @@ private:
|
||||
estimator_aid_source_2d_s _aid_src_fake_pos{};
|
||||
estimator_aid_source_1d_s _aid_src_fake_hgt{};
|
||||
|
||||
estimator_aid_source_1d_s _aid_src_ev_hgt{};
|
||||
estimator_aid_source_2d_s _aid_src_ev_pos{};
|
||||
estimator_aid_source_3d_s _aid_src_ev_vel{};
|
||||
estimator_aid_source_1d_s _aid_src_ev_yaw{};
|
||||
|
||||
estimator_aid_source_1d_s _aid_src_gnss_yaw{};
|
||||
estimator_aid_source_1d_s _aid_src_gnss_hgt{};
|
||||
estimator_aid_source_2d_s _aid_src_gnss_pos{};
|
||||
estimator_aid_source_3d_s _aid_src_gnss_vel{};
|
||||
estimator_aid_source_3d_s _aid_src_gnss_pos{};
|
||||
estimator_aid_source_1d_s _aid_src_gnss_yaw{};
|
||||
|
||||
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{};
|
||||
estimator_aid_source_2d_s _aid_src_aux_vel{};
|
||||
|
||||
// output predictor states
|
||||
Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad)
|
||||
@@ -737,32 +734,23 @@ 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;
|
||||
// horizontal and vertical position aid source
|
||||
void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source_2d_s &aid_src) const;
|
||||
void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source_1d_s &aid_src) const;
|
||||
|
||||
// 2d & 3d velocity aid source
|
||||
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source_2d_s &aid_src) const;
|
||||
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source_3d_s &aid_src) const;
|
||||
|
||||
// horizontal and vertical position fusion
|
||||
void fuseHorizontalPosition(estimator_aid_source_2d_s &pos_aid_src);
|
||||
void fuseVerticalPosition(estimator_aid_source_1d_s &hgt_aid_src);
|
||||
|
||||
// 2d & 3d velocity fusion
|
||||
void fuseVelocity(estimator_aid_source_2d_s &vel_aid_src);
|
||||
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);
|
||||
|
||||
bool fuseVerticalVelocity(const Vector3f &innov, float innov_gate, const Vector3f &obs_var,
|
||||
Vector3f &innov_var, Vector2f &test_ratio);
|
||||
|
||||
bool fuseHorizontalPosition(const Vector3f &innov, float innov_gate, const Vector3f &obs_var,
|
||||
Vector3f &innov_var, Vector2f &test_ratiov);
|
||||
|
||||
bool fuseVerticalPosition(float innov, float innov_gate, float obs_var,
|
||||
float &innov_var, float &test_ratio);
|
||||
|
||||
void updateGpsYaw(const gpsSample &gps_sample);
|
||||
void updateGpsVel(const gpsSample &gps_sample);
|
||||
void updateGpsPos(const gpsSample &gps_sample);
|
||||
|
||||
void fuseGpsVel();
|
||||
void fuseGpsPos();
|
||||
|
||||
// calculate optical flow body angular rate compensation
|
||||
// returns false if bias corrected body rate data is unavailable
|
||||
@@ -1125,68 +1113,54 @@ private:
|
||||
|
||||
void resetGpsDriftCheckFilters();
|
||||
|
||||
bool resetEstimatorAidStatusFlags(estimator_aid_source_1d_s &status) const
|
||||
{
|
||||
if (status.timestamp_sample != 0) {
|
||||
status.timestamp_sample = 0;
|
||||
status.fusion_enabled = false;
|
||||
status.innovation_rejected = false;
|
||||
status.fused = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void resetEstimatorAidStatus(estimator_aid_source_1d_s &status) const
|
||||
{
|
||||
if (resetEstimatorAidStatusFlags(status)) {
|
||||
// only bother resetting if timestamp_sample is set
|
||||
if (status.timestamp_sample != 0) {
|
||||
status.timestamp_sample = 0;
|
||||
|
||||
// preserve status.time_last_fuse
|
||||
|
||||
status.observation = 0;
|
||||
status.observation_variance = 0;
|
||||
|
||||
status.innovation = 0;
|
||||
status.innovation_variance = 0;
|
||||
status.test_ratio = 0;
|
||||
status.test_ratio = INFINITY;
|
||||
|
||||
status.fusion_enabled = false;
|
||||
status.innovation_rejected = true;
|
||||
status.fused = false;
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool resetEstimatorAidStatusFlags(T &status) const
|
||||
{
|
||||
if (status.timestamp_sample != 0) {
|
||||
status.timestamp_sample = 0;
|
||||
|
||||
for (size_t i = 0; i < (sizeof(status.fusion_enabled) / sizeof(status.fusion_enabled[0])); i++) {
|
||||
status.fusion_enabled[i] = false;
|
||||
status.innovation_rejected[i] = false;
|
||||
status.fused[i] = false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void resetEstimatorAidStatus(T &status) const
|
||||
{
|
||||
if (resetEstimatorAidStatusFlags(status)) {
|
||||
for (size_t i = 0; i < (sizeof(status.fusion_enabled) / sizeof(status.fusion_enabled[0])); i++) {
|
||||
// only bother resetting if timestamp_sample is set
|
||||
if (status.timestamp_sample != 0) {
|
||||
status.timestamp_sample = 0;
|
||||
|
||||
// preserve status.time_last_fuse
|
||||
|
||||
for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) {
|
||||
status.observation[i] = 0;
|
||||
status.observation_variance[i] = 0;
|
||||
|
||||
status.innovation[i] = 0;
|
||||
status.innovation_variance[i] = 0;
|
||||
status.test_ratio[i] = 0;
|
||||
status.test_ratio[i] = INFINITY;
|
||||
}
|
||||
|
||||
status.fusion_enabled = false;
|
||||
status.innovation_rejected = true;
|
||||
status.fused = false;
|
||||
}
|
||||
}
|
||||
|
||||
void setEstimatorAidStatusTestRatio(estimator_aid_source_1d_s &status, float innovation_gate) const
|
||||
{
|
||||
if (PX4_ISFINITE(status.innovation) && PX4_ISFINITE(status.innovation_variance)) {
|
||||
if (PX4_ISFINITE(status.innovation) && PX4_ISFINITE(status.innovation_variance) && (status.innovation_variance > 0.f)) {
|
||||
status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
|
||||
status.innovation_rejected = (status.test_ratio > 1.f);
|
||||
|
||||
@@ -1199,16 +1173,24 @@ private:
|
||||
template <typename T>
|
||||
void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const
|
||||
{
|
||||
bool innovation_rejected = false;
|
||||
|
||||
for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) {
|
||||
if (PX4_ISFINITE(status.innovation[i]) && PX4_ISFINITE(status.innovation_variance[i])) {
|
||||
if (PX4_ISFINITE(status.innovation[i]) && PX4_ISFINITE(status.innovation_variance[i]) && (status.innovation_variance[i] > 0.f)) {
|
||||
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 (status.test_ratio[i] > 1.f) {
|
||||
innovation_rejected = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
status.test_ratio[i] = INFINITY;
|
||||
status.innovation_rejected[i] = true;
|
||||
innovation_rejected = true;
|
||||
}
|
||||
}
|
||||
|
||||
// if any of the innovations are rejected, then the overall innovation is rejected
|
||||
status.innovation_rejected = innovation_rejected;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -557,7 +557,7 @@ void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &v
|
||||
|
||||
hpos[0] = _aid_src_gnss_pos.innovation[0];
|
||||
hpos[1] = _aid_src_gnss_pos.innovation[1];
|
||||
vpos = _aid_src_gnss_pos.innovation[2];
|
||||
vpos = _aid_src_gnss_hgt.innovation;
|
||||
}
|
||||
|
||||
void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const
|
||||
@@ -568,7 +568,7 @@ void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float
|
||||
|
||||
hpos[0] = _aid_src_gnss_pos.innovation_variance[0];
|
||||
hpos[1] = _aid_src_gnss_pos.innovation_variance[1];
|
||||
vpos = _aid_src_gnss_pos.innovation_variance[2];
|
||||
vpos = _aid_src_gnss_hgt.innovation_variance;
|
||||
}
|
||||
|
||||
void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
|
||||
@@ -577,35 +577,38 @@ void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &v
|
||||
vvel = _aid_src_gnss_vel.test_ratio[2];
|
||||
|
||||
hpos = fmaxf(_aid_src_gnss_pos.test_ratio[0], _aid_src_gnss_pos.test_ratio[1]);
|
||||
vpos = _aid_src_gnss_pos.test_ratio[2];
|
||||
vpos = _aid_src_gnss_hgt.test_ratio;
|
||||
}
|
||||
|
||||
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_hgt.innovation;
|
||||
}
|
||||
|
||||
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_hgt.innovation_variance;
|
||||
}
|
||||
|
||||
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_hgt.test_ratio;
|
||||
}
|
||||
|
||||
void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
|
||||
@@ -714,7 +717,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());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -731,8 +734,14 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
|
||||
// If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal position error
|
||||
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
|
||||
// and using state variances for accuracy reporting is overly optimistic in these situations
|
||||
if (_horizontal_deadreckon_time_exceeded && _control_status.flags.gps) {
|
||||
hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
|
||||
if (_horizontal_deadreckon_time_exceeded) {
|
||||
if (_control_status.flags.gps) {
|
||||
hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_pos) {
|
||||
hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm());
|
||||
}
|
||||
}
|
||||
|
||||
*ekf_eph = hpos_err;
|
||||
@@ -759,11 +768,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, Vector2f(_aid_src_ev_vel.innovation).norm());
|
||||
}
|
||||
|
||||
hvel_err = math::max(hvel_err, vel_err_conservative);
|
||||
@@ -908,12 +917,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);
|
||||
}
|
||||
|
||||
@@ -924,25 +933,28 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
||||
|
||||
// return the combined vertical position innovation test ratio
|
||||
float hgt_sum = 0.f;
|
||||
int n_hgt_sources = 0;
|
||||
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
hgt_sum += sqrtf(_aid_src_baro_hgt.test_ratio);
|
||||
n_hgt_sources++;
|
||||
}
|
||||
|
||||
if (_control_status.flags.gps_hgt) {
|
||||
hgt_sum += sqrtf(_aid_src_gnss_pos.test_ratio[2]);
|
||||
hgt_sum += sqrtf(_aid_src_gnss_hgt.test_ratio);
|
||||
n_hgt_sources++;
|
||||
}
|
||||
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
hgt_sum += sqrtf(_aid_src_rng_hgt.test_ratio);
|
||||
n_hgt_sources++;
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
hgt_sum += sqrtf(_ev_pos_test_ratio(1));
|
||||
hgt_sum += sqrtf(_aid_src_ev_hgt.test_ratio);
|
||||
n_hgt_sources++;
|
||||
}
|
||||
|
||||
const int n_hgt_sources = getNumberOfActiveVerticalPositionAidingSources();
|
||||
|
||||
if (n_hgt_sources > 0) {
|
||||
hgt = math::max(hgt_sum / static_cast<float>(n_hgt_sources), FLT_MIN);
|
||||
|
||||
@@ -1498,7 +1510,6 @@ void Ekf::stopGpsPosFusion()
|
||||
if (_control_status.flags.gps) {
|
||||
ECL_INFO("stopping GPS position fusion");
|
||||
_control_status.flags.gps = false;
|
||||
stopGpsHgtFusion();
|
||||
|
||||
resetEstimatorAidStatus(_aid_src_gnss_pos);
|
||||
}
|
||||
@@ -1571,18 +1582,20 @@ 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 pos fusion");
|
||||
_control_status.flags.ev_pos = false;
|
||||
resetEstimatorAidStatus(_aid_src_ev_pos);
|
||||
}
|
||||
}
|
||||
|
||||
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 vel fusion");
|
||||
_control_status.flags.ev_vel = false;
|
||||
resetEstimatorAidStatus(_aid_src_ev_vel);
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopEvYawFusion()
|
||||
|
||||
@@ -341,8 +341,7 @@ 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
|
||||
Vector2f _ev_vel_test_ratio{}; // EV velocity innovation consistency check ratios
|
||||
Vector2f _ev_pos_test_ratio{}; // EV position innovation consistency check ratios
|
||||
|
||||
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
|
||||
|
||||
@@ -89,7 +89,7 @@ void Ekf::startEvHgtFusion()
|
||||
{
|
||||
if (!_control_status.flags.ev_hgt) {
|
||||
if (_params.height_sensor_ref == HeightSensor::EV) {
|
||||
_rng_hgt_b_est.reset();
|
||||
_ev_hgt_b_est.reset();
|
||||
_height_sensor_ref = HeightSensor::EV;
|
||||
resetHeightToEv();
|
||||
|
||||
|
||||
@@ -43,7 +43,7 @@ void Ekf::controlFakeHgtFusion()
|
||||
auto &fake_hgt = _aid_src_fake_hgt;
|
||||
|
||||
// clear
|
||||
resetEstimatorAidStatusFlags(fake_hgt);
|
||||
resetEstimatorAidStatus(fake_hgt);
|
||||
|
||||
// If we aren't doing any aiding, fake position measurements at the last known vertical position to constrain drift
|
||||
const bool fake_hgt_data_ready = isTimedOut(fake_hgt.time_last_fuse, (uint64_t)2e5); // Fuse fake height at a limited rate
|
||||
|
||||
@@ -43,11 +43,11 @@ void Ekf::controlFakePosFusion()
|
||||
auto &fake_pos = _aid_src_fake_pos;
|
||||
|
||||
// clear
|
||||
resetEstimatorAidStatusFlags(fake_pos);
|
||||
resetEstimatorAidStatus(fake_pos);
|
||||
|
||||
// If we aren't doing any aiding, fake position measurements at the last known position to constrain drift
|
||||
// During intial tilt aligment, fake position is used to perform a "quasi-stationary" leveling of the EKF
|
||||
const bool fake_pos_data_ready = isTimedOut(fake_pos.time_last_fuse[0], (uint64_t)2e5); // Fuse fake position at a limited rate
|
||||
const bool fake_pos_data_ready = isTimedOut(fake_pos.time_last_fuse, (uint64_t)2e5); // Fuse fake position at a limited rate
|
||||
|
||||
if (fake_pos_data_ready) {
|
||||
const bool continuing_conditions_passing = !isHorizontalAidingActive();
|
||||
@@ -58,7 +58,7 @@ void Ekf::controlFakePosFusion()
|
||||
if (continuing_conditions_passing) {
|
||||
fuseFakePosition();
|
||||
|
||||
const bool is_fusion_failing = isTimedOut(fake_pos.time_last_fuse[0], (uint64_t)4e5);
|
||||
const bool is_fusion_failing = isTimedOut(fake_pos.time_last_fuse, (uint64_t)4e5);
|
||||
|
||||
if (is_fusion_failing) {
|
||||
resetFakePosFusion();
|
||||
@@ -100,8 +100,7 @@ void Ekf::resetFakePosFusion()
|
||||
resetHorizontalPositionToLastKnown();
|
||||
resetHorizontalVelocityToZero();
|
||||
|
||||
_aid_src_fake_pos.time_last_fuse[0] = _imu_sample_delayed.time_us;
|
||||
_aid_src_fake_pos.time_last_fuse[1] = _imu_sample_delayed.time_us;
|
||||
_aid_src_fake_pos.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
void Ekf::stopFakePosFusion()
|
||||
@@ -144,16 +143,25 @@ void Ekf::fuseFakePosition()
|
||||
|
||||
setEstimatorAidStatusTestRatio(fake_pos, innov_gate);
|
||||
|
||||
// fuse
|
||||
for (int i = 0; i < 2; i++) {
|
||||
// always protect against extreme values that could result in a NaN
|
||||
fake_pos.fusion_enabled[i] = fake_pos.test_ratio[i] < sq(100.0f / innov_gate);
|
||||
fake_pos.fusion_enabled = true;
|
||||
|
||||
if (fake_pos.fusion_enabled[i] && !fake_pos.innovation_rejected[i]) {
|
||||
if (fuseVelPosHeight(fake_pos.innovation[i], fake_pos.innovation_variance[i], 3 + i)) {
|
||||
fake_pos.fused[i] = true;
|
||||
fake_pos.time_last_fuse[i] = _imu_sample_delayed.time_us;
|
||||
}
|
||||
// always protect against extreme values that could result in a NaN
|
||||
if (!fake_pos.innovation_rejected) {
|
||||
if ((fake_pos.test_ratio[0] > sq(100.0f / innov_gate)) || (fake_pos.test_ratio[1] > sq(100.0f / innov_gate))) {
|
||||
fake_pos.innovation_rejected = true;
|
||||
}
|
||||
}
|
||||
|
||||
// fuse
|
||||
if (fake_pos.fusion_enabled && !fake_pos.innovation_rejected) {
|
||||
if (fuseVelPosHeight(fake_pos.innovation[0], fake_pos.innovation_variance[0], 3)
|
||||
&& fuseVelPosHeight(fake_pos.innovation[1], fake_pos.innovation_variance[1], 4)
|
||||
) {
|
||||
fake_pos.fused = true;
|
||||
fake_pos.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
|
||||
} else {
|
||||
fake_pos.fused = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -45,17 +45,59 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
|
||||
return;
|
||||
}
|
||||
|
||||
_gps_hgt_b_est.predict(_dt_ekf_avg);
|
||||
auto &aid_src = _aid_src_gnss_hgt;
|
||||
HeightBiasEstimator &bias_est = _gps_hgt_b_est;
|
||||
|
||||
bias_est.predict(_dt_ekf_avg);
|
||||
|
||||
if (_gps_data_ready) {
|
||||
const bool continuing_conditions_passing = !_gps_intermittent && _gps_checks_passed && _NED_origin_initialised;
|
||||
const bool starting_conditions_passing = continuing_conditions_passing;
|
||||
|
||||
const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
|
||||
const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
|
||||
|
||||
const float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);
|
||||
|
||||
const float measurement = gps_sample.hgt - getEkfGlobalOriginAltitude();
|
||||
const float measurement_var = getGpsHeightVariance(gps_sample);
|
||||
|
||||
// GNSS position, vertical position GNSS measurement has opposite sign to earth z axis
|
||||
updateVerticalPositionAidSrcStatus(gps_sample.time_us,
|
||||
-(measurement - bias_est.getBias()),
|
||||
measurement_var + bias_est.getBiasVar(),
|
||||
innov_gate,
|
||||
aid_src);
|
||||
|
||||
// update the bias estimator before updating the main filter but after
|
||||
// using its current state to compute the vertical position innovation
|
||||
if (gps_checks_passing && !gps_checks_failing
|
||||
&& PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var)
|
||||
) {
|
||||
const float noise = sqrtf(measurement_var);
|
||||
bias_est.setMaxStateNoise(noise);
|
||||
bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
|
||||
bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9));
|
||||
}
|
||||
|
||||
// determine if we should use GNSS height aiding
|
||||
const bool continuing_conditions_passing = (_params.gnss_ctrl & GnssCtrl::VPOS)
|
||||
&& PX4_ISFINITE(gps_sample.hgt)
|
||||
&& _NED_origin_initialised
|
||||
&& _gps_checks_passed;
|
||||
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& _gps_checks_passed
|
||||
&& gps_checks_passing
|
||||
&& !gps_checks_failing
|
||||
&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL);
|
||||
|
||||
if (_control_status.flags.gps_hgt) {
|
||||
if (continuing_conditions_passing) {
|
||||
/* fuseGpsHgt(); */ // Done in fuseGpsPos
|
||||
aid_src.fusion_enabled = true;
|
||||
|
||||
const bool is_fusion_failing = isTimedOut(_aid_src_gnss_pos.time_last_fuse[2], _params.hgt_fusion_timeout_max);
|
||||
if (continuing_conditions_passing) {
|
||||
|
||||
fuseVerticalPosition(aid_src);
|
||||
|
||||
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
|
||||
|
||||
if (isHeightResetRequired()) {
|
||||
// All height sources are failing
|
||||
@@ -95,7 +137,7 @@ void Ekf::startGpsHgtFusion(const gpsSample &gps_sample)
|
||||
_gps_hgt_b_est.setBias(_state.pos(2) + (gps_sample.hgt - getEkfGlobalOriginAltitude()));
|
||||
|
||||
// Reset the timeout value here because the fusion isn't done at the same place and would immediately trigger a timeout
|
||||
_aid_src_gnss_pos.time_last_fuse[2] = _imu_sample_delayed.time_us;
|
||||
_aid_src_gnss_hgt.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
_control_status.flags.gps_hgt = true;
|
||||
@@ -118,7 +160,7 @@ void Ekf::resetHeightToGps(const gpsSample &gps_sample)
|
||||
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
|
||||
|
||||
_aid_src_gnss_pos.time_last_fuse[2] = _imu_sample_delayed.time_us;
|
||||
_aid_src_gnss_hgt.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
void Ekf::stopGpsHgtFusion()
|
||||
|
||||
@@ -52,22 +52,42 @@ void Ekf::controlGpsFusion()
|
||||
const gpsSample &gps_sample{_gps_sample_delayed};
|
||||
|
||||
updateGpsYaw(gps_sample);
|
||||
updateGpsVel(gps_sample);
|
||||
updateGpsPos(gps_sample);
|
||||
|
||||
const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
|
||||
const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
|
||||
|
||||
controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing);
|
||||
|
||||
// GNSS velocity
|
||||
const Vector3f velocity{gps_sample.vel};
|
||||
const float vel_var = sq(gps_sample.sacc);
|
||||
const Vector3f vel_obs_var{vel_var, vel_var, vel_var * sq(1.5f)};
|
||||
updateVelocityAidSrcStatus(gps_sample.time_us, velocity, vel_obs_var, fmaxf(_params.gps_vel_innov_gate, 1.f), _aid_src_gnss_vel);
|
||||
_aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL);
|
||||
|
||||
// GNSS position
|
||||
const float pos_var_lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
|
||||
float pos_var = sq(fmaxf(gps_sample.hacc, pos_var_lower_limit));
|
||||
|
||||
if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
|
||||
// if we are not using another source of aiding, then we are reliant on the GPS
|
||||
// observations to constrain attitude errors and must limit the observation noise value.
|
||||
float upper_limit = fmaxf(_params.pos_noaid_noise, pos_var_lower_limit);
|
||||
pos_var = fminf(pos_var, upper_limit);
|
||||
}
|
||||
|
||||
updateHorizontalPositionAidSrcStatus(gps_sample.time_us, gps_sample.pos, Vector2f(pos_var, pos_var), fmaxf(_params.gps_pos_innov_gate, 1.f), _aid_src_gnss_pos);
|
||||
_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
|
||||
|
||||
// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
|
||||
if (gps_checks_passing && !gps_checks_failing) {
|
||||
_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.sacc);
|
||||
_yawEstimator.setVelocity(velocity.xy(), gps_sample.sacc);
|
||||
}
|
||||
|
||||
// Determine if we should use GPS aiding for velocity and horizontal position
|
||||
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
|
||||
const bool mandatory_conditions_passing = _control_status.flags.tilt_align
|
||||
const bool mandatory_conditions_passing = ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))
|
||||
&& _control_status.flags.tilt_align
|
||||
&& _control_status.flags.yaw_align
|
||||
&& _NED_origin_initialised;
|
||||
|
||||
@@ -79,16 +99,11 @@ void Ekf::controlGpsFusion()
|
||||
if (continuing_conditions_passing
|
||||
|| !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
|
||||
|
||||
if (_params.gnss_ctrl & GnssCtrl::VEL) {
|
||||
fuseGpsVel();
|
||||
}
|
||||
|
||||
if ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VPOS)) {
|
||||
fuseGpsPos();
|
||||
}
|
||||
fuseVelocity(_aid_src_gnss_vel);
|
||||
fuseHorizontalPosition(_aid_src_gnss_pos);
|
||||
|
||||
if (shouldResetGpsFusion()) {
|
||||
const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);
|
||||
const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1'000'000);
|
||||
|
||||
/* A reset is not performed when getting GPS back after a significant period of no data
|
||||
* because the timeout could have been caused by bad GPS.
|
||||
@@ -98,7 +113,7 @@ void Ekf::controlGpsFusion()
|
||||
&& _control_status.flags.in_air
|
||||
&& !was_gps_signal_lost
|
||||
&& _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit
|
||||
&& isTimedOut(_ekfgsf_yaw_reset_time, 5000000)) {
|
||||
&& isTimedOut(_ekfgsf_yaw_reset_time, 5'000'000)) {
|
||||
// The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
|
||||
// to improve its estimate if the previous reset was not successful.
|
||||
if (resetYawToEKFGSF()) {
|
||||
@@ -116,8 +131,8 @@ void Ekf::controlGpsFusion()
|
||||
ECL_WARN("GPS fusion timeout - resetting");
|
||||
}
|
||||
|
||||
resetVelocityToGps(_gps_sample_delayed);
|
||||
resetHorizontalPositionToGps(_gps_sample_delayed);
|
||||
resetVelocityToGps(gps_sample);
|
||||
resetHorizontalPositionToGps(gps_sample);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
@@ -42,7 +42,7 @@
|
||||
void Ekf::updateGpsYaw(const gpsSample &gps_sample)
|
||||
{
|
||||
auto &gps_yaw = _aid_src_gnss_yaw;
|
||||
resetEstimatorAidStatusFlags(gps_yaw);
|
||||
resetEstimatorAidStatus(gps_yaw);
|
||||
|
||||
if (PX4_ISFINITE(gps_sample.yaw)) {
|
||||
// initially populate for estimator_aid_src_gnss_yaw logging
|
||||
@@ -64,155 +64,3 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample)
|
||||
gps_yaw.timestamp_sample = gps_sample.time_us;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::updateGpsVel(const gpsSample &gps_sample)
|
||||
{
|
||||
auto &gps_vel = _aid_src_gnss_vel;
|
||||
resetEstimatorAidStatus(gps_vel);
|
||||
|
||||
const float vel_var = sq(gps_sample.sacc);
|
||||
const Vector3f obs_var{vel_var, vel_var, vel_var * sq(1.5f)};
|
||||
|
||||
// innovation gate size
|
||||
const float innov_gate = fmaxf(_params.gps_vel_innov_gate, 1.f);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
gps_vel.observation[i] = gps_sample.vel(i);
|
||||
gps_vel.observation_variance[i] = obs_var(i);
|
||||
|
||||
gps_vel.innovation[i] = _state.vel(i) - gps_sample.vel(i);
|
||||
gps_vel.innovation_variance[i] = P(4 + i, 4 + i) + obs_var(i);
|
||||
}
|
||||
|
||||
setEstimatorAidStatusTestRatio(gps_vel, 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 && gps_vel.innovation_rejected[2]) {
|
||||
const float innov_limit = innov_gate * sqrtf(gps_vel.innovation_variance[2]);
|
||||
gps_vel.innovation[2] = math::constrain(gps_vel.innovation[2], -innov_limit, innov_limit);
|
||||
gps_vel.innovation_rejected[2] = false;
|
||||
}
|
||||
|
||||
gps_vel.timestamp_sample = gps_sample.time_us;
|
||||
}
|
||||
|
||||
void Ekf::updateGpsPos(const gpsSample &gps_sample)
|
||||
{
|
||||
auto &gps_pos = _aid_src_gnss_pos;
|
||||
resetEstimatorAidStatus(gps_pos);
|
||||
|
||||
const float height_measurement = gps_sample.hgt - getEkfGlobalOriginAltitude();
|
||||
const float height_measurement_var = getGpsHeightVariance(gps_sample);
|
||||
|
||||
Vector3f position;
|
||||
position(0) = gps_sample.pos(0);
|
||||
position(1) = gps_sample.pos(1);
|
||||
|
||||
// vertical position - gps measurement has opposite sign to earth z axis
|
||||
position(2) = -(height_measurement - _gps_hgt_b_est.getBias());
|
||||
|
||||
const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
|
||||
|
||||
Vector3f obs_var;
|
||||
|
||||
if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
|
||||
// if we are using other sources of aiding, then relax the upper observation
|
||||
// noise limit which prevents bad GPS perturbing the position estimate
|
||||
obs_var(0) = obs_var(1) = sq(fmaxf(gps_sample.hacc, lower_limit));
|
||||
|
||||
} else {
|
||||
// if we are not using another source of aiding, then we are reliant on the GPS
|
||||
// observations to constrain attitude errors and must limit the observation noise value.
|
||||
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
|
||||
obs_var(0) = obs_var(1) = sq(math::constrain(gps_sample.hacc, lower_limit, upper_limit));
|
||||
}
|
||||
|
||||
obs_var(2) = height_measurement_var + _gps_hgt_b_est.getBiasVar();
|
||||
|
||||
// innovation gate size
|
||||
float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
gps_pos.observation[i] = position(i);
|
||||
gps_pos.observation_variance[i] = obs_var(i);
|
||||
|
||||
gps_pos.innovation[i] = _state.pos(i) - position(i);
|
||||
gps_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
|
||||
}
|
||||
|
||||
setEstimatorAidStatusTestRatio(gps_pos, 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 && gps_pos.innovation_rejected[2]) {
|
||||
const float innov_limit = innov_gate * sqrtf(gps_pos.innovation_variance[2]);
|
||||
gps_pos.innovation[2] = math::constrain(gps_pos.innovation[2], -innov_limit, innov_limit);
|
||||
gps_pos.innovation_rejected[2] = false;
|
||||
}
|
||||
|
||||
gps_pos.timestamp_sample = gps_sample.time_us;
|
||||
|
||||
// update the bias estimator before updating the main filter but after
|
||||
// using its current state to compute the vertical position innovation
|
||||
_gps_hgt_b_est.setMaxStateNoise(height_measurement_var);
|
||||
_gps_hgt_b_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
|
||||
_gps_hgt_b_est.fuseBias(height_measurement - (-_state.pos(2)), height_measurement_var + P(9, 9));
|
||||
}
|
||||
|
||||
void Ekf::fuseGpsVel()
|
||||
{
|
||||
// velocity
|
||||
auto &gps_vel = _aid_src_gnss_vel;
|
||||
|
||||
// vx & vy
|
||||
gps_vel.fusion_enabled[0] = true;
|
||||
gps_vel.fusion_enabled[1] = true;
|
||||
|
||||
if (!gps_vel.innovation_rejected[0] && !gps_vel.innovation_rejected[1]) {
|
||||
for (int i = 0; i < 2; i++) {
|
||||
if (fuseVelPosHeight(gps_vel.innovation[i], gps_vel.innovation_variance[i], i)) {
|
||||
gps_vel.fused[i] = true;
|
||||
gps_vel.time_last_fuse[i] = _imu_sample_delayed.time_us;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// vz
|
||||
gps_vel.fusion_enabled[2] = true;
|
||||
|
||||
if (gps_vel.fusion_enabled[2] && !gps_vel.innovation_rejected[2]) {
|
||||
if (fuseVelPosHeight(gps_vel.innovation[2], gps_vel.innovation_variance[2], 2)) {
|
||||
gps_vel.fused[2] = true;
|
||||
gps_vel.time_last_fuse[2] = _imu_sample_delayed.time_us;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseGpsPos()
|
||||
{
|
||||
auto &gps_pos = _aid_src_gnss_pos;
|
||||
|
||||
// x & y
|
||||
gps_pos.fusion_enabled[0] = true;
|
||||
gps_pos.fusion_enabled[1] = true;
|
||||
|
||||
if (!gps_pos.innovation_rejected[0] && !gps_pos.innovation_rejected[1]) {
|
||||
for (int i = 0; i < 2; i++) {
|
||||
if (fuseVelPosHeight(gps_pos.innovation[i], gps_pos.innovation_variance[i], 3 + i)) {
|
||||
gps_pos.fused[i] = true;
|
||||
gps_pos.time_last_fuse[i] = _imu_sample_delayed.time_us;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// z
|
||||
gps_pos.fusion_enabled[2] = _control_status.flags.gps_hgt;
|
||||
|
||||
if (gps_pos.fusion_enabled[2] && !gps_pos.innovation_rejected[2]) {
|
||||
if (fuseVelPosHeight(gps_pos.innovation[2], gps_pos.innovation_variance[2], 5)) {
|
||||
gps_pos.fused[2] = true;
|
||||
gps_pos.time_last_fuse[2] = _imu_sample_delayed.time_us;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -178,7 +178,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
|
||||
}
|
||||
|
||||
if (_control_status.flags.gps_hgt) {
|
||||
checks[1] = {ReferenceType::GNSS, _aid_src_gnss_pos.innovation[2], _aid_src_gnss_pos.innovation_variance[2]};
|
||||
checks[1] = {ReferenceType::GNSS, _aid_src_gnss_hgt.innovation, _aid_src_gnss_hgt.innovation_variance};
|
||||
}
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
@@ -190,11 +190,11 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
checks[4] = {ReferenceType::GROUND, _ev_pos_innov(2), _ev_pos_innov_var(2)};
|
||||
checks[4] = {ReferenceType::GROUND, _aid_src_ev_hgt.innovation, _aid_src_ev_hgt.innovation_variance};
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_vel) {
|
||||
checks[5] = {ReferenceType::GROUND, _ev_vel_innov(2), _ev_vel_innov_var(2)};
|
||||
checks[5] = {ReferenceType::GROUND, _aid_src_ev_vel.innovation[2], _aid_src_ev_vel.innovation_variance[2]};
|
||||
}
|
||||
|
||||
// Compute the check based on innovation ratio for all the sources
|
||||
|
||||
@@ -41,7 +41,7 @@
|
||||
void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt)
|
||||
{
|
||||
// reset flags
|
||||
resetEstimatorAidStatusFlags(baro_hgt);
|
||||
resetEstimatorAidStatus(baro_hgt);
|
||||
|
||||
// innovation gate size
|
||||
const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
|
||||
@@ -109,7 +109,7 @@ void Ekf::fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt)
|
||||
void Ekf::updateRngHgt(estimator_aid_source_1d_s &rng_hgt)
|
||||
{
|
||||
// reset flags
|
||||
resetEstimatorAidStatusFlags(rng_hgt);
|
||||
resetEstimatorAidStatus(rng_hgt);
|
||||
|
||||
// measurement variance - user parameter defined
|
||||
const float measurement_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
|
||||
@@ -174,12 +174,13 @@ void Ekf::fuseEvHgt()
|
||||
// calculate the innovation assuming the external vision observation is in local NED frame
|
||||
const float obs = measurement - bias;
|
||||
const float obs_var = measurement_var + bias_var;
|
||||
_ev_pos_innov(2) = _state.pos(2) - obs;
|
||||
|
||||
// innovation gate size
|
||||
float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f);
|
||||
|
||||
// _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));
|
||||
updateVerticalPositionAidSrcStatus(_ev_sample_delayed.time_us, obs, obs_var, innov_gate, _aid_src_ev_hgt);
|
||||
|
||||
_aid_src_ev_hgt.fusion_enabled = _control_status.flags.ev_hgt;
|
||||
|
||||
fuseVerticalPosition(_aid_src_ev_hgt);
|
||||
}
|
||||
|
||||
@@ -171,25 +171,25 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b
|
||||
aid_src_mag.observation[i] = mag_observation(i);
|
||||
aid_src_mag.observation_variance[i] = R_MAG;
|
||||
aid_src_mag.innovation[i] = mag_innov(i);
|
||||
aid_src_mag.fusion_enabled[i] = _control_status.flags.mag_3D && update_all_states;
|
||||
}
|
||||
|
||||
aid_src_mag.fusion_enabled = _control_status.flags.mag_3D && update_all_states;
|
||||
|
||||
// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
|
||||
if (_control_status.flags.synthetic_mag_z) {
|
||||
aid_src_mag.innovation[2] = 0.0f;
|
||||
aid_src_mag.innovation_rejected[2] = false;
|
||||
}
|
||||
|
||||
const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
|
||||
setEstimatorAidStatusTestRatio(aid_src_mag, innov_gate);
|
||||
|
||||
// Perform an innovation consistency check and report the result
|
||||
_innov_check_fail_status.flags.reject_mag_x = aid_src_mag.innovation_rejected[0];
|
||||
_innov_check_fail_status.flags.reject_mag_y = aid_src_mag.innovation_rejected[1];
|
||||
_innov_check_fail_status.flags.reject_mag_z = aid_src_mag.innovation_rejected[2];
|
||||
_innov_check_fail_status.flags.reject_mag_x = (aid_src_mag.test_ratio[0] > 1.f);
|
||||
_innov_check_fail_status.flags.reject_mag_y = (aid_src_mag.test_ratio[1] > 1.f);
|
||||
_innov_check_fail_status.flags.reject_mag_z = (aid_src_mag.test_ratio[2] > 1.f);
|
||||
|
||||
// if any axis fails, abort the mag fusion
|
||||
if (aid_src_mag.innovation_rejected[0] || aid_src_mag.innovation_rejected[1] || aid_src_mag.innovation_rejected[2]) {
|
||||
if (aid_src_mag.innovation_rejected) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -197,6 +197,8 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b
|
||||
SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion;
|
||||
Vector24f Kfusion;
|
||||
|
||||
bool fused[3] {false, false, false};
|
||||
|
||||
// update the states and covariance using sequential fusion of the magnetometer components
|
||||
for (uint8_t index = 0; index <= 2; index++) {
|
||||
|
||||
@@ -397,37 +399,27 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b
|
||||
Kfusion(21) = HKZ23*HKZ24;
|
||||
}
|
||||
|
||||
const bool is_fused = measurementUpdate(Kfusion, Hfusion, aid_src_mag.innovation[index]);
|
||||
|
||||
if (is_fused) {
|
||||
aid_src_mag.fused[index] = true;
|
||||
aid_src_mag.time_last_fuse[index] = _imu_sample_delayed.time_us;
|
||||
if (measurementUpdate(Kfusion, Hfusion, aid_src_mag.innovation[index])) {
|
||||
fused[index] = true;
|
||||
limitDeclination();
|
||||
|
||||
} else {
|
||||
aid_src_mag.fused[index] = false;
|
||||
}
|
||||
|
||||
|
||||
switch (index) {
|
||||
case 0:
|
||||
_fault_status.flags.bad_mag_x = !is_fused;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
_fault_status.flags.bad_mag_y = !is_fused;
|
||||
break;
|
||||
|
||||
case 2:
|
||||
_fault_status.flags.bad_mag_z = !is_fused;
|
||||
break;
|
||||
}
|
||||
|
||||
if (is_fused) {
|
||||
limitDeclination();
|
||||
fused[index] = false;
|
||||
}
|
||||
}
|
||||
|
||||
return aid_src_mag.fused[0] && aid_src_mag.fused[1] && aid_src_mag.fused[2];
|
||||
_fault_status.flags.bad_mag_x = !fused[0];
|
||||
_fault_status.flags.bad_mag_y = !fused[1];
|
||||
_fault_status.flags.bad_mag_z = !fused[2];
|
||||
|
||||
if (fused[0] && fused[1] && fused[2]) {
|
||||
aid_src_mag.fused = true;
|
||||
aid_src_mag.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
return true;
|
||||
}
|
||||
|
||||
aid_src_mag.fused = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
// update quaternion states and covariances using the yaw innovation and yaw observation variance
|
||||
|
||||
@@ -50,7 +50,7 @@
|
||||
void Ekf::updateSideslip(estimator_aid_source_1d_s &sideslip) const
|
||||
{
|
||||
// reset flags
|
||||
resetEstimatorAidStatusFlags(sideslip);
|
||||
resetEstimatorAidStatus(sideslip);
|
||||
|
||||
const float R = sq(_params.beta_noise); // observation noise variance
|
||||
|
||||
|
||||
@@ -33,7 +33,6 @@
|
||||
|
||||
/**
|
||||
* @file vel_pos_fusion.cpp
|
||||
* Function for fusing gps and baro measurements/
|
||||
*
|
||||
* @author Roman Bast <bapstroman@gmail.com>
|
||||
* @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
|
||||
@@ -44,213 +43,148 @@
|
||||
#include <mathlib/mathlib.h>
|
||||
#include "ekf.h"
|
||||
|
||||
bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var,
|
||||
Vector3f &innov_var, Vector2f &test_ratio)
|
||||
void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
|
||||
const float innov_gate, estimator_aid_source_2d_s &aid_src) const
|
||||
{
|
||||
innov_var(0) = P(4, 4) + obs_var(0);
|
||||
innov_var(1) = P(5, 5) + obs_var(1);
|
||||
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate) * innov_var(0)),
|
||||
sq(innov(1)) / (sq(innov_gate) * innov_var(1)));
|
||||
resetEstimatorAidStatus(aid_src);
|
||||
|
||||
const bool innov_check_pass = (test_ratio(0) <= 1.0f);
|
||||
for (int i = 0; i < 2; i++) {
|
||||
aid_src.observation[i] = obs(i);
|
||||
aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
|
||||
|
||||
if (innov_check_pass) {
|
||||
_innov_check_fail_status.flags.reject_hor_vel = false;
|
||||
|
||||
bool fuse_vx = fuseVelPosHeight(innov(0), innov_var(0), 0);
|
||||
bool fuse_vy = fuseVelPosHeight(innov(1), innov_var(1), 1);
|
||||
|
||||
return fuse_vx && fuse_vy;
|
||||
|
||||
} else {
|
||||
_innov_check_fail_status.flags.reject_hor_vel = true;
|
||||
return false;
|
||||
aid_src.observation_variance[i] = obs_var(i);
|
||||
aid_src.innovation_variance[i] = P(4 + i, 4 + i) + aid_src.observation_variance[i];
|
||||
}
|
||||
|
||||
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
|
||||
|
||||
aid_src.timestamp_sample = time_us;
|
||||
}
|
||||
|
||||
bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var,
|
||||
Vector3f &innov_var, Vector2f &test_ratio)
|
||||
void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var,
|
||||
const float innov_gate, estimator_aid_source_3d_s &aid_src) const
|
||||
{
|
||||
innov_var(2) = P(6, 6) + obs_var(2);
|
||||
test_ratio(1) = sq(innov(2)) / (sq(innov_gate) * innov_var(2));
|
||||
_vert_vel_innov_ratio = innov(2) / sqrtf(innov_var(2));
|
||||
_vert_vel_fuse_time_us = _imu_sample_delayed.time_us;
|
||||
bool innov_check_pass = (test_ratio(1) <= 1.0f);
|
||||
|
||||
// if there is bad vertical acceleration data, then don't reject measurement,
|
||||
// but limit innovation to prevent spikes that could destabilise the filter
|
||||
float innovation;
|
||||
|
||||
if (_fault_status.flags.bad_acc_vertical && !innov_check_pass) {
|
||||
const float innov_limit = innov_gate * sqrtf(innov_var(2));
|
||||
innovation = math::constrain(innov(2), -innov_limit, innov_limit);
|
||||
innov_check_pass = true;
|
||||
|
||||
} else {
|
||||
innovation = innov(2);
|
||||
}
|
||||
|
||||
if (innov_check_pass) {
|
||||
_innov_check_fail_status.flags.reject_ver_vel = false;
|
||||
|
||||
return fuseVelPosHeight(innovation, innov_var(2), 2);
|
||||
|
||||
} else {
|
||||
_innov_check_fail_status.flags.reject_ver_vel = true;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var,
|
||||
Vector3f &innov_var, Vector2f &test_ratio)
|
||||
{
|
||||
|
||||
innov_var(0) = P(7, 7) + obs_var(0);
|
||||
innov_var(1) = P(8, 8) + obs_var(1);
|
||||
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate) * innov_var(0)),
|
||||
sq(innov(1)) / (sq(innov_gate) * innov_var(1)));
|
||||
|
||||
const bool innov_check_pass = test_ratio(0) <= 1.0f;
|
||||
|
||||
if (innov_check_pass) {
|
||||
_innov_check_fail_status.flags.reject_hor_pos = false;
|
||||
|
||||
bool fuse_x = fuseVelPosHeight(innov(0), innov_var(0), 3);
|
||||
bool fuse_y = fuseVelPosHeight(innov(1), innov_var(1), 4);
|
||||
|
||||
return fuse_x && fuse_y;
|
||||
|
||||
} else {
|
||||
_innov_check_fail_status.flags.reject_hor_pos = true;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const float obs_var,
|
||||
float &innov_var, float &test_ratio)
|
||||
{
|
||||
innov_var = P(9, 9) + obs_var;
|
||||
test_ratio = sq(innov) / (sq(innov_gate) * innov_var);
|
||||
_vert_pos_innov_ratio = innov / sqrtf(innov_var);
|
||||
_vert_pos_fuse_attempt_time_us = _imu_sample_delayed.time_us;
|
||||
bool innov_check_pass = test_ratio <= 1.0f;
|
||||
|
||||
// if there is bad vertical acceleration data, then don't reject measurement,
|
||||
// but limit innovation to prevent spikes that could destabilise the filter
|
||||
float innovation;
|
||||
|
||||
if (_fault_status.flags.bad_acc_vertical && !innov_check_pass) {
|
||||
const float innov_limit = innov_gate * sqrtf(innov_var);
|
||||
innovation = math::constrain(innov, -innov_limit, innov_limit);
|
||||
innov_check_pass = true;
|
||||
|
||||
} else {
|
||||
innovation = innov;
|
||||
}
|
||||
|
||||
if (innov_check_pass) {
|
||||
_innov_check_fail_status.flags.reject_ver_pos = false;
|
||||
|
||||
return fuseVelPosHeight(innovation, innov_var, 5);
|
||||
|
||||
} else {
|
||||
_innov_check_fail_status.flags.reject_ver_pos = true;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
resetEstimatorAidStatus(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);
|
||||
aid_src.observation[i] = obs(i);
|
||||
aid_src.innovation[i] = _state.vel(i) - aid_src.observation[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);
|
||||
aid_src.observation_variance[i] = obs_var(i);
|
||||
aid_src.innovation_variance[i] = P(4 + i, 4 + i) + aid_src.observation_variance[i];
|
||||
}
|
||||
|
||||
setEstimatorAidStatusTestRatio(vel_aid_src, innov_gate);
|
||||
setEstimatorAidStatusTestRatio(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;
|
||||
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
|
||||
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance[2]);
|
||||
aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
|
||||
aid_src.innovation_rejected = false;
|
||||
}
|
||||
|
||||
vel_aid_src.timestamp_sample = sample_time_us;
|
||||
aid_src.timestamp_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
|
||||
void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var,
|
||||
const float innov_gate, estimator_aid_source_1d_s &aid_src) const
|
||||
{
|
||||
resetEstimatorAidStatus(pos_aid_src);
|
||||
resetEstimatorAidStatus(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);
|
||||
aid_src.observation = obs;
|
||||
aid_src.innovation = _state.pos(2) - aid_src.observation;
|
||||
|
||||
pos_aid_src.innovation[i] = _state.pos(i) - position(i);
|
||||
pos_aid_src.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
|
||||
}
|
||||
aid_src.observation_variance = obs_var;
|
||||
aid_src.innovation_variance = P(9, 9) + aid_src.observation_variance;
|
||||
|
||||
setEstimatorAidStatusTestRatio(pos_aid_src, innov_gate);
|
||||
setEstimatorAidStatusTestRatio(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;
|
||||
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
|
||||
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
|
||||
aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
|
||||
aid_src.innovation_rejected = false;
|
||||
}
|
||||
|
||||
pos_aid_src.timestamp_sample = sample_time_us;
|
||||
aid_src.timestamp_sample = time_us;
|
||||
}
|
||||
|
||||
void Ekf::fuseVelocity(estimator_aid_source_3d_s& vel_aid_src)
|
||||
void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
|
||||
const float innov_gate, estimator_aid_source_2d_s &aid_src) const
|
||||
{
|
||||
// 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] = _imu_sample_delayed.time_us;
|
||||
}
|
||||
}
|
||||
resetEstimatorAidStatus(aid_src);
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
aid_src.observation[i] = obs(i);
|
||||
aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i];
|
||||
|
||||
aid_src.observation_variance[i] = obs_var(i);
|
||||
aid_src.innovation_variance[i] = P(7 + i, 7 + i) + aid_src.observation_variance[i];
|
||||
}
|
||||
|
||||
// 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] = _imu_sample_delayed.time_us;
|
||||
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
|
||||
|
||||
aid_src.timestamp_sample = time_us;
|
||||
}
|
||||
|
||||
void Ekf::fuseVelocity(estimator_aid_source_2d_s &aid_src)
|
||||
{
|
||||
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
|
||||
// vx, vy
|
||||
if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], 0)
|
||||
&& fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 1)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
|
||||
} else {
|
||||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fusePosition(estimator_aid_source_3d_s& pos_aid_src)
|
||||
void Ekf::fuseVelocity(estimator_aid_source_3d_s &aid_src)
|
||||
{
|
||||
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
|
||||
// vx, vy, vz
|
||||
if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], 0)
|
||||
&& fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 1)
|
||||
&& fuseVelPosHeight(aid_src.innovation[2], aid_src.innovation_variance[2], 2)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
|
||||
} else {
|
||||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseHorizontalPosition(estimator_aid_source_2d_s &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] = _imu_sample_delayed.time_us;
|
||||
}
|
||||
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
|
||||
if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], 3)
|
||||
&& fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 4)
|
||||
) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
|
||||
} else {
|
||||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseVerticalPosition(estimator_aid_source_1d_s &aid_src)
|
||||
{
|
||||
// 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] = _imu_sample_delayed.time_us;
|
||||
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
|
||||
if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, 5)) {
|
||||
aid_src.fused = true;
|
||||
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user