Changing initial estimate error covariance to be much smaller, and fixing some debug output