Added the linear algebra required to perform square-root EKF