WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
StateSpaceUtil.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <array>
8#include <cmath>
9#include <concepts>
10#include <limits>
11#include <random>
12
13#include <Eigen/Eigenvalues>
14#include <Eigen/QR>
15#include <wpi/Algorithm.h>
16#include <wpi/SymbolExports.h>
17
18#include "frc/EigenCore.h"
19#include "frc/geometry/Pose2d.h"
20
21namespace frc {
22
23/**
24 * Creates a cost matrix from the given vector for use with LQR.
25 *
26 * The cost matrix is constructed using Bryson's rule. The inverse square of
27 * each tolerance is placed on the cost matrix diagonal. If a tolerance is
28 * infinity, its cost matrix entry is set to zero.
29 *
30 * @param tolerances An array. For a Q matrix, its elements are the maximum
31 * allowed excursions of the states from the reference. For an
32 * R matrix, its elements are the maximum allowed excursions
33 * of the control inputs from no actuation.
34 * @return State excursion or control effort cost matrix.
35 */
36template <std::same_as<double>... Ts>
37constexpr Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
38 Ts... tolerances) {
39 Matrixd<sizeof...(Ts), sizeof...(Ts)> result;
40
41 for (int row = 0; row < result.rows(); ++row) {
42 for (int col = 0; col < result.cols(); ++col) {
43 if (row != col) {
44 result(row, col) = 0.0;
45 }
46 }
47 }
48
50 [&](int i, double tolerance) {
51 if (tolerance == std::numeric_limits<double>::infinity()) {
52 result(i, i) = 0.0;
53 } else {
54 result(i, i) = 1.0 / (tolerance * tolerance);
55 }
56 },
57 tolerances...);
58
59 return result;
60}
61
62/**
63 * Creates a covariance matrix from the given vector for use with Kalman
64 * filters.
65 *
66 * Each element is squared and placed on the covariance matrix diagonal.
67 *
68 * @param stdDevs An array. For a Q matrix, its elements are the standard
69 * deviations of each state from how the model behaves. For an R
70 * matrix, its elements are the standard deviations for each
71 * output measurement.
72 * @return Process noise or measurement noise covariance matrix.
73 */
74template <std::same_as<double>... Ts>
75constexpr Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
76 Matrixd<sizeof...(Ts), sizeof...(Ts)> result;
77
78 for (int row = 0; row < result.rows(); ++row) {
79 for (int col = 0; col < result.cols(); ++col) {
80 if (row != col) {
81 result(row, col) = 0.0;
82 }
83 }
84 }
85
86 wpi::for_each([&](int i, double stdDev) { result(i, i) = stdDev * stdDev; },
87 stdDevs...);
88
89 return result;
90}
91
92/**
93 * Creates a cost matrix from the given vector for use with LQR.
94 *
95 * The cost matrix is constructed using Bryson's rule. The inverse square of
96 * each element in the input is placed on the cost matrix diagonal. If a
97 * tolerance is infinity, its cost matrix entry is set to zero.
98 *
99 * @param costs An array. For a Q matrix, its elements are the maximum allowed
100 * excursions of the states from the reference. For an R matrix,
101 * its elements are the maximum allowed excursions of the control
102 * inputs from no actuation.
103 * @return State excursion or control effort cost matrix.
104 */
105template <size_t N>
106constexpr Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
107 Matrixd<N, N> result;
108
109 for (int row = 0; row < result.rows(); ++row) {
110 for (int col = 0; col < result.cols(); ++col) {
111 if (row == col) {
112 if (costs[row] == std::numeric_limits<double>::infinity()) {
113 result(row, col) = 0.0;
114 } else {
115 result(row, col) = 1.0 / (costs[row] * costs[row]);
116 }
117 } else {
118 result(row, col) = 0.0;
119 }
120 }
121 }
122
123 return result;
124}
125
126/**
127 * Creates a covariance matrix from the given vector for use with Kalman
128 * filters.
129 *
130 * Each element is squared and placed on the covariance matrix diagonal.
131 *
132 * @param stdDevs An array. For a Q matrix, its elements are the standard
133 * deviations of each state from how the model behaves. For an R
134 * matrix, its elements are the standard deviations for each
135 * output measurement.
136 * @return Process noise or measurement noise covariance matrix.
137 */
138template <size_t N>
139constexpr Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
140 Matrixd<N, N> result;
141
142 for (int row = 0; row < result.rows(); ++row) {
143 for (int col = 0; col < result.cols(); ++col) {
144 if (row == col) {
145 result(row, col) = stdDevs[row] * stdDevs[row];
146 } else {
147 result(row, col) = 0.0;
148 }
149 }
150 }
151
152 return result;
153}
154
155template <std::same_as<double>... Ts>
156Vectord<sizeof...(Ts)> MakeWhiteNoiseVector(Ts... stdDevs) {
157 std::random_device rd;
158 std::mt19937 gen{rd()};
159
160 Vectord<sizeof...(Ts)> result;
162 [&](int i, double stdDev) {
163 // Passing a standard deviation of 0.0 to std::normal_distribution is
164 // undefined behavior
165 if (stdDev == 0.0) {
166 result(i) = 0.0;
167 } else {
168 std::normal_distribution distr{0.0, stdDev};
169 result(i) = distr(gen);
170 }
171 },
172 stdDevs...);
173 return result;
174}
175
176/**
177 * Creates a vector of normally distributed white noise with the given noise
178 * intensities for each element.
179 *
180 * @param stdDevs An array whose elements are the standard deviations of each
181 * element of the noise vector.
182 * @return White noise vector.
183 */
184template <int N>
185Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
186 std::random_device rd;
187 std::mt19937 gen{rd()};
188
189 Vectord<N> result;
190 for (size_t i = 0; i < stdDevs.size(); ++i) {
191 // Passing a standard deviation of 0.0 to std::normal_distribution is
192 // undefined behavior
193 if (stdDevs[i] == 0.0) {
194 result(i) = 0.0;
195 } else {
196 std::normal_distribution distr{0.0, stdDevs[i]};
197 result(i) = distr(gen);
198 }
199 }
200 return result;
201}
202
203/**
204 * Converts a Pose2d into a vector of [x, y, theta].
205 *
206 * @param pose The pose that is being represented.
207 *
208 * @return The vector.
209 * @deprecated Create the vector manually instead. If you were using this as an
210 * intermediate step for constructing affine transformations, use
211 * Pose2d.ToMatrix() instead.
212 */
213[[deprecated("Use Pose2d.ToMatrix() instead.")]]
214WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
215 return Eigen::Vector3d{{pose.Translation().X().value(),
216 pose.Translation().Y().value(),
217 pose.Rotation().Radians().value()}};
218}
219
220/**
221 * Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
222 *
223 * @param pose The pose that is being represented.
224 *
225 * @return The vector.
226 * @deprecated Create the vector manually instead. If you were using this as an
227 * intermediate step for constructing affine transformations, use
228 * Pose2d.ToMatrix() instead.
229 */
230[[deprecated("Use Pose2d.ToMatrix() instead.")]]
231WPILIB_DLLEXPORT constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
232 return Eigen::Vector4d{{pose.Translation().X().value(),
233 pose.Translation().Y().value(), pose.Rotation().Cos(),
234 pose.Rotation().Sin()}};
235}
236
237/**
238 * Returns true if (A, B) is a stabilizable pair.
239 *
240 * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
241 * any, have absolute values less than one, where an eigenvalue is
242 * uncontrollable if rank([λI - A, B]) < n where n is the number of states.
243 *
244 * @tparam States Number of states.
245 * @tparam Inputs Number of inputs.
246 * @param A System matrix.
247 * @param B Input matrix.
248 */
249template <int States, int Inputs>
251 const Matrixd<States, Inputs>& B) {
252 Eigen::EigenSolver<Matrixd<States, States>> es{A, false};
253
254 for (int i = 0; i < A.rows(); ++i) {
255 if (std::norm(es.eigenvalues()[i]) < 1) {
256 continue;
257 }
258
259 if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) {
260 Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
261 E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
262 States>::Identity() -
263 A,
264 B;
265
266 Eigen::ColPivHouseholderQR<
267 Eigen::Matrix<std::complex<double>, States, States + Inputs>>
268 qr{E};
269 if (qr.rank() < States) {
270 return false;
271 }
272 } else {
273 Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
274 E << es.eigenvalues()[i] *
275 Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
276 A,
277 B;
278
279 Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
280 if (qr.rank() < A.rows()) {
281 return false;
282 }
283 }
284 }
285 return true;
286}
287
289 const Matrixd<1, 1>& A, const Matrixd<1, 1>& B);
291 const Matrixd<2, 2>& A, const Matrixd<2, 1>& B);
292extern template WPILIB_DLLEXPORT bool
294 const Eigen::MatrixXd& B);
295
296/**
297 * Returns true if (A, C) is a detectable pair.
298 *
299 * (A, C) is detectable if and only if the unobservable eigenvalues of A, if
300 * any, have absolute values less than one, where an eigenvalue is unobservable
301 * if rank([λI - A; C]) < n where n is the number of states.
302 *
303 * @tparam States Number of states.
304 * @tparam Outputs Number of outputs.
305 * @param A System matrix.
306 * @param C Output matrix.
307 */
308template <int States, int Outputs>
310 const Matrixd<Outputs, States>& C) {
311 return IsStabilizable<States, Outputs>(A.transpose(), C.transpose());
312}
313
314/**
315 * Converts a Pose2d into a vector of [x, y, theta].
316 *
317 * @param pose The pose that is being represented.
318 *
319 * @return The vector.
320 * @deprecated Create the vector manually instead. If you were using this as an
321 * intermediate step for constructing affine transformations, use
322 * Pose2d.ToMatrix() instead.
323 */
324[[deprecated("Use Pose2d.ToMatrix() instead.")]]
325WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) {
326 return Eigen::Vector3d{
327 {pose.X().value(), pose.Y().value(), pose.Rotation().Radians().value()}};
328}
329
330/**
331 * Clamps input vector between system's minimum and maximum allowable input.
332 *
333 * @tparam Inputs Number of inputs.
334 * @param u Input vector to clamp.
335 * @param umin The minimum input magnitude.
336 * @param umax The maximum input magnitude.
337 * @return Clamped input vector.
338 */
339template <int Inputs>
341 const Vectord<Inputs>& umin,
342 const Vectord<Inputs>& umax) {
343 Vectord<Inputs> result;
344 for (int i = 0; i < Inputs; ++i) {
345 result(i) = std::clamp(u(i), umin(i), umax(i));
346 }
347 return result;
348}
349
350/**
351 * Renormalize all inputs if any exceeds the maximum magnitude. Useful for
352 * systems such as differential drivetrains.
353 *
354 * @tparam Inputs Number of inputs.
355 * @param u The input vector.
356 * @param maxMagnitude The maximum magnitude any input can have.
357 * @return The normalizedInput
358 */
359template <int Inputs>
361 double maxMagnitude) {
362 double maxValue = u.template lpNorm<Eigen::Infinity>();
363
364 if (maxValue > maxMagnitude) {
365 return u * maxMagnitude / maxValue;
366 }
367 return u;
368}
369} // namespace frc
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
Represents a 2D pose containing translational and rotational elements.
Definition Pose2d.h:28
constexpr const Rotation2d & Rotation() const
Returns the underlying rotation.
Definition Pose2d.h:128
constexpr units::meter_t Y() const
Returns the Y component of the pose's translation.
Definition Pose2d.h:121
constexpr units::meter_t X() const
Returns the X component of the pose's translation.
Definition Pose2d.h:114
constexpr const Translation2d & Translation() const
Returns the underlying translation.
Definition Pose2d.h:107
constexpr double Cos() const
Returns the cosine of the rotation.
Definition Rotation2d.h:231
constexpr double Sin() const
Returns the sine of the rotation.
Definition Rotation2d.h:238
constexpr units::radian_t Radians() const
Returns the radian value of the rotation.
Definition Rotation2d.h:216
constexpr units::meter_t X() const
Returns the X component of the translation.
Definition Translation2d.h:83
constexpr units::meter_t Y() const
Returns the Y component of the translation.
Definition Translation2d.h:90
Definition CAN.h:11
WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d &pose)
Converts a Pose2d into a vector of [x, y, theta].
Definition StateSpaceUtil.h:214
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition EigenCore.h:21
template WPILIB_DLLEXPORT bool IsStabilizable< 2, 1 >(const Matrixd< 2, 2 > &A, const Matrixd< 2, 1 > &B)
Eigen::Vector< double, Size > Vectord
Definition EigenCore.h:12
template WPILIB_DLLEXPORT bool IsStabilizable< Eigen::Dynamic, Eigen::Dynamic >(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B)
bool IsStabilizable(const Matrixd< States, States > &A, const Matrixd< States, Inputs > &B)
Returns true if (A, B) is a stabilizable pair.
Definition StateSpaceUtil.h:250
constexpr Vectord< Inputs > ClampInputMaxMagnitude(const Vectord< Inputs > &u, const Vectord< Inputs > &umin, const Vectord< Inputs > &umax)
Clamps input vector between system's minimum and maximum allowable input.
Definition StateSpaceUtil.h:340
Vectord< sizeof...(Ts)> MakeWhiteNoiseVector(Ts... stdDevs)
Definition StateSpaceUtil.h:156
constexpr Matrixd< sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs)
Creates a covariance matrix from the given vector for use with Kalman filters.
Definition StateSpaceUtil.h:75
bool IsDetectable(const Matrixd< States, States > &A, const Matrixd< Outputs, States > &C)
Returns true if (A, C) is a detectable pair.
Definition StateSpaceUtil.h:309
template WPILIB_DLLEXPORT bool IsStabilizable< 1, 1 >(const Matrixd< 1, 1 > &A, const Matrixd< 1, 1 > &B)
WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseToVector(const Pose2d &pose)
Converts a Pose2d into a vector of [x, y, theta].
Definition StateSpaceUtil.h:325
WPILIB_DLLEXPORT constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d &pose)
Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
Definition StateSpaceUtil.h:231
Vectord< Inputs > DesaturateInputVector(const Vectord< Inputs > &u, double maxMagnitude)
Renormalize all inputs if any exceeds the maximum magnitude.
Definition StateSpaceUtil.h:360
constexpr Matrixd< sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances)
Creates a cost matrix from the given vector for use with LQR.
Definition StateSpaceUtil.h:37
constexpr void for_each(F &&f, Ts &&... elems)
Calls f(i, elem) for each element of elems where i is the index of the element in elems and elem is t...
Definition Algorithm.h:29