commit 3c5b4d312fd8ca63996bab194aa58161f6ff5831
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Tue Feb 11 17:22:14 2020 -0800
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Feb 22 14:09:57 2020 -0800
tree f43b9c14e7f70dcde0f28312ebcbe278d5757b61
parent 2215d924fab63d7d022c736875f4e3e4c766c922

Incorporate IMU measurements into EKF

The changes in this commit forced me to up the tolerances more than I
would've liked on some of the 2019 simulation tests, partially because
the artificial disturbances in the 2019 tests don't introduce valid
accelerometer readings.

Change-Id: I66758e36d76b127ddeedfbcfadb2d77acf1f2997

frc971/control_loops/drivetrain/localizer.h

diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 2589d4b..91fd16a 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -3,6 +3,7 @@
 
 #include "aos/events/event_loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/drivetrain/hybrid_ekf.h"
 #include "frc971/control_loops/pose.h"
 
@@ -48,13 +49,30 @@
   virtual void Update(const ::Eigen::Matrix<double, 2, 1> &U,
                       ::aos::monotonic_clock::time_point now,
                       double left_encoder, double right_encoder,
-                      double gyro_rate, double longitudinal_accelerometer) = 0;
+                      double gyro_rate, const Eigen::Vector3d &accel) = 0;
   // Reset the absolute position of the estimator.
   virtual void ResetPosition(::aos::monotonic_clock::time_point t, double x,
                              double y, double theta, double theta_uncertainty,
                              bool reset_theta) = 0;
+  flatbuffers::Offset<LocalizerState> PopulateStatus(
+      flatbuffers::FlatBufferBuilder *fbb) {
+    LocalizerState::Builder builder(*fbb);
+    builder.add_x(x());
+    builder.add_y(y());
+    builder.add_theta(theta());
+    builder.add_left_velocity(left_velocity());
+    builder.add_right_velocity(right_velocity());
+    builder.add_left_encoder(left_encoder());
+    builder.add_right_encoder(right_encoder());
+    builder.add_left_voltage_error(left_voltage_error());
+    builder.add_right_voltage_error(right_voltage_error());
+    builder.add_angular_error(angular_error());
+    builder.add_longitudinal_velocity_offset(longitudinal_velocity_offset());
+    builder.add_lateral_velocity(lateral_velocity());
+    return builder.Finish();
+  }
   // There are several subtly different norms floating around for state
-  // matrices. In order to avoid that mess, we jus tprovide direct accessors for
+  // matrices. In order to avoid that mess, we just provide direct accessors for
   // the values that most people care about.
   virtual double x() const = 0;
   virtual double y() const = 0;
@@ -65,6 +83,9 @@
   virtual double right_encoder() const = 0;
   virtual double left_voltage_error() const = 0;
   virtual double right_voltage_error() const = 0;
+  virtual double angular_error() const = 0;
+  virtual double longitudinal_velocity_offset() const = 0;
+  virtual double lateral_velocity() const = 0;
   virtual TargetSelectorInterface *target_selector() = 0;
 };
 
@@ -110,8 +131,9 @@
   void Update(const ::Eigen::Matrix<double, 2, 1> &U,
               ::aos::monotonic_clock::time_point now, double left_encoder,
               double right_encoder, double gyro_rate,
-              double /*longitudinal_accelerometer*/) override {
-    ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, now);
+              const Eigen::Vector3d &accel) override {
+    ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, accel,
+                               now);
   }
 
   void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
@@ -119,9 +141,10 @@
                      bool /*reset_theta*/) override {
     const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
     const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
-    ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,
-                               right_encoder, 0, 0, 0,
-                               0).finished(),
+    ekf_.ResetInitialState(t,
+                           (Ekf::State() << x, y, theta, left_encoder, 0,
+                            right_encoder, 0, 0, 0, 0, 0, 0)
+                               .finished(),
                            ekf_.P());
   };
 
@@ -146,6 +169,15 @@
   double right_voltage_error() const override {
     return ekf_.X_hat(StateIdx::kRightVoltageError);
   }
+  double angular_error() const override {
+    return ekf_.X_hat(StateIdx::kAngularError);
+  }
+  double longitudinal_velocity_offset() const override {
+    return ekf_.X_hat(StateIdx::kLongitudinalVelocityOffset);
+  }
+  double lateral_velocity() const override {
+    return ekf_.X_hat(StateIdx::kLateralVelocity);
+  }
 
   TrivialTargetSelector *target_selector() override {
     return &target_selector_;