Added test method in EKFTests just to see if addFeature() doesn't throw exceptions. Also fixed intialization of Matrix dimension in DerivativeHelper.dRq_times_a_by_dq.