[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.

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,

wpimath/src/main/native/include/frc/StateSpaceUtil.h

@@ -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>

wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h

@@ -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 {

wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h

@@ -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.
    */

wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h

@@ -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.

wpimath/src/main/native/include/frc/estimator/AngleStatistics.h

@@ -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>

wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h

@@ -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.

wpimath/src/main/native/include/frc/estimator/KalmanFilter.h

@@ -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 {

wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h

@@ -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 {

wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h

@@ -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;
 

wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h

@@ -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,

wpimath/src/main/native/include/frc/system/Discretization.h

@@ -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.
  */

wpimath/src/main/native/include/frc/system/LinearSystem.h

@@ -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 {

wpimath/src/main/native/include/frc/system/LinearSystemLoop.h

@@ -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 {

wpimath/src/main/native/include/frc/system/NumericalJacobian.h

@@ -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,