Lindsay Kleeman presented a solution to a numerical rounding problem found in the Extended Kalman Filter. In this post, I summarize the paper by showing where the error is and how it can be solved.

Philippe Giguere’s Website informed me about Kleeman’s presentation a few months ago. On slide number 28, it is explained that the problem comes from the equation for the covariance update:

‘’‘ matlab
Sigma_t = (I - K_t H_t) Sigma^{p}_t
’‘’
Where Sigma_t is the updated covariance of the state (mu_t), I is the identity matrix, K_t is the Kalman Gain matrix, H is the measurement matrix and Sigma^{p}_t is the predicted covariance at time t. Indeed, the computation of Sigma_t involves a substraction, which can result in loss of symmetry and positive definiteness due to rounding errors during this substraction.

Simply put, this means that if you use an EKF filter long enough, it may suddenly crash because your pc is making some number roundings which leads to matrix inconsistency.

According to Kleeman, we could instead use the covariance update equation in Joseph’s form. It involves an additional computation burden, but saves us from crashing the filter unexpectedly. Using the notation from Probabilistic Robotics (Thrun, Burgard and Fox), Joseph’s form looks like this:
‘’‘ matlab
Sigma_t = (I - K_t H_t) Sigma^{p}_t (I - K_t H_t)’ + K_t Q_t K_t'
‘’‘
Where Q_t is the measurement noise covariance.

Enjoy!