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

[wpimath] Refactor StateSpaceUtil into separate files (#8421)

Browse Source 
* Moved makeWhiteNoiseVector() to random.Normal.normal()
* Moved isControllable() and isDetectable() to system.LinearSystemUtil
* Renamed makeCostMatrix() to costMatrix() (Java)
* Renamed makeCovarianceMatrix() to covarianceMatrix() (Java)
* Renamed MakeCostMatrix() to CostMatrix() (C++)
* Renamed MakeCovMatrix() to CovarianceMatrix() (C++)
* Removed deprecated poseTo3dVector(), poseTo4dVector(), poseToVector()
* Removed clampInputMaxMagnitude()
* We don't use it, and Eigen has this functionality built in via `u =
u.array().min(u_max.array()).max(u_min.array());`
* Simplified implementation of desaturateInputVector()
This commit is contained in:
Tyler Veness
2025-11-29 10:28:38 -08:00
committed by GitHub
parent c8e6ce1ca4
commit a79f86ade3
51 changed files with 755 additions and 741 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/ExampleGlobalMeasurementSensor.java
View File

@@ -7,7 +7,7 @@ package org.wpilib.examples.differentialdriveposeestimator;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.util.StateSpaceUtil;
import org.wpilib.math.random.Normal;
import org.wpilib.math.util.Units;
/** This dummy class represents a global measurement sensor, such as a computer vision solution. */
@@ -22,8 +22,7 @@ public final class ExampleGlobalMeasurementSensor {
* @param estimatedRobotPose The robot pose.
*/
public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
var rand =
StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
var rand = Normal.normal(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
return new Pose2d(
estimatedRobotPose.getX() + rand.get(0, 0),
estimatedRobotPose.getY() + rand.get(1, 0),

5
wpilibjExamples/src/main/java/org/wpilib/examples/mecanumdriveposeestimator/ExampleGlobalMeasurementSensor.java
View File

@@ -7,7 +7,7 @@ package org.wpilib.examples.mecanumdriveposeestimator;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.util.StateSpaceUtil;
import org.wpilib.math.random.Normal;
import org.wpilib.math.util.Units;
/** This dummy class represents a global measurement sensor, such as a computer vision solution. */
@@ -22,8 +22,7 @@ public final class ExampleGlobalMeasurementSensor {
* @param estimatedRobotPose The robot pose.
*/
public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
var rand =
StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
var rand = Normal.normal(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
return new Pose2d(
estimatedRobotPose.getX() + rand.get(0, 0),
estimatedRobotPose.getY() + rand.get(1, 0),

5
wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/ExampleGlobalMeasurementSensor.java
View File

@@ -7,7 +7,7 @@ package org.wpilib.examples.swervedriveposeestimator;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.util.StateSpaceUtil;
import org.wpilib.math.random.Normal;
import org.wpilib.math.util.Units;
/** This dummy class represents a global measurement sensor, such as a computer vision solution. */
@@ -22,8 +22,7 @@ public final class ExampleGlobalMeasurementSensor {
* @param estimatedRobotPose The robot pose.
*/
public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
var rand =
StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
var rand = Normal.normal(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
return new Pose2d(
estimatedRobotPose.getX() + rand.get(0, 0),
estimatedRobotPose.getY() + rand.get(1, 0),