Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
- LinearQuadraticRegulator
- Kalman filters
- Pose estimators
- LinearSystemLoop

Fixes #2925.
This commit is contained in:
Peter Johnson
2025-12-06 09:17:02 -08:00
committed by GitHub
parent 0d1dd84e86
commit baa6379267
20 changed files with 95 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
wpilibc/src/main/native/cppcs/RobotBase.cpp
View File

@@ -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;
}
}

10
wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
View File

@@ -126,6 +126,16 @@ public abstract class RobotBase implements AutoCloseable {
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
}

15
wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java
View File

@@ -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,
}

6
wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
View File

@@ -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 class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
reset();
MathSharedStore.getMathShared()
.reportUsage(MathUsageId.kController_LinearQuadraticRegulator, 1);
}
/**
@@ -163,6 +167,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
reset();
MathSharedStore.getMathShared()
.reportUsage(MathUsageId.kController_LinearQuadraticRegulator, 1);
}
/**

4
wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
View File

@@ -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 class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
}
m_P = m_initP;
MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 2);
}
/**

4
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
View File

@@ -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 class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
m_initP = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
reset();
MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 1);
}
/**

2
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
View File

@@ -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 class PoseEstimator<T> {
m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
}
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_PoseEstimator, 1);
}
/**

2
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
View File

@@ -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 class PoseEstimator3d<T> {
m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
}
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_PoseEstimator3d, 1);
}
/**

3
wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java
View File

@@ -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 class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
m_K = new Matrix<>(S.getStorage().solve(C.times(P).getStorage()).transpose());
reset();
MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 4);
}
/** Resets the observer. */

3
wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
View File

@@ -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 class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
m_pts = new MerweScaledSigmaPoints<>(states);
reset();
MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 3);
}
static <S extends Num, C extends Num>

3
wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
View File

@@ -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 class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs ex
m_nextR = new Matrix<>(new SimpleMatrix(controller.getK().getNumCols(), 1));
reset(m_nextR);
MathSharedStore.getMathShared().reportUsage(MathUsageId.kSystem_LinearSystemLoop, 1);
}
/**

4
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
View File

@@ -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;

4
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -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);
}
/**

2
wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
View File

@@ -118,6 +118,8 @@ class KalmanFilter {
}
Reset();
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kEstimator_KalmanFilter, 1);
}
/**

2
wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
View File

@@ -70,6 +70,8 @@ class WPILIB_DLLEXPORT PoseEstimator {
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kEstimator_PoseEstimator, 1);
}
/**

2
wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
View File

@@ -76,6 +76,8 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kEstimator_PoseEstimator3d, 1);
}
/**

2
wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h
View File

@@ -149,6 +149,8 @@ class SteadyStateKalmanFilter {
}
Reset();
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kEstimator_KalmanFilter, 4);
}
SteadyStateKalmanFilter(SteadyStateKalmanFilter&&) = default;

4
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -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);
}
/**

2
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
View File

@@ -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;

5
wpimath/src/main/native/include/wpimath/MathShared.h
View File

@@ -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 {