commit 8cdc9f66098ef31ceb8f1ef46bc764a7a658ab59
author James Kuszmaul <jabukuszmaul@gmail.com> Sun Mar 12 14:01:14 2023 -0700
committer James Kuszmaul <jabukuszmaul@gmail.com> Sun Mar 12 14:03:19 2023 -0700
tree eb2bbdd64a2e6590a4f97fbf87bbbb26ed8eb7e2
parent a8e0d6effb48e3fa8b2fc8f50d1e4952717f8f08

Crank down gains on roborio puppet localizer

It turns out that the roborio had some much slower-than-expected time
constants on accepting corrections from the localizer pi. It should be
treating the localizer pi as ~ground truth.

Change-Id: Icfbeb8a3759488599a91d4d7d17e77e796373f07
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>

frc971/control_loops/drivetrain/drivetrain_plotter.ts

diff --git a/frc971/control_loops/drivetrain/drivetrain_plotter.ts b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
index 5610e97..a609532 100644
--- a/frc971/control_loops/drivetrain/drivetrain_plotter.ts
+++ b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
@@ -18,6 +18,8 @@
       "frc971.control_loops.drivetrain.Position");
   const status = aosPlotter.addMessageSource(
       '/drivetrain', 'frc971.control_loops.drivetrain.Status');
+  const localizerOuput = aosPlotter.addMessageSource(
+      '/localizer', 'frc971.controls.LocalizerOutput');
   const output = aosPlotter.addMessageSource(
       '/drivetrain', 'frc971.control_loops.drivetrain.Output');
   const gyroReading = aosPlotter.addMessageSource(
@@ -279,11 +281,10 @@
   xPositionPlot.plot.getAxisLabels().setXLabel(TIME);
   xPositionPlot.plot.getAxisLabels().setYLabel('X Position (m)');
 
-  const localizerX = xPositionPlot.addMessageLine(status, ['x']);
-  localizerX.setColor(RED);
-  const splineX =
-      xPositionPlot.addMessageLine(status, ['trajectory_logging', 'x']);
-  splineX.setColor(GREEN);
+  xPositionPlot.addMessageLine(status, ['x']).setColor(RED);
+  xPositionPlot.addMessageLine(status, ['trajectory_logging', 'x'])
+      .setColor(GREEN);
+  xPositionPlot.addMessageLine(localizerOuput, ['x']).setColor(BLUE);
 
   // Absolute Y Position
   const yPositionPlot = aosPlotter.addPlot(element);
@@ -291,11 +292,10 @@
   yPositionPlot.plot.getAxisLabels().setXLabel(TIME);
   yPositionPlot.plot.getAxisLabels().setYLabel('Y Position (m)');
 
-  const localizerY = yPositionPlot.addMessageLine(status, ['y']);
-  localizerY.setColor(RED);
-  const splineY =
-      yPositionPlot.addMessageLine(status, ['trajectory_logging', 'y']);
-  splineY.setColor(GREEN);
+  yPositionPlot.addMessageLine(status, ['y']).setColor(RED);
+  yPositionPlot.addMessageLine(status, ['trajectory_logging', 'y'])
+      .setColor(GREEN);
+  yPositionPlot.addMessageLine(localizerOuput, ['y']).setColor(BLUE);
 
   // Gyro
   const gyroPlot = aosPlotter.addPlot(element);

frc971/control_loops/drivetrain/localization/puppet_localizer.cc

diff --git a/frc971/control_loops/drivetrain/localization/puppet_localizer.cc b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
index 33095f2..77b9fbe 100644
--- a/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
@@ -91,7 +91,7 @@
         static_cast<float>(localizer_output_fetcher_->y()),
         static_cast<float>(localizer_output_fetcher_->theta())};
     Eigen::Matrix3f R = Eigen::Matrix3f::Zero();
-    R.diagonal() << 0.01, 0.01, 1e-4;
+    R.diagonal() << 1e-4, 1e-4, 1e-6;
     const Input U_correct = ekf_.MostRecentInput();
     observations_.CorrectKnownH(Eigen::Vector3f::Zero(), &U_correct,
                                 Corrector(state_at_capture.value(), Z), R, now);