Changing initial estimate error covariance to be much smaller, and fixing some debug output
Updating EKF interface test
Updating EKF interface test
Merge pull request #28 from ayrton04/master

Small change for filter init and updating EKF interface test.