WPILibC++ 2024.3.2
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>
37Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
38 Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
39 auto& diag = result.diagonal();
41 [&](int i, double tolerance) {
42 if (tolerance == std::numeric_limits<double>::infinity()) {
43 diag(i) = 0.0;
44 } else {
45 diag(i) = 1.0 / std::pow(tolerance, 2);
46 }
47 },
48 tolerances...);
49 return result;
50}
51
52/**
53 * Creates a covariance matrix from the given vector for use with Kalman
54 * filters.
55 *
56 * Each element is squared and placed on the covariance matrix diagonal.
57 *
58 * @param stdDevs An array. For a Q matrix, its elements are the standard
59 * deviations of each state from how the model behaves. For an R
60 * matrix, its elements are the standard deviations for each
61 * output measurement.
62 * @return Process noise or measurement noise covariance matrix.
63 */
64template <std::same_as<double>... Ts>
65Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
66 Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
67 auto& diag = result.diagonal();
68 wpi::for_each([&](int i, double stdDev) { diag(i) = std::pow(stdDev, 2); },
69 stdDevs...);
70 return result;
71}
72
73/**
74 * Creates a cost matrix from the given vector for use with LQR.
75 *
76 * The cost matrix is constructed using Bryson's rule. The inverse square of
77 * each element in the input is placed on the cost matrix diagonal. If a
78 * tolerance is infinity, its cost matrix entry is set to zero.
79 *
80 * @param costs An array. For a Q matrix, its elements are the maximum allowed
81 * excursions of the states from the reference. For an R matrix,
82 * its elements are the maximum allowed excursions of the control
83 * inputs from no actuation.
84 * @return State excursion or control effort cost matrix.
85 */
86template <size_t N>
87Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
88 Eigen::DiagonalMatrix<double, N> result;
89 auto& diag = result.diagonal();
90 for (size_t i = 0; i < costs.size(); ++i) {
91 if (costs[i] == std::numeric_limits<double>::infinity()) {
92 diag(i) = 0.0;
93 } else {
94 diag(i) = 1.0 / std::pow(costs[i], 2);
95 }
96 }
97 return result;
98}
99
100/**
101 * Creates a covariance matrix from the given vector for use with Kalman
102 * filters.
103 *
104 * Each element is squared and placed on the covariance matrix diagonal.
105 *
106 * @param stdDevs An array. For a Q matrix, its elements are the standard
107 * deviations of each state from how the model behaves. For an R
108 * matrix, its elements are the standard deviations for each
109 * output measurement.
110 * @return Process noise or measurement noise covariance matrix.
111 */
112template <size_t N>
113Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
114 Eigen::DiagonalMatrix<double, N> result;
115 auto& diag = result.diagonal();
116 for (size_t i = 0; i < N; ++i) {
117 diag(i) = std::pow(stdDevs[i], 2);
118 }
119 return result;
120}
121
122template <std::same_as<double>... Ts>
123Vectord<sizeof...(Ts)> MakeWhiteNoiseVector(Ts... stdDevs) {
124 std::random_device rd;
125 std::mt19937 gen{rd()};
126
127 Vectord<sizeof...(Ts)> result;
129 [&](int i, double stdDev) {
130 // Passing a standard deviation of 0.0 to std::normal_distribution is
131 // undefined behavior
132 if (stdDev == 0.0) {
133 result(i) = 0.0;
134 } else {
135 std::normal_distribution distr{0.0, stdDev};
136 result(i) = distr(gen);
137 }
138 },
139 stdDevs...);
140 return result;
141}
142
143/**
144 * Creates a vector of normally distributed white noise with the given noise
145 * intensities for each element.
146 *
147 * @param stdDevs An array whose elements are the standard deviations of each
148 * element of the noise vector.
149 * @return White noise vector.
150 */
151template <int N>
152Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
153 std::random_device rd;
154 std::mt19937 gen{rd()};
155
156 Vectord<N> result;
157 for (size_t i = 0; i < stdDevs.size(); ++i) {
158 // Passing a standard deviation of 0.0 to std::normal_distribution is
159 // undefined behavior
160 if (stdDevs[i] == 0.0) {
161 result(i) = 0.0;
162 } else {
163 std::normal_distribution distr{0.0, stdDevs[i]};
164 result(i) = distr(gen);
165 }
166 }
167 return result;
168}
169
170/**
171 * Converts a Pose2d into a vector of [x, y, theta].
172 *
173 * @param pose The pose that is being represented.
174 *
175 * @return The vector.
176 */
178Eigen::Vector3d PoseTo3dVector(const Pose2d& pose);
179
180/**
181 * Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
182 *
183 * @param pose The pose that is being represented.
184 *
185 * @return The vector.
186 */
188Eigen::Vector4d PoseTo4dVector(const Pose2d& pose);
189
190/**
191 * Returns true if (A, B) is a stabilizable pair.
192 *
193 * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
194 * any, have absolute values less than one, where an eigenvalue is
195 * uncontrollable if rank([λI - A, B]) < n where n is the number of states.
196 *
197 * @tparam States Number of states.
198 * @tparam Inputs Number of inputs.
199 * @param A System matrix.
200 * @param B Input matrix.
201 */
202template <int States, int Inputs>
204 const Matrixd<States, Inputs>& B) {
205 Eigen::EigenSolver<Matrixd<States, States>> es{A, false};
206
207 for (int i = 0; i < A.rows(); ++i) {
208 if (std::norm(es.eigenvalues()[i]) < 1) {
209 continue;
210 }
211
212 if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) {
213 Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
214 E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
215 States>::Identity() -
216 A,
217 B;
218
219 Eigen::ColPivHouseholderQR<
220 Eigen::Matrix<std::complex<double>, States, States + Inputs>>
221 qr{E};
222 if (qr.rank() < States) {
223 return false;
224 }
225 } else {
226 Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
227 E << es.eigenvalues()[i] *
228 Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
229 A,
230 B;
231
232 Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
233 if (qr.rank() < A.rows()) {
234 return false;
235 }
236 }
237 }
238 return true;
239}
240
242 const Matrixd<1, 1>& A, const Matrixd<1, 1>& B);
244 const Matrixd<2, 2>& A, const Matrixd<2, 1>& B);
245extern template WPILIB_DLLEXPORT bool
246IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
247 const Eigen::MatrixXd& B);
248
249/**
250 * Returns true if (A, C) is a detectable pair.
251 *
252 * (A, C) is detectable if and only if the unobservable eigenvalues of A, if
253 * any, have absolute values less than one, where an eigenvalue is unobservable
254 * if rank([λI - A; C]) < n where n is the number of states.
255 *
256 * @tparam States Number of states.
257 * @tparam Outputs Number of outputs.
258 * @param A System matrix.
259 * @param C Output matrix.
260 */
261template <int States, int Outputs>
263 const Matrixd<Outputs, States>& C) {
264 return IsStabilizable<States, Outputs>(A.transpose(), C.transpose());
265}
266
267/**
268 * Converts a Pose2d into a vector of [x, y, theta].
269 *
270 * @param pose The pose that is being represented.
271 *
272 * @return The vector.
273 */
275Eigen::Vector3d PoseToVector(const Pose2d& pose);
276
277/**
278 * Clamps input vector between system's minimum and maximum allowable input.
279 *
280 * @tparam Inputs Number of inputs.
281 * @param u Input vector to clamp.
282 * @param umin The minimum input magnitude.
283 * @param umax The maximum input magnitude.
284 * @return Clamped input vector.
285 */
286template <int Inputs>
288 const Vectord<Inputs>& umin,
289 const Vectord<Inputs>& umax) {
290 Vectord<Inputs> result;
291 for (int i = 0; i < Inputs; ++i) {
292 result(i) = std::clamp(u(i), umin(i), umax(i));
293 }
294 return result;
295}
296
297/**
298 * Renormalize all inputs if any exceeds the maximum magnitude. Useful for
299 * systems such as differential drivetrains.
300 *
301 * @tparam Inputs Number of inputs.
302 * @param u The input vector.
303 * @param maxMagnitude The maximum magnitude any input can have.
304 * @return The normalizedInput
305 */
306template <int Inputs>
308 double maxMagnitude) {
309 double maxValue = u.template lpNorm<Eigen::Infinity>();
310
311 if (maxValue > maxMagnitude) {
312 return u * maxMagnitude / maxValue;
313 }
314 return u;
315}
316} // namespace frc
#define WPILIB_DLLEXPORT
Definition: SymbolExports.h:36
Represents a 2D pose containing translational and rotational elements.
Definition: Pose2d.h:23
Definition: AprilTagPoseEstimator.h:15
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition: EigenCore.h:21
WPILIB_DLLEXPORT Eigen::Vector3d PoseToVector(const Pose2d &pose)
Converts a Pose2d into a vector of [x, y, theta].
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12
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:287
Matrixd< sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs)
Creates a covariance matrix from the given vector for use with Kalman filters.
Definition: StateSpaceUtil.h:65
template WPILIB_DLLEXPORT bool IsStabilizable< 2, 1 >(const Matrixd< 2, 2 > &A, const Matrixd< 2, 1 > &B)
WPILIB_DLLEXPORT Eigen::Vector3d PoseTo3dVector(const Pose2d &pose)
Converts a Pose2d into a vector of [x, y, theta].
bool IsStabilizable(const Matrixd< States, States > &A, const Matrixd< States, Inputs > &B)
Returns true if (A, B) is a stabilizable pair.
Definition: StateSpaceUtil.h:203
Vectord< sizeof...(Ts)> MakeWhiteNoiseVector(Ts... stdDevs)
Definition: StateSpaceUtil.h:123
bool IsDetectable(const Matrixd< States, States > &A, const Matrixd< Outputs, States > &C)
Returns true if (A, C) is a detectable pair.
Definition: StateSpaceUtil.h:262
template WPILIB_DLLEXPORT bool IsStabilizable< 1, 1 >(const Matrixd< 1, 1 > &A, const Matrixd< 1, 1 > &B)
Vectord< Inputs > DesaturateInputVector(const Vectord< Inputs > &u, double maxMagnitude)
Renormalize all inputs if any exceeds the maximum magnitude.
Definition: StateSpaceUtil.h:307
Matrixd< sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances)
Creates a cost matrix from the given vector for use with LQR.
Definition: StateSpaceUtil.h:37
WPILIB_DLLEXPORT Eigen::Vector4d PoseTo4dVector(const Pose2d &pose)
Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
auto pow(const UnitType &value) noexcept -> unit_t< typename units::detail::power_of_unit< power, typename units::traits::unit_t_traits< UnitType >::unit_type >::type, typename units::traits::unit_t_traits< UnitType >::underlying_type, linear_scale >
computes the value of value raised to the power
Definition: base.h:2806
compound_unit< energy::joules, inverse< mass::kilogram > > rd