Added code for actually adding the new IDPFeaure to the state vector in EKF.java.
Also added method stub for updating the covariance when a new feature gets added.