Added coordinate system transformations and initial Kalman state