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

ekf2: zvup sequential fusion

This commit is contained in:
bresch 2024-01-29 15:52:51 +01:00 committed by Daniel Agar
parent e8b3778f81
commit 80f20e619c
1 changed files with 2 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp
View File

@ -58,16 +58,15 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye
if (continuing_conditions_passing) {
Vector3f vel_obs{0.f, 0.f, 0.f};
Vector3f innovation = ekf.state().vel - vel_obs;
// Set a low variance initially for faster leveling and higher
// later to let the states follow the measurements
const float obs_var = ekf.control_status_flags().tilt_align ? sq(0.2f) : sq(0.001f);
Vector3f innov_var = ekf.getVelocityVariance() + obs_var;
for (unsigned i = 0; i < 3; i++) {
ekf.fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i);
const float innovation = ekf.state().vel(i) - vel_obs(i);
ekf.fuseVelPosHeight(innovation, innov_var(i), State::vel.idx + i);
}
_time_last_zero_velocity_fuse = imu_delayed.time_us;