fuxiaoer/KF-GINS
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix a bug in EKFUpate

Browse Source
This commit is contained in:
liqiang 2022-10-31 20:48:49 +08:00
parent 7b92124871
commit fa56ea3e2c
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/kf-gins/gi_engine.cpp
View File

@ -386,7 +386,10 @@ void GIEngine::EKFUpdate(Eigen::MatrixXd &dz, Eigen::MatrixXd &H, Eigen::MatrixX
I.resizeLike(Cov_); I.resizeLike(Cov_);
I.setIdentity(); I.setIdentity();
I = I - K * H; I = I - K * H;
dx_ = dx_ + K * dz; // 如果每次更新后都进行状态反馈，则更新前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(); Cov_ = I * Cov_ * I.transpose() + K * R * K.transpose();
} }