forked from Archive/PX4-Autopilot
ekf2: use Joseph stabilized update in direct state observations
This commit is contained in:
parent
9d9766c6cf
commit
d501d8e1d4
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue