Started implementation of unscented kalman filter for state estimation