Actually wrote code :O initialization of EKF slam