covariance update equation

[sunbingfeng]

The right equation to update covariance matrix may be the one attached as follows:

But in your code , you used a simplified one, and commented out the right one.

void MsckfVio::measurementUpdate(
    const MatrixXd& H, const VectorXd& r) {
...
  // Update state covariance.
  MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin;
  //state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() +
  //  K*K.transpose()*Feature::observation_noise;
  state_server.state_cov = I_KH*state_server.state_cov;
...

I am thinking if you have tried to use the right one, but it did not work as expected?

Thanks!

[ke-sun]

Please correct me if I am wrong. I though the two equations are equivalent if K is the optimal Kalman gain. The advantage of the uncommented code is less operation.

[sunbingfeng]

I found a trustworthy explanation in Paper(Quaternion kinematics for the error-state Kalman filter, P63).