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

ekf2: ZeroGyroUpdate move to fuseDirectStateMeasurement

Browse Source
This commit is contained in:
Daniel Agar
2024-02-26 12:32:59 -05:00
parent 08a2a6c836
commit 8001132d33
1 changed files with 4 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp
+4 -16
View File
@@ -60,25 +60,13 @@ bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
if (zero_gyro_update_data_ready) { if (zero_gyro_update_data_ready) {
Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt; Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt;
Vector3f innovation = ekf.state().gyro_bias - gyro_bias;
const float obs_var = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f)); const float obs_var = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f));
const Vector3f innov_var = ekf.getGyroBiasVariance() + obs_var; for (unsigned i = 0; i < 3; i++) {
const float innovation = ekf.state().gyro_bias(i) - gyro_bias(i);
for (int i = 0; i < 3; i++) { const float innov_var = ekf.getGyroBiasVariance()(i) + obs_var;
Ekf::VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. ekf.fuseDirectStateMeasurement(innovation, innov_var, obs_var, State::gyro_bias.idx + i);
const unsigned state_index = State::gyro_bias.idx + i;
// calculate kalman gain K = PHS, where S = 1/innovation variance
for (int row = 0; row < State::size; row++) {
K(row) = ekf.stateCovariance(row, state_index) / innov_var(i);
}
Ekf::VectorState H{};
H(State::gyro_bias.idx + i) = 1.f;
ekf.measurementUpdate(K, H, obs_var, innovation(i), innov_var(i));
} }
// Reset the integrators // Reset the integrators