Factoize innovation covariance so that we don't have to calculate inverses
Add factorization to Extended Kalman filters