Fixed the diagonal when initializing the covariance matrix in EKF.
(Previously, it was just 0. I now arbitrarily set it to 0.1)
(1) Fixed the updating of vrv matrix in addFeature. It should be created

based on the observed distance to the new feature. This step was

previously just skipped.

(2) In EKF.getP(), made the method return a clone of P so that P's

integrity will not get compromised if accessed by an outside object.

(3) Re-organized the functions. Moved them around.

(4) Also changed the create methods to return Matrix objects instead of

double[][] to be consistent.

(5) Added computeDistance() function in PointDouble.