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

ekf2: use Joseph stabilized update in direct state observations

This commit is contained in:
bresch 2024-02-23 16:24:54 +01:00 committed by Daniel Agar
parent 9d9766c6cf
commit d501d8e1d4
5 changed files with 36 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -66,7 +66,7 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye
for (unsigned i = 0; i < 3; i++) {
const float innovation = ekf.state().vel(i) - vel_obs(i);
ekf.fuseDirectStateMeasurement(innovation, innov_var(i), State::vel.idx + i);
ekf.fuseDirectStateMeasurement(innovation, innov_var(i), obs_var, State::vel.idx + i);
}
_time_last_zero_velocity_fuse = imu_delayed.time_us;

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

@ -328,7 +328,7 @@ public:
}
// fuse single direct state measurement (eg NED velocity, NED position, mag earth field, etc)
bool fuseDirectStateMeasurement(const float innov, const float innov_var, const int state_index);
bool fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index);
// gyro bias
const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s

46
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -839,38 +839,44 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
}
}
bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const int state_index)
bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index)
{
VectorState Kfusion; // Kalman gain vector for any single observation - sequential fusion is used.
VectorState K; // Kalman gain vector for any single observation - sequential fusion is used.
// calculate kalman gain K = PHS, where S = 1/innovation variance
for (int row = 0; row < State::size; row++) {
Kfusion(row) = P(row, state_index) / innov_var;
K(row) = P(row, state_index) / innov_var;
}
clearInhibitedStateKalmanGains(Kfusion);
clearInhibitedStateKalmanGains(K);
SquareMatrixState KHP;
// Efficient implementation of the Joseph stabilized covariance update
// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"
for (unsigned row = 0; row < State::size; row++) {
for (unsigned column = 0; column < State::size; column++) {
KHP(row, column) = Kfusion(row) * P(state_index, column);
// Step 1: conventional update
VectorState PH = P.row(state_index);
for (unsigned i = 0; i < State::size; i++) {
for (unsigned j = 0; j <= i; j++) {
P(i, j) = P(i, j) - K(i) * PH(j);
P(j, i) = P(i, j);
}
}
const bool healthy = checkAndFixCovarianceUpdate(KHP);
// Step 2: stabilized update
PH = P.row(state_index);
if (healthy) {
// apply the covariance corrections
P -= KHP;
constrainStateVariances();
// apply the state corrections
fuse(Kfusion, innov);
return true;
for (unsigned i = 0; i < State::size; i++) {
for (unsigned j = 0; j <= i; j++) {
float s = .5f * (P(i, j) - PH(i) * K(j) + P(i, j) - PH(j) * K(i));
P(i, j) = s + K(i) * R * K(j);
P(j, i) = P(i, j);
}
}
return false;
constrainStateVariances();
// apply the state corrections
fuse(K, innov);
return true;
}

6
src/modules/ekf2/EKF/position_fusion.cpp
View File

@ -80,8 +80,8 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
{
// x & y
if (!aid_src.innovation_rejected
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::pos.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::pos.idx + 1)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
@ -97,7 +97,7 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
{
// z
if (!aid_src.innovation_rejected
&& fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)
&& fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance, State::pos.idx + 2)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;

10
src/modules/ekf2/EKF/velocity_fusion.cpp
View File

@ -83,8 +83,8 @@ void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
{
// vx, vy
if (!aid_src.innovation_rejected
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
@ -100,9 +100,9 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
{
// vx, vy, vz
if (!aid_src.innovation_rejected
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
&& fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2)
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1)
&& fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], aid_src.observation_variance[2], State::vel.idx + 2)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;