mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[docs] Set Doxygen extract_all to true and fix Doxygen failures (#3695)
The template argument order for UnscentedTransform was reversed to match all the other UKF classes. Since UnscentedTransform is intended as a class for internal use only, this shouldn't cause much breakage.
This commit is contained in:
Tyler Veness
@@ -126,6 +126,7 @@ doxygen {
|
||||
|
||||
case_sense_names false
|
||||
extension_mapping 'inc=C++'
|
||||
extract_all true
|
||||
extract_static true
|
||||
full_path_names true
|
||||
generate_html true
|
||||
|
||||
@@ -58,10 +58,11 @@ int32_t HALSIM_RegisterSimValueChangedCallback(HAL_SimValueHandle handle,
|
||||
void HALSIM_CancelSimValueChangedCallback(int32_t uid);
|
||||
|
||||
/**
|
||||
* Register a callback for HAL_SimValueReset(). The callback is called with
|
||||
* the old value.
|
||||
* Register a callback for HAL_SimValueReset(). The callback is called with the
|
||||
* old value.
|
||||
*
|
||||
* @param handle simulated value handle
|
||||
* @param param parameter for callback
|
||||
* @param callback callback
|
||||
* @param initialNotify ignored (present for consistency)
|
||||
*/
|
||||
|
||||
130
upstream_utils/drake-fix-doxygen.patch
Normal file
130
upstream_utils/drake-fix-doxygen.patch
Normal file
@@ -0,0 +1,130 @@
|
||||
diff --git a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
|
||||
index 5d7a316f3..dc08be95e 100644
|
||||
--- a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
|
||||
+++ b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
|
||||
@@ -9,18 +9,19 @@
|
||||
namespace drake {
|
||||
namespace math {
|
||||
|
||||
-/// 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
|
||||
-///
|
||||
-/// @throws std::exception if Q is not positive semi-definite.
|
||||
-/// @throws std::exception if R is not positive definite.
|
||||
-///
|
||||
-/// Based on the Schur Vector approach outlined in this paper:
|
||||
-/// "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
|
||||
-/// by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
|
||||
-///
|
||||
+/**
|
||||
+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
|
||||
+
|
||||
+@throws std::exception if Q is not positive semi-definite.
|
||||
+@throws std::exception if R is not positive definite.
|
||||
+
|
||||
+Based on the Schur Vector approach outlined in this paper:
|
||||
+"On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
|
||||
+by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
|
||||
+*/
|
||||
WPILIB_DLLEXPORT
|
||||
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
@@ -28,49 +29,50 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& Q,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& R);
|
||||
|
||||
-/// 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ₖ.
|
||||
-///
|
||||
-/// @verbatim
|
||||
-/// ∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
-/// J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
-/// k=0
|
||||
-/// @endverbatim
|
||||
-///
|
||||
-/// 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ₖ:
|
||||
-///
|
||||
-/// @verbatim
|
||||
-/// ∞
|
||||
-/// J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
-/// k=0
|
||||
-/// @endverbatim
|
||||
-///
|
||||
-/// This can be refactored as:
|
||||
-///
|
||||
-/// @verbatim
|
||||
-/// ∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
-/// J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
-/// k=0
|
||||
-/// @endverbatim
|
||||
-///
|
||||
-/// @throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
|
||||
-/// @throws std::runtime_error if R is not positive definite.
|
||||
-///
|
||||
+/**
|
||||
+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ₖ.
|
||||
+
|
||||
+@verbatim
|
||||
+ ∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
+J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
+ k=0
|
||||
+@endverbatim
|
||||
+
|
||||
+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ₖ:
|
||||
+
|
||||
+@verbatim
|
||||
+ ∞
|
||||
+J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
+ k=0
|
||||
+@endverbatim
|
||||
+
|
||||
+This can be refactored as:
|
||||
+
|
||||
+@verbatim
|
||||
+ ∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
+J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
+ k=0
|
||||
+@endverbatim
|
||||
+
|
||||
+@throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
|
||||
+@throws std::runtime_error if R is not positive definite.
|
||||
+*/
|
||||
WPILIB_DLLEXPORT
|
||||
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
@@ -63,7 +63,8 @@ def main():
|
||||
|
||||
apply_patches(root, [
|
||||
"upstream_utils/drake-replace-dense-with-core.patch",
|
||||
"upstream_utils/drake-dllexport-dare.patch"
|
||||
"upstream_utils/drake-dllexport-dare.patch",
|
||||
"upstream_utils/drake-fix-doxygen.patch"
|
||||
])
|
||||
|
||||
|
||||
|
||||
@@ -87,10 +87,11 @@ inline void ReportError(int32_t status, const char* fileName, int lineNumber,
|
||||
* instead.
|
||||
*
|
||||
* @param[out] status error code
|
||||
* @param[in] message error message details
|
||||
* @param[in] fileName source file name
|
||||
* @param[in] lineNumber source line number
|
||||
* @param[in] funcName source function name
|
||||
* @param[in] format error message format
|
||||
* @param[in] args error message format args
|
||||
* @return runtime error object
|
||||
*/
|
||||
[[nodiscard]] RuntimeError MakeErrorV(int32_t status, const char* fileName,
|
||||
|
||||
@@ -280,7 +280,7 @@ enum class BuiltInWidgets {
|
||||
* <br>Custom properties:
|
||||
* <table>
|
||||
* <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
|
||||
* <tr><td>Range</td><td>{@link Range}</td><td>k16G</td><td>The accelerometer
|
||||
* <tr><td>Range</td><td>Range</td><td>k16G</td><td>The accelerometer
|
||||
* range</td></tr> <tr><td>Show value</td><td>Boolean</td><td>true</td>
|
||||
* <td>Show or hide the acceleration values</td></tr>
|
||||
* <tr><td>Precision</td><td>Number</td><td>2</td>
|
||||
|
||||
@@ -49,14 +49,14 @@ bool IsTimingPaused();
|
||||
/**
|
||||
* Advance the simulator time and wait for all notifiers to run.
|
||||
*
|
||||
* @param deltaSeconds the amount to advance (in seconds)
|
||||
* @param delta the amount to advance (in seconds)
|
||||
*/
|
||||
void StepTiming(units::second_t delta);
|
||||
|
||||
/**
|
||||
* Advance the simulator time and return immediately.
|
||||
*
|
||||
* @param deltaSeconds the amount to advance (in seconds)
|
||||
* @param delta the amount to advance (in seconds)
|
||||
*/
|
||||
void StepTimingAsync(units::second_t delta);
|
||||
|
||||
|
||||
@@ -16,13 +16,23 @@ import edu.wpi.first.math.system.NumericalJacobian;
|
||||
import java.util.function.BiFunction;
|
||||
|
||||
/**
|
||||
* Kalman filters combine predictions from a model and measurements to give an estimate of the true
|
||||
* system state. This is useful because many states cannot be measured directly as a result of
|
||||
* A Kalman filter combines predictions from a model and measurements to give an estimate of the
|
||||
* true system state. This is useful because many states cannot be measured directly as a result of
|
||||
* sensor noise, or because the state is "hidden".
|
||||
*
|
||||
* <p>The Extended Kalman filter is just like the {@link KalmanFilter Kalman filter}, but we make a
|
||||
* linear approximation of nonlinear dynamics and/or nonlinear measurement models. This means that
|
||||
* the EKF works with nonlinear systems.
|
||||
* <p>Kalman filters use a K gain matrix to determine whether to trust the model or measurements
|
||||
* more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum
|
||||
* of squares error in the state estimate. This K gain is used to correct the state estimate by some
|
||||
* amount of the difference between the actual measurements and the measurements predicted by the
|
||||
* model.
|
||||
*
|
||||
* <p>An extended Kalman filter supports nonlinear state and measurement models. It propagates the
|
||||
* error covariance by linearizing the models around the state estimate, then applying the linear
|
||||
* Kalman filter equations.
|
||||
*
|
||||
* <p>For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
|
||||
* theory".
|
||||
*/
|
||||
@SuppressWarnings("ClassTypeParameterName")
|
||||
public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
|
||||
|
||||
@@ -18,13 +18,21 @@ import org.ejml.simple.SimpleMatrix;
|
||||
|
||||
/**
|
||||
* A Kalman filter combines predictions from a model and measurements to give an estimate of the
|
||||
* true ystem state. This is useful because many states cannot be measured directly as a result of
|
||||
* true system state. This is useful because many states cannot be measured directly as a result of
|
||||
* sensor noise, or because the state is "hidden".
|
||||
*
|
||||
* <p>The Unscented Kalman filter is similar to the {@link KalmanFilter Kalman filter}, except that
|
||||
* it propagates carefully chosen points called sigma points through the non-linear model to obtain
|
||||
* an estimate of the true covariance (as opposed to a linearized version of it). This means that
|
||||
* the UKF works with nonlinear systems.
|
||||
* <p>Kalman filters use a K gain matrix to determine whether to trust the model or measurements
|
||||
* more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum
|
||||
* of squares error in the state estimate. This K gain is used to correct the state estimate by some
|
||||
* amount of the difference between the actual measurements and the measurements predicted by the
|
||||
* model.
|
||||
*
|
||||
* <p>An unscented Kalman filter uses nonlinear state and measurement models. It propagates the
|
||||
* error covariance using sigma points chosen to approximate the true probability distribution.
|
||||
*
|
||||
* <p>For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
|
||||
* theory".
|
||||
*/
|
||||
@SuppressWarnings({"MemberName", "ClassTypeParameterName"})
|
||||
public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
|
||||
|
||||
@@ -9,18 +9,19 @@
|
||||
namespace drake {
|
||||
namespace math {
|
||||
|
||||
/// 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
|
||||
///
|
||||
/// @throws std::exception if Q is not positive semi-definite.
|
||||
/// @throws std::exception if R is not positive definite.
|
||||
///
|
||||
/// Based on the Schur Vector approach outlined in this paper:
|
||||
/// "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
|
||||
/// by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
|
||||
///
|
||||
/**
|
||||
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
|
||||
|
||||
@throws std::exception if Q is not positive semi-definite.
|
||||
@throws std::exception if R is not positive definite.
|
||||
|
||||
Based on the Schur Vector approach outlined in this paper:
|
||||
"On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
|
||||
by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
|
||||
*/
|
||||
WPILIB_DLLEXPORT
|
||||
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
@@ -28,49 +29,50 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& Q,
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& R);
|
||||
|
||||
/// 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ₖ.
|
||||
///
|
||||
/// @verbatim
|
||||
/// ∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
/// J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
/// k=0
|
||||
/// @endverbatim
|
||||
///
|
||||
/// 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ₖ:
|
||||
///
|
||||
/// @verbatim
|
||||
/// ∞
|
||||
/// J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
/// k=0
|
||||
/// @endverbatim
|
||||
///
|
||||
/// This can be refactored as:
|
||||
///
|
||||
/// @verbatim
|
||||
/// ∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
/// J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
/// k=0
|
||||
/// @endverbatim
|
||||
///
|
||||
/// @throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
|
||||
/// @throws std::runtime_error if R is not positive definite.
|
||||
///
|
||||
/**
|
||||
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ₖ.
|
||||
|
||||
@verbatim
|
||||
∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
k=0
|
||||
@endverbatim
|
||||
|
||||
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ₖ:
|
||||
|
||||
@verbatim
|
||||
∞
|
||||
J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
k=0
|
||||
@endverbatim
|
||||
|
||||
This can be refactored as:
|
||||
|
||||
@verbatim
|
||||
∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
k=0
|
||||
@endverbatim
|
||||
|
||||
@throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
|
||||
@throws std::runtime_error if R is not positive definite.
|
||||
*/
|
||||
WPILIB_DLLEXPORT
|
||||
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
|
||||
const Eigen::Ref<const Eigen::MatrixXd>& A,
|
||||
|
||||
@@ -262,6 +262,8 @@ Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose);
|
||||
* 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.
|
||||
*
|
||||
* @tparam States The number of states.
|
||||
* @tparam Inputs The number of inputs.
|
||||
* @param A System matrix.
|
||||
* @param B Input matrix.
|
||||
*/
|
||||
@@ -278,6 +280,8 @@ bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
|
||||
* 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.
|
||||
*
|
||||
* @tparam States The number of states.
|
||||
* @tparam Outputs The number of outputs.
|
||||
* @param A System matrix.
|
||||
* @param C Output matrix.
|
||||
*/
|
||||
@@ -313,7 +317,10 @@ Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose);
|
||||
/**
|
||||
* Clamps input vector between system's minimum and maximum allowable input.
|
||||
*
|
||||
* @tparam Inputs The number of inputs.
|
||||
* @param u Input vector to clamp.
|
||||
* @param umin The minimum input magnitude.
|
||||
* @param umax The maximum input magnitude.
|
||||
* @return Clamped input vector.
|
||||
*/
|
||||
template <int Inputs>
|
||||
@@ -332,9 +339,9 @@ Eigen::Vector<double, Inputs> ClampInputMaxMagnitude(
|
||||
* Normalize all inputs if any excedes the maximum magnitude. Useful for systems
|
||||
* such as differential drivetrains.
|
||||
*
|
||||
* @tparam Inputs The number of inputs.
|
||||
* @param u The input vector.
|
||||
* @param maxMagnitude The maximum magnitude any input can have.
|
||||
* @param <I> The number of inputs.
|
||||
* @return The normalizedInput
|
||||
*/
|
||||
template <int Inputs>
|
||||
|
||||
@@ -32,6 +32,9 @@ namespace frc {
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*
|
||||
* @tparam States The number of states.
|
||||
* @tparam Inputs the number of inputs.
|
||||
*/
|
||||
template <int States, int Inputs>
|
||||
class ControlAffinePlantInversionFeedforward {
|
||||
|
||||
@@ -24,6 +24,9 @@ namespace frc {
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*
|
||||
* @tparam States The number of states.
|
||||
* @tparam Inputs The number of inputs.
|
||||
*/
|
||||
template <int States, int Inputs>
|
||||
class LinearPlantInversionFeedforward {
|
||||
@@ -31,6 +34,7 @@ class LinearPlantInversionFeedforward {
|
||||
/**
|
||||
* Constructs a feedforward with the given plant.
|
||||
*
|
||||
* @tparam Outputs The number of outputs.
|
||||
* @param plant The plant being controlled.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
|
||||
@@ -32,6 +32,9 @@ namespace detail {
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*
|
||||
* @tparam States Number of states.
|
||||
* @tparam Inputs Number of inputs.
|
||||
*/
|
||||
template <int States, int Inputs>
|
||||
class LinearQuadraticRegulatorImpl {
|
||||
@@ -266,6 +269,7 @@ class LinearQuadraticRegulator
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @tparam Outputs The number of outputs.
|
||||
* @param plant The plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
|
||||
@@ -15,6 +15,7 @@ namespace frc {
|
||||
* Subtracts a and b while normalizing the resulting value in the selected row
|
||||
* as if it were an angle.
|
||||
*
|
||||
* @tparam States The number of states.
|
||||
* @param a A vector to subtract from.
|
||||
* @param b A vector to subtract with.
|
||||
* @param angleStateIdx The row containing angles to be normalized.
|
||||
@@ -33,6 +34,7 @@ Eigen::Vector<double, States> AngleResidual(
|
||||
* Returns a function that subtracts two vectors while normalizing the resulting
|
||||
* value in the selected row as if it were an angle.
|
||||
*
|
||||
* @tparam States The number of states.
|
||||
* @param angleStateIdx The row containing angles to be normalized.
|
||||
*/
|
||||
template <int States>
|
||||
@@ -48,6 +50,7 @@ AngleResidual(int angleStateIdx) {
|
||||
* Adds a and b while normalizing the resulting value in the selected row as an
|
||||
* angle.
|
||||
*
|
||||
* @tparam States The number of states.
|
||||
* @param a A vector to add with.
|
||||
* @param b A vector to add with.
|
||||
* @param angleStateIdx The row containing angles to be normalized.
|
||||
@@ -66,6 +69,7 @@ Eigen::Vector<double, States> AngleAdd(const Eigen::Vector<double, States>& a,
|
||||
* Returns a function that adds two vectors while normalizing the resulting
|
||||
* value in the selected row as an angle.
|
||||
*
|
||||
* @tparam States The number of states.
|
||||
* @param angleStateIdx The row containing angles to be normalized.
|
||||
*/
|
||||
template <int States>
|
||||
@@ -79,9 +83,12 @@ AngleAdd(int angleStateIdx) {
|
||||
* Computes the mean of sigmas with the weights Wm while computing a special
|
||||
* angle mean for a select row.
|
||||
*
|
||||
* @tparam CovDim Dimension of covariance of sigma points after passing through
|
||||
* the transform.
|
||||
* @tparam States The number of states.
|
||||
* @param sigmas Sigma points.
|
||||
* @param Wm Weights for the mean.
|
||||
* @param angleStateIdx The row containing the angles.
|
||||
* @param angleStatesIdx The row containing the angles.
|
||||
*/
|
||||
template <int CovDim, int States>
|
||||
Eigen::Vector<double, CovDim> AngleMean(
|
||||
@@ -103,6 +110,9 @@ Eigen::Vector<double, CovDim> AngleMean(
|
||||
* Returns a function that computes the mean of sigmas with the weights Wm while
|
||||
* computing a special angle mean for a select row.
|
||||
*
|
||||
* @tparam CovDim Dimension of covariance of sigma points after passing through
|
||||
* the transform.
|
||||
* @tparam States The number of states.
|
||||
* @param angleStateIdx The row containing the angles.
|
||||
*/
|
||||
template <int CovDim, int States>
|
||||
|
||||
@@ -19,11 +19,35 @@
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* A Kalman filter combines predictions from a model and measurements to give an
|
||||
* estimate of the true system state. This is useful because many states cannot
|
||||
* be measured directly as a result of sensor noise, or because the state is
|
||||
* "hidden".
|
||||
*
|
||||
* Kalman filters use a K gain matrix to determine whether to trust the model or
|
||||
* measurements more. Kalman filter theory uses statistics to compute an optimal
|
||||
* K gain which minimizes the sum of squares error in the state estimate. This K
|
||||
* gain is used to correct the state estimate by some amount of the difference
|
||||
* between the actual measurements and the measurements predicted by the model.
|
||||
*
|
||||
* An extended Kalman filter supports nonlinear state and measurement models. It
|
||||
* propagates the error covariance by linearizing the models around the state
|
||||
* estimate, then applying the linear Kalman filter equations.
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
|
||||
* "Stochastic control theory".
|
||||
*
|
||||
* @tparam States The number of states.
|
||||
* @tparam Inputs The number of inputs.
|
||||
* @tparam Outputs The number of outputs.
|
||||
*/
|
||||
template <int States, int Inputs, int Outputs>
|
||||
class ExtendedKalmanFilter {
|
||||
public:
|
||||
/**
|
||||
* Constructs an Extended Kalman filter.
|
||||
* Constructs an extended Kalman filter.
|
||||
*
|
||||
* @param f A vector-valued function of x and u that returns
|
||||
* the derivative of the state vector.
|
||||
@@ -81,7 +105,7 @@ class ExtendedKalmanFilter {
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs an Extended Kalman filter.
|
||||
* Constructs an extended Kalman filter.
|
||||
*
|
||||
* @param f A vector-valued function of x and u that returns
|
||||
* the derivative of the state vector.
|
||||
|
||||
@@ -39,6 +39,10 @@ namespace detail {
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
|
||||
* "Stochastic control theory".
|
||||
*
|
||||
* @tparam States The number of states.
|
||||
* @tparam Inputs The number of inputs.
|
||||
* @tparam Outputs The number of outputs.
|
||||
*/
|
||||
template <int States, int Inputs, int Outputs>
|
||||
class KalmanFilterImpl {
|
||||
|
||||
@@ -19,11 +19,11 @@ namespace frc {
|
||||
* version seen in most publications. Unless you know better, this should be
|
||||
* your default choice.
|
||||
*
|
||||
* @tparam States The dimensionality of the state. 2*States+1 weights will be
|
||||
* generated.
|
||||
*
|
||||
* [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
|
||||
* Inference in Dynamic State-Space Models" (Doctoral dissertation)
|
||||
*
|
||||
* @tparam States The dimensionality of the state. 2*States+1 weights will be
|
||||
* generated.
|
||||
*/
|
||||
template <int States>
|
||||
class MerweScaledSigmaPoints {
|
||||
|
||||
@@ -20,6 +20,30 @@
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* A Kalman filter combines predictions from a model and measurements to give an
|
||||
* estimate of the true system state. This is useful because many states cannot
|
||||
* be measured directly as a result of sensor noise, or because the state is
|
||||
* "hidden".
|
||||
*
|
||||
* Kalman filters use a K gain matrix to determine whether to trust the model or
|
||||
* measurements more. Kalman filter theory uses statistics to compute an optimal
|
||||
* K gain which minimizes the sum of squares error in the state estimate. This K
|
||||
* gain is used to correct the state estimate by some amount of the difference
|
||||
* between the actual measurements and the measurements predicted by the model.
|
||||
*
|
||||
* An unscented Kalman filter uses nonlinear state and measurement models. It
|
||||
* propagates the error covariance using sigma points chosen to approximate the
|
||||
* true probability distribution.
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
|
||||
* "Stochastic control theory".
|
||||
*
|
||||
* @tparam States The number of states.
|
||||
* @tparam Inputs The number of inputs.
|
||||
* @tparam Outputs The number of outputs.
|
||||
*/
|
||||
template <int States, int Inputs, int Outputs>
|
||||
class UnscentedKalmanFilter {
|
||||
public:
|
||||
@@ -331,7 +355,7 @@ class UnscentedKalmanFilter {
|
||||
}
|
||||
|
||||
// Mean and covariance of prediction passed through UT
|
||||
auto [yHat, Py] = UnscentedTransform<States, Rows>(
|
||||
auto [yHat, Py] = UnscentedTransform<Rows, States>(
|
||||
sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY);
|
||||
Py += discR;
|
||||
|
||||
|
||||
@@ -16,17 +16,21 @@ namespace frc {
|
||||
*
|
||||
* This works in conjunction with the UnscentedKalmanFilter class.
|
||||
*
|
||||
* @tparam States Number of states.
|
||||
* @tparam CovDim Dimension of covariance of sigma points after passing through
|
||||
* the transform.
|
||||
* @param sigmas List of sigma points.
|
||||
* @param Wm Weights for the mean.
|
||||
* @param Wc Weights for the covariance.
|
||||
* @tparam CovDim Dimension of covariance of sigma points after passing
|
||||
* through the transform.
|
||||
* @tparam States Number of states.
|
||||
* @param sigmas List of sigma points.
|
||||
* @param Wm Weights for the mean.
|
||||
* @param Wc Weights for the covariance.
|
||||
* @param meanFunc A function that computes the mean of 2 * States + 1 state
|
||||
* vectors using a given set of weights.
|
||||
* @param residualFunc A function that computes the residual of two state
|
||||
* vectors (i.e. it subtracts them.)
|
||||
*
|
||||
* @return Tuple of x, mean of sigma points; P, covariance of sigma points after
|
||||
* passing through the transform.
|
||||
*/
|
||||
template <int States, int CovDim>
|
||||
template <int CovDim, int States>
|
||||
std::tuple<Eigen::Vector<double, CovDim>, Eigen::Matrix<double, CovDim, CovDim>>
|
||||
UnscentedTransform(const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
|
||||
const Eigen::Vector<double, 2 * States + 1>& Wm,
|
||||
|
||||
@@ -13,6 +13,7 @@ namespace frc {
|
||||
/**
|
||||
* Discretizes the given continuous A matrix.
|
||||
*
|
||||
* @tparam States Number of states.
|
||||
* @param contA Continuous system matrix.
|
||||
* @param dt Discretization timestep.
|
||||
* @param discA Storage for discrete system matrix.
|
||||
@@ -27,6 +28,8 @@ void DiscretizeA(const Eigen::Matrix<double, States, States>& contA,
|
||||
/**
|
||||
* Discretizes the given continuous A and B matrices.
|
||||
*
|
||||
* @tparam States Number of states.
|
||||
* @tparam Inputs Number of inputs.
|
||||
* @param contA Continuous system matrix.
|
||||
* @param contB Continuous input matrix.
|
||||
* @param dt Discretization timestep.
|
||||
@@ -54,6 +57,7 @@ void DiscretizeAB(const Eigen::Matrix<double, States, States>& contA,
|
||||
/**
|
||||
* Discretizes the given continuous A and Q matrices.
|
||||
*
|
||||
* @tparam States Number of states.
|
||||
* @param contA Continuous system matrix.
|
||||
* @param contQ Continuous process noise covariance matrix.
|
||||
* @param dt Discretization timestep.
|
||||
@@ -105,6 +109,7 @@ void DiscretizeAQ(const Eigen::Matrix<double, States, States>& contA,
|
||||
* using a taylor series to several terms and still be substantially cheaper
|
||||
* than taking the big exponential.
|
||||
*
|
||||
* @tparam States Number of states.
|
||||
* @param contA Continuous system matrix.
|
||||
* @param contQ Continuous process noise covariance matrix.
|
||||
* @param dt Discretization timestep.
|
||||
@@ -149,6 +154,7 @@ void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
|
||||
* Returns a discretized version of the provided continuous measurement noise
|
||||
* covariance matrix.
|
||||
*
|
||||
* @tparam Outputs Number of outputs.
|
||||
* @param R Continuous measurement noise covariance matrix.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
|
||||
@@ -22,6 +22,10 @@ namespace frc {
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*
|
||||
* @tparam States Number of states.
|
||||
* @tparam Inputs Number of inputs.
|
||||
* @tparam Outputs Number of outputs.
|
||||
*/
|
||||
template <int States, int Inputs, int Outputs>
|
||||
class LinearSystem {
|
||||
|
||||
@@ -27,6 +27,10 @@ namespace frc {
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*
|
||||
* @tparam States Number of states.
|
||||
* @tparam Inputs Number of inputs.
|
||||
* @tparam Outputs Number of outputs.
|
||||
*/
|
||||
template <int States, int Inputs, int Outputs>
|
||||
class LinearSystemLoop {
|
||||
|
||||
@@ -41,10 +41,11 @@ auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& x) {
|
||||
* @tparam States Number of rows in x.
|
||||
* @tparam Inputs Number of rows in u.
|
||||
* @tparam F Function object type.
|
||||
* @tparam Args... Remaining arguments to f(x, u, ...).
|
||||
* @tparam Args... Types of remaining arguments to f(x, u, ...).
|
||||
* @param f Vector-valued function from which to compute Jacobian.
|
||||
* @param x State vector.
|
||||
* @param u Input vector.
|
||||
* @param args Remaining arguments to f(x, u, ...).
|
||||
*/
|
||||
template <int Rows, int States, int Inputs, typename F, typename... Args>
|
||||
auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
|
||||
@@ -62,10 +63,11 @@ auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
|
||||
* @tparam States Number of rows in x.
|
||||
* @tparam Inputs Number of rows in u.
|
||||
* @tparam F Function object type.
|
||||
* @tparam Args... Remaining arguments to f(x, u, ...).
|
||||
* @tparam Args... Types of remaining arguments to f(x, u, ...).
|
||||
* @param f Vector-valued function from which to compute Jacobian.
|
||||
* @param x State vector.
|
||||
* @param u Input vector.
|
||||
* @param args Remaining arguments to f(x, u, ...).
|
||||
*/
|
||||
template <int Rows, int States, int Inputs, typename F, typename... Args>
|
||||
auto NumericalJacobianU(F&& f, const Eigen::Vector<double, States>& x,
|
||||
|
||||
@@ -580,7 +580,6 @@ int WPI_WaitForObjectsTimeout(const WPI_Handle* handles, int handles_count,
|
||||
* @param handle handle
|
||||
* @param manual_reset true for manual reset, false for automatic reset
|
||||
* @param initial_state true to make the handle initially in signaled state
|
||||
* @return Event handle
|
||||
*/
|
||||
void WPI_CreateSignalObject(WPI_Handle handle, int manual_reset,
|
||||
int initial_state);
|
||||
|
||||
@@ -19,25 +19,27 @@ class raw_istream;
|
||||
class raw_ostream;
|
||||
|
||||
/**
|
||||
* Get size of unsigned LEB128 data
|
||||
* @val: value
|
||||
* Get size of unsigned LEB128 data.
|
||||
*
|
||||
* Determine the number of bytes required to encode an unsigned LEB128 datum.
|
||||
* The algorithm is taken from Appendix C of the DWARF 3 spec. For information
|
||||
* on the encodings refer to section "7.6 - Variable Length Data". Return
|
||||
* the number of bytes required.
|
||||
*
|
||||
* @param val LEB128 data.
|
||||
*/
|
||||
uint64_t SizeUleb128(uint64_t val);
|
||||
|
||||
/**
|
||||
* Write unsigned LEB128 data
|
||||
* @dest: the address where the ULEB128 data is to be stored
|
||||
* @val: value to be stored
|
||||
* Write unsigned LEB128 data.
|
||||
*
|
||||
* Encode an unsigned LEB128 encoded datum. The algorithm is taken
|
||||
* from Appendix C of the DWARF 3 spec. For information on the
|
||||
* encodings refer to section "7.6 - Variable Length Data". Return
|
||||
* the number of bytes written.
|
||||
*
|
||||
* @param dest The address where the ULEB128 data is to be stored.
|
||||
* @param val Value to be stored.
|
||||
*/
|
||||
uint64_t WriteUleb128(SmallVectorImpl<char>& dest, uint64_t val);
|
||||
|
||||
@@ -48,32 +50,34 @@ uint64_t WriteUleb128(SmallVectorImpl<char>& dest, uint64_t val);
|
||||
* from Appendix C of the DWARF 3 spec. For information on the
|
||||
* encodings refer to section "7.6 - Variable Length Data".
|
||||
*
|
||||
* @param os output stream
|
||||
* @param val value to be stored
|
||||
* @param os Output stream.
|
||||
* @param val Value to be stored.
|
||||
*/
|
||||
void WriteUleb128(raw_ostream& os, uint64_t val);
|
||||
|
||||
/**
|
||||
* Read unsigned LEB128 data
|
||||
* @addr: the address where the ULEB128 data is stored
|
||||
* @ret: address to store the result
|
||||
* Read unsigned LEB128 data.
|
||||
*
|
||||
* Decode an unsigned LEB128 encoded datum. The algorithm is taken
|
||||
* from Appendix C of the DWARF 3 spec. For information on the
|
||||
* encodings refer to section "7.6 - Variable Length Data". Return
|
||||
* the number of bytes read.
|
||||
*
|
||||
* @param addr The address where the ULEB128 data is stored.
|
||||
* @param ret Address to store the result.
|
||||
*/
|
||||
uint64_t ReadUleb128(const char* addr, uint64_t* ret);
|
||||
|
||||
/**
|
||||
* Read unsigned LEB128 data from a stream
|
||||
* @is: the input stream where the ULEB128 data is to be read from
|
||||
* @ret: address to store the result
|
||||
* Read unsigned LEB128 data from a stream.
|
||||
*
|
||||
* Decode an unsigned LEB128 encoded datum. The algorithm is taken
|
||||
* from Appendix C of the DWARF 3 spec. For information on the
|
||||
* encodings refer to section "7.6 - Variable Length Data". Return
|
||||
* false on stream error, true on success.
|
||||
*
|
||||
* @param is The input stream where the ULEB128 data is to be read from.
|
||||
* @param ret Address to store the result.
|
||||
*/
|
||||
bool ReadUleb128(raw_istream& is, uint64_t* ret);
|
||||
|
||||
@@ -92,8 +96,8 @@ class Uleb128Reader {
|
||||
* If a value is returned, internal state is reset so it's safe to immediately
|
||||
* call this function again to decode another value.
|
||||
*
|
||||
* @param in input data; modified as data is consumed (any unconsumed data
|
||||
* is left when function returns)
|
||||
* @param in Input data; modified as data is consumed (any unconsumed data
|
||||
* is left when function returns).
|
||||
* @return value (in std::optional)
|
||||
*/
|
||||
std::optional<uint64_t> ReadOne(span<const uint8_t>* in);
|
||||
|
||||
Reference in New Issue
Block a user