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

ekf2: ZeroGyroUpdate move to fuseDirectStateMeasurement

This commit is contained in:
Daniel Agar 2024-02-23 11:40:12 -05:00
parent 08a2a6c836
commit 8001132d33
1 changed files with 4 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp
View File

@ -60,25 +60,13 @@ bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
if (zero_gyro_update_data_ready) {
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 Vector3f innov_var = ekf.getGyroBiasVariance() + obs_var;
for (int i = 0; i < 3; i++) {
Ekf::VectorState K; // Kalman gain vector for any single observation - sequential fusion is used.
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));
for (unsigned i = 0; i < 3; i++) {
const float innovation = ekf.state().gyro_bias(i) - gyro_bias(i);
const float innov_var = ekf.getGyroBiasVariance()(i) + obs_var;
ekf.fuseDirectStateMeasurement(innovation, innov_var, obs_var, State::gyro_bias.idx + i);
}
// Reset the integrators