*Fixed equations in EKF.updateFromReobservedFeatures()

(1) Fixed the computation for the lower left matrix. The formula was
lacking a final transpose operation after every operation was done.
(2) Moved the code for addition of columns based on the transposition of
the last rows to after the last rows have been initialized. Also looped
only until toAdd.size()-1 because the last entry (lower right matrix)
shouldn't be transposed anymore.
Fixed bug in EKF.addFeature():
moved numFeatures++ to the end because this caused errors
(indexOutOfBoundsException) in the addition logic
Added unit test for adding a new feature without any other EKF
update/operation happening before it.
Added constants to EKF. These constants were the ones mentioned in EKF
SLAM for dummies, which would probably be set by trial and error.
Completed the EKFTests.testAddOneFeature() method by checking for the
values in P. Previously, I was only checking for the correct row and
column size of P.
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.
EKF.updateFromReobservedFeature(): Fixed calculation of observedHeading
by subtracting the device's heading so it becomes relative.
Possible bug fix to formula for double integration. Unsure.
Reverted to Duke's original version.
In EKF.updateFromReobservedFeatures(): changed conversion of P to
double[][] to use extractSubMatrix() instead of calling toArray then
casting.
EKF: made a createH() method. 
DevicePose: made a toString() method.
EKFTests: made a new test that just checks if reobserving one feature
will cause exceptions.
Merged from vins-integration branch
Fixed EKF.deleteFeature(). Forgot to execute numFeatures--
Fixed implementation of IntegrateMotionEstimation:
(1) Divided by 2000000 instead of just 2000.
(2) Created rotation matrix so that acc values are with respect to the
world.
Fixed orientation. There was a synchronization problem when recording
the SensorEntry (nextSensorEntryToAdd)
Fixed bug in IntegrateMotionEstimation.estimateDisplacementAndHeading()
where dividing by 0 was possible, causing NaNs to appear.
EKF.getCurrDevicePose(): just returns degrees int the device pose.
Added listener to Driver to check if OpenCV is done loading
Decreased FeatureUpdate setter visibility to package
Removed FeatureService because nobody wants that
Implemented conversion of Feature Locations to Metric Units
Made C1 Camera follow the World Coordinate System
Implemented rough best fit scalar value for conversion
Noted areas where issues arise (EKF and DriverActivity)