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

ekf2: add and share centralized method to clear inhibited state Kalman gains

This commit is contained in:
Daniel Agar 2023-02-24 14:37:05 -05:00
parent 7098970a38
commit d45aeae1de
3 changed files with 37 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -769,20 +769,47 @@ private:
// Return the magnetic declination in radians to be used by the alignment and fusion processing
float getMagDeclination();
bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
void clearInhibitedStateKalmanGains(Vector24f &K) const
{
for (unsigned i = 0; i < 3; i++) {
// gyro bias: states 10, 11, 12
for (unsigned i = 0; i < 3; i++) {
if (_gyro_bias_inhibit[i]) {
K(10 + i) = 0.0f;
K(10 + i) = 0.f;
}
}
// accel bias: states 13, 14, 15
for (unsigned i = 0; i < 3; i++) {
if (_accel_bias_inhibit[i]) {
K(13 + i) = 0.0f;
K(13 + i) = 0.f;
}
}
// mag I: states 16, 17, 18
if (!_control_status.flags.mag_3D) {
K(16) = 0.f;
K(17) = 0.f;
K(18) = 0.f;
}
// mag B: states 19, 20, 21
if (!_control_status.flags.mag_3D) {
K(19) = 0.f;
K(20) = 0.f;
K(21) = 0.f;
}
// wind: states 22, 23
if (!_control_status.flags.wind) {
K(22) = 0.f;
K(23) = 0.f;
}
}
bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
{
clearInhibitedStateKalmanGains(K);
const Vector24f KS = K * innovation_variance;
SquareMatrix24f KHP;

12
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -258,7 +258,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
// only calculate gains for states we are using
Vector24f Kfusion;
for (uint8_t row = 0; row <= 15; row++) {
for (uint8_t row = 0; row < _k_num_states; row++) {
for (uint8_t col = 0; col <= 3; col++) {
Kfusion(row) += P(row, col) * H_YAW(col);
}
@ -266,16 +266,6 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
Kfusion(row) *= heading_innov_var_inv;
}
if (_control_status.flags.wind) {
for (uint8_t row = 22; row <= 23; row++) {
for (uint8_t col = 0; col <= 3; col++) {
Kfusion(row) += P(row, col) * H_YAW(col);
}
Kfusion(row) *= heading_innov_var_inv;
}
}
// define the innovation gate size
float gate_sigma = math::max(_params.heading_innov_gate, 1.f);

12
src/modules/ekf2/EKF/vel_pos_fusion.cpp
View File

@ -200,17 +200,7 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
Kfusion(row) = P(row, state_index) / innov_var;
}
for (unsigned i = 0; i < 3; i++) {
// gyro bias: states 10, 11, 12
if (_gyro_bias_inhibit[i]) {
Kfusion(10 + i) = 0.0f;
}
// accel bias: states 13, 14, 15
if (_accel_bias_inhibit[i]) {
Kfusion(13 + i) = 0.0f;
}
}
clearInhibitedStateKalmanGains(Kfusion);
SquareMatrix24f KHP;