Fixed how the covariance matrix was updated in
EKF.predictFromINSUpdate(). Followed the calculation in EKF SLAM for
dummies to update top 3 rows, then use their transpositions to update
first three colums.