Fix issues in Kalman filter
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