Namespaces
namespace	detail

Classes
class	LinearSystem
	A plant defined using state-space notation. More...
class	SimulatedAnnealing
	An implementation of the Simulated Annealing stochastic nonlinear optimization method. More...
class	Odometry
	Class for odometry. More...
class	EdgeCounterFilter
	A rising edge counter for boolean streams. More...
class	DCMotor
	Holds the constants for a DC motor. More...
class	ExtendedKalmanFilter
	A Kalman filter combines predictions from a model and measurements to give an estimate of the true system state. More...
class	TrapezoidProfile
	A trapezoid-shaped velocity profile. More...
class	LinearQuadraticRegulator
	Contains the controller coefficients and logic for a linear-quadratic regulator (LQR). More...
class	EllipticalRegionConstraint
	Enforces a particular constraint only within an elliptical region. More...
class	DifferentialDriveAccelerationLimiter
	Filters the provided voltages to limit a differential drive's linear and angular acceleration. More...
class	Ellipse2d
	Represents a 2d ellipse space containing translational, rotational, and scaling components. More...
struct	MecanumDriveWheelAccelerations
	Represents the wheel accelerations for a mecanum drive drivetrain. More...
class	PoseEstimator
	This class wraps odometry to fuse latency-compensated vision measurements with encoder measurements. More...
class	Transform2d
	Represents a transformation for a Pose2d in the pose's frame. More...
class	LinearFilter
	This class implements a linear, digital filter. More...
class	DifferentialDriveOdometry
	Class for differential drive odometry. More...
class	LinearPlantInversionFeedforward
	Constructs a plant inversion model-based feedforward from a LinearSystem. More...
class	DifferentialDriveOdometry3d
	Class for differential drive odometry. More...
struct	Twist3d
	A change in distance along a 3D arc since the last pose update. More...
class	Translation2d
	Represents a translation in 2D space. More...
class	Kinematics
	Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel velocities. More...
class	BangBangController
	Implements a bang-bang controller, which outputs either 0 or 1 depending on whether the measurement is less than the setpoint. More...
class	Pose2d
	Represents a 2D pose containing translational and rotational elements. More...
class	KalmanFilterLatencyCompensator
	This class incorporates time-delayed measurements into a Kalman filter's state estimate. More...
class	SwerveDriveOdometry
	Class for swerve drive odometry. More...
class	ElevatorFeedforward
	A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting against the force of gravity). More...
class	SwerveDriveKinematics
	Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module states (velocity and angle). More...
class	TimeInterpolatableBuffer
	The TimeInterpolatableBuffer provides an easy way to estimate past measurements. More...
class	Translation3d
	Represents a translation in 3D space. More...
class	SwerveDrivePoseEstimator
	This class wraps Swerve Drive Odometry to fuse latency-compensated vision measurements with swerve drive encoder distance measurements. More...
class	DifferentialDriveKinematics
	Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel velocities for a differential drive. More...
struct	Twist2d
	A change in distance along a 2D arc since the last pose update. More...
class	Odometry3d
	Class for odometry. More...
class	TravelingSalesman
	Given a list of poses, this class finds the shortest possible route that visits each pose exactly once and returns to the origin pose. More...
class	Models
	Linear system factories. More...
class	SwerveDriveKinematicsConstraint
	A class that enforces constraints on the swerve drive kinematics. More...
class	DifferentialDrivePoseEstimator
	This class wraps Differential Drive Odometry to fuse latency-compensated vision measurements with differential drive encoder measurements. More...
class	Rotation3d
	A rotation in a 3D coordinate frame, represented by a quaternion. More...
class	ProfiledPIDController
	Implements a PID control loop whose setpoint is constrained by a trapezoid profile. More...
class	S3SigmaPoints
	Generates sigma points and weights according to Papakonstantinou's paper[1] for the UnscentedKalmanFilter class. More...
class	CoordinateSystem
	A helper class that converts Pose3d objects between different standard coordinate frames. More...
struct	SwerveModulePosition
	Represents the position of one swerve module. More...
class	Rotation2d
	A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine). More...
class	ControlAffinePlantInversionFeedforward
	Constructs a control-affine plant inversion model-based feedforward from given model dynamics. More...
class	PIDController
	Implements a PID control loop. More...
struct	MecanumDriveWheelPositions
	Represents the wheel positions for a mecanum drive drivetrain. More...
class	Transform3d
	Represents a transformation for a Pose3d in the pose's frame. More...
class	TrajectoryConfig
	Represents the configuration for generating a trajectory. More...
class	ArmFeedforward
	A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting against the force of gravity on a beam suspended at an angle). More...
class	TrajectoryParameterizer
	Class used to parameterize a trajectory by time. More...
class	SlewRateLimiter
	A class that limits the rate of change of an input value. More...
class	ExponentialProfile
	A Exponential-shaped velocity profile. More...
class	DifferentialDriveFeedforward
	A helper class which computes the feedforward outputs for a differential drive drivetrain. More...
struct	DifferentialDriveWheelAccelerations
	Represents the wheel accelerations for a differential drive drivetrain. More...
class	TrajectoryGenerator
	Helper class used to generate trajectories with various constraints. More...
class	Spline
	Represents a two-dimensional parametric spline that interpolates between two points. More...
class	CentripetalAccelerationConstraint
	A constraint on the maximum absolute centripetal acceleration allowed when traversing a trajectory. More...
class	QuinticHermiteSpline
	Represents a hermite spline of degree 5. More...
class	UnscentedKalmanFilter
	A Kalman filter combines predictions from a model and measurements to give an estimate of the true system state. More...
struct	ChassisAccelerations
	Represents robot chassis accelerations. More...
class	ct_matrix
	Compile-time wrapper for Eigen::Matrix. More...
class	SplineParameterizer
	Class used to parameterize a spline by its arc length. More...
class	DifferentialDriveKinematicsConstraint
	A class that enforces constraints on the differential drive kinematics. More...
struct	SwerveModuleVelocity
	Represents the velocity of one swerve module. More...
class	RectangularRegionConstraint
	Enforces a particular constraint only within a rectangular region. More...
class	LTVUnicycleController
	The linear time-varying unicycle controller has a similar form to the LQR, but the model used to compute the controller gain is the nonlinear unicycle model linearized around the drivetrain's current state. More...
class	KalmanFilter
	A Kalman filter combines predictions from a model and measurements to give an estimate of the true system state. More...
class	CubicHermiteSpline
	Represents a hermite spline of degree 3. More...
class	LTVDifferentialDriveController
	The linear time-varying differential drive controller has a similar form to the LQR, but the model used to compute the controller gain is the nonlinear differential drive model linearized around the drivetrain's current state. More...
struct	DifferentialDriveWheelVelocities
	Represents the wheel velocities for a differential drive drivetrain. More...
class	MaxVelocityConstraint
	Represents a constraint that enforces a max velocity. More...
class	Rectangle2d
	Represents a 2d rectangular space containing translational, rotational, and scaling components. More...
class	Debouncer
	A simple debounce filter for boolean streams. More...
class	MecanumDriveOdometry
	Class for mecanum drive odometry. More...
class	SwerveDrivePoseEstimator3d
	This class wraps Swerve Drive Odometry to fuse latency-compensated vision measurements with swerve drive encoder distance measurements. More...
class	MerweScaledSigmaPoints
	Generates sigma points and weights according to Van der Merwe's 2004 dissertation[1] for the UnscentedKalmanFilter class. More...
class	MecanumDrivePoseEstimator3d
	This class wraps Mecanum Drive Odometry to fuse latency-compensated vision measurements with mecanum drive encoder velocity measurements. More...
class	Quaternion
	Represents a quaternion. More...
struct	SwerveModuleAcceleration
	Represents the accelerations of one swerve module. More...
class	LinearSystemLoop
	Combines a controller, feedforward, and observer for controlling a mechanism with full state feedback. More...
class	MecanumDrivePoseEstimator
	This class wraps Mecanum Drive Odometry to fuse latency-compensated vision measurements with mecanum drive encoder velocity measurements. More...
class	DifferentialDrivePoseEstimator3d
	This class wraps Differential Drive Odometry to fuse latency-compensated vision measurements with differential drive encoder measurements. More...
class	MathShared
class	MathSharedStore
class	SteadyStateKalmanFilter
	A Kalman filter combines predictions from a model and measurements to give an estimate of the true system state. More...
class	MedianFilter
	A class that implements a moving-window median filter. More...
struct	ChassisVelocities
	Represents robot chassis velocities. More...
class	PoseEstimator3d
	This class wraps odometry to fuse latency-compensated vision measurements with encoder measurements. More...
struct	DifferentialDriveWheelVoltages
	Motor voltages for a differential drive. More...
class	MecanumDriveKinematics
	Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel velocities. More...
class	MecanumDriveOdometry3d
	Class for mecanum drive odometry. More...
class	MecanumDriveKinematicsConstraint
	A class that enforces constraints on the mecanum drive kinematics. More...
class	TrajectoryConstraint
	An interface for defining user-defined velocity and acceleration constraints while generating trajectories. More...
class	SplineHelper
	Helper class that is used to generate cubic and quintic splines from user provided waypoints. More...
struct	MecanumDriveWheelVelocities
	Represents the wheel velocities for a mecanum drive drivetrain. More...
class	SwerveDriveOdometry3d
	Class for swerve drive odometry. More...
struct	DifferentialDriveWheelPositions
	Represents the wheel positions for a differential drive drivetrain. More...
class	SimpleMotorFeedforward
	A helper class that computes feedforward voltages for a simple permanent-magnet DC motor. More...
class	Pose3d
	Represents a 3D pose containing translational and rotational elements. More...
class	ImplicitModelFollower
	Contains the controller coefficients and logic for an implicit model follower. More...
class	DifferentialDriveVoltageConstraint
	A class that enforces constraints on differential drive voltage expenditure based on the motor dynamics and the drive kinematics. More...
class	Trajectory
	Represents a time-parameterized trajectory. More...
class	CoordinateAxis
	A class representing a coordinate system axis within the NWU coordinate system. More...

Concepts
concept	EigenMatrixLike
concept	SigmaPoints

Typedefs
template<int States, int Inputs, int Outputs>
using	EKF = ExtendedKalmanFilter<States, Inputs, Outputs>
template<int States, int Inputs>
using	LQR = LinearQuadraticRegulator<States, Inputs>
template<int States, int Inputs, int Outputs>
using	MerweUKF
template<int States, int Inputs, int Outputs>
using	S3UKF
template<int States, int Inputs, int Outputs, SigmaPoints< States > SigmaPoints>
using	UKF = UnscentedKalmanFilter<States, Inputs, Outputs, SigmaPoints>
template<typename Scalar, int Rows>
using	ct_vector = ct_matrix<Scalar, Rows, 1>
template<typename Scalar, int Cols>
using	ct_row_vector = ct_matrix<Scalar, 1, Cols>
using	ct_matrix2d = ct_matrix<double, 2, 2>
using	ct_matrix3d = ct_matrix<double, 3, 3>
using	ct_vector2d = ct_vector<double, 2>
using	ct_vector3d = ct_vector<double, 3>
template<int States, int Inputs, int Outputs>
using	KF = KalmanFilter<States, Inputs, Outputs>
template<int Size>
using	Vectord = Eigen::Vector<double, Size>
template<int Rows, int Cols, int Options = Eigen::AutoAlign \| ((Rows == 1 && Cols != 1) ? Eigen::RowMajor : (Cols == 1 && Rows != 1) ? Eigen::ColMajor : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION), int MaxRows = Rows, int MaxCols = Cols>
using	Matrixd = Eigen::Matrix<double, Rows, Cols, Options, MaxRows, MaxCols>
template<int States, int Inputs, int Outputs>
using	SteadyStateKF = SteadyStateKalmanFilter<States, Inputs, Outputs>

Enumerations
enum class	DAREError { QNotSymmetric , QNotPositiveSemidefinite , RNotSymmetric , RNotPositiveDefinite , ABNotStabilizable , ACNotDetectable }
	Errors the DARE solver can encounter. More...

Functions
template class	EXPORT_TEMPLATE_DECLARE (WPILIB_DLLEXPORT) LinearQuadraticRegulator< 1
WPILIB_DLLEXPORT void	to_json (wpi::util::json &json, const Translation2d &state)
WPILIB_DLLEXPORT void	from_json (const wpi::util::json &json, Translation2d &state)
WPILIB_DLLEXPORT void	to_json (wpi::util::json &json, const Pose2d &pose)
WPILIB_DLLEXPORT void	from_json (const wpi::util::json &json, Pose2d &pose)
template<int States>
void	DiscretizeA (const Matrixd< States, States > &contA, wpi::units::second_t dt, Matrixd< States, States > *discA)
	Discretizes the given continuous A matrix.
template<int States, int Inputs>
void	DiscretizeAB (const Matrixd< States, States > &contA, const Matrixd< States, Inputs > &contB, wpi::units::second_t dt, Matrixd< States, States > discA, Matrixd< States, Inputs > discB)
	Discretizes the given continuous A and B matrices.
template<int States>
void	DiscretizeAQ (const Matrixd< States, States > &contA, const Matrixd< States, States > &contQ, wpi::units::second_t dt, Matrixd< States, States > discA, Matrixd< States, States > discQ)
	Discretizes the given continuous A and Q matrices.
template<int Outputs>
Matrixd< Outputs, Outputs >	DiscretizeR (const Matrixd< Outputs, Outputs > &R, wpi::units::second_t dt)
	Returns a discretized version of the provided continuous measurement noise covariance matrix.
template<typename ModuleTranslation, typename... ModuleTranslations>
	SwerveDriveKinematics (ModuleTranslation, ModuleTranslations...) -> SwerveDriveKinematics< 1+sizeof...(ModuleTranslations)>
WPILIB_DLLEXPORT void	to_json (wpi::util::json &json, const Translation3d &state)
WPILIB_DLLEXPORT void	from_json (const wpi::util::json &json, Translation3d &state)
template<typename F, typename T>
T	RK4 (F &&f, T x, wpi::units::second_t dt)
	Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
template<typename F, typename T, typename U>
T	RK4 (F &&f, T x, U u, wpi::units::second_t dt)
	Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
template<typename F, typename T>
T	RK4 (F &&f, wpi::units::second_t t, T y, wpi::units::second_t dt)
	Performs 4th order Runge-Kutta integration of dy/dt = f(t, y) for dt.
template<typename F, typename T, typename U>
T	RKDP (F &&f, T x, U u, wpi::units::second_t dt, double maxError=1e-6)
	Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt.
template<typename F, typename T>
T	RKDP (F &&f, wpi::units::second_t t, T y, wpi::units::second_t dt, double maxError=1e-6)
	Performs adaptive Dormand-Prince integration of dy/dt = f(t, y) for dt.
WPILIB_DLLEXPORT void	to_json (wpi::util::json &json, const Rotation3d &rotation)
WPILIB_DLLEXPORT void	from_json (const wpi::util::json &json, Rotation3d &rotation)
template<std::same_as< double >... Ts>
constexpr Eigen::Matrix< double, sizeof...(Ts), sizeof...(Ts)>	CostMatrix (Ts... tolerances)
	Creates a cost matrix from the given vector for use with LQR.
template<size_t N>
constexpr Eigen::Matrix< double, N, N >	CostMatrix (const std::array< double, N > &costs)
	Creates a cost matrix from the given vector for use with LQR.
WPILIB_DLLEXPORT Eigen::MatrixXd	CostMatrix (const std::span< const double > costs)
	Creates a cost matrix from the given vector for use with LQR.
template<std::same_as< double >... Ts>
constexpr Eigen::Matrix< double, sizeof...(Ts), sizeof...(Ts)>	CovarianceMatrix (Ts... stdDevs)
	Creates a covariance matrix from the given vector for use with Kalman filters.
template<size_t N>
constexpr Eigen::Matrix< double, N, N >	CovarianceMatrix (const std::array< double, N > &stdDevs)
	Creates a covariance matrix from the given vector for use with Kalman filters.
WPILIB_DLLEXPORT Eigen::MatrixXd	CovarianceMatrix (const std::span< const double > stdDevs)
	Creates a covariance matrix from the given vector for use with Kalman filters.
template<int Inputs>
Eigen::Vector< double, Inputs >	DesaturateInputVector (const Eigen::Vector< double, Inputs > &u, double maxMagnitude)
	Renormalize all inputs if any exceeds the maximum magnitude.
template WPILIB_DLLEXPORT Eigen::VectorXd	DesaturateInputVector< Eigen::Dynamic > (const Eigen::VectorXd &u, double maxMagnitude)
WPILIB_DLLEXPORT void	to_json (wpi::util::json &json, const Rotation2d &rotation)
WPILIB_DLLEXPORT void	from_json (const wpi::util::json &json, Rotation2d &rotation)
constexpr std::string_view	to_string (const DAREError &error)
	Converts the given DAREError enum to a string.
template<int States, int Inputs>
wpi::util::expected< Eigen::Matrix< double, States, States >, DAREError >	DARE (const Eigen::Matrix< double, States, States > &A, const Eigen::Matrix< double, States, Inputs > &B, const Eigen::Matrix< double, States, States > &Q, const Eigen::Matrix< double, Inputs, Inputs > &R, bool checkPreconditions=true)
	Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation:
template<int States, int Inputs>
wpi::util::expected< Eigen::Matrix< double, States, States >, DAREError >	DARE (const Eigen::Matrix< double, States, States > &A, const Eigen::Matrix< double, States, Inputs > &B, const Eigen::Matrix< double, States, States > &Q, const Eigen::Matrix< double, Inputs, Inputs > &R, const Eigen::Matrix< double, States, Inputs > &N, bool checkPreconditions=true)
	Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation:
template<typename Derived> requires std::derived_from<Derived, Eigen::MatrixBase<Derived>>
	ct_matrix (const Derived &) -> ct_matrix< typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime >
template<int States>
Vectord< States >	AngleResidual (const Vectord< States > &a, const Vectord< States > &b, int angleStateIdx)
	Subtracts a and b while normalizing the resulting value in the selected row as if it were an angle.
template<int States>
std::function< Vectord< States >(const Vectord< States > &, const Vectord< States > &)>	AngleResidual (int angleStateIdx)
	Returns a function that subtracts two vectors while normalizing the resulting value in the selected row as if it were an angle.
template<int States>
Vectord< States >	AngleAdd (const Vectord< States > &a, const Vectord< States > &b, int angleStateIdx)
	Adds a and b while normalizing the resulting value in the selected row as an angle.
template<int States>
std::function< Vectord< States >(const Vectord< States > &, const Vectord< States > &)>	AngleAdd (int angleStateIdx)
	Returns a function that adds two vectors while normalizing the resulting value in the selected row as an angle.
template<int CovDim, int NumSigmas>
Vectord< CovDim >	AngleMean (const Matrixd< CovDim, NumSigmas > &sigmas, const Vectord< NumSigmas > &Wm, int angleStatesIdx)
	Computes the mean of sigmas with the weights Wm while computing a special angle mean for a select row.
template<int CovDim, int NumSigmas>
std::function< Vectord< CovDim >(const Matrixd< CovDim, NumSigmas > &, const Vectord< NumSigmas > &)>	AngleMean (int angleStateIdx)
	Returns a function that computes the mean of sigmas with the weights Wm while computing a special angle mean for a select row.
template<std::same_as< double >... Ts>
Eigen::Vector< double, sizeof...(Ts)>	Normal (Ts... stdDevs)
	Creates a vector of normally distributed random values with the given standard deviations for each element.
template<int N>
Eigen::Vector< double, N >	Normal (const std::array< double, N > &stdDevs)
	Creates a vector of normally distributed random values with the given standard deviations for each element.
WPILIB_DLLEXPORT Eigen::VectorXd	Normal (const std::span< const double > stdDevs)
	Creates a vector of normally distributed random values with the given standard deviations for each element.
WPILIB_DLLEXPORT void	to_json (wpi::util::json &json, const Quaternion &quaternion)
WPILIB_DLLEXPORT void	from_json (const wpi::util::json &json, Quaternion &quaternion)
template<typename T> requires std::is_arithmetic_v<T> \|\| wpi::units::traits::is_unit_t_v<T>
constexpr T	ApplyDeadband (T value, T deadband, T maxMagnitude=T{1.0})
	Returns 0.0 if the given value is within the specified range around zero.
template<typename T, int N> requires std::is_arithmetic_v<T> \|\| wpi::units::traits::is_unit_t_v<T>
Eigen::Vector< T, N >	ApplyDeadband (const Eigen::Vector< T, N > &value, T deadband, T maxMagnitude=T{1.0})
	Returns a zero vector if the given vector is within the specified distance from the origin.
template<typename T> requires std::is_arithmetic_v<T> \|\| wpi::units::traits::is_unit_t_v<T>
constexpr T	CopyDirectionPow (T value, double exponent, T maxMagnitude=T{1.0})
	Raises the input to the power of the given exponent while preserving its sign.
template<typename T, int N> requires std::is_arithmetic_v<T> \|\| wpi::units::traits::is_unit_t_v<T>
Eigen::Vector< T, N >	CopyDirectionPow (const Eigen::Vector< T, N > &value, double exponent, T maxMagnitude=T{1.0})
	Raises the norm of the input to the power of the given exponent while preserving its direction.
template<typename T>
constexpr T	InputModulus (T input, T minimumInput, T maximumInput)
	Returns modulus of input.
template<typename T> requires std::is_arithmetic_v<T> \|\| wpi::units::traits::is_unit_t_v<T>
constexpr bool	IsNear (T expected, T actual, T tolerance)
	Checks if the given value matches an expected value within a certain tolerance.
template<typename T> requires std::is_arithmetic_v<T> \|\| wpi::units::traits::is_unit_t_v<T>
constexpr bool	IsNear (T expected, T actual, T tolerance, T min, T max)
	Checks if the given value matches an expected value within a certain tolerance.
WPILIB_DLLEXPORT constexpr wpi::units::radian_t	AngleModulus (wpi::units::radian_t angle)
	Wraps an angle to the range -π to π radians (-180 to 180 degrees).
constexpr std::signed_integral auto	FloorDiv (std::signed_integral auto x, std::signed_integral auto y)
	Returns the largest (closest to positive infinity) `int` value that is less than or equal to the algebraic quotient.
constexpr std::signed_integral auto	FloorMod (std::signed_integral auto x, std::signed_integral auto y)
	Returns the floor modulus of the `int` arguments.
constexpr Translation2d	SlewRateLimit (const Translation2d &current, const Translation2d &next, wpi::units::second_t dt, wpi::units::meters_per_second_t maxVelocity)
	Limits translation velocity.
constexpr Translation3d	SlewRateLimit (const Translation3d &current, const Translation3d &next, wpi::units::second_t dt, wpi::units::meters_per_second_t maxVelocity)
	Limits translation velocity.
WPILIB_DLLEXPORT constexpr wpi::math::Pose3d	ObjectToRobotPose (const wpi::math::Pose3d &objectInField, const wpi::math::Transform3d &cameraToObject, const wpi::math::Transform3d &robotToCamera)
	Returns the robot's pose in the field coordinate system given an object's field-relative pose, the transformation from the camera's pose to the object's pose (obtained via computer vision), and the transformation from the robot's pose to the camera's pose.
template<int States, int Inputs>
bool	IsStabilizable (const Eigen::Matrix< double, States, States > &A, const Eigen::Matrix< double, States, Inputs > &B)
	Returns true if (A, B) is a stabilizable pair.
template WPILIB_DLLEXPORT bool	IsStabilizable< 1, 1 > (const Eigen::Matrix< double, 1, 1 > &A, const Eigen::Matrix< double, 1, 1 > &B)
template WPILIB_DLLEXPORT bool	IsStabilizable< 2, 1 > (const Eigen::Matrix< double, 2, 2 > &A, const Eigen::Matrix< double, 2, 1 > &B)
template WPILIB_DLLEXPORT bool	IsStabilizable< Eigen::Dynamic, Eigen::Dynamic > (const Eigen::MatrixXd &A, const Eigen::MatrixXd &B)
template<int States, int Outputs>
bool	IsDetectable (const Eigen::Matrix< double, States, States > &A, const Eigen::Matrix< double, Outputs, States > &C)
	Returns true if (A, C) is a detectable pair.
template WPILIB_DLLEXPORT bool	IsDetectable< Eigen::Dynamic, Eigen::Dynamic > (const Eigen::MatrixXd &A, const Eigen::MatrixXd &C)
template<int CovDim, int NumSigmas>
std::tuple< Vectord< CovDim >, Matrixd< CovDim, CovDim > >	SquareRootUnscentedTransform (const Matrixd< CovDim, NumSigmas > &sigmas, const Vectord< NumSigmas > &Wm, const Vectord< NumSigmas > &Wc, std::function< Vectord< CovDim >(const Matrixd< CovDim, NumSigmas > &, const Vectord< NumSigmas > &)> meanFunc, std::function< Vectord< CovDim >(const Vectord< CovDim > &, const Vectord< CovDim > &)> residualFunc, const Matrixd< CovDim, CovDim > &squareRootR)
	Computes unscented transform of a set of sigma points and weights.
template<int Rows, int Cols, typename F>
auto	NumericalJacobian (F &&f, const Vectord< Cols > &x)
	Returns numerical Jacobian with respect to x for f(x).
template<typename F>
Eigen::MatrixXd	NumericalJacobian (F &&f, const Eigen::VectorXd &x)
	Returns numerical Jacobian with respect to x for f(x).
template<int Rows, int States, int Inputs, typename F, typename... Args>
auto	NumericalJacobianX (F &&f, const Vectord< States > &x, const Vectord< Inputs > &u, Args &&... args)
	Returns numerical Jacobian with respect to x for f(x, u, ...).
template<typename F, typename... Args>
auto	NumericalJacobianX (F &&f, const Eigen::VectorXd &x, const Eigen::VectorXd &u, Args &&... args)
	Returns numerical Jacobian with respect to x for f(x, u, ...).
template<int Rows, int States, int Inputs, typename F, typename... Args>
auto	NumericalJacobianU (F &&f, const Vectord< States > &x, const Vectord< Inputs > &u, Args &&... args)
	Returns numerical Jacobian with respect to u for f(x, u, ...).
template<typename F, typename... Args>
auto	NumericalJacobianU (F &&f, const Eigen::VectorXd &x, const Eigen::VectorXd &u, Args &&... args)
	Returns numerical Jacobian with respect to u for f(x, u, ...).
WPILIB_DLLEXPORT void	to_json (wpi::util::json &json, const Pose3d &pose)
WPILIB_DLLEXPORT void	from_json (const wpi::util::json &json, Pose3d &pose)
WPILIB_DLLEXPORT void	to_json (wpi::util::json &json, const Trajectory::State &state)
WPILIB_DLLEXPORT void	from_json (const wpi::util::json &json, Trajectory::State &state)

Typedef Documentation

◆ ct_matrix2d

using wpi::math::ct_matrix2d = ct_matrix<double, 2, 2>

◆ ct_matrix3d

using wpi::math::ct_matrix3d = ct_matrix<double, 3, 3>

◆ ct_row_vector

template<typename Scalar, int Cols>

using wpi::math::ct_row_vector = ct_matrix<Scalar, 1, Cols>

◆ ct_vector

template<typename Scalar, int Rows>

using wpi::math::ct_vector = ct_matrix<Scalar, Rows, 1>

◆ ct_vector2d

using wpi::math::ct_vector2d = ct_vector<double, 2>

◆ ct_vector3d

using wpi::math::ct_vector3d = ct_vector<double, 3>

◆ EKF

template<int States, int Inputs, int Outputs>

using wpi::math::EKF = ExtendedKalmanFilter<States, Inputs, Outputs>

◆ KF

template<int States, int Inputs, int Outputs>

using wpi::math::KF = KalmanFilter<States, Inputs, Outputs>

◆ LQR

template<int States, int Inputs>

using wpi::math::LQR = LinearQuadraticRegulator<States, Inputs>

◆ Matrixd

template<int Rows, int Cols, int Options = Eigen::AutoAlign | ((Rows == 1 && Cols != 1) ? Eigen::RowMajor : (Cols == 1 && Rows != 1) ? Eigen::ColMajor : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION), int MaxRows = Rows, int MaxCols = Cols>

using wpi::math::Matrixd = Eigen::Matrix<double, Rows, Cols, Options, MaxRows, MaxCols>

◆ MerweUKF

template<int States, int Inputs, int Outputs>

using wpi::math::MerweUKF

Initial value:

UnscentedKalmanFilter<States, Inputs, Outputs,

MerweScaledSigmaPoints<States>>

wpi::math::MerweScaledSigmaPoints

Generates sigma points and weights according to Van der Merwe's 2004 dissertation[1] for the Unscente...

Definition MerweScaledSigmaPoints.hpp:28

wpi::math::UnscentedKalmanFilter

A Kalman filter combines predictions from a model and measurements to give an estimate of the true sy...

Definition UnscentedKalmanFilter.hpp:62

◆ S3UKF

template<int States, int Inputs, int Outputs>

using wpi::math::S3UKF

Initial value:

UnscentedKalmanFilter<States, Inputs, Outputs, S3SigmaPoints<States>>

◆ SteadyStateKF

template<int States, int Inputs, int Outputs>

using wpi::math::SteadyStateKF = SteadyStateKalmanFilter<States, Inputs, Outputs>

◆ UKF

template<int States, int Inputs, int Outputs, SigmaPoints< States > SigmaPoints>

using wpi::math::UKF = UnscentedKalmanFilter<States, Inputs, Outputs, SigmaPoints>

◆ Vectord

template<int Size>

using wpi::math::Vectord = Eigen::Vector<double, Size>

Enumeration Type Documentation

◆ DAREError

enum class wpi::math::DAREError

strong

Errors the DARE solver can encounter.

Enumerator
QNotSymmetric	Q was not symmetric.
QNotPositiveSemidefinite	Q was not positive semidefinite.
RNotSymmetric	R was not symmetric.
RNotPositiveDefinite	R was not positive definite.
ABNotStabilizable	(A, B) pair was not stabilizable.
ACNotDetectable	(A, C) pair where Q = CᵀC was not detectable.

Function Documentation

◆ AngleAdd() [1/2]

template<int States>

Vectord< States > wpi::math::AngleAdd	(	const Vectord< States > &	a,
		const Vectord< States > &	b,
		int	angleStateIdx )

Adds a and b while normalizing the resulting value in the selected row as an angle.

Template Parameters

States Number of states.

Parameters

a	A vector to add with.
b	A vector to add with.
angleStateIdx	The row containing angles to be normalized.

◆ AngleAdd() [2/2]

template<int States>

std::function< Vectord< States >(const Vectord< States > &, const Vectord< States > &)> wpi::math::AngleAdd ( int angleStateIdx )

Returns a function that adds two vectors while normalizing the resulting value in the selected row as an angle.

Template Parameters

States Number of states.

Parameters

angleStateIdx The row containing angles to be normalized.

◆ AngleMean() [1/2]

template<int CovDim, int NumSigmas>

Vectord< CovDim > wpi::math::AngleMean	(	const Matrixd< CovDim, NumSigmas > &	sigmas,
		const Vectord< NumSigmas > &	Wm,
		int	angleStatesIdx )

Computes the mean of sigmas with the weights Wm while computing a special angle mean for a select row.

Template Parameters

CovDim	Dimension of covariance of sigma points after passing through the transform.
NumSigmas	Number of sigma points.

Parameters

sigmas	Sigma points.
Wm	Weights for the mean.
angleStatesIdx	The row containing the angles.

◆ AngleMean() [2/2]

template<int CovDim, int NumSigmas>

std::function< Vectord< CovDim >(const Matrixd< CovDim, NumSigmas > &, const Vectord< NumSigmas > &)> wpi::math::AngleMean ( int angleStateIdx )

Returns a function that computes the mean of sigmas with the weights Wm while computing a special angle mean for a select row.

Template Parameters

CovDim	Dimension of covariance of sigma points after passing through the transform.
NumSigmas	Number of sigma points.

Parameters

angleStateIdx The row containing the angles.

◆ AngleModulus()

WPILIB_DLLEXPORT constexpr wpi::units::radian_t wpi::math::AngleModulus ( wpi::units::radian_t angle )

constexpr

Wraps an angle to the range -π to π radians (-180 to 180 degrees).

Parameters

angle Angle to wrap.

◆ AngleResidual() [1/2]

template<int States>

Vectord< States > wpi::math::AngleResidual	(	const Vectord< States > &	a,
		const Vectord< States > &	b,
		int	angleStateIdx )

Subtracts a and b while normalizing the resulting value in the selected row as if it were an angle.

Template Parameters

States Number of states.

Parameters

a	A vector to subtract from.
b	A vector to subtract with.
angleStateIdx	The row containing angles to be normalized.

◆ AngleResidual() [2/2]

template<int States>

std::function< Vectord< States >(const Vectord< States > &, const Vectord< States > &)> wpi::math::AngleResidual ( int angleStateIdx )

Returns a function that subtracts two vectors while normalizing the resulting value in the selected row as if it were an angle.

Template Parameters

States Number of states.

Parameters

angleStateIdx The row containing angles to be normalized.

◆ ApplyDeadband() [1/2]

template<typename T, int N>
requires std::is_arithmetic_v<T> || wpi::units::traits::is_unit_t_v<T>

Eigen::Vector< T, N > wpi::math::ApplyDeadband	(	const Eigen::Vector< T, N > &	value,
		T	deadband,
		T	maxMagnitude = T{1.0} )

Returns a zero vector if the given vector is within the specified distance from the origin.

The remaining distance between the deadband and the maximum distance is scaled from the origin to the maximum distance.

Parameters

value	Value to clip.
deadband	Distance from origin.
maxMagnitude	The maximum distance from the origin of the input (defaults to 1). Can be infinite.

Returns: The value after the deadband is applied.

◆ ApplyDeadband() [2/2]

template<typename T>
requires std::is_arithmetic_v<T> || wpi::units::traits::is_unit_t_v<T>

T wpi::math::ApplyDeadband	(	T	value,
		T	deadband,
		T	maxMagnitude = T{1.0} )

constexpr

Returns 0.0 if the given value is within the specified range around zero.

The remaining range between the deadband and the maximum magnitude is scaled from 0.0 to the maximum magnitude.

Parameters

value	Value to clip.
deadband	Range around zero.
maxMagnitude	The maximum magnitude of the input (defaults to 1). Can be infinite.

Returns: The value after the deadband is applied.

◆ CopyDirectionPow() [1/2]

template<typename T, int N>
requires std::is_arithmetic_v<T> || wpi::units::traits::is_unit_t_v<T>

Eigen::Vector< T, N > wpi::math::CopyDirectionPow	(	const Eigen::Vector< T, N > &	value,
		double	exponent,
		T	maxMagnitude = T{1.0} )

Raises the norm of the input to the power of the given exponent while preserving its direction.

The function normalizes the input value to the range [0, 1] based on the maximum magnitude so that the output stays in the range.

This is useful for applying smoother or more aggressive control response curves (e.g. joystick input shaping).

Parameters

value	The input vector to transform.
exponent	The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be positive.
maxMagnitude	The maximum expected distance from origin of input (defaults to 1). Must be positive.

Returns: The transformed value with the same direction and norm scaled to the input range.

◆ CopyDirectionPow() [2/2]

template<typename T>
requires std::is_arithmetic_v<T> || wpi::units::traits::is_unit_t_v<T>

T wpi::math::CopyDirectionPow	(	T	value,
		double	exponent,
		T	maxMagnitude = T{1.0} )

constexpr

Raises the input to the power of the given exponent while preserving its sign.

The function normalizes the input value to the range [0, 1] based on the maximum magnitude so that the output stays in the range.

This is useful for applying smoother or more aggressive control response curves (e.g. joystick input shaping).

Parameters

value	The input value to transform.
exponent	The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be positive.
maxMagnitude	The maximum expected absolute value of input (defaults to 1). Must be positive.

Returns: The transformed value with the same sign and scaled to the input range.

◆ CostMatrix() [1/3]

template<size_t N>

Eigen::Matrix< double, N, N > wpi::math::CostMatrix ( const std::array< double, N > & costs )

constexpr

Creates a cost matrix from the given vector for use with LQR.

The cost matrix is constructed using Bryson's rule. The inverse square of each element in the input is placed on the cost matrix diagonal. If a tolerance is infinity, its cost matrix entry is set to zero.

Parameters

costs An array. For a Q matrix, its elements are the maximum allowed excursions of the states from the reference. For an R matrix, its elements are the maximum allowed excursions of the control inputs from no actuation.

Returns: State excursion or control effort cost matrix.

◆ CostMatrix() [2/3]

WPILIB_DLLEXPORT Eigen::MatrixXd wpi::math::CostMatrix ( const std::span< const double > costs )

Creates a cost matrix from the given vector for use with LQR.

The cost matrix is constructed using Bryson's rule. The inverse square of each element in the input is placed on the cost matrix diagonal. If a tolerance is infinity, its cost matrix entry is set to zero.

Parameters

costs A possibly variable length container. For a Q matrix, its elements are the maximum allowed excursions of the states from the reference. For an R matrix, its elements are the maximum allowed excursions of the control inputs from no actuation.

Returns: State excursion or control effort cost matrix.

◆ CostMatrix() [3/3]

template<std::same_as< double >... Ts>

Eigen::Matrix< double, sizeof...(Ts), sizeof...(Ts)> wpi::math::CostMatrix ( Ts... tolerances )

constexpr

Creates a cost matrix from the given vector for use with LQR.

The cost matrix is constructed using Bryson's rule. The inverse square of each tolerance is placed on the cost matrix diagonal. If a tolerance is infinity, its cost matrix entry is set to zero.

Parameters

tolerances An array. For a Q matrix, its elements are the maximum allowed excursions of the states from the reference. For an R matrix, its elements are the maximum allowed excursions of the control inputs from no actuation.

Returns: State excursion or control effort cost matrix.

◆ CovarianceMatrix() [1/3]

template<size_t N>

Eigen::Matrix< double, N, N > wpi::math::CovarianceMatrix ( const std::array< double, N > & stdDevs )

constexpr

Creates a covariance matrix from the given vector for use with Kalman filters.

Each element is squared and placed on the covariance matrix diagonal.

Parameters

stdDevs An array. For a Q matrix, its elements are the standard deviations of each state from how the model behaves. For an R matrix, its elements are the standard deviations for each output measurement.

Returns: Process noise or measurement noise covariance matrix.

◆ CovarianceMatrix() [2/3]

WPILIB_DLLEXPORT Eigen::MatrixXd wpi::math::CovarianceMatrix ( const std::span< const double > stdDevs )

Creates a covariance matrix from the given vector for use with Kalman filters.

Each element is squared and placed on the covariance matrix diagonal.

Parameters

stdDevs A possibly variable length container. For a Q matrix, its elements are the standard deviations of each state from how the model behaves. For an R matrix, its elements are the standard deviations for each output measurement.

Returns: Process noise or measurement noise covariance matrix.

◆ CovarianceMatrix() [3/3]

template<std::same_as< double >... Ts>

Eigen::Matrix< double, sizeof...(Ts), sizeof...(Ts)> wpi::math::CovarianceMatrix ( Ts... stdDevs )

constexpr

Creates a covariance matrix from the given vector for use with Kalman filters.

Each element is squared and placed on the covariance matrix diagonal.

Parameters

stdDevs An array. For a Q matrix, its elements are the standard deviations of each state from how the model behaves. For an R matrix, its elements are the standard deviations for each output measurement.

Returns: Process noise or measurement noise covariance matrix.

◆ ct_matrix()

template<typename Derived>
requires std::derived_from<Derived, Eigen::MatrixBase<Derived>>

wpi::math::ct_matrix ( const Derived & ) ->ct_matrix< typenameDerived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime >

◆ DARE() [1/2]

template<int States, int Inputs>

wpi::util::expected< Eigen::Matrix< double, States, States >, DAREError > wpi::math::DARE	(	const Eigen::Matrix< double, States, States > &	A,
		const Eigen::Matrix< double, States, Inputs > &	B,
		const Eigen::Matrix< double, States, States > &	Q,
		const Eigen::Matrix< double, Inputs, Inputs > &	R,
		bool	checkPreconditions = true )

Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation:

AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0

Template Parameters

States	Number of states.
Inputs	Number of inputs.

Parameters

A	The system matrix.
B	The input matrix.
Q	The state cost matrix.
R	The input cost matrix.
checkPreconditions	Whether to check preconditions (30% less time if user is sure precondtions are already met).

Returns: Solution to the DARE on success, or DAREError on failure.

◆ DARE() [2/2]

template<int States, int Inputs>

wpi::util::expected< Eigen::Matrix< double, States, States >, DAREError > wpi::math::DARE	(	const Eigen::Matrix< double, States, States > &	A,
		const Eigen::Matrix< double, States, Inputs > &	B,
		const Eigen::Matrix< double, States, States > &	Q,
		const Eigen::Matrix< double, Inputs, Inputs > &	R,
		const Eigen::Matrix< double, States, Inputs > &	N,
		bool	checkPreconditions = true )

Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation:

AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0

This is equivalent to solving the original DARE:

A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0

where A₂ and Q₂ are a change of variables:

A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ

This overload of the DARE is useful for finding the control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.

    ∞ [xₖ]ᵀ[Q  N][xₖ]
J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
   k=0

This is a more general form of the following. The linear-quadratic regulator is the feedback control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:

    ∞
J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
   k=0

This can be refactored as:

    ∞ [xₖ]ᵀ[Q 0][xₖ]
J = Σ [uₖ] [0 R][uₖ] ΔT
   k=0

Template Parameters

States	Number of states.
Inputs	Number of inputs.

Parameters

A	The system matrix.
B	The input matrix.
Q	The state cost matrix.
R	The input cost matrix.
N	The state-input cross cost matrix.
checkPreconditions	Whether to check preconditions (30% less time if user is sure precondtions are already met).

Returns: Solution to the DARE on success, or DAREError on failure.

◆ DesaturateInputVector()

template<int Inputs>

Eigen::Vector< double, Inputs > wpi::math::DesaturateInputVector	(	const Eigen::Vector< double, Inputs > &	u,
		double	maxMagnitude )

Renormalize all inputs if any exceeds the maximum magnitude.

Useful for systems such as differential drivetrains.

Template Parameters

Inputs Number of inputs.

Parameters

u	The input vector.
maxMagnitude	The maximum magnitude any input can have.

Returns: The normalizedInput

◆ DesaturateInputVector< Eigen::Dynamic >()

template WPILIB_DLLEXPORT Eigen::VectorXd wpi::math::DesaturateInputVector< Eigen::Dynamic >	(	const Eigen::VectorXd &	u,
		double	maxMagnitude )

extern

◆ DiscretizeA()

template<int States>

void wpi::math::DiscretizeA	(	const Matrixd< States, States > &	contA,
		wpi::units::second_t	dt,
		Matrixd< States, States > *	discA )

Discretizes the given continuous A matrix.

Template Parameters

States Number of states.

Parameters

contA	Continuous system matrix.
dt	Discretization timestep.
discA	Storage for discrete system matrix.

◆ DiscretizeAB()

template<int States, int Inputs>

void wpi::math::DiscretizeAB	(	const Matrixd< States, States > &	contA,
		const Matrixd< States, Inputs > &	contB,
		wpi::units::second_t	dt,
		Matrixd< States, States > *	discA,
		Matrixd< States, Inputs > *	discB )

Discretizes the given continuous A and B matrices.

Template Parameters

States	Number of states.
Inputs	Number of inputs.

Parameters

contA	Continuous system matrix.
contB	Continuous input matrix.
dt	Discretization timestep.
discA	Storage for discrete system matrix.
discB	Storage for discrete input matrix.

◆ DiscretizeAQ()

template<int States>

void wpi::math::DiscretizeAQ	(	const Matrixd< States, States > &	contA,
		const Matrixd< States, States > &	contQ,
		wpi::units::second_t	dt,
		Matrixd< States, States > *	discA,
		Matrixd< States, States > *	discQ )

Discretizes the given continuous A and Q matrices.

Template Parameters

States Number of states.

Parameters

contA	Continuous system matrix.
contQ	Continuous process noise covariance matrix.
dt	Discretization timestep.
discA	Storage for discrete system matrix.
discQ	Storage for discrete process noise covariance matrix.

◆ DiscretizeR()

template<int Outputs>

Matrixd< Outputs, Outputs > wpi::math::DiscretizeR	(	const Matrixd< Outputs, Outputs > &	R,
		wpi::units::second_t	dt )

Returns a discretized version of the provided continuous measurement noise covariance matrix.

Template Parameters

Outputs Number of outputs.

Parameters

R	Continuous measurement noise covariance matrix.
dt	Discretization timestep.

◆ EXPORT_TEMPLATE_DECLARE()

template class wpi::math::EXPORT_TEMPLATE_DECLARE ( WPILIB_DLLEXPORT )

extern

◆ FloorDiv()

std::signed_integral auto wpi::math::FloorDiv	(	std::signed_integral auto	x,
		std::signed_integral auto	y )

constexpr

Returns the largest (closest to positive infinity) int value that is less than or equal to the algebraic quotient.

Parameters

x	the dividend
y	the divisor

Returns: the largest (closest to positive infinity) int value that is less than or equal to the algebraic quotient.

◆ FloorMod()

std::signed_integral auto wpi::math::FloorMod	(	std::signed_integral auto	x,
		std::signed_integral auto	y )

constexpr

Returns the floor modulus of the int arguments.

The floor modulus is r = x - (floorDiv(x, y) * y), has the same sign as the divisor y or is zero, and is in the range of -std::abs(y) < r < +std::abs(y).

Parameters

x	the dividend
y	the divisor

Returns: the floor modulus x - (floorDiv(x, y) * y)

◆ from_json() [1/8]

WPILIB_DLLEXPORT void wpi::math::from_json	(	const wpi::util::json &	json,
		Pose2d &	pose )

◆ from_json() [2/8]

WPILIB_DLLEXPORT void wpi::math::from_json	(	const wpi::util::json &	json,
		Pose3d &	pose )

◆ from_json() [3/8]

WPILIB_DLLEXPORT void wpi::math::from_json	(	const wpi::util::json &	json,
		Quaternion &	quaternion )

◆ from_json() [4/8]

WPILIB_DLLEXPORT void wpi::math::from_json	(	const wpi::util::json &	json,
		Rotation2d &	rotation )

◆ from_json() [5/8]

WPILIB_DLLEXPORT void wpi::math::from_json	(	const wpi::util::json &	json,
		Rotation3d &	rotation )

◆ from_json() [6/8]

WPILIB_DLLEXPORT void wpi::math::from_json	(	const wpi::util::json &	json,
		Trajectory::State &	state )

◆ from_json() [7/8]

WPILIB_DLLEXPORT void wpi::math::from_json	(	const wpi::util::json &	json,
		Translation2d &	state )

◆ from_json() [8/8]

WPILIB_DLLEXPORT void wpi::math::from_json	(	const wpi::util::json &	json,
		Translation3d &	state )

◆ InputModulus()

template<typename T>

T wpi::math::InputModulus	(	T	input,
		T	minimumInput,
		T	maximumInput )

constexpr

Returns modulus of input.

Parameters

input	Input value to wrap.
minimumInput	The minimum value expected from the input.
maximumInput	The maximum value expected from the input.

◆ IsDetectable()

template<int States, int Outputs>

bool wpi::math::IsDetectable	(	const Eigen::Matrix< double, States, States > &	A,
		const Eigen::Matrix< double, Outputs, States > &	C )

Returns true if (A, C) is a detectable pair.

(A, C) is detectable if and only if the unobservable eigenvalues of A, if any, have absolute values less than one, where an eigenvalue is unobservable if rank([λI - A; C]) < n where n is the number of states.

Template Parameters

States	Number of states.
Outputs	Number of outputs.

Parameters

A	System matrix.
C	Output matrix.

◆ IsDetectable< Eigen::Dynamic, Eigen::Dynamic >()

template WPILIB_DLLEXPORT bool wpi::math::IsDetectable< Eigen::Dynamic, Eigen::Dynamic >	(	const Eigen::MatrixXd &	A,
		const Eigen::MatrixXd &	C )

extern

◆ IsNear() [1/2]

template<typename T>
requires std::is_arithmetic_v<T> || wpi::units::traits::is_unit_t_v<T>

bool wpi::math::IsNear	(	T	expected,
		T	actual,
		T	tolerance )

constexpr

Checks if the given value matches an expected value within a certain tolerance.

Parameters

expected	The expected value
actual	The actual value
tolerance	The allowed difference between the actual and the expected value

Returns: Whether or not the actual value is within the allowed tolerance

◆ IsNear() [2/2]

template<typename T>
requires std::is_arithmetic_v<T> || wpi::units::traits::is_unit_t_v<T>

bool wpi::math::IsNear	(	T	expected,
		T	actual,
		T	tolerance,
		T	min,
		T	max )

constexpr

Checks if the given value matches an expected value within a certain tolerance.

Supports continuous input for cases like absolute encoders.

Continuous input means that the min and max value are considered to be the same point, and tolerances can be checked across them. A common example would be for absolute encoders: calling isNear(2, 359, 5, 0, 360) returns true because 359 is 1 away from 360 (which is treated as the same as 0) and 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the given tolerance of 5.

Parameters

expected	The expected value
actual	The actual value
tolerance	The allowed difference between the actual and the expected value
min	Smallest value before wrapping around to the largest value
max	Largest value before wrapping around to the smallest value

Returns: Whether or not the actual value is within the allowed tolerance

◆ IsStabilizable()

template<int States, int Inputs>

bool wpi::math::IsStabilizable	(	const Eigen::Matrix< double, States, States > &	A,
		const Eigen::Matrix< double, States, Inputs > &	B )

Returns true if (A, B) is a stabilizable pair.

(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have absolute values less than one, where an eigenvalue is uncontrollable if rank([λI - A, B]) < n where n is the number of states.

Template Parameters

States	Number of states.
Inputs	Number of inputs.

Parameters

A	System matrix.
B	Input matrix.

◆ IsStabilizable< 1, 1 >()

template WPILIB_DLLEXPORT bool wpi::math::IsStabilizable< 1, 1 >	(	const Eigen::Matrix< double, 1, 1 > &	A,
		const Eigen::Matrix< double, 1, 1 > &	B )

extern

◆ IsStabilizable< 2, 1 >()

template WPILIB_DLLEXPORT bool wpi::math::IsStabilizable< 2, 1 >	(	const Eigen::Matrix< double, 2, 2 > &	A,
		const Eigen::Matrix< double, 2, 1 > &	B )

extern

◆ IsStabilizable< Eigen::Dynamic, Eigen::Dynamic >()

template WPILIB_DLLEXPORT bool wpi::math::IsStabilizable< Eigen::Dynamic, Eigen::Dynamic >	(	const Eigen::MatrixXd &	A,
		const Eigen::MatrixXd &	B )

extern

◆ Normal() [1/3]

template<int N>

Eigen::Vector< double, N > wpi::math::Normal ( const std::array< double, N > & stdDevs )

Creates a vector of normally distributed random values with the given standard deviations for each element.

Parameters

stdDevs An array whose elements are the standard deviations of each element of the random vector.

Returns: Vector of normally distributed values.

◆ Normal() [2/3]

WPILIB_DLLEXPORT Eigen::VectorXd wpi::math::Normal ( const std::span< const double > stdDevs )

Creates a vector of normally distributed random values with the given standard deviations for each element.

Parameters

stdDevs A possibly variable length container whose elements are the standard deviations of each element of the random vector.

Returns: Vector of normally distributed values.

◆ Normal() [3/3]

template<std::same_as< double >... Ts>

Eigen::Vector< double, sizeof...(Ts)> wpi::math::Normal ( Ts... stdDevs )

Creates a vector of normally distributed random values with the given standard deviations for each element.

Parameters

stdDevs An array whose elements are the standard deviations of each element of the random vector.

Returns: Vector of normally distributed values.

◆ NumericalJacobian() [1/2]

template<typename F>

Eigen::MatrixXd wpi::math::NumericalJacobian	(	F &&	f,
		const Eigen::VectorXd &	x )

Returns numerical Jacobian with respect to x for f(x).

Parameters

f	Vector-valued function from which to compute Jacobian.
x	Vector argument.

◆ NumericalJacobian() [2/2]

template<int Rows, int Cols, typename F>

auto wpi::math::NumericalJacobian	(	F &&	f,
		const Vectord< Cols > &	x )

Returns numerical Jacobian with respect to x for f(x).

Template Parameters

Rows	Number of rows in result of f(x).
Cols	Number of columns in result of f(x).

Parameters

f	Vector-valued function from which to compute Jacobian.
x	Vector argument.

◆ NumericalJacobianU() [1/2]

template<typename F, typename... Args>

auto wpi::math::NumericalJacobianU	(	F &&	f,
		const Eigen::VectorXd &	x,
		const Eigen::VectorXd &	u,
		Args &&...	args )

Returns numerical Jacobian with respect to u for f(x, u, ...).

Template Parameters

F	Function object type.
Args...	Types of remaining arguments to f(x, u, ...).

Parameters

f	Vector-valued function from which to compute Jacobian.
x	State vector.
u	Input vector.
args	Remaining arguments to f(x, u, ...).

◆ NumericalJacobianU() [2/2]

template<int Rows, int States, int Inputs, typename F, typename... Args>

auto wpi::math::NumericalJacobianU	(	F &&	f,
		const Vectord< States > &	x,
		const Vectord< Inputs > &	u,
		Args &&...	args )

Returns numerical Jacobian with respect to u for f(x, u, ...).

Template Parameters

Rows	Number of rows in result of f(x, u, ...).
States	Number of rows in x.
Inputs	Number of rows in u.
F	Function object type.
Args...	Types of remaining arguments to f(x, u, ...).

Parameters

f	Vector-valued function from which to compute Jacobian.
x	State vector.
u	Input vector.
args	Remaining arguments to f(x, u, ...).

◆ NumericalJacobianX() [1/2]

template<typename F, typename... Args>

auto wpi::math::NumericalJacobianX	(	F &&	f,
		const Eigen::VectorXd &	x,
		const Eigen::VectorXd &	u,
		Args &&...	args )

Returns numerical Jacobian with respect to x for f(x, u, ...).

Template Parameters

F	Function object type.
Args...	Types of remaining arguments to f(x, u, ...).

Parameters

f	Vector-valued function from which to compute Jacobian.
x	State vector.
u	Input vector.
args	Remaining arguments to f(x, u, ...).

◆ NumericalJacobianX() [2/2]

template<int Rows, int States, int Inputs, typename F, typename... Args>

auto wpi::math::NumericalJacobianX	(	F &&	f,
		const Vectord< States > &	x,
		const Vectord< Inputs > &	u,
		Args &&...	args )

Returns numerical Jacobian with respect to x for f(x, u, ...).

Template Parameters

Rows	Number of rows in result of f(x, u, ...).
States	Number of rows in x.
Inputs	Number of rows in u.
F	Function object type.
Args...	Types of remaining arguments to f(x, u, ...).

Parameters

f	Vector-valued function from which to compute Jacobian.
x	State vector.
u	Input vector.
args	Remaining arguments to f(x, u, ...).

◆ ObjectToRobotPose()

WPILIB_DLLEXPORT constexpr wpi::math::Pose3d wpi::math::ObjectToRobotPose	(	const wpi::math::Pose3d &	objectInField,
		const wpi::math::Transform3d &	cameraToObject,
		const wpi::math::Transform3d &	robotToCamera )

constexpr

Returns the robot's pose in the field coordinate system given an object's field-relative pose, the transformation from the camera's pose to the object's pose (obtained via computer vision), and the transformation from the robot's pose to the camera's pose.

The object could be a target or a fiducial marker.

Parameters

objectInField	An object's field-relative pose.
cameraToObject	The transformation from the camera's pose to the object's pose. This comes from computer vision.
robotToCamera	The transformation from the robot's pose to the camera's pose. This can either be a constant for a rigidly mounted camera, or variable if the camera is mounted to a turret. If the camera was mounted 3 inches in front of the "origin" (usually physical center) of the robot, this would be wpi::math::Transform3d{3_in, 0_in, 0_in, wpi::math::Rotation3d{}}.

Returns: The robot's field-relative pose.

◆ RK4() [1/3]

template<typename F, typename T, typename U>

T wpi::math::RK4	(	F &&	f,
		T	x,
		U	u,
		wpi::units::second_t	dt )

Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.

Parameters

f	The function to integrate. It must take two arguments x and u.
x	The initial value of x.
u	The value u held constant over the integration period.
dt	The time over which to integrate.

◆ RK4() [2/3]

template<typename F, typename T>

T wpi::math::RK4	(	F &&	f,
		T	x,
		wpi::units::second_t	dt )

Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.

Parameters

f	The function to integrate. It must take one argument x.
x	The initial value of x.
dt	The time over which to integrate.

◆ RK4() [3/3]

template<typename F, typename T>

T wpi::math::RK4	(	F &&	f,
		wpi::units::second_t	t,
		T	y,
		wpi::units::second_t	dt )

Performs 4th order Runge-Kutta integration of dy/dt = f(t, y) for dt.

Parameters

f	The function to integrate. It must take two arguments t and y.
t	The initial value of t.
y	The initial value of y.
dt	The time over which to integrate.

◆ RKDP() [1/2]

template<typename F, typename T, typename U>

T wpi::math::RKDP	(	F &&	f,
		T	x,
		U	u,
		wpi::units::second_t	dt,
		double	maxError = 1e-6 )

Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt.

Parameters

f	The function to integrate. It must take two arguments x and u.
x	The initial value of x.
u	The value u held constant over the integration period.
dt	The time over which to integrate.
maxError	The maximum acceptable truncation error. Usually a small number like 1e-6.

◆ RKDP() [2/2]

template<typename F, typename T>

T wpi::math::RKDP	(	F &&	f,
		wpi::units::second_t	t,
		T	y,
		wpi::units::second_t	dt,
		double	maxError = 1e-6 )

Performs adaptive Dormand-Prince integration of dy/dt = f(t, y) for dt.

Parameters

f	The function to integrate. It must take two arguments t and y.
t	The initial value of t.
y	The initial value of y.
dt	The time over which to integrate.
maxError	The maximum acceptable truncation error. Usually a small number like 1e-6.

◆ SlewRateLimit() [1/2]

Translation2d wpi::math::SlewRateLimit	(	const Translation2d &	current,
		const Translation2d &	next,
		wpi::units::second_t	dt,
		wpi::units::meters_per_second_t	maxVelocity )

constexpr

Limits translation velocity.

Parameters

current	Translation at current timestep.
next	Translation at next timestep.
dt	Timestep duration.
maxVelocity	Maximum translation velocity.

Returns: Returns the next Translation2d limited to maxVelocity

◆ SlewRateLimit() [2/2]

Translation3d wpi::math::SlewRateLimit	(	const Translation3d &	current,
		const Translation3d &	next,
		wpi::units::second_t	dt,
		wpi::units::meters_per_second_t	maxVelocity )

constexpr

Limits translation velocity.

Parameters

current	Translation at current timestep.
next	Translation at next timestep.
dt	Timestep duration.
maxVelocity	Maximum translation velocity.

Returns: Returns the next Translation3d limited to maxVelocity

◆ SquareRootUnscentedTransform()

template<int CovDim, int NumSigmas>

std::tuple< Vectord< CovDim >, Matrixd< CovDim, CovDim > > wpi::math::SquareRootUnscentedTransform	(	const Matrixd< CovDim, NumSigmas > &	sigmas,
		const Vectord< NumSigmas > &	Wm,
		const Vectord< NumSigmas > &	Wc,
		std::function< Vectord< CovDim >(const Matrixd< CovDim, NumSigmas > &, const Vectord< NumSigmas > &)>	meanFunc,
		std::function< Vectord< CovDim >(const Vectord< CovDim > &, const Vectord< CovDim > &)>	residualFunc,
		const Matrixd< CovDim, CovDim > &	squareRootR )

Computes unscented transform of a set of sigma points and weights.

CovDim returns the mean and square-root covariance of the sigma points in a tuple.

This works in conjunction with the UnscentedKalmanFilter class. For use with square-root form UKFs.

Template Parameters

CovDim	Dimension of covariance of sigma points after passing through the transform.
NumSigmas	Number of sigma points.

Parameters

sigmas	List of sigma points.
Wm	Weights for the mean.
Wc	Weights for the covariance.
meanFunc	A function that computes the mean of NumSigmas state vectors using a given set of weights.
residualFunc	A function that computes the residual of two state vectors (i.e. it subtracts them.)
squareRootR	Square-root of the noise covariance of the sigma points.

Returns: Tuple of x, mean of sigma points; S, square-root covariance of sigmas.

◆ SwerveDriveKinematics()

template<typename ModuleTranslation, typename... ModuleTranslations>

wpi::math::SwerveDriveKinematics	(	ModuleTranslation	,
		ModuleTranslations...	)->SwerveDriveKinematics< 1+sizeof...(ModuleTranslations)>

◆ to_json() [1/8]

WPILIB_DLLEXPORT void wpi::math::to_json	(	wpi::util::json &	json,
		const Pose2d &	pose )

◆ to_json() [2/8]

WPILIB_DLLEXPORT void wpi::math::to_json	(	wpi::util::json &	json,
		const Pose3d &	pose )

◆ to_json() [3/8]

WPILIB_DLLEXPORT void wpi::math::to_json	(	wpi::util::json &	json,
		const Quaternion &	quaternion )

◆ to_json() [4/8]

WPILIB_DLLEXPORT void wpi::math::to_json	(	wpi::util::json &	json,
		const Rotation2d &	rotation )

◆ to_json() [5/8]

WPILIB_DLLEXPORT void wpi::math::to_json	(	wpi::util::json &	json,
		const Rotation3d &	rotation )

◆ to_json() [6/8]

WPILIB_DLLEXPORT void wpi::math::to_json	(	wpi::util::json &	json,
		const Trajectory::State &	state )

◆ to_json() [7/8]

WPILIB_DLLEXPORT void wpi::math::to_json	(	wpi::util::json &	json,
		const Translation2d &	state )

◆ to_json() [8/8]

WPILIB_DLLEXPORT void wpi::math::to_json	(	wpi::util::json &	json,
		const Translation3d &	state )

◆ to_string()

std::string_view wpi::math::to_string ( const DAREError & error )

constexpr

Converts the given DAREError enum to a string.

Namespaces

Classes

Concepts

Typedefs

Enumerations

Functions

Typedef Documentation

◆ ct_matrix2d

◆ ct_matrix3d

◆ ct_row_vector

◆ ct_vector

◆ ct_vector2d

◆ ct_vector3d

◆ EKF

◆ KF

◆ LQR

◆ Matrixd

◆ MerweUKF

◆ S3UKF

◆ SteadyStateKF

◆ UKF

◆ Vectord

Enumeration Type Documentation

◆ DAREError

Function Documentation

◆ AngleAdd() [1/2]

◆ AngleAdd() [2/2]

◆ AngleMean() [1/2]

◆ AngleMean() [2/2]

◆ AngleModulus()

◆ AngleResidual() [1/2]

◆ AngleResidual() [2/2]

◆ ApplyDeadband() [1/2]

◆ ApplyDeadband() [2/2]

◆ CopyDirectionPow() [1/2]

◆ CopyDirectionPow() [2/2]

◆ CostMatrix() [1/3]

◆ CostMatrix() [2/3]

◆ CostMatrix() [3/3]

◆ CovarianceMatrix() [1/3]

◆ CovarianceMatrix() [2/3]

◆ CovarianceMatrix() [3/3]

◆ ct_matrix()

◆ DARE() [1/2]

◆ DARE() [2/2]

◆ DesaturateInputVector()

◆ DesaturateInputVector< Eigen::Dynamic >()

◆ DiscretizeA()

◆ DiscretizeAB()

◆ DiscretizeAQ()

◆ DiscretizeR()

◆ EXPORT_TEMPLATE_DECLARE()

◆ FloorDiv()

◆ FloorMod()

◆ from_json() [1/8]

◆ from_json() [2/8]

◆ from_json() [3/8]

◆ from_json() [4/8]

◆ from_json() [5/8]

◆ from_json() [6/8]

◆ from_json() [7/8]

◆ from_json() [8/8]

◆ InputModulus()

◆ IsDetectable()

◆ IsDetectable< Eigen::Dynamic, Eigen::Dynamic >()

◆ IsNear() [1/2]

◆ IsNear() [2/2]

◆ IsStabilizable()

◆ IsStabilizable< 1, 1 >()

◆ IsStabilizable< 2, 1 >()

◆ IsStabilizable< Eigen::Dynamic, Eigen::Dynamic >()

◆ Normal() [1/3]

◆ Normal() [2/3]

◆ Normal() [3/3]

◆ NumericalJacobian() [1/2]

◆ NumericalJacobian() [2/2]

◆ NumericalJacobianU() [1/2]

◆ NumericalJacobianU() [2/2]

◆ NumericalJacobianX() [1/2]

◆ NumericalJacobianX() [2/2]