Changed the extract methods in EKF to re-use the extractSubMatrix()
method instead of having their own logic. Also updated affected code.