dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: velocity/position fusion helper minor consistency cleanup

This commit is contained in:
Daniel Agar 2024-02-14 21:01:32 -05:00
parent c338891677
commit 22b957696d
4 changed files with 66 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF/auxvel_fusion.cpp
View File

@ -42,10 +42,10 @@ void Ekf::controlAuxVelFusion()
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);
updateHorizontalVelocityAidSrcStatus(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()) {
fuseVelocity(_aid_src_aux_vel);
fuseHorizontalVelocity(_aid_src_aux_vel);
}
}
}

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

@ -828,7 +828,7 @@ private:
void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_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_source2d_s &aid_src) const;
void updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const;
// horizontal and vertical position fusion
@ -836,7 +836,7 @@ private:
void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src);
// 2d & 3d velocity fusion
void fuseVelocity(estimator_aid_source2d_s &vel_aid_src);
void fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src);
void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
#if defined(CONFIG_EKF2_TERRAIN)

52
src/modules/ekf2/EKF/position_fusion.cpp
View File

@ -33,6 +33,25 @@
#include "ekf.h"
void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
const float innov_gate, estimator_aid_source2d_s &aid_src) const
{
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] = math::max(sq(0.01f), obs_var(i));
const int state_index = State::pos.idx + i;
aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
}
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
aid_src.timestamp_sample = time_us;
}
void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var,
const float innov_gate, estimator_aid_source1d_s &aid_src) const
{
@ -57,30 +76,11 @@ void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const floa
aid_src.timestamp_sample = time_us;
}
void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
const float innov_gate, estimator_aid_source2d_s &aid_src) const
{
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] = math::max(sq(0.01f), obs_var(i));
const int state_index = State::pos.idx + i;
aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
}
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
aid_src.timestamp_sample = time_us;
}
void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
{
// x & y
if (!aid_src.innovation_rejected) {
if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx)
if (!aid_src.innovation_rejected
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
) {
aid_src.fused = true;
@ -92,17 +92,19 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
aid_src.fused = false;
}
}
}
void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
{
// z
if (!aid_src.innovation_rejected) {
if (fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) {
if (!aid_src.innovation_rejected
&& fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
_time_last_hgt_fuse = _time_delayed_us;
}
} else {
aid_src.fused = false;
}
}

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

@ -33,7 +33,7 @@
#include "ekf.h"
void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
void Ekf::updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
const float innov_gate, estimator_aid_source2d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
@ -79,11 +79,11 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &ob
aid_src.timestamp_sample = time_us;
}
void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
{
if (!aid_src.innovation_rejected) {
// vx, vy
if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
if (!aid_src.innovation_rejected
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
) {
aid_src.fused = true;
@ -95,13 +95,12 @@ void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
aid_src.fused = false;
}
}
}
void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
{
if (!aid_src.innovation_rejected) {
// vx, vy, vz
if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0)
if (!aid_src.innovation_rejected
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
&& fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2)
) {
@ -115,4 +114,3 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
aid_src.fused = false;
}
}
}