From fa56ea3e2cb9880edc423857757a658139c096ae Mon Sep 17 00:00:00 2001 From: liqiang Date: Mon, 31 Oct 2022 20:48:49 +0800 Subject: [PATCH] fix a bug in EKFUpate --- src/kf-gins/gi_engine.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/kf-gins/gi_engine.cpp b/src/kf-gins/gi_engine.cpp index 2c062db..64fadee 100644 --- a/src/kf-gins/gi_engine.cpp +++ b/src/kf-gins/gi_engine.cpp @@ -385,8 +385,11 @@ void GIEngine::EKFUpdate(Eigen::MatrixXd &dz, Eigen::MatrixXd &H, Eigen::MatrixX Eigen::MatrixXd I; I.resizeLike(Cov_); I.setIdentity(); - I = I - K * H; - dx_ = dx_ + K * dz; + I = I - K * H; + // 如果每次更新后都进行状态反馈，则更新前dx_一直为0，下式可以简化为：dx_ = K * dz; + // if state feedback is performed after every update, dx_ is always zero before the update + // the following formula can be simplified as : dx_ = K * dz; + dx_ = dx_ + K * (dz - H * dx_); Cov_ = I * Cov_ * I.transpose() + K * R * K.transpose(); }