From 0c822b45ab49b201455417d135474fc2f7b00f86 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Mon, 20 May 2024 16:53:22 -0700
Subject: [PATCH] [wpimath] Fix Eigen maybe-uninitialized warnings (#6636)

---
 .../frc/estimator/ExtendedKalmanFilter.h      |  2 +-
 .../frc/estimator/ExtendedKalmanFilter.inc    | 30 ++++++----
 .../frc/estimator/UnscentedKalmanFilter.inc   | 55 +++++++++++++------
 3 files changed, 57 insertions(+), 30 deletions(-)

diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
index 68eb75b881..ec8a3ec7d6 100644
--- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
@@ -236,7 +236,7 @@ class ExtendedKalmanFilter {
   std::function<OutputVector(const OutputVector&, const OutputVector&)>
       m_residualFuncY;
   std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
-  StateVector m_xHat;
+  StateVector m_xHat = StateVector::Zero();
   StateMatrix m_P;
   StateMatrix m_contQ;
   Matrixd<Outputs, Outputs> m_contR;
diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
index 0b0e9de4a3..b31d35b742 100644
--- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
@@ -5,6 +5,7 @@
 #pragma once
 
 #include <functional>
+#include <utility>
 
 #include <Eigen/Cholesky>
 
@@ -23,15 +24,16 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
     std::function<OutputVector(const StateVector&, const InputVector&)> h,
     const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
     units::second_t dt)
-    : m_f(f), m_h(h) {
+    : m_f(std::move(f)), m_h(std::move(h)) {
   m_contQ = MakeCovMatrix(stateStdDevs);
   m_contR = MakeCovMatrix(measurementStdDevs);
-  m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
-  m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
+  m_residualFuncY = [](const OutputVector& a,
+                       const OutputVector& b) -> OutputVector { return a - b; };
+  m_addFuncX = [](const StateVector& a, const StateVector& b) -> StateVector {
+    return a + b;
+  };
   m_dt = dt;
 
-  Reset();
-
   StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
       m_f, m_xHat, InputVector::Zero());
   Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
@@ -61,13 +63,14 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
         residualFuncY,
     std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
     units::second_t dt)
-    : m_f(f), m_h(h), m_residualFuncY(residualFuncY), m_addFuncX(addFuncX) {
+    : m_f(std::move(f)),
+      m_h(std::move(h)),
+      m_residualFuncY(std::move(residualFuncY)),
+      m_addFuncX(std::move(addFuncX)) {
   m_contQ = MakeCovMatrix(stateStdDevs);
   m_contR = MakeCovMatrix(measurementStdDevs);
   m_dt = dt;
 
-  Reset();
-
   StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
       m_f, m_xHat, InputVector::Zero());
   Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
@@ -114,9 +117,14 @@ void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
     const InputVector& u, const Vectord<Rows>& y,
     std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
     const Matrixd<Rows, Rows>& R) {
-  auto residualFuncY = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
-  auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
-  Correct<Rows>(u, y, h, R, residualFuncY, addFuncX);
+  auto residualFuncY = [](const Vectord<Rows>& a,
+                          const Vectord<Rows>& b) -> Vectord<Rows> {
+    return a - b;
+  };
+  auto addFuncX = [](const StateVector& a,
+                     const StateVector& b) -> StateVector { return a + b; };
+  Correct<Rows>(u, y, std::move(h), R, std::move(residualFuncY),
+                std::move(addFuncX));
 }
 
 template <int States, int Inputs, int Outputs>
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
index c693197017..03cfd192b4 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
@@ -5,6 +5,7 @@
 #pragma once
 
 #include <functional>
+#include <utility>
 
 #include <Eigen/Cholesky>
 
@@ -23,16 +24,24 @@ UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
     std::function<OutputVector(const StateVector&, const InputVector&)> h,
     const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
     units::second_t dt)
-    : m_f(f), m_h(h) {
+    : m_f(std::move(f)), m_h(std::move(h)) {
   m_contQ = MakeCovMatrix(stateStdDevs);
   m_contR = MakeCovMatrix(measurementStdDevs);
-  m_meanFuncX = [](auto sigmas, auto Wm) -> StateVector { return sigmas * Wm; };
-  m_meanFuncY = [](auto sigmas, auto Wc) -> OutputVector {
+  m_meanFuncX = [](const Matrixd<States, 2 * States + 1>& sigmas,
+                   const Vectord<2 * States + 1>& Wm) -> StateVector {
+    return sigmas * Wm;
+  };
+  m_meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
+                   const Vectord<2 * States + 1>& Wc) -> OutputVector {
     return sigmas * Wc;
   };
-  m_residualFuncX = [](auto a, auto b) -> StateVector { return a - b; };
-  m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
-  m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
+  m_residualFuncX = [](const StateVector& a,
+                       const StateVector& b) -> StateVector { return a - b; };
+  m_residualFuncY = [](const OutputVector& a,
+                       const OutputVector& b) -> OutputVector { return a - b; };
+  m_addFuncX = [](const StateVector& a, const StateVector& b) -> StateVector {
+    return a + b;
+  };
   m_dt = dt;
 
   Reset();
@@ -55,13 +64,13 @@ UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
         residualFuncY,
     std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
     units::second_t dt)
-    : m_f(f),
-      m_h(h),
-      m_meanFuncX(meanFuncX),
-      m_meanFuncY(meanFuncY),
-      m_residualFuncX(residualFuncX),
-      m_residualFuncY(residualFuncY),
-      m_addFuncX(addFuncX) {
+    : m_f(std::move(f)),
+      m_h(std::move(h)),
+      m_meanFuncX(std::move(meanFuncX)),
+      m_meanFuncY(std::move(meanFuncY)),
+      m_residualFuncX(std::move(residualFuncX)),
+      m_residualFuncY(std::move(residualFuncY)),
+      m_addFuncX(std::move(addFuncX)) {
   m_contQ = MakeCovMatrix(stateStdDevs);
   m_contR = MakeCovMatrix(measurementStdDevs);
   m_dt = dt;
@@ -103,13 +112,23 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
     const InputVector& u, const Vectord<Rows>& y,
     std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
     const Matrixd<Rows, Rows>& R) {
-  auto meanFuncY = [](auto sigmas, auto Wc) -> Vectord<Rows> {
+  auto meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
+                      const Vectord<2 * States + 1>& Wc) -> Vectord<Rows> {
     return sigmas * Wc;
   };
-  auto residualFuncX = [](auto a, auto b) -> StateVector { return a - b; };
-  auto residualFuncY = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
-  auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
-  Correct<Rows>(u, y, h, R, meanFuncY, residualFuncY, residualFuncX, addFuncX);
+  auto residualFuncX = [](const StateVector& a,
+                          const StateVector& b) -> StateVector {
+    return a - b;
+  };
+  auto residualFuncY = [](const Vectord<Rows>& a,
+                          const Vectord<Rows>& b) -> Vectord<Rows> {
+    return a - b;
+  };
+  auto addFuncX = [](const StateVector& a,
+                     const StateVector& b) -> StateVector { return a + b; };
+  Correct<Rows>(u, y, std::move(h), R, std::move(meanFuncY),
+                std::move(residualFuncY), std::move(residualFuncX),
+                std::move(addFuncX));
 }
 
 template <int States, int Inputs, int Outputs>