[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.
2026-06-25 01:41:43 +00:00 · 2021-10-29 15:07:05 -07:00
parent a939cd9c89
commit 2cb171f6f5
26 changed files with 362 additions and 105 deletions
--- a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
+++ b/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>
--- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
+++ b/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.
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
+++ b/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 {
--- a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
+++ b/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 {
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
+++ b/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;

--- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
+++ b/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,