From 0ce9133b551556ba881ced04e89b965abdb2aff7 Mon Sep 17 00:00:00 2001
From: Claudius Tewari <cttdev@users.noreply.github.com>
Date: Thu, 29 Oct 2020 18:10:48 -0700
Subject: [PATCH] [wpimath] Address issues with LinearSystemLoop reset() and
 matrix initialization (#2819)

This address some problems with the LinearSystemLoop class that were discovered through testing.

The initial state estimate of the observer was set to the provided initial state rather than zero as previously, a non zero initial state passed into reset() would lead to a discrepancy between the current state estimate and the actual system state.
---
 .../wpilibj/system/LinearSystemLoop.java      | 23 ++++++++++---------
 .../include/frc/system/LinearSystemLoop.h     | 15 ++++++------
 2 files changed, 20 insertions(+), 18 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java
index f972dec170..d44ca62e23 100644
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java
@@ -272,29 +272,30 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num,
   }
 
   /**
-   * Zeroes reference r, controller output u and plant output y.
-   * The previous reference for PlantInversionFeedforward is set to the
-   * initial reference.
-   * @param initialReference The initial reference.
+   * Zeroes reference r and controller output u. The previous reference
+   * of the PlantInversionFeedforward and the initial state estimate of
+   * the KalmanFilter are set to the initial state provided.
+   *
+   * @param initialState The initial state.
    */
-  public void reset(Matrix<States, N1> initialReference) {
-    m_controller.reset();
-    m_feedforward.reset(initialReference);
-    m_observer.reset();
+  public void reset(Matrix<States, N1> initialState) {
     m_nextR.fill(0.0);
+    m_controller.reset();
+    m_feedforward.reset(initialState);
+    m_observer.setXhat(initialState);
   }
 
   /**
-   * Returns difference between reoid predict(double dtSference r and x-hat.
+   * Returns difference between reference r and current state x-hat.
    *
-   * @return the
+   * @return The state error matrix.
    */
   public Matrix<States, N1> getError() {
     return getController().getR().minus(m_observer.getXhat());
   }
 
   /**
-   * Returns difference between reference r and x-hat.
+   * Returns difference between reference r and current state x-hat.
    *
    * @param index The index of the error matrix to return.
    * @return The error at that index.
diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
index 01ef1adb3a..d5f25fb25f 100644
--- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
+++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
@@ -227,20 +227,21 @@ class LinearSystemLoop {
   }
 
   /**
-   * Zeroes reference r, controller output u and plant output y.
-   * The previous reference for PlantInversionFeedforward is set to the
-   * initial reference.
-   * @param initialReference The initial reference.
+   * Zeroes reference r and controller output u. The previous reference
+   * of the PlantInversionFeedforward and the initial state estimate of
+   * the KalmanFilter are set to the initial state provided.
+   *
+   * @param initialState The initial state.
    */
   void Reset(Eigen::Matrix<double, States, 1> initialState) {
+    m_nextR.setZero();
     m_controller.Reset();
     m_feedforward.Reset(initialState);
-    m_observer.Reset();
-    m_nextR.setZero();
+    m_observer.SetXhat(initialState);
   }
 
   /**
-   * Returns difference between reference r and x-hat.
+   * Returns difference between reference r and current state x-hat.
    */
   const Eigen::Matrix<double, States, 1> Error() const {
     return m_controller.R() - m_observer.Xhat();