Skip to content

Fix issues in Kalman filter

Igor Bogoslavskyi requested to merge 505-fix-kf-issues into master

Description

In our code we had a line:

jac_mat = cov_mat * jac_mat;

It should have been:

jac_mat = jac_mat * cov_mat;

This actually fixes the test in the kalman filter package that worked just by luck (or by fitting the parameters to satisfy the test).

I have also verified that now the kalman filter implementation provides exactly the same results as all of these (independently implemented in python for the sake of debugging):

  • vanilla KF implementation
  • KF implementation that uses scalar updates
  • square-root KF implementation

Potentially related to #505 (closed)

Notes for Reviewer

I have also changed the matrix m_GQ_factor that was stored as a pointer to the original matrix to be a concrete matrix type. This mitigates the situation when a matrix GQ passed into the filter has been creates as auto result of Eigen operation. In this case it would not be of matrix type and a const reference to a temporary matrix would be transmitted to the filter, which we would store in the pointer m_GQ_factor. I can try to invest some time into finding a way to restrict using our constructors, but the easy fix was to just store an additional copy of the input GQ matrix. I am open to any suggestions on how to better handle this.

Checklist

  • Documentation was updated
Edited by Igor Bogoslavskyi

Merge request reports