Added upper triangular matrix multiplication and finished the EKF code