[wpimath] Add usage reporting for state-space classes (#8453)

PeterJohnson · web-flow · commit baa637926708 · 2025-12-06T09:17:02.000-08:00
- LinearQuadraticRegulator - Kalman filters - Pose estimators - LinearSystemLoop Fixes #2925.
diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -146,6 +146,22 @@ class WPILibMathShared : public wpi::math::MathShared {
         HAL_Report(HALUsageReporting::kResourceType_PathWeaverTrajectory,
                    count);
         break;
+      case wpi::math::MathUsageId::kController_LinearQuadraticRegulator:
+        HAL_Report(HALUsageReporting::kResourceType_LinearQuadraticRegulator,
+                   count);
+        break;
+      case wpi::math::MathUsageId::kEstimator_KalmanFilter:
+        HAL_Report(HALUsageReporting::kResourceType_KalmanFilter, count);
+        break;
+      case wpi::math::MathUsageId::kEstimator_PoseEstimator:
+        HAL_Report(HALUsageReporting::kResourceType_PoseEstimator, count);
+        break;
+      case wpi::math::MathUsageId::kEstimator_PoseEstimator3d:
+        HAL_Report(HALUsageReporting::kResourceType_PoseEstimator3d, count);
+        break;
+      case wpi::math::MathUsageId::kSystem_LinearSystemLoop:
+        HAL_Report(HALUsageReporting::kResourceType_LinearSystemLoop, count);
+        break;
     }
   }
 
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
@@ -126,6 +126,16 @@ public void reportUsage(MathUsageId id, int count) {
                   HAL.report(tResourceType.kResourceType_BangBangController, count);
               case kTrajectory_PathWeaver ->
                   HAL.report(tResourceType.kResourceType_PathWeaverTrajectory, count);
+              case kController_LinearQuadraticRegulator ->
+                  HAL.report(tResourceType.kResourceType_LinearQuadraticRegulator, count);
+              case kEstimator_KalmanFilter ->
+                  HAL.report(tResourceType.kResourceType_KalmanFilter, count);
+              case kEstimator_PoseEstimator ->
+                  HAL.report(tResourceType.kResourceType_PoseEstimator, count);
+              case kEstimator_PoseEstimator3d ->
+                  HAL.report(tResourceType.kResourceType_PoseEstimator3d, count);
+              case kSystem_LinearSystemLoop ->
+                  HAL.report(tResourceType.kResourceType_LinearSystemLoop, count);
               default -> {
                 // NOP
               }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java b/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java
@@ -41,4 +41,19 @@ public enum MathUsageId {
 
   /** PathWeaver Trajectory. */
   kTrajectory_PathWeaver,
+
+  /** Linear Quadratic Regulator. */
+  kController_LinearQuadraticRegulator,
+
+  /** Kalman Filter. */
+  kEstimator_KalmanFilter,
+
+  /** Pose Estimator. */
+  kEstimator_PoseEstimator,
+
+  /** 3D Pose Estimator. */
+  kEstimator_PoseEstimator3d,
+
+  /** Linear System Loop. */
+  kSystem_LinearSystemLoop,
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
@@ -5,6 +5,8 @@
 package edu.wpi.first.math.controller;
 
 import edu.wpi.first.math.DARE;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.StateSpaceUtil;
@@ -124,6 +126,8 @@ public LinearQuadraticRegulator(
     m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
 
     reset();
+    MathSharedStore.getMathShared()
+        .reportUsage(MathUsageId.kController_LinearQuadraticRegulator, 1);
   }
 
   /**
@@ -163,6 +167,8 @@ public LinearQuadraticRegulator(
     m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
 
     reset();
+    MathSharedStore.getMathShared()
+        .reportUsage(MathUsageId.kController_LinearQuadraticRegulator, 1);
   }
 
   /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
@@ -5,6 +5,8 @@
 package edu.wpi.first.math.estimator;
 
 import edu.wpi.first.math.DARE;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
@@ -163,6 +165,8 @@ public ExtendedKalmanFilter(
     }
 
     m_P = m_initP;
+
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 2);
   }
 
   /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
@@ -5,6 +5,8 @@
 package edu.wpi.first.math.estimator;
 
 import edu.wpi.first.math.DARE;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
@@ -87,6 +89,8 @@ public KalmanFilter(
     m_initP = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
 
     reset();
+
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 1);
   }
 
   /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
@@ -5,6 +5,7 @@
 package edu.wpi.first.math.estimator;
 
 import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -79,6 +80,7 @@ public PoseEstimator(
       m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
     }
     setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_PoseEstimator, 1);
   }
 
   /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
@@ -5,6 +5,7 @@
 package edu.wpi.first.math.estimator;
 
 import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -87,6 +88,7 @@ public PoseEstimator3d(
       m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
     }
     setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_PoseEstimator3d, 1);
   }
 
   /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java
@@ -5,6 +5,8 @@
 package edu.wpi.first.math.estimator;
 
 import edu.wpi.first.math.DARE;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
@@ -107,6 +109,7 @@ public SteadyStateKalmanFilter(
     m_K = new Matrix<>(S.getStorage().solve(C.times(P).getStorage()).transpose());
 
     reset();
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 4);
   }
 
   /** Resets the observer. */
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
@@ -4,6 +4,8 @@
 
 package edu.wpi.first.math.estimator;
 
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
@@ -163,6 +165,7 @@ public UnscentedKalmanFilter(
     m_pts = new MerweScaledSigmaPoints<>(states);
 
     reset();
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 3);
   }
 
   static <S extends Num, C extends Num>
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
@@ -4,6 +4,8 @@
 
 package edu.wpi.first.math.system;
 
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.StateSpaceUtil;
@@ -132,6 +134,7 @@ public LinearSystemLoop(
 
     m_nextR = new Matrix<>(new SimpleMatrix(controller.getK().getNumCols(), 1));
     reset(m_nextR);
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kSystem_LinearSystemLoop, 1);
   }
 
   /**
diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
@@ -135,6 +135,8 @@ class LinearQuadraticRegulator {
     }
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kController_LinearQuadraticRegulator, 1);
   }
 
   /**
@@ -194,6 +196,8 @@ class LinearQuadraticRegulator {
     }
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kController_LinearQuadraticRegulator, 1);
   }
 
   LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
@@ -137,6 +137,8 @@ class ExtendedKalmanFilter {
       m_initP = StateMatrix::Zero();
     }
     m_P = m_initP;
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kEstimator_KalmanFilter, 2);
   }
 
   /**
@@ -221,6 +223,8 @@ class ExtendedKalmanFilter {
       m_initP = StateMatrix::Zero();
     }
     m_P = m_initP;
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kEstimator_KalmanFilter, 2);
   }
 
   /**
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
@@ -118,6 +118,8 @@ class KalmanFilter {
     }
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kEstimator_KalmanFilter, 1);
   }
 
   /**
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -70,6 +70,8 @@ class WPILIB_DLLEXPORT PoseEstimator {
     }
 
     SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kEstimator_PoseEstimator, 1);
   }
 
   /**
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
@@ -76,6 +76,8 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
     }
 
     SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kEstimator_PoseEstimator3d, 1);
   }
 
   /**
diff --git a/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h
@@ -149,6 +149,8 @@ class SteadyStateKalmanFilter {
     }
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kEstimator_KalmanFilter, 4);
   }
 
   SteadyStateKalmanFilter(SteadyStateKalmanFilter&&) = default;
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
@@ -107,6 +107,8 @@ class UnscentedKalmanFilter {
     m_dt = dt;
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kEstimator_KalmanFilter, 3);
   }
 
   /**
@@ -166,6 +168,8 @@ class UnscentedKalmanFilter {
     m_dt = dt;
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kEstimator_KalmanFilter, 3);
   }
 
   /**
diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
@@ -129,6 +129,8 @@ class LinearSystemLoop {
         m_clampFunc(clampFunction) {
     m_nextR.setZero();
     Reset(m_nextR);
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kSystem_LinearSystemLoop, 1);
   }
 
   LinearSystemLoop(LinearSystemLoop&&) = default;
diff --git a/wpimath/src/main/native/include/wpimath/MathShared.h b/wpimath/src/main/native/include/wpimath/MathShared.h
@@ -26,6 +26,11 @@ enum class MathUsageId {
   kController_ProfiledPIDController,
   kController_BangBangController,
   kTrajectory_PathWeaver,
+  kController_LinearQuadraticRegulator,
+  kEstimator_KalmanFilter,
+  kEstimator_PoseEstimator,
+  kEstimator_PoseEstimator3d,
+  kSystem_LinearSystemLoop,
 };
 
 class WPILIB_DLLEXPORT MathShared {

Original file line number	Diff line number	Diff line change
`@@ -5,6 +5,8 @@`
`5`	`5`	`package edu.wpi.first.math.estimator;`
`6`	`6`
`7`	`7`	`import edu.wpi.first.math.DARE;`
	`8`	`+import edu.wpi.first.math.MathSharedStore;`
	`9`	`+import edu.wpi.first.math.MathUsageId;`
`8`	`10`	`import edu.wpi.first.math.Matrix;`
`9`	`11`	`import edu.wpi.first.math.Nat;`
`10`	`12`	`import edu.wpi.first.math.Num;`
`@@ -163,6 +165,8 @@ public ExtendedKalmanFilter(`
`163`	`165`	`}`
`164`	`166`
`165`	`167`	`m_P = m_initP;`
	`168`	`+`
	`169`	`+ MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 2);`
`166`	`170`	`}`
`167`	`171`
`168`	`172`	`/**`
Original file line number	Diff line number	Diff line change
`@@ -5,6 +5,7 @@`
`5`	`5`	`package edu.wpi.first.math.estimator;`
`6`	`6`
`7`	`7`	`import edu.wpi.first.math.MathSharedStore;`
	`8`	`+import edu.wpi.first.math.MathUsageId;`
`8`	`9`	`import edu.wpi.first.math.MathUtil;`
`9`	`10`	`import edu.wpi.first.math.Matrix;`
`10`	`11`	`import edu.wpi.first.math.Nat;`
`@@ -79,6 +80,7 @@ public PoseEstimator(`
`79`	`80`	`m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));`
`80`	`81`	`}`
`81`	`82`	`setVisionMeasurementStdDevs(visionMeasurementStdDevs);`
	`83`	`+ MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_PoseEstimator, 1);`
`82`	`84`	`}`
`83`	`85`
`84`	`86`	`/**`