Started working on the full state kinematic estimator.