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); 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()) { 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; 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 // 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; 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 // horizontal and vertical position fusion
@ -836,7 +836,7 @@ private:
void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src); void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src);
// 2d & 3d velocity fusion // 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); void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
#if defined(CONFIG_EKF2_TERRAIN) #if defined(CONFIG_EKF2_TERRAIN)

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

@ -33,6 +33,25 @@
#include "ekf.h" #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, 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 const float innov_gate, estimator_aid_source1d_s &aid_src) const
{ {
@ -57,52 +76,35 @@ void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const floa
aid_src.timestamp_sample = time_us; 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) void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
{ {
// x & y // x & y
if (!aid_src.innovation_rejected) { if (!aid_src.innovation_rejected
if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx) && 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) && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
) { ) {
aid_src.fused = true; aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us; aid_src.time_last_fuse = _time_delayed_us;
_time_last_hor_pos_fuse = _time_delayed_us; _time_last_hor_pos_fuse = _time_delayed_us;
} else { } else {
aid_src.fused = false; aid_src.fused = false;
}
} }
} }
void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
{ {
// z // z
if (!aid_src.innovation_rejected) { if (!aid_src.innovation_rejected
if (fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) { && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)
aid_src.fused = true; ) {
aid_src.time_last_fuse = _time_delayed_us; aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
_time_last_hgt_fuse = _time_delayed_us; _time_last_hgt_fuse = _time_delayed_us;
}
} else {
aid_src.fused = false;
} }
} }

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

@ -33,8 +33,8 @@
#include "ekf.h" #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 const float innov_gate, estimator_aid_source2d_s &aid_src) const
{ {
resetEstimatorAidStatus(aid_src); resetEstimatorAidStatus(aid_src);
@ -79,40 +79,38 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &ob
aid_src.timestamp_sample = time_us; 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
// vx, vy if (!aid_src.innovation_rejected
if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx) && 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[1], aid_src.innovation_variance[1], State::vel.idx + 1)
) { ) {
aid_src.fused = true; aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us; aid_src.time_last_fuse = _time_delayed_us;
_time_last_hor_vel_fuse = _time_delayed_us; _time_last_hor_vel_fuse = _time_delayed_us;
} else { } else {
aid_src.fused = false; aid_src.fused = false;
}
} }
} }
void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
{ {
if (!aid_src.innovation_rejected) { // vx, vy, vz
// vx, vy, vz if (!aid_src.innovation_rejected
if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0) && 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[1], aid_src.innovation_variance[1], State::vel.idx + 1)
&& fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2) && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2)
) { ) {
aid_src.fused = true; aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us; aid_src.time_last_fuse = _time_delayed_us;
_time_last_hor_vel_fuse = _time_delayed_us; _time_last_hor_vel_fuse = _time_delayed_us;
_time_last_ver_vel_fuse = _time_delayed_us; _time_last_ver_vel_fuse = _time_delayed_us;
} else { } else {
aid_src.fused = false; aid_src.fused = false;
}
} }
} }