forked from Archive/PX4-Autopilot
ekf2: add and share centralized method to clear inhibited state Kalman gains
This commit is contained in:
parent
7098970a38
commit
d45aeae1de
|
@ -769,20 +769,47 @@ private:
|
||||||
// Return the magnetic declination in radians to be used by the alignment and fusion processing
|
// Return the magnetic declination in radians to be used by the alignment and fusion processing
|
||||||
float getMagDeclination();
|
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
|
// gyro bias: states 10, 11, 12
|
||||||
|
for (unsigned i = 0; i < 3; i++) {
|
||||||
if (_gyro_bias_inhibit[i]) {
|
if (_gyro_bias_inhibit[i]) {
|
||||||
K(10 + i) = 0.0f;
|
K(10 + i) = 0.f;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// accel bias: states 13, 14, 15
|
// accel bias: states 13, 14, 15
|
||||||
|
for (unsigned i = 0; i < 3; i++) {
|
||||||
if (_accel_bias_inhibit[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;
|
const Vector24f KS = K * innovation_variance;
|
||||||
SquareMatrix24f KHP;
|
SquareMatrix24f KHP;
|
||||||
|
|
||||||
|
|
|
@ -258,7 +258,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
|
||||||
// only calculate gains for states we are using
|
// only calculate gains for states we are using
|
||||||
Vector24f Kfusion;
|
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++) {
|
for (uint8_t col = 0; col <= 3; col++) {
|
||||||
Kfusion(row) += P(row, col) * H_YAW(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;
|
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
|
// define the innovation gate size
|
||||||
float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
|
float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
|
||||||
|
|
||||||
|
|
|
@ -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;
|
Kfusion(row) = P(row, state_index) / innov_var;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (unsigned i = 0; i < 3; i++) {
|
clearInhibitedStateKalmanGains(Kfusion);
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
SquareMatrix24f KHP;
|
SquareMatrix24f KHP;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue