Publish the x, y, theta state of the drivetrain

This gives us a start of an on-field position to follow.

Change-Id: Ibdd3f06e1856efd451ceb504801ed32605b5e560
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 3f6c32f..9396aed 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -38,6 +38,12 @@
 
   void Zero(::frc971::control_loops::DrivetrainQueue::Output *output) override;
 
+  // Computes the xy state change given the change in the lr state.
+  ::Eigen::Matrix<double, 3, 1> PredictState(
+      const ::Eigen::Matrix<double, 3, 1> &xytheta_state,
+      const ::Eigen::Matrix<double, 7, 1> &state,
+      const ::Eigen::Matrix<double, 7, 1> &previous_state) const;
+
   double last_gyro_rate_ = 0.0;
 
   const DrivetrainConfig<double> dt_config_;
@@ -66,6 +72,16 @@
   bool has_been_enabled_ = false;
 
   double last_accel_ = 0.0;
+
+  // Current xytheta state of the robot.  This is essentially the kalman filter
+  // integrated up in a direction.
+  // [x, y, theta, vl, vr, left_error, right_error]
+  ::Eigen::Matrix<double, 7, 1> xytheta_state_ =
+      ::Eigen::Matrix<double, 7, 1>::Zero();
+
+  // Last kalman filter state.
+  ::Eigen::Matrix<double, 7, 1> last_state_ =
+      ::Eigen::Matrix<double, 7, 1>::Zero();
 };
 
 }  // namespace drivetrain