Repository: RIT-VEX-U/Core
Pull request #103: Add an Unscented Kalman Filter 
Author: login: PascalSkylake
Created: 2025-03-18T18:51:38Z | Merged: 2025-03-21T05:51:06Z

Closes: issue #69

# Description
The Unscented Kalman Filter is a nonlinear state estimator. You use it to combine the outputs of a model of a system, and measurements of the system to estimate the true state over time.

More specifically, this pr implements a square-root Unscented Kalman Filter, where the lower-triangular square-root of the covariance matrix is maintained by the filter instead of a normal covariance matrix.

# Usage
This example is for a DC motor which simultaneously estimate position, velocity, kV, and kA:
Define the system model f(x, u), this is the derivative of your state with respect to time:
```cpp
auto f = [](const Eigen::Vector<double, 4>& xhat, const Eigen::Vector<double, 1>& u) -> Eigen::Vector<double, 4> {
    double v = xhat(1);
    double kV = xhat(2);
    double kA = xhat(3);
    double a = (u(0) - kV * v) / kA;
    Eigen::Vector<double, 4> xdot{v, a, 0, 0};
    return xdot;
};
```
Define the measurement function h(x, u), this computes the expected measurement at a given state:
```cpp
auto h = [](const Eigen::Vector<double, 4>& xhat, const Eigen::Vector<double, 1>& u) -> Eigen::Vector<double, 3> {
    double p = xhat(0);
    double v = xhat(1);
    double kV = xhat(2);
    double kA = xhat(3);
    double I = (u(0) - kV * v) / kA;
    Eigen::Vector<double, 3> z{p, v, I};
    return z;
};
```
Define the noise standard deviations for the system model and measurement model (noise is additive)
```cpp
Eigen::Vector<double, 4> f_stddevs{2.5e-4, 7e-2, 1e-10, 1e-10};
Eigen::Vector<double, 3> h_stddevs{0.02, 0.1, 0.1};
```
Create UKF object, we need to tell it which integration method to use, in this case RK4. The template parameters are <# states, # inputs, # measurement outputs>:
```cpp
SRUKF<4, 1, 3> srukf(f, h, RK4_with_input<4, 1>, f_stddevs, h_stddevs);
```

The filter has two functions that operate it, predict and correct. They do what they sound like they do, predict propagates the state and covariance forward by some timestep, and correct corrects the state and covariance by applying some measurement.

Usually these are called in pairs, unless you want to apply multiple measurements during the same timestep.
We assume here that there are vectors "inputs" and "measurements" already defined, these must be provided at each timestep.
In this example, we will say that the timestep is 10ms (update rate of VEX electronics)
```cpp
srukf.predict(inputs, 0.01);
srukf.correct(inputs, measurements);
```

To obtain the current state estimate, call:
```cpp
Eigen::Vector<double, 4> xhat = srukf.xhat();
```

# Testing

Various systems were tested (in simulation, not real life) to determine whether the state estimate converged correctly, to prove that the math is correct:
Four state system modeling a DC motor, [pos, vel, kV, kA]
Differential drive robot with LIDAR on an empty square field, [x, y, theta]
Glider with INS [x, y, z, vx, vy, vz]

Depending on the setup of the initial covariance matrix, and noise matrices, the filter converged for all these systems.

# References
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models
https://www.researchgate.net/publication/2478046

The Square-Root Unscented Kalman Filter for State and Parameter-Estimation
https://www.researchgate.net/publication/3908304

A Scaled Spherical Simplex Filter (S3F) with a decreased n + 2 sigma points set size and equivalent 2n + 1 Unscented Kalman Filter (UKF) accuracy
https://www.sciencedirect.com/science/article/pii/S0888327020308190

https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h

https://github.com/wpilibsuite/allwpilib/pull/7850


Diff:
diff --git a/include/utils/math/eigen_interface.h b/include/utils/math/eigen_interface.h
new file mode 100644
index 00000000..0cb86ee6
--- /dev/null
+++ b/include/utils/math/eigen_interface.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include <Eigen/Dense>
+
+/**
+ * This header only serves to let you use Eigen's vectors and matrices without
+ * having to write out
+ * Eigen::Vector<double, DIM>
+ * Eigen::Matrix<double, ROWS, COLS>
+ * 
+ * include this and use:
+ * EVec<DIM>
+ * EMAT<ROWS, COLS>
+ */
+
+template <int DIM>
+using EVec = Eigen::Vector<double, DIM>;
+
+template <int ROWS, int COLS>
+using EMat = Eigen::Matrix<double, ROWS, COLS>;
diff --git a/include/utils/math/estimator/srukf.h b/include/utils/math/estimator/srukf.h
new file mode 100644
index 00000000..db416e9c
--- /dev/null
+++ b/include/utils/math/estimator/srukf.h
@@ -0,0 +1,683 @@
+#pragma once
+
+#include <functional>
+
+#include "../core/include/utils/math/eigen_interface.h"
+#include "../core/include/utils/math/numerical/numerical_integration.h"
+
+// Forward declare the sigma points class, it is at the bottom of this file.
+template <int STATES> class ScaledSphericalSimplexSigmaPoints;
+
+// Forward declare the Unscented Transform function, it is after the SRUKF class itself.
+template <int COV_DIM, int STATES, int NUM_SIGMAS>
+std::tuple<EVec<COV_DIM>, EMat<COV_DIM, COV_DIM>> square_root_ut(
+  const EMat<COV_DIM, NUM_SIGMAS> &sigmas, const EVec<NUM_SIGMAS> &Wm, const EVec<NUM_SIGMAS> &Wc,
+  const std::function<EVec<COV_DIM>(const EMat<COV_DIM, NUM_SIGMAS> &, const EVec<NUM_SIGMAS> &)> &mean_func,
+  const std::function<EVec<COV_DIM>(const EVec<COV_DIM> &, const EVec<COV_DIM> &)> &residual_func,
+  const EMat<COV_DIM, COV_DIM> &square_root_R
+);
+
+/**
+ * Kalman filters combine predictions from a model and measurements to estimate
+ * a system's true state.
+ *
+ * The Unscented Kalman Filter is a nonlinear estimator, meaning that the model
+ * used to predict how the state changes over time can be nonlinear. The model
+ * that determines the expected measurement given the current state can also be
+ * nonlinear.
+ *
+ * At each timestep, sigma points are generated close to the mean, they are all
+ * propagated forward in time according to the nonlinear model. The Unscented
+ * Transform uses the propagated sigma points to compute the prior state and
+ * covariance.
+ *
+ * When correcting the state and covariance with a measurement, sigma points are
+ * again generated, but are transformed into the measurement space using the
+ * measurement function. A Kalman gain matrix K is then computed, and used to
+ * update the state and covariance.
+ *
+ * To read more about Kalman filters and the standard UKF read:
+ * https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
+ *
+ * This implementation is somewhat non-standard. The square-root form of the UKF
+ * (SR-UKF) is used, and the way the sigma points are generated is different than
+ * most implementations. The square-root form is used to ensure that the
+ * covariance matrix remains positive definite.
+ *
+ * To learn more about the SR-UKF, and see the exact formulation that most of this
+ * implementation follows, read:
+ * https://www.researchgate.net/publication/3908304
+ *
+ * The sigma points are not generated symmetrically around the mean, instead they
+ * are generated as vertices of a simplex. Using N = # of states, this method uses
+ * N + 2 sigma points instead of the standard 2N + 1 sigma points. This reduces
+ * computation up to 50%. To learn more about this method, read:
+ * https://www.sciencedirect.com/science/article/pii/S0888327020308190
+ * 
+ * This filter uses a method of "recalibrating" by essentially applying a measurement
+ * twice instead of once, and only using it if it is more accurate than before the
+ * measurement was applied. To learn more about this framework for nonlinear filters,
+ * read:
+ * https://arxiv.org/pdf/2407.05717
+ *
+ * @tparam STATES Dimension of the state vector.
+ * @tparam INPUTS Dimension of the control input vector.
+ * @tparam OUTPUTS Dimension of the measurement vector.
+ */
+template <int STATES, int INPUTS, int OUTPUTS> class SquareRootUnscentedKalmanFilter {
+  public:
+    static constexpr int NUM_SIGMAS = STATES + 2;
+
+    using StateVector = EVec<STATES>;
+    using InputVector = EVec<INPUTS>;
+    using OutputVector = EVec<OUTPUTS>;
+
+    using StateMatrix = EMat<STATES, STATES>;
+
+    using WithInputIntegrator = std::function<EVec<STATES>(
+      const WithInputDerivative<STATES, INPUTS> &f, const EVec<STATES> &x, const EVec<INPUTS> &u, const double &h
+    )>;
+
+    /**
+     * Constructs an Unscented Kalman Filter.
+     *
+     * @param f A vector valued function of x and u that returns the derivative of
+     * the state vector with respect to time.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param integrator A function from "numerical_integration.h" that integrates
+     * a differential equation of the form f(x, u).
+     * @param state_stddevs Standard deviations of the states in the model.
+     * @param measurement_stddevs Standard deviations of the measurements.
+     */
+    SquareRootUnscentedKalmanFilter(
+      const std::function<StateVector(const StateVector &, const InputVector &)> &f,
+      const std::function<OutputVector(const StateVector &, const InputVector &)> &h,
+      const WithInputIntegrator &integrator, const StateVector &state_stddevs, const OutputVector &measurement_stddevs
+    )
+        : m_f(f), m_h(h), m_integrator(integrator) {
+        m_sqrt_Q = state_stddevs.asDiagonal();
+        m_measurement_stddevs = measurement_stddevs;
+        m_mean_func_X = [](const EMat<STATES, NUM_SIGMAS> &sigmas, const EVec<NUM_SIGMAS> &Wm) -> StateVector {
+            return sigmas * Wm;
+        };
+
+        m_mean_func_Y = [](const EMat<OUTPUTS, NUM_SIGMAS> &sigmas, const EVec<NUM_SIGMAS> &Wc) -> OutputVector {
+            return sigmas * Wc;
+        };
+        m_residual_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a - b; };
+        m_residual_func_Y = [](const OutputVector &a, const OutputVector &b) -> OutputVector { return a - b; };
+        m_add_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a + b; };
+
+        reset();
+    }
+
+    /**
+     * Constructs an Unscented Kalman Filter with custom mean, residual, and
+     * addition functions. The most common use for these functions is when you
+     * are estimating angles whose arithmetic operations need to be wrapped.
+     *
+     * @param f A vector valued function of x and u that returns the derivative of
+     * the state vector with respect to time.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param integrator A function from "numerical_integration.h" that integrates
+     * a differential equation of the form f(x, u).
+     * @param state_stddevs Standard deviations of the states in the model.
+     * @param measurement_stddevs Standard deviations of the measurements.
+     * @param mean_func_X A function that computes the mean of a matrix
+     * containing NUM_SIGMAS state sigma points with a set of weights for each.
+     * @param mean_func_Y A function that computes the mean of a matrix
+     * containing NUM_SIGMAS measurement sigma points with a set of weights for each.
+     * @param residual_func_X A function that computes the residual of two state
+     * vectors, usually by simple subtraction.
+     * @param residual_func_Y A function that computes the residual of two measurement
+     * vectors, usually by simple subtraction.
+     * @param add_funx_X A function that adds two state vectors.
+     */
+    SquareRootUnscentedKalmanFilter(
+      const std::function<StateVector(const StateVector &, const InputVector &)> &f,
+      const std::function<OutputVector(const StateVector &, const InputVector &)> &h,
+      const WithInputIntegrator &integrator, const StateVector &state_stddevs, const OutputVector &measurement_stddevs,
+      const std::function<StateVector(const EMat<STATES, NUM_SIGMAS> &, const EVec<NUM_SIGMAS> &)> &mean_func_X,
+      const std::function<OutputVector(const EMat<OUTPUTS, NUM_SIGMAS> &, const EVec<NUM_SIGMAS> &)> &mean_func_Y,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &residual_func_X,
+      const std::function<OutputVector(const OutputVector &, const OutputVector &)> &residual_func_Y,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &add_func_X
+    )
+        : m_f(f), m_h(h), m_integrator(integrator), m_mean_func_X(mean_func_X), m_mean_func_Y(mean_func_Y),
+          m_residual_func_X(residual_func_X), m_residual_func_Y(residual_func_Y), m_add_func_X(add_func_X) {
+        m_sqrt_Q = state_stddevs.asDiagonal();
+        m_measurement_stddevs = measurement_stddevs;
+
+        reset();
+    }
+
+    /**
+     * Returns the square-root covariance matrix S.
+     */
+    StateMatrix S() const { return m_S; }
+
+    /**
+     * Returns one element of the square-root covariance matrix S.
+     *
+     * @param i Row of S.
+     * @param j Column of S.
+     */
+    double S(int i, int j) const { return m_S(i, j); }
+
+    /**
+     * Set the current square-root covariance matrix S.
+     *
+     * @param S The new square-root covariance matrix S.
+     */
+    void set_S(const StateMatrix &S) { m_S = S; }
+
+    /**
+     * Returns the reconstructed covariance matrix P.
+     */
+    StateMatrix P() const { return m_S * m_S.transpose(); }
+
+    /**
+     * Set the current square-root covariance matrix S to the square-root of P.
+     *
+     * @param P The covariance matrix P.
+     */
+    void set_P(const StateMatrix &P) { m_S = P.llt().matrixL(); }
+
+    /**
+     * Returns the current state estimate x-hat.
+     */
+    StateVector xhat() const { return m_xhat; }
+
+    /**
+     * Returns one element of the current state estimate x-hat.
+     *
+     * @param i Row of x-hat.
+     */
+    double xhat(int i) const { return m_xhat(i); }
+
+    /**
+     * Set the current state estimate x-hat.
+     */
+    void set_xhat(const StateVector &xhat) { m_xhat = xhat; }
+
+    /**
+     * Set one element of the current state estimate x-hat.
+     *
+     * @param i Row of x-hat.
+     */
+    void set_xhat(int i, double value) { m_xhat(i) = value; }
+
+    /**
+     * Resets the filter.
+     * Make sure to explicitly set S after calling this.
+     */
+    void reset() {
+        m_xhat.setZero();
+        m_S.setZero();
+        m_sigmas_F.setZero();
+    }
+
+    /**
+     * Projects the state into the future by dt seconds with control input u.
+     *
+     * @param u The control input.
+     * @param dt The timestep in seconds.
+     */
+    void predict(const InputVector &u, double dt) {
+        // Our noise is continuous, so we need to discretize
+        const EMat<STATES, STATES> Q = m_sqrt_Q * sqrt(dt);
+
+        // Generate sigma points around the state mean
+        //
+        // equation (17)
+        EMat<STATES, NUM_SIGMAS> sigmas = m_pts.square_root_sigma_points(m_xhat, m_S);
+
+        // Project each sigma point forward in time according to the
+        // dynamics f(x, u)
+        //
+        //   sigmas  = 𝒳ₖ₋₁
+        //   sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability
+        //
+        // equation (18)
+        for (int i = 0; i < NUM_SIGMAS; ++i) {
+            StateVector x = sigmas.template block<STATES, 1>(0, i);
+            m_sigmas_F.template block<STATES, 1>(0, i) = m_integrator(m_f, x, u, dt);
+        }
+
+        // Pass the predicted sigmas (𝒳) through the Unscented Transform
+        // to compute the prior state mean and covariance
+        //
+        // equations (18) (19) and (20)
+        auto [xhat, S] = square_root_ut<STATES, STATES>(
+          m_sigmas_F, m_pts.Wm(), m_pts.Wc(), m_mean_func_X, m_residual_func_X,
+          Q.template triangularView<Eigen::Lower>()
+        );
+
+        m_xhat = xhat;
+        m_S = S;
+    }
+
+    /**
+     * Correct the state estimate using the measurements in y.
+     *
+     * @param u The control input used in the last predict step.
+     * @param y The vector of measurements.
+     */
+    void correct(const InputVector &u, const OutputVector &y) {
+        correct<OUTPUTS>(
+          u, y, m_h, m_measurement_stddevs, m_mean_func_Y, m_residual_func_Y, m_residual_func_X, m_add_func_X
+        );
+    }
+
+    /**
+     * Correct the state estimate using the measurements in y, and custom standard
+     * deviations. This is useful for when the noise in the measurements vary.
+     *
+     * @param u The control input used in the last predict step.
+     * @param y The vector of measurements.
+     * @param measurement_stddevs The vector of standard deviations for each
+     * measurement to be used for this correct step.
+     */
+    void correct(const InputVector &u, const OutputVector &y, const EVec<OUTPUTS> &measurement_stddevs) {
+        correct<OUTPUTS>(
+          u, y, m_h, measurement_stddevs, m_mean_func_Y, m_residual_func_Y, m_residual_func_X, m_add_func_X
+        );
+    }
+
+    /**
+     * Correct the state estimate using the measurements in y, a custom measurement
+     * function, and custom standard deviations. This is useful for when a different
+     * set of measurements are being applied.
+     *
+     * @param u The control input used in the last predict step.
+     * @param y The vector of measurements.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param measurement_stddevs The vector of standard deviations for each
+     * measurement to be used for this correct step.
+     */
+    template <int ROWS>
+    void correct(
+      const InputVector &u, const EVec<ROWS> &y,
+      const std::function<EVec<ROWS>(const StateVector &, const InputVector &)> &h,
+      const EVec<ROWS> &measurement_stddevs
+    ) {
+        auto mean_func_Y = [](const EMat<OUTPUTS, NUM_SIGMAS> &sigmas, const EVec<NUM_SIGMAS> &Wc) -> EVec<ROWS> {
+            return sigmas * Wc;
+        };
+        auto residual_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a - b; };
+        auto residual_func_Y = [](const EVec<ROWS> &a, const EVec<ROWS> &b) -> EVec<ROWS> { return a - b; };
+        auto add_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a + b; };
+
+        correct<ROWS>(u, y, h, measurement_stddevs, mean_func_Y, residual_func_Y, residual_func_X, add_func_X);
+    }
+
+    /**
+     * Correct the state estimate using the measurements in y, a custom measurement
+     * function, custom standard deviations, and custom mean, residual, and addition
+     * functions. This is useful for when a different set of measurements are being
+     * applied, and they require custom arithmetic functions.
+     *
+     * @param u The control input used in the last predict step.
+     * @param y The vector of measurements.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param measurement_stddevs The vector of standard deviations for each
+     * measurement to be used for this correct step.
+     * @param mean_func_Y A function that computes the mean of a matrix
+     * containing NUM_SIGMAS measurement sigma points with a set of weights for each.
+     * @param residual_func_X A function that computes the residual of two state
+     * vectors, usually by simple subtraction.
+     * @param residual_func_Y A function that computes the residual of two measurement
+     * vectors, usually by simple subtraction.
+     * @param add_funx_X A function that adds two state vectors.
+     */
+    template <int ROWS>
+    void correct(
+      const InputVector &u, const EVec<ROWS> &y,
+      const std::function<EVec<ROWS>(const StateVector &, const InputVector &)> &h,
+      const EVec<ROWS> measurement_stddevs,
+      const std::function<EVec<ROWS>(const EMat<ROWS, NUM_SIGMAS> &, const EVec<NUM_SIGMAS> &)> &mean_func_Y,
+      const std::function<EVec<ROWS>(const EVec<ROWS> &, const EVec<ROWS> &)> &residual_func_Y,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &residual_func_X,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &add_func_X
+    ) {
+
+      EMat<ROWS, ROWS> sqrt_R = measurement_stddevs.asDiagonal();
+
+      // Generate new sigma points from the prior mean and covariance
+      // and transform them into measurement space using h(x, u)
+      //
+      //   sigmas  = 𝒳
+      //   sigmasH = 𝒴
+      //
+      // This differs from equation (22) which uses
+      // the prior sigma points, regenerating them allows
+      // multiple measurement updates per time update
+      EMat<ROWS, NUM_SIGMAS> sigmas_H;
+      EMat<STATES, NUM_SIGMAS> sigmas = m_pts.square_root_sigma_points(m_xhat, m_S);
+      for (int i = 0; i < NUM_SIGMAS; ++i) {
+        sigmas_H.template block<ROWS, 1>(0, i) =
+            h(sigmas.template block<STATES, 1>(0, i), u);
+      }
+  
+      // Pass the predicted measurement sigmas through the Unscented Transform
+      // to compute the mean predicted measurement and square-root innovation
+      // covariance.
+      //
+      // equations (23) (24) and (25)
+      auto [yhat, Sy] = square_root_ut<ROWS, STATES, NUM_SIGMAS>(
+          sigmas_H, m_pts.Wm(), m_pts.Wc(), mean_func_Y, residual_func_Y,
+          sqrt_R.template triangularView<Eigen::Lower>());
+  
+      // Compute cross covariance of the predicted state and measurement sigma
+      // points given as:
+      //
+      //           L+1
+      //   P_{xy} = Σ Wᵢ⁽ᶜ⁾[𝒳ᵢ - x̂][𝒴ᵢ - ŷ⁻]ᵀ
+      //           i=0
+      //
+      // equation (26)
+      EMat<STATES, ROWS> Pxy;
+      Pxy.setZero();
+      for (int i = 0; i < NUM_SIGMAS; ++i) {
+        Pxy += m_pts.Wc(i) *
+               (residual_func_X(m_sigmas_F.template block<STATES, 1>(0, i),
+                                m_xhat)) *
+               (residual_func_Y(sigmas_H.template block<ROWS, 1>(0, i), yhat))
+                   .transpose();
+      }
+  
+      // Compute the Kalman gain. We use Eigen's forward and backward substitution
+      // to solve. The equation in the paper uses MATLAB's / operator, but Eigen's
+      // solvers act like the \ operator, so we need to rearrange the equation to
+      // use those.
+      //
+      //   K = (P_{xy} / S_{y}ᵀ) / S_{y}
+      //   K = (S_{y} \ P_{xy})ᵀ / S_{y}
+      //   K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ
+      //
+      // equation (27)
+        EMat<STATES, ROWS> K = (Sy.transpose().template triangularView<Eigen::Upper>().solve(
+                                  Sy.template triangularView<Eigen::Lower>().solve(Pxy.transpose())
+                                ))
+              .transpose();
+  
+      // Compute the posterior state mean
+      //
+      //   x̂ = x̂⁻ + K(y − ŷ⁻)
+      //
+      // second part of equation (27)
+      EVec<STATES> xhat_dot = K * residual_func_Y(y, yhat);
+      EVec<STATES> xhat = add_func_X(m_xhat, xhat_dot);
+      EMat<STATES, STATES> S = m_S;
+  
+      // RECALIBRATE
+  
+      // Add the change of xhat to each of the sigma points in 𝒳.
+      for (int i = 0; i < NUM_SIGMAS; i++) {
+          sigmas.template block<STATES, 1>(0, i) += (xhat_dot);
+      }
+  
+      // Pass those sigma points through the measurement function to transform
+      // them into measurement space.
+      for (int i = 0; i < NUM_SIGMAS; ++i) {
+          sigmas_H.template block<ROWS, 1>(0, i) = h(sigmas.template block<STATES, 1>(0, i), u);
+      }
+  
+      // Perform a second unscented transform, this time on the recalibrated
+      // measurement sigma points.
+      auto [yhat_k, Sy_k] = square_root_ut<ROWS, STATES, NUM_SIGMAS>(
+          sigmas_H, m_pts.Wm(), m_pts.Wc(), mean_func_Y, residual_func_Y,
+          sqrt_R.template triangularView<Eigen::Lower>());
+  
+      // Compute the cross covariance of the recalibrated sigma points.
+      Pxy.setZero();
+      for (int i = 0; i < NUM_SIGMAS; ++i) {
+          Pxy += m_pts.Wc(i) *
+                  (residual_func_X(sigmas.template block<STATES, 1>(0, i),
+                                  xhat)) *
+                  (residual_func_Y(sigmas_H.template block<ROWS, 1>(0, i), yhat_k))
+                      .transpose();
+      }
+  
+      // Compute the intermediate matrix U for downdating
+      // the square-root covariance
+      //
+      // equation (28)
+      const EMat<STATES, ROWS> U = K * Sy;
+  
+      // Downdate the posterior square-root state covariance
+      //
+      // equation (29)
+      for (int i = 0; i < ROWS; i++) {
+        Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(
+            S, U.template block<STATES, 1>(0, i), -1);
+      }
+  
+      // BACK OUT
+  
+      // We only use the posterior state and covariance if it is more certain
+      // than the prior.
+      if (m_S.trace() > S.trace()) {
+          m_xhat = xhat;
+          m_S = S;
+      }
+    }
+
+  private:
+    std::function<StateVector(const StateVector &, const InputVector &)> m_f;
+    std::function<OutputVector(const StateVector &, const InputVector &)> m_h;
+    std::function<StateVector(const EMat<STATES, NUM_SIGMAS> &, const EVec<NUM_SIGMAS> &)> m_mean_func_X;
+    std::function<OutputVector(const EMat<OUTPUTS, NUM_SIGMAS> &, const EVec<NUM_SIGMAS> &)> m_mean_func_Y;
+    std::function<StateVector(const StateVector &, const StateVector &)> m_residual_func_X;
+    std::function<OutputVector(const OutputVector &, const OutputVector &)> m_residual_func_Y;
+    std::function<StateVector(const StateVector &, const StateVector &)> m_add_func_X;
+    StateVector m_xhat;
+    StateMatrix m_S;
+    StateMatrix m_sqrt_Q;
+    EVec<OUTPUTS> m_measurement_stddevs;
+    EMat<STATES, NUM_SIGMAS> m_sigmas_F;
+
+    ScaledSphericalSimplexSigmaPoints<STATES> m_pts;
+
+    WithInputIntegrator m_integrator;
+};
+
+/**
+ * Computes the Unscented Transform of a set of sigma points and their weights.
+ * The mean and square-root covariance of the sigma points are returned in a tuple.
+ *
+ * @tparam COV_DIM Dimension of the covariance of the sigma points after they are
+ * passed through a transforming function.
+ * @tparam STATES Dimension of the state vector.
+ *
+ * @param sigmas Matrix containing the sigma points, each column is one sigma point.
+ * @param Wm The weights for the mean.
+ * @param Wc The weights for the covariance.
+ * @param mean_func A function that computes the mean of NUM_SIGMAS sigma points
+ * using a given set of weights.
+ * @param residual_func A function that computes the residual of two sigma points.
+ * This is usually a simple subtraction.
+ * @param sqrt_R Square-root of the noise covariance of the sigma points.
+ *
+ * @return Tuple of x, and S, the mean and square-root covariance of the sigma points.
+ */
+template <int COV_DIM, int STATES, int NUM_SIGMAS>
+std::tuple<EVec<COV_DIM>, EMat<COV_DIM, COV_DIM>> square_root_ut(
+  const EMat<COV_DIM, NUM_SIGMAS> &sigmas, const EVec<NUM_SIGMAS> &Wm, const EVec<NUM_SIGMAS> &Wc,
+  const std::function<EVec<COV_DIM>(const EMat<COV_DIM, NUM_SIGMAS> &, const EVec<NUM_SIGMAS> &)> &mean_func,
+  const std::function<EVec<COV_DIM>(const EVec<COV_DIM> &, const EVec<COV_DIM> &)> &residual_func,
+  const EMat<COV_DIM, COV_DIM> &sqrt_R
+) {
+    // New mean is usually just the sum of the sigmas * weights:
+    //
+    //      L+1
+    //   x̂ = Σ Wᵢ⁽ᵐ⁾𝒳ᵢ
+    //      i=0
+    //
+    // equations (19) and (23) in the paper show this,
+    // but we allow a custom function, usually for angle wrapping
+    EVec<COV_DIM> x = mean_func(sigmas, Wm);
+
+    // Form an intermediate matrix S⁻ as:
+    //
+    //   [√{W₁⁽ᶜ⁾}(𝒳_{1:L+1} - x̂) √{Rᵛ}]
+    //
+    // the part of equations (20) and (24) within the "qr{}"
+    EMat<COV_DIM, NUM_SIGMAS - 1 + COV_DIM> S_bar;
+    for (int i = 0; i < NUM_SIGMAS - 1; i++) {
+        S_bar.template block<COV_DIM, 1>(0, i) =
+          std::sqrt(Wc[1]) * residual_func(sigmas.template block<COV_DIM, 1>(0, i + 1), x);
+    }
+    S_bar.template block<COV_DIM, COV_DIM>(0, NUM_SIGMAS - 1) = sqrt_R;
+
+    // Compute the square-root covariance of the sigma points.
+    //
+    // We transpose S⁻ first because we formed it by horizontally
+    // concatenating each part; it should be vertical so we can take
+    // the QR decomposition as defined in the "QR Decomposition" passage
+    // of section 3. "EFFICIENT SQUARE-ROOT IMPLEMENTATION"
+    //
+    // The resulting matrix R is the square-root covariance S, but it
+    // is upper triangular, so we need to transpose it.
+    //
+    // equations (20) and (24)
+    EMat<COV_DIM, COV_DIM> S = S_bar.transpose()
+                                 .householderQr()
+                                 .matrixQR()
+                                 .template block<COV_DIM, COV_DIM>(0, 0)
+                                 .template triangularView<Eigen::Upper>()
+                                 .transpose();
+
+    // Update or downdate the square-root covariance with (𝒳₀-x̂)
+    // depending on whether its weight (W₀⁽ᶜ⁾) is positive or negative.
+    //
+    // equations (21) and (25)
+    Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(
+      S, residual_func(sigmas.template block<COV_DIM, 1>(0, 0), x), Wc[0]
+    );
+
+    return std::make_tuple(x, S);
+}
+
+/**
+ * Generates sigma points and weights according to the paper [1]
+ * This is very different from Wan and Merwe's formulation.
+ *
+ * This only requires N + 2 sigma points instead of 2N + 1 sigma points.
+ * Rather than generating sigma points symmetrically around the mean, it
+ * generates them as vertices of an N-simplex.
+ *
+ * The performance of the filter using this reduced set of sigma points is
+ * identical to the standard method, so there is no downside to using it here.
+ *
+ * [1] A Scaled Spherical Simplex Filter (S3F) with a decreased n + 2 sigma
+ *     points set size and equivalent 2n + 1 Unscented Kalman Filter (UKF)
+ *     accuracy
+ *
+ * @tparam STATES the dimension of the state. NUM_SIGMAS sigma points and
+ * weights will be generated.
+ */
+template <int STATES> class ScaledSphericalSimplexSigmaPoints {
+  public:
+    static constexpr int NUM_SIGMAS = STATES + 2;
+
+    /**
+     * Constructs a sigma point generator for Spherical Simplex sigma points
+     *
+     * @param alpha Determines the spread of the sigma points around the mean.
+     * Smaller values are closer to the mean, this is usually a small value.
+     * @param beta Incorporates prior knowledge of the distribution of the state.
+     * For Gaussian distributions, beta = 2 is optimal.
+     */
+    ScaledSphericalSimplexSigmaPoints(double alpha = 0.001, double beta = 2) { compute_weights(alpha, beta); }
+
+    /**
+     * Returns the number of sigma points, for simplex sigma points this is N+2.
+     */
+    int num_sigmas() { return NUM_SIGMAS; }
+
+    /**
+     * Computes the sigma points given a mean (x) and square-root covariance (S).
+     *
+     * @param x Vector of the means.
+     * @param S Square-root covariance.
+     *
+     * @return Matrix containing the sigma points. Each column contains one sigma
+     * point in the same space as x. The first column is the same as the mean,
+     * with the others arranged around the mean.
+     */
+    EMat<STATES, NUM_SIGMAS> square_root_sigma_points(const EVec<STATES> &x, const EMat<STATES, STATES> &S) {
+
+        EMat<STATES, NUM_SIGMAS> C = EMat<STATES, NUM_SIGMAS>::Zero();
+
+        for (int row = 0; row < STATES; row++) {
+            C.row(row).segment(1, row + 1).setConstant(-q(row) / (row + 1));
+        }
+        C.diagonal(2) = q;
+
+        EMat<STATES, NUM_SIGMAS> sigmas = S * C;
+        sigmas.colwise() += x;
+
+        return sigmas;
+    }
+
+    /**
+     * Returns a vector containing the weights of each sigma point for the mean.
+     */
+    const EVec<NUM_SIGMAS> &Wm() const { return m_Wm; }
+
+    /**
+     * Returns a vector containing the weights of each sigma point for the covariance.
+     */
+    const EVec<NUM_SIGMAS> &Wc() const { return m_Wc; }
+
+    /**
+     * Returns the weight for the i-th sigma point for the mean.
+     *
+     * @param i Element of the weights vector to return.
+     */
+    double Wm(int i) const { return m_Wm(i); }
+
+    /**
+     * Returns the weight for the i-th sigma point for the covariance.
+     *
+     * @param i Element of the weights vector to return.
+     */
+    double Wc(int i) const { return m_Wc(i); }
+
+  private:
+    EVec<NUM_SIGMAS> m_Wm;
+    EVec<NUM_SIGMAS> m_Wc;
+    EVec<STATES> q;
+
+    /**
+     * Computes the weights for the sigma points.
+     *
+     * @param alpha Determines the spread of the sigma points around the mean.
+     * Smaller values are closer to the mean, this is usually a small value.
+     * @param beta Incorporates prior knowledge of the distribution of the state.
+     * For Gaussian distributions, beta = 2 is optimal.
+     */
+    void compute_weights(double alpha, double beta) {
+        double c = 1 / (alpha * alpha * (STATES + 1));
+        m_Wm = EVec<NUM_SIGMAS>::Constant(c);
+        m_Wc = EVec<NUM_SIGMAS>::Constant(c);
+
+        m_Wm(0) = 1 - (1 / (alpha * alpha));
+        m_Wc(0) = 1 - (1 / (alpha * alpha)) + (1 - alpha * alpha + beta);
+
+        EVec<STATES> t;
+        for (int i = 0; i < STATES; i++) {
+            t(i) = i + 1;
+        }
+
+        q = alpha * ((t * (STATES + 1)).cwiseQuotient(t + EVec<STATES>::Ones())).cwiseSqrt();
+    }
+};
+
+// allow using both names
+template <int STATES, int INPUTS, int OUTPUTS> using SRUKF = SquareRootUnscentedKalmanFilter<STATES, INPUTS, OUTPUTS>;


Reviews:
user: login: cowsed
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-03-18T19:36:02Z
user: login: PascalSkylake
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-18T19:51:02Z
user: login: cowsed
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-03-19T00:58:27Z
user: login: cowsed
body: Looks good i think (notebook it tho lol)
state: APPROVED
author_association: COLLABORATOR
submitted_at: 2025-03-20T23:05:50Z

Review comments:
diff_hunk: @@ -0,0 +1,616 @@
+#pragma once
+
+#include <../vendor/eigen/Eigen/Dense>
path: include/utils/math/estimator/srukf.h
user: login: cowsed
body: You should be able to just do `#include "Eigen/Dense"` nowadays
created_at: 2025-03-18T19:04:58Z
author_association: COLLABORATOR
diff_hunk: @@ -0,0 +1,616 @@
+#pragma once
+
+#include <../vendor/eigen/Eigen/Dense>
+#include <functional>
+#include <tuple>
+
+#include "../core/include/utils/math/numerical/numerical_integration.h"
+
+// Forward declare the sigma points class, it is at the bottom of this file.
+template <int STATES> class ScaledSphericalSimplexSigmaPoints;
+
+// Forward declare the Unscented Transform function, it is after the SRUKF class itself.
+template <int COV_DIM, int STATES>
+std::tuple<Eigen::Vector<double, COV_DIM>, Eigen::Matrix<double, COV_DIM, COV_DIM>> square_root_ut(
+  const Eigen::Matrix<double, COV_DIM, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wm, const Eigen::Vector<double, STATES + 2> &Wc,
+  const std::function<Eigen::Vector<double, COV_DIM>(const Eigen::Matrix<double, COV_DIM, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func,
+  const std::function<Eigen::Vector<double, COV_DIM>(const Eigen::Vector<double, COV_DIM> &, const Eigen::Vector<double, COV_DIM> &)> &residual_func,
+  const Eigen::Matrix<double, COV_DIM, COV_DIM> &square_root_R
+);
+
+/**
+ * Kalman filters combine predictions from a model and measurements to estimate
+ * a system's true state.
+ *
+ * The Unscented Kalman Filter is a nonlinear estimator, meaning that the model
+ * used to predict how the state changes over time can be nonlinear. The model
+ * that determines the expected measurement given the current state can also be
+ * nonlinear.
+ *
+ * At each timestep, sigma points are generated close to the mean, they are all
+ * propagated forward in time according to the nonlinear model. The Unscented
+ * Transform uses the propagated sigma points to compute the prior state and
+ * covariance.
+ *
+ * When correcting the state and covariance with a measurement, sigma points are
+ * again generated, but are transformed into the measurement space using the
+ * measurement function. A Kalman gain matrix K is then computed, and used to
+ * update the state and covariance.
+ *
+ * To read more about Kalman filters and the standard UKF read:
+ * https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
+ *
+ * This implementation is somewhat non-standard. The square-root form of the UKF
+ * (SR-UKF) is used, and the way the sigma points are generated is different than
+ * most implementations. The square-root form is used to ensure that the
+ * covariance matrix remains positive definite.
+ *
+ * To learn more about the SR-UKF, and see the exact formulation that most of this
+ * implementation follows, read:
+ * https://www.researchgate.net/publication/3908304
+ *
+ * The sigma points are not generated symmetrically around the mean, instead they
+ * are generated as vertices of a simplex. Using N = # of states, this method uses
+ * N + 2 sigma points instead of the standard 2N + 1 sigma points. This reduces
+ * computation up to 50%. To learn more about this method, read:
+ * https://www.sciencedirect.com/science/article/pii/S0888327020308190
+ *
+ * @tparam STATES Dimension of the state vector.
+ * @tparam INPUTS Dimension of the control input vector.
+ * @tparam OUTPUTS Dimension of the measurement vector.
+ */
+template <int STATES, int INPUTS, int OUTPUTS> class SquareRootUnscentedKalmanFilter {
+  public:
+    using StateVector = Eigen::Vector<double, STATES>;
+    using InputVector = Eigen::Vector<double, INPUTS>;
+    using OutputVector = Eigen::Vector<double, OUTPUTS>;
+
+    using StateMatrix = Eigen::Matrix<double, STATES, STATES>;
+
+    using WithInputIntegrator = std::function<Eigen::Vector<double, STATES>(
+      const WithInputDerivative<STATES, INPUTS> &f, const Eigen::Vector<double, STATES> &x, const Eigen::Vector<double, INPUTS> &u, const double &h
+    )>;
+
+    /**
+     * Constructs an Unscented Kalman Filter.
+     *
+     * @param f A vector valued function of x and u that returns the derivative of
+     * the state vector with respect to time.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param integrator A function from "numerical_integration.h" that integrates
+     * a differential equation of the form f(x, u).
+     * @param state_stddevs Standard deviations of the states in the model.
+     * @param measurement_stddevs Standard deviations of the measurements.
+     */
+    SquareRootUnscentedKalmanFilter(
+      const std::function<StateVector(const StateVector &, const InputVector &)> &f,
+      const std::function<OutputVector(const StateVector &, const InputVector &)> &h, const WithInputIntegrator &integrator, const StateVector &state_stddevs,
+      const OutputVector &measurement_stddevs
+    )
+        : m_f(f), m_h(h), m_integrator(integrator) {
+        m_sqrt_Q = state_stddevs.asDiagonal();
+        m_measurement_stddevs = measurement_stddevs;
+        m_mean_func_X = [](const Eigen::Matrix<double, STATES, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wm) -> StateVector {
+            return sigmas * Wm;
+        };
+
+        m_mean_func_Y = [](const Eigen::Matrix<double, OUTPUTS, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wc) -> OutputVector {
+            return sigmas * Wc;
+        };
+        m_residual_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a - b; };
+        m_residual_func_Y = [](const OutputVector &a, const OutputVector &b) -> OutputVector { return a - b; };
+        m_add_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a + b; };
+
+        reset();
+    }
+
+    /**
+     * Constructs an Unscented Kalman Filter with custom mean, residual, and
+     * addition functions. The most common use for these functions is when you
+     * are estimating angles whose arithmetic operations need to be wrapped.
+     *
+     * @param f A vector valued function of x and u that returns the derivative of
+     * the state vector with respect to time.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param integrator A function from "numerical_integration.h" that integrates
+     * a differential equation of the form f(x, u).
+     * @param state_stddevs Standard deviations of the states in the model.
+     * @param measurement_stddevs Standard deviations of the measurements.
+     * @param mean_func_X A function that computes the mean of a matrix
+     * containing STATES + 2 state sigma points with a set of weights for each.
+     * @param mean_func_Y A function that computes the mean of a matrix
+     * containing STATES + 2 measurement sigma points with a set of weights for each.
+     * @param residual_func_X A function that computes the residual of two state
+     * vectors, usually by simple subtraction.
+     * @param residual_func_Y A function that computes the residual of two measurement
+     * vectors, usually by simple subtraction.
+     * @param add_funx_X A function that adds two state vectors.
+     */
+    SquareRootUnscentedKalmanFilter(
+      const std::function<StateVector(const StateVector &, const InputVector &)> &f,
+      const std::function<OutputVector(const StateVector &, const InputVector &)> &h, const WithInputIntegrator &integrator, const StateVector &state_stddevs,
+      const OutputVector &measurement_stddevs,
+      const std::function<StateVector(const Eigen::Matrix<double, STATES, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func_X,
+      const std::function<OutputVector(const Eigen::Matrix<double, OUTPUTS, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func_Y,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &residual_func_X,
+      const std::function<OutputVector(const OutputVector &, const OutputVector &)> &residual_func_Y,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &add_func_X
+    )
+        : m_f(f), m_h(h), m_integrator(integrator), m_mean_func_X(mean_func_X), m_mean_func_Y(mean_func_Y), m_residual_func_X(residual_func_X),
+          m_residual_func_Y(residual_func_Y), m_add_func_X(add_func_X) {
+        m_sqrt_Q = state_stddevs.asDiagonal();
+        m_measurement_stddevs = measurement_stddevs;
+
+        reset();
+    }
+
+    /**
+     * Returns the square-root covariance matrix S.
+     *
+     * @return the square-root covariance matrix S.
+     */
+    StateMatrix S() const { return m_S; }
+
+    /**
+     * Returns one element of the square-root covariance matrix S.
+     *
+     * @param i Row of S.
+     * @param j Column of S.
+     *
+     * @return one element of the square-root covariance matrix S.
+     */
+    double S(int i, int j) const { return m_S(i, j); }
+
+    /**
+     * Set the current square-root covariance matrix S.
+     *
+     * @param S The new square-root covariance matrix S.
+     */
+    void set_S(const StateMatrix &S) { m_S = S; }
+
+    /**
+     * Returns the reconstructed covariance matrix P.
+     *
+     * @return the reconstructed covariance matrix P.
+     */
+    StateMatrix P() const { return m_S * m_S.transpose(); }
+
+    /**
+     * Set the current square-root covariance matrix S to the square-root of P.
+     *
+     * @param P The covariance matrix P.
+     */
+    void set_P(const StateMatrix &P) { m_S = P.llt().matrixL(); }
+
+    /**
+     * Returns the current state estimate x-hat.
+     *
+     * @return the current state estimate x-hat.
+     */
+    StateVector xhat() const { return m_xhat; }
+
+    /**
+     * Returns one element of the current state estimate x-hat.
+     *
+     * @param i Row of x-hat.
+     *
+     * @return one element of the current state estimate x-hat.
+     */
+    double xhat(int i) const { return m_xhat(i); }
+
+    /**
+     * Set the current state estimate x-hat.
+     */
+    void set_xhat(const StateVector &xhat) { m_xhat = xhat; }
+
+    /**
+     * Set one element of the current state estimate x-hat.
+     *
+     * @param i Row of x-hat.
+     */
+    void set_xhat(int i, double value) { m_xhat(i) = value; }
+
+    /**
+     * Resets the filter.
+     * Make sure to explicitly set S after calling this.
+     */
+    void reset() {
+        m_xhat.setZero();
+        m_S.setZero();
+        m_sigmas_F.setZero();
+    }
+
+    /**
+     * Projects the state into the future by dt seconds with control input u.
+     *
+     * @param u The control input.
+     * @param dt The timestep in seconds.
+     */
+    void predict(const InputVector &u, double dt) {
+        // Generate sigma points around the state mean
+        //
+        // equation (17)
+        Eigen::Matrix<double, STATES, STATES + 2> sigmas = m_pts.square_root_sigma_points(m_xhat, m_S);
+
+        // Project each sigma point forward in time according to the
+        // dynamics f(x, u)
+        //
+        //   sigmas  = 𝒳ₖ₋₁
path: include/utils/math/estimator/srukf.h
user: login: cowsed
body: Hurray for unicode
created_at: 2025-03-18T19:22:06Z
author_association: COLLABORATOR
diff_hunk: @@ -0,0 +1,616 @@
+#pragma once
+
+#include <../vendor/eigen/Eigen/Dense>
+#include <functional>
+#include <tuple>
+
+#include "../core/include/utils/math/numerical/numerical_integration.h"
+
+// Forward declare the sigma points class, it is at the bottom of this file.
+template <int STATES> class ScaledSphericalSimplexSigmaPoints;
+
+// Forward declare the Unscented Transform function, it is after the SRUKF class itself.
+template <int COV_DIM, int STATES>
+std::tuple<Eigen::Vector<double, COV_DIM>, Eigen::Matrix<double, COV_DIM, COV_DIM>> square_root_ut(
+  const Eigen::Matrix<double, COV_DIM, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wm, const Eigen::Vector<double, STATES + 2> &Wc,
+  const std::function<Eigen::Vector<double, COV_DIM>(const Eigen::Matrix<double, COV_DIM, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func,
+  const std::function<Eigen::Vector<double, COV_DIM>(const Eigen::Vector<double, COV_DIM> &, const Eigen::Vector<double, COV_DIM> &)> &residual_func,
+  const Eigen::Matrix<double, COV_DIM, COV_DIM> &square_root_R
+);
+
+/**
+ * Kalman filters combine predictions from a model and measurements to estimate
+ * a system's true state.
+ *
+ * The Unscented Kalman Filter is a nonlinear estimator, meaning that the model
+ * used to predict how the state changes over time can be nonlinear. The model
+ * that determines the expected measurement given the current state can also be
+ * nonlinear.
+ *
+ * At each timestep, sigma points are generated close to the mean, they are all
+ * propagated forward in time according to the nonlinear model. The Unscented
+ * Transform uses the propagated sigma points to compute the prior state and
+ * covariance.
+ *
+ * When correcting the state and covariance with a measurement, sigma points are
+ * again generated, but are transformed into the measurement space using the
+ * measurement function. A Kalman gain matrix K is then computed, and used to
+ * update the state and covariance.
+ *
+ * To read more about Kalman filters and the standard UKF read:
+ * https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
+ *
+ * This implementation is somewhat non-standard. The square-root form of the UKF
+ * (SR-UKF) is used, and the way the sigma points are generated is different than
+ * most implementations. The square-root form is used to ensure that the
+ * covariance matrix remains positive definite.
+ *
+ * To learn more about the SR-UKF, and see the exact formulation that most of this
+ * implementation follows, read:
+ * https://www.researchgate.net/publication/3908304
+ *
+ * The sigma points are not generated symmetrically around the mean, instead they
+ * are generated as vertices of a simplex. Using N = # of states, this method uses
+ * N + 2 sigma points instead of the standard 2N + 1 sigma points. This reduces
+ * computation up to 50%. To learn more about this method, read:
+ * https://www.sciencedirect.com/science/article/pii/S0888327020308190
+ *
+ * @tparam STATES Dimension of the state vector.
+ * @tparam INPUTS Dimension of the control input vector.
+ * @tparam OUTPUTS Dimension of the measurement vector.
+ */
+template <int STATES, int INPUTS, int OUTPUTS> class SquareRootUnscentedKalmanFilter {
+  public:
+    using StateVector = Eigen::Vector<double, STATES>;
+    using InputVector = Eigen::Vector<double, INPUTS>;
+    using OutputVector = Eigen::Vector<double, OUTPUTS>;
+
+    using StateMatrix = Eigen::Matrix<double, STATES, STATES>;
+
+    using WithInputIntegrator = std::function<Eigen::Vector<double, STATES>(
+      const WithInputDerivative<STATES, INPUTS> &f, const Eigen::Vector<double, STATES> &x, const Eigen::Vector<double, INPUTS> &u, const double &h
+    )>;
+
+    /**
+     * Constructs an Unscented Kalman Filter.
+     *
+     * @param f A vector valued function of x and u that returns the derivative of
+     * the state vector with respect to time.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param integrator A function from "numerical_integration.h" that integrates
+     * a differential equation of the form f(x, u).
+     * @param state_stddevs Standard deviations of the states in the model.
+     * @param measurement_stddevs Standard deviations of the measurements.
+     */
+    SquareRootUnscentedKalmanFilter(
+      const std::function<StateVector(const StateVector &, const InputVector &)> &f,
+      const std::function<OutputVector(const StateVector &, const InputVector &)> &h, const WithInputIntegrator &integrator, const StateVector &state_stddevs,
+      const OutputVector &measurement_stddevs
+    )
+        : m_f(f), m_h(h), m_integrator(integrator) {
+        m_sqrt_Q = state_stddevs.asDiagonal();
+        m_measurement_stddevs = measurement_stddevs;
+        m_mean_func_X = [](const Eigen::Matrix<double, STATES, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wm) -> StateVector {
+            return sigmas * Wm;
+        };
+
+        m_mean_func_Y = [](const Eigen::Matrix<double, OUTPUTS, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wc) -> OutputVector {
+            return sigmas * Wc;
+        };
+        m_residual_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a - b; };
+        m_residual_func_Y = [](const OutputVector &a, const OutputVector &b) -> OutputVector { return a - b; };
+        m_add_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a + b; };
+
+        reset();
+    }
+
+    /**
+     * Constructs an Unscented Kalman Filter with custom mean, residual, and
+     * addition functions. The most common use for these functions is when you
+     * are estimating angles whose arithmetic operations need to be wrapped.
+     *
+     * @param f A vector valued function of x and u that returns the derivative of
+     * the state vector with respect to time.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param integrator A function from "numerical_integration.h" that integrates
+     * a differential equation of the form f(x, u).
+     * @param state_stddevs Standard deviations of the states in the model.
+     * @param measurement_stddevs Standard deviations of the measurements.
+     * @param mean_func_X A function that computes the mean of a matrix
+     * containing STATES + 2 state sigma points with a set of weights for each.
+     * @param mean_func_Y A function that computes the mean of a matrix
+     * containing STATES + 2 measurement sigma points with a set of weights for each.
+     * @param residual_func_X A function that computes the residual of two state
+     * vectors, usually by simple subtraction.
+     * @param residual_func_Y A function that computes the residual of two measurement
+     * vectors, usually by simple subtraction.
+     * @param add_funx_X A function that adds two state vectors.
+     */
+    SquareRootUnscentedKalmanFilter(
+      const std::function<StateVector(const StateVector &, const InputVector &)> &f,
+      const std::function<OutputVector(const StateVector &, const InputVector &)> &h, const WithInputIntegrator &integrator, const StateVector &state_stddevs,
+      const OutputVector &measurement_stddevs,
+      const std::function<StateVector(const Eigen::Matrix<double, STATES, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func_X,
+      const std::function<OutputVector(const Eigen::Matrix<double, OUTPUTS, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func_Y,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &residual_func_X,
+      const std::function<OutputVector(const OutputVector &, const OutputVector &)> &residual_func_Y,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &add_func_X
+    )
+        : m_f(f), m_h(h), m_integrator(integrator), m_mean_func_X(mean_func_X), m_mean_func_Y(mean_func_Y), m_residual_func_X(residual_func_X),
+          m_residual_func_Y(residual_func_Y), m_add_func_X(add_func_X) {
+        m_sqrt_Q = state_stddevs.asDiagonal();
+        m_measurement_stddevs = measurement_stddevs;
+
+        reset();
+    }
+
+    /**
+     * Returns the square-root covariance matrix S.
+     *
+     * @return the square-root covariance matrix S.
+     */
+    StateMatrix S() const { return m_S; }
+
+    /**
+     * Returns one element of the square-root covariance matrix S.
+     *
+     * @param i Row of S.
+     * @param j Column of S.
+     *
+     * @return one element of the square-root covariance matrix S.
+     */
+    double S(int i, int j) const { return m_S(i, j); }
+
+    /**
+     * Set the current square-root covariance matrix S.
+     *
+     * @param S The new square-root covariance matrix S.
+     */
+    void set_S(const StateMatrix &S) { m_S = S; }
+
+    /**
+     * Returns the reconstructed covariance matrix P.
+     *
+     * @return the reconstructed covariance matrix P.
+     */
+    StateMatrix P() const { return m_S * m_S.transpose(); }
+
+    /**
+     * Set the current square-root covariance matrix S to the square-root of P.
+     *
+     * @param P The covariance matrix P.
+     */
+    void set_P(const StateMatrix &P) { m_S = P.llt().matrixL(); }
+
+    /**
+     * Returns the current state estimate x-hat.
+     *
+     * @return the current state estimate x-hat.
+     */
+    StateVector xhat() const { return m_xhat; }
+
+    /**
+     * Returns one element of the current state estimate x-hat.
+     *
+     * @param i Row of x-hat.
+     *
+     * @return one element of the current state estimate x-hat.
+     */
+    double xhat(int i) const { return m_xhat(i); }
+
+    /**
+     * Set the current state estimate x-hat.
+     */
+    void set_xhat(const StateVector &xhat) { m_xhat = xhat; }
+
+    /**
+     * Set one element of the current state estimate x-hat.
+     *
+     * @param i Row of x-hat.
+     */
+    void set_xhat(int i, double value) { m_xhat(i) = value; }
+
+    /**
+     * Resets the filter.
+     * Make sure to explicitly set S after calling this.
+     */
+    void reset() {
+        m_xhat.setZero();
+        m_S.setZero();
+        m_sigmas_F.setZero();
+    }
+
+    /**
+     * Projects the state into the future by dt seconds with control input u.
+     *
+     * @param u The control input.
+     * @param dt The timestep in seconds.
+     */
+    void predict(const InputVector &u, double dt) {
+        // Generate sigma points around the state mean
+        //
+        // equation (17)
+        Eigen::Matrix<double, STATES, STATES + 2> sigmas = m_pts.square_root_sigma_points(m_xhat, m_S);
+
+        // Project each sigma point forward in time according to the
+        // dynamics f(x, u)
+        //
+        //   sigmas  = 𝒳ₖ₋₁
+        //   sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability
+        //
+        // equation (18)
+        for (int i = 0; i < m_pts.num_sigmas(); ++i) {
+            StateVector x = sigmas.template block<STATES, 1>(0, i);
+            m_sigmas_F.template block<STATES, 1>(0, i) = m_integrator(m_f, x, u, dt);
+        }
+
+        // Pass the predicted sigmas (𝒳) through the Unscented Transform
+        // to compute the prior state mean and covariance
+        //
+        // equations (18) (19) and (20)
+        auto [xhat, S] = square_root_ut<STATES, STATES>(
+          m_sigmas_F, m_pts.Wm(), m_pts.Wc(), m_mean_func_X, m_residual_func_X, m_sqrt_Q.template triangularView<Eigen::Lower>()
+        );
+
+        m_xhat = xhat;
+        m_S = S;
+    }
+
+    /**
+     * Correct the state estimate using the measurements in y.
+     *
+     * @param u The control input used in the last predict step.
+     * @param y The vector of measurements.
+     */
+    void correct(const InputVector &u, const OutputVector &y) {
+        correct<OUTPUTS>(u, y, m_h, m_measurement_stddevs, m_mean_func_Y, m_residual_func_Y, m_residual_func_X, m_add_func_X);
+    }
+
+    /**
+     * Correct the state estimate using the measurements in y, and custom standard
+     * deviations. This is useful for when the noise in the measurements vary.
+     *
+     * @param u The control input used in the last predict step.
+     * @param y The vector of measurements.
+     * @param measurement_stddevs The vector of standard deviations for each
+     * measurement to be used for this correct step.
+     */
+    void correct(const InputVector &u, const OutputVector &y, const Eigen::Vector<double, OUTPUTS> &measurement_stddevs) {
+        correct<OUTPUTS>(u, y, m_h, measurement_stddevs, m_mean_func_Y, m_residual_func_Y, m_residual_func_X, m_add_func_X);
+    }
+
+    /**
+     * Correct the state estimate using the measurements in y, a custom measurement
+     * function, and custom standard deviations. This is useful for when a different
+     * set of measurements are being applied.
+     *
+     * @param u The control input used in the last predict step.
+     * @param y The vector of measurements.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param measurement_stddevs The vector of standard deviations for each
+     * measurement to be used for this correct step.
+     */
+    template <int ROWS>
+    void correct(
+      const InputVector &u, const Eigen::Vector<double, ROWS> &y, const std::function<Eigen::Vector<double, ROWS>(const StateVector &, const InputVector &)> &h,
+      const Eigen::Vector<double, ROWS> &measurement_stddevs
+    ) {
+        auto mean_func_Y = [](
+                             const Eigen::Matrix<double, ROWS, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wc
+                           ) -> Eigen::Vector<double, ROWS> { return sigmas * Wc; };
+        auto residual_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a - b; };
+        auto residual_func_Y = [](const Eigen::Vector<double, ROWS> &a, const Eigen::Vector<double, ROWS> &b) -> Eigen::Vector<double, ROWS> { return a - b; };
+        auto add_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a + b; };
+
+        correct<ROWS>(u, y, h, measurement_stddevs, mean_func_Y, residual_func_Y, residual_func_X, add_func_X);
+    }
+
+    /**
+     * Correct the state estimate using the measurements in y, a custom measurement
+     * function, custom standard deviations, and custom mean, residual, and addition
+     * functions. This is useful for when a different set of measurements are being
+     * applied, and they require custom arithmetic functions.
+     *
+     * @param u The control input used in the last predict step.
+     * @param y The vector of measurements.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param measurement_stddevs The vector of standard deviations for each
+     * measurement to be used for this correct step.
+     * @param mean_func_Y A function that computes the mean of a matrix
+     * containing STATES + 2 measurement sigma points with a set of weights for each.
+     * @param residual_func_X A function that computes the residual of two state
+     * vectors, usually by simple subtraction.
+     * @param residual_func_Y A function that computes the residual of two measurement
+     * vectors, usually by simple subtraction.
+     * @param add_funx_X A function that adds two state vectors.
+     */
+    template <int ROWS>
+    void correct(
+      const InputVector &u, const Eigen::Vector<double, ROWS> &y, const std::function<Eigen::Vector<double, ROWS>(const StateVector &, const InputVector &)> &h,
+      const Eigen::Vector<double, ROWS> measurement_stddevs,
+      const std::function<Eigen::Vector<double, ROWS>(const Eigen::Matrix<double, ROWS, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func_Y,
+      const std::function<Eigen::Vector<double, ROWS>(const Eigen::Vector<double, ROWS> &, const Eigen::Vector<double, ROWS> &)> &residual_func_Y,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &residual_func_X,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &add_func_X
+    ) {
+
+        Eigen::Matrix<double, ROWS, ROWS> sqrt_R = measurement_stddevs.asDiagonal();
+
+        // Generate new sigma points from the prior mean and covariance
+        // and transform them into measurement space using h(x, u)
+        //
+        //   sigmas  = 𝒳
+        //   sigmasH = 𝒴
+        //
+        // This differs from equation (22) which uses
+        // the prior sigma points, regenerating them allows
+        // multiple measurement updates per time update
+        Eigen::Matrix<double, ROWS, STATES + 2> sigmas_H;
+        Eigen::Matrix<double, STATES, STATES + 2> sigmas = m_pts.square_root_sigma_points(m_xhat, m_S);
+        for (int i = 0; i < m_pts.num_sigmas(); ++i) {
+            sigmas_H.template block<ROWS, 1>(0, i) = h(sigmas.template block<STATES, 1>(0, i), u);
+        }
+
+        // Pass the predicted measurement sigmas through the Unscented Transform
+        // to compute the mean predicted measurement and square-root innovation
+        // covariance.
+        //
+        // equations (23) (24) and (25)
+        auto [yhat, Sy] =
+          square_root_ut<ROWS, STATES>(sigmas_H, m_pts.Wm(), m_pts.Wc(), mean_func_Y, residual_func_Y, sqrt_R.template triangularView<Eigen::Lower>());
+
+        // Compute cross covariance of the predicted state and measurement sigma
+        // points given as:
+        //
+        //           n+1
+        //   P_{xy} = Σ Wᵢ⁽ᶜ⁾[𝒳ᵢ - x̂][𝒴ᵢ - ŷ⁻]ᵀ
+        //           i=0
+        //
+        // equation (26)
+        Eigen::Matrix<double, STATES, ROWS> Pxy;
+        Pxy.setZero();
+        for (int i = 0; i < m_pts.num_sigmas(); ++i) {
+            Pxy += m_pts.Wc(i) * (residual_func_X(m_sigmas_F.template block<STATES, 1>(0, i), m_xhat)) *
+                   (residual_func_Y(sigmas_H.template block<ROWS, 1>(0, i), yhat)).transpose();
+        }
+
+        // Compute the Kalman gain. We use Eigen's QR decomposition to solve. This
+        // is equivalent to MATLAB's \ operator, so we need to rearrange to use
+        // that.
+        //
+        //   K = (P_{xy} / S_{y}ᵀ) / S_{y}
+        //   K = (S_{y} \ P_{xy})ᵀ / S_{y}
+        //   K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ
+        //
+        // equation (27)
+        Eigen::Matrix<double, STATES, ROWS> K = (Sy.transpose().fullPivHouseholderQr().solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))).transpose();
+
+        // Compute the posterior state mean
+        //
+        //   x̂ = x̂⁻ + K(y − ŷ⁻)
+        //
+        // second part of equation (27)
+        m_xhat = add_func_X(m_xhat, K * residual_func_Y(y, yhat));
+
+        // Compute the intermediate matrix U for downdating
+        // the square-root covariance
+        //
+        // equation (28)
+        Eigen::Matrix<double, STATES, ROWS> U = K * Sy;
+
+        // Downdate the posterior square-root state covariance
+        //
+        // equation (29)
+        for (int i = 0; i < ROWS; i++) {
+            Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(m_S, U.template block<STATES, 1>(0, i), -1);
+        }
+    }
+
+  private:
+    std::function<StateVector(const StateVector &, const InputVector &)> m_f;
+    std::function<OutputVector(const StateVector &, const InputVector &)> m_h;
+    std::function<StateVector(const Eigen::Matrix<double, STATES, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> m_mean_func_X;
path: include/utils/math/estimator/srukf.h
user: login: cowsed
body: This and the following line have a lot going on. Is there a handy name to give this type of functions. anything like
```c++
using XMeanFunc = std::function<...>;
using YMeanFunc = std::function<...>;
...
XResidualFunc m_mean_func_x;
YResidualFunc m_mean_func_y
```
Also, STATES+2 is the number of sigma points, right? Since you're writing STATES+2 everywhere it might be handy to have a name for that and make a (static constexpr) member like
```c++
class SRUKF{
     static constexpr int NUM_SIGMA_PTS = STATES+2;
...
    Eigen::Matrix<double, STATES, NUM_SIGMA_PTS> m_sigmas_F;

} 
```
and etc, i think itll make it a little clearer what those matrices represent.
created_at: 2025-03-18T19:29:15Z
author_association: COLLABORATOR
diff_hunk: @@ -0,0 +1,616 @@
+#pragma once
+
+#include <../vendor/eigen/Eigen/Dense>
+#include <functional>
+#include <tuple>
+
+#include "../core/include/utils/math/numerical/numerical_integration.h"
+
+// Forward declare the sigma points class, it is at the bottom of this file.
+template <int STATES> class ScaledSphericalSimplexSigmaPoints;
+
+// Forward declare the Unscented Transform function, it is after the SRUKF class itself.
+template <int COV_DIM, int STATES>
+std::tuple<Eigen::Vector<double, COV_DIM>, Eigen::Matrix<double, COV_DIM, COV_DIM>> square_root_ut(
+  const Eigen::Matrix<double, COV_DIM, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wm, const Eigen::Vector<double, STATES + 2> &Wc,
+  const std::function<Eigen::Vector<double, COV_DIM>(const Eigen::Matrix<double, COV_DIM, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func,
+  const std::function<Eigen::Vector<double, COV_DIM>(const Eigen::Vector<double, COV_DIM> &, const Eigen::Vector<double, COV_DIM> &)> &residual_func,
+  const Eigen::Matrix<double, COV_DIM, COV_DIM> &square_root_R
+);
+
+/**
+ * Kalman filters combine predictions from a model and measurements to estimate
+ * a system's true state.
+ *
+ * The Unscented Kalman Filter is a nonlinear estimator, meaning that the model
+ * used to predict how the state changes over time can be nonlinear. The model
+ * that determines the expected measurement given the current state can also be
+ * nonlinear.
+ *
+ * At each timestep, sigma points are generated close to the mean, they are all
+ * propagated forward in time according to the nonlinear model. The Unscented
+ * Transform uses the propagated sigma points to compute the prior state and
+ * covariance.
+ *
+ * When correcting the state and covariance with a measurement, sigma points are
+ * again generated, but are transformed into the measurement space using the
+ * measurement function. A Kalman gain matrix K is then computed, and used to
+ * update the state and covariance.
+ *
+ * To read more about Kalman filters and the standard UKF read:
+ * https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
+ *
+ * This implementation is somewhat non-standard. The square-root form of the UKF
+ * (SR-UKF) is used, and the way the sigma points are generated is different than
+ * most implementations. The square-root form is used to ensure that the
+ * covariance matrix remains positive definite.
+ *
+ * To learn more about the SR-UKF, and see the exact formulation that most of this
+ * implementation follows, read:
+ * https://www.researchgate.net/publication/3908304
+ *
+ * The sigma points are not generated symmetrically around the mean, instead they
+ * are generated as vertices of a simplex. Using N = # of states, this method uses
+ * N + 2 sigma points instead of the standard 2N + 1 sigma points. This reduces
+ * computation up to 50%. To learn more about this method, read:
+ * https://www.sciencedirect.com/science/article/pii/S0888327020308190
+ *
+ * @tparam STATES Dimension of the state vector.
+ * @tparam INPUTS Dimension of the control input vector.
+ * @tparam OUTPUTS Dimension of the measurement vector.
+ */
+template <int STATES, int INPUTS, int OUTPUTS> class SquareRootUnscentedKalmanFilter {
+  public:
+    using StateVector = Eigen::Vector<double, STATES>;
+    using InputVector = Eigen::Vector<double, INPUTS>;
+    using OutputVector = Eigen::Vector<double, OUTPUTS>;
+
+    using StateMatrix = Eigen::Matrix<double, STATES, STATES>;
+
+    using WithInputIntegrator = std::function<Eigen::Vector<double, STATES>(
+      const WithInputDerivative<STATES, INPUTS> &f, const Eigen::Vector<double, STATES> &x, const Eigen::Vector<double, INPUTS> &u, const double &h
+    )>;
+
+    /**
+     * Constructs an Unscented Kalman Filter.
+     *
+     * @param f A vector valued function of x and u that returns the derivative of
+     * the state vector with respect to time.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param integrator A function from "numerical_integration.h" that integrates
+     * a differential equation of the form f(x, u).
+     * @param state_stddevs Standard deviations of the states in the model.
+     * @param measurement_stddevs Standard deviations of the measurements.
+     */
+    SquareRootUnscentedKalmanFilter(
+      const std::function<StateVector(const StateVector &, const InputVector &)> &f,
+      const std::function<OutputVector(const StateVector &, const InputVector &)> &h, const WithInputIntegrator &integrator, const StateVector &state_stddevs,
+      const OutputVector &measurement_stddevs
+    )
+        : m_f(f), m_h(h), m_integrator(integrator) {
+        m_sqrt_Q = state_stddevs.asDiagonal();
+        m_measurement_stddevs = measurement_stddevs;
+        m_mean_func_X = [](const Eigen::Matrix<double, STATES, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wm) -> StateVector {
+            return sigmas * Wm;
+        };
+
+        m_mean_func_Y = [](const Eigen::Matrix<double, OUTPUTS, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wc) -> OutputVector {
+            return sigmas * Wc;
+        };
+        m_residual_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a - b; };
+        m_residual_func_Y = [](const OutputVector &a, const OutputVector &b) -> OutputVector { return a - b; };
+        m_add_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a + b; };
+
+        reset();
+    }
+
+    /**
+     * Constructs an Unscented Kalman Filter with custom mean, residual, and
+     * addition functions. The most common use for these functions is when you
+     * are estimating angles whose arithmetic operations need to be wrapped.
+     *
+     * @param f A vector valued function of x and u that returns the derivative of
+     * the state vector with respect to time.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param integrator A function from "numerical_integration.h" that integrates
+     * a differential equation of the form f(x, u).
+     * @param state_stddevs Standard deviations of the states in the model.
+     * @param measurement_stddevs Standard deviations of the measurements.
+     * @param mean_func_X A function that computes the mean of a matrix
+     * containing STATES + 2 state sigma points with a set of weights for each.
+     * @param mean_func_Y A function that computes the mean of a matrix
+     * containing STATES + 2 measurement sigma points with a set of weights for each.
+     * @param residual_func_X A function that computes the residual of two state
+     * vectors, usually by simple subtraction.
+     * @param residual_func_Y A function that computes the residual of two measurement
+     * vectors, usually by simple subtraction.
+     * @param add_funx_X A function that adds two state vectors.
+     */
+    SquareRootUnscentedKalmanFilter(
+      const std::function<StateVector(const StateVector &, const InputVector &)> &f,
+      const std::function<OutputVector(const StateVector &, const InputVector &)> &h, const WithInputIntegrator &integrator, const StateVector &state_stddevs,
+      const OutputVector &measurement_stddevs,
+      const std::function<StateVector(const Eigen::Matrix<double, STATES, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func_X,
+      const std::function<OutputVector(const Eigen::Matrix<double, OUTPUTS, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func_Y,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &residual_func_X,
+      const std::function<OutputVector(const OutputVector &, const OutputVector &)> &residual_func_Y,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &add_func_X
+    )
+        : m_f(f), m_h(h), m_integrator(integrator), m_mean_func_X(mean_func_X), m_mean_func_Y(mean_func_Y), m_residual_func_X(residual_func_X),
+          m_residual_func_Y(residual_func_Y), m_add_func_X(add_func_X) {
+        m_sqrt_Q = state_stddevs.asDiagonal();
+        m_measurement_stddevs = measurement_stddevs;
+
+        reset();
+    }
+
+    /**
+     * Returns the square-root covariance matrix S.
+     *
+     * @return the square-root covariance matrix S.
+     */
+    StateMatrix S() const { return m_S; }
+
+    /**
+     * Returns one element of the square-root covariance matrix S.
+     *
+     * @param i Row of S.
+     * @param j Column of S.
+     *
+     * @return one element of the square-root covariance matrix S.
+     */
+    double S(int i, int j) const { return m_S(i, j); }
+
+    /**
+     * Set the current square-root covariance matrix S.
+     *
+     * @param S The new square-root covariance matrix S.
+     */
+    void set_S(const StateMatrix &S) { m_S = S; }
+
+    /**
+     * Returns the reconstructed covariance matrix P.
+     *
+     * @return the reconstructed covariance matrix P.
+     */
+    StateMatrix P() const { return m_S * m_S.transpose(); }
+
+    /**
+     * Set the current square-root covariance matrix S to the square-root of P.
+     *
+     * @param P The covariance matrix P.
+     */
+    void set_P(const StateMatrix &P) { m_S = P.llt().matrixL(); }
+
+    /**
+     * Returns the current state estimate x-hat.
+     *
+     * @return the current state estimate x-hat.
+     */
+    StateVector xhat() const { return m_xhat; }
+
+    /**
+     * Returns one element of the current state estimate x-hat.
+     *
+     * @param i Row of x-hat.
+     *
+     * @return one element of the current state estimate x-hat.
+     */
+    double xhat(int i) const { return m_xhat(i); }
+
+    /**
+     * Set the current state estimate x-hat.
+     */
+    void set_xhat(const StateVector &xhat) { m_xhat = xhat; }
+
+    /**
+     * Set one element of the current state estimate x-hat.
+     *
+     * @param i Row of x-hat.
+     */
+    void set_xhat(int i, double value) { m_xhat(i) = value; }
+
+    /**
+     * Resets the filter.
+     * Make sure to explicitly set S after calling this.
+     */
+    void reset() {
+        m_xhat.setZero();
+        m_S.setZero();
+        m_sigmas_F.setZero();
+    }
+
+    /**
+     * Projects the state into the future by dt seconds with control input u.
+     *
+     * @param u The control input.
+     * @param dt The timestep in seconds.
+     */
+    void predict(const InputVector &u, double dt) {
+        // Generate sigma points around the state mean
+        //
+        // equation (17)
+        Eigen::Matrix<double, STATES, STATES + 2> sigmas = m_pts.square_root_sigma_points(m_xhat, m_S);
+
+        // Project each sigma point forward in time according to the
+        // dynamics f(x, u)
+        //
+        //   sigmas  = 𝒳ₖ₋₁
+        //   sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability
+        //
+        // equation (18)
+        for (int i = 0; i < m_pts.num_sigmas(); ++i) {
+            StateVector x = sigmas.template block<STATES, 1>(0, i);
+            m_sigmas_F.template block<STATES, 1>(0, i) = m_integrator(m_f, x, u, dt);
+        }
+
+        // Pass the predicted sigmas (𝒳) through the Unscented Transform
+        // to compute the prior state mean and covariance
+        //
+        // equations (18) (19) and (20)
+        auto [xhat, S] = square_root_ut<STATES, STATES>(
+          m_sigmas_F, m_pts.Wm(), m_pts.Wc(), m_mean_func_X, m_residual_func_X, m_sqrt_Q.template triangularView<Eigen::Lower>()
+        );
+
+        m_xhat = xhat;
+        m_S = S;
+    }
+
+    /**
+     * Correct the state estimate using the measurements in y.
+     *
+     * @param u The control input used in the last predict step.
+     * @param y The vector of measurements.
+     */
+    void correct(const InputVector &u, const OutputVector &y) {
+        correct<OUTPUTS>(u, y, m_h, m_measurement_stddevs, m_mean_func_Y, m_residual_func_Y, m_residual_func_X, m_add_func_X);
+    }
+
+    /**
+     * Correct the state estimate using the measurements in y, and custom standard
+     * deviations. This is useful for when the noise in the measurements vary.
+     *
+     * @param u The control input used in the last predict step.
+     * @param y The vector of measurements.
+     * @param measurement_stddevs The vector of standard deviations for each
+     * measurement to be used for this correct step.
+     */
+    void correct(const InputVector &u, const OutputVector &y, const Eigen::Vector<double, OUTPUTS> &measurement_stddevs) {
+        correct<OUTPUTS>(u, y, m_h, measurement_stddevs, m_mean_func_Y, m_residual_func_Y, m_residual_func_X, m_add_func_X);
+    }
+
+    /**
+     * Correct the state estimate using the measurements in y, a custom measurement
+     * function, and custom standard deviations. This is useful for when a different
+     * set of measurements are being applied.
+     *
+     * @param u The control input used in the last predict step.
+     * @param y The vector of measurements.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param measurement_stddevs The vector of standard deviations for each
+     * measurement to be used for this correct step.
+     */
+    template <int ROWS>
+    void correct(
+      const InputVector &u, const Eigen::Vector<double, ROWS> &y, const std::function<Eigen::Vector<double, ROWS>(const StateVector &, const InputVector &)> &h,
+      const Eigen::Vector<double, ROWS> &measurement_stddevs
+    ) {
+        auto mean_func_Y = [](
+                             const Eigen::Matrix<double, ROWS, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wc
+                           ) -> Eigen::Vector<double, ROWS> { return sigmas * Wc; };
+        auto residual_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a - b; };
+        auto residual_func_Y = [](const Eigen::Vector<double, ROWS> &a, const Eigen::Vector<double, ROWS> &b) -> Eigen::Vector<double, ROWS> { return a - b; };
+        auto add_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a + b; };
+
+        correct<ROWS>(u, y, h, measurement_stddevs, mean_func_Y, residual_func_Y, residual_func_X, add_func_X);
+    }
+
+    /**
+     * Correct the state estimate using the measurements in y, a custom measurement
+     * function, custom standard deviations, and custom mean, residual, and addition
+     * functions. This is useful for when a different set of measurements are being
+     * applied, and they require custom arithmetic functions.
+     *
+     * @param u The control input used in the last predict step.
+     * @param y The vector of measurements.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param measurement_stddevs The vector of standard deviations for each
+     * measurement to be used for this correct step.
+     * @param mean_func_Y A function that computes the mean of a matrix
+     * containing STATES + 2 measurement sigma points with a set of weights for each.
+     * @param residual_func_X A function that computes the residual of two state
+     * vectors, usually by simple subtraction.
+     * @param residual_func_Y A function that computes the residual of two measurement
+     * vectors, usually by simple subtraction.
+     * @param add_funx_X A function that adds two state vectors.
+     */
+    template <int ROWS>
+    void correct(
+      const InputVector &u, const Eigen::Vector<double, ROWS> &y, const std::function<Eigen::Vector<double, ROWS>(const StateVector &, const InputVector &)> &h,
+      const Eigen::Vector<double, ROWS> measurement_stddevs,
+      const std::function<Eigen::Vector<double, ROWS>(const Eigen::Matrix<double, ROWS, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func_Y,
+      const std::function<Eigen::Vector<double, ROWS>(const Eigen::Vector<double, ROWS> &, const Eigen::Vector<double, ROWS> &)> &residual_func_Y,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &residual_func_X,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &add_func_X
+    ) {
+
+        Eigen::Matrix<double, ROWS, ROWS> sqrt_R = measurement_stddevs.asDiagonal();
+
+        // Generate new sigma points from the prior mean and covariance
+        // and transform them into measurement space using h(x, u)
+        //
+        //   sigmas  = 𝒳
+        //   sigmasH = 𝒴
+        //
+        // This differs from equation (22) which uses
+        // the prior sigma points, regenerating them allows
+        // multiple measurement updates per time update
+        Eigen::Matrix<double, ROWS, STATES + 2> sigmas_H;
+        Eigen::Matrix<double, STATES, STATES + 2> sigmas = m_pts.square_root_sigma_points(m_xhat, m_S);
+        for (int i = 0; i < m_pts.num_sigmas(); ++i) {
+            sigmas_H.template block<ROWS, 1>(0, i) = h(sigmas.template block<STATES, 1>(0, i), u);
+        }
+
+        // Pass the predicted measurement sigmas through the Unscented Transform
+        // to compute the mean predicted measurement and square-root innovation
+        // covariance.
+        //
+        // equations (23) (24) and (25)
+        auto [yhat, Sy] =
+          square_root_ut<ROWS, STATES>(sigmas_H, m_pts.Wm(), m_pts.Wc(), mean_func_Y, residual_func_Y, sqrt_R.template triangularView<Eigen::Lower>());
+
+        // Compute cross covariance of the predicted state and measurement sigma
+        // points given as:
+        //
+        //           n+1
+        //   P_{xy} = Σ Wᵢ⁽ᶜ⁾[𝒳ᵢ - x̂][𝒴ᵢ - ŷ⁻]ᵀ
+        //           i=0
+        //
+        // equation (26)
+        Eigen::Matrix<double, STATES, ROWS> Pxy;
+        Pxy.setZero();
+        for (int i = 0; i < m_pts.num_sigmas(); ++i) {
+            Pxy += m_pts.Wc(i) * (residual_func_X(m_sigmas_F.template block<STATES, 1>(0, i), m_xhat)) *
+                   (residual_func_Y(sigmas_H.template block<ROWS, 1>(0, i), yhat)).transpose();
+        }
+
+        // Compute the Kalman gain. We use Eigen's QR decomposition to solve. This
+        // is equivalent to MATLAB's \ operator, so we need to rearrange to use
+        // that.
+        //
+        //   K = (P_{xy} / S_{y}ᵀ) / S_{y}
+        //   K = (S_{y} \ P_{xy})ᵀ / S_{y}
+        //   K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ
+        //
+        // equation (27)
+        Eigen::Matrix<double, STATES, ROWS> K = (Sy.transpose().fullPivHouseholderQr().solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))).transpose();
+
+        // Compute the posterior state mean
+        //
+        //   x̂ = x̂⁻ + K(y − ŷ⁻)
+        //
+        // second part of equation (27)
+        m_xhat = add_func_X(m_xhat, K * residual_func_Y(y, yhat));
+
+        // Compute the intermediate matrix U for downdating
+        // the square-root covariance
+        //
+        // equation (28)
+        Eigen::Matrix<double, STATES, ROWS> U = K * Sy;
+
+        // Downdate the posterior square-root state covariance
+        //
+        // equation (29)
+        for (int i = 0; i < ROWS; i++) {
+            Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(m_S, U.template block<STATES, 1>(0, i), -1);
+        }
+    }
+
+  private:
+    std::function<StateVector(const StateVector &, const InputVector &)> m_f;
+    std::function<OutputVector(const StateVector &, const InputVector &)> m_h;
+    std::function<StateVector(const Eigen::Matrix<double, STATES, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> m_mean_func_X;
path: include/utils/math/estimator/srukf.h
user: login: cowsed
body: or if you're feeling really crazy have ScaledSphericalSimplexSigmaPoints know how to calculate that then have SRUKF say `NUM_SIGMA_PTS = ScaledSphericalSimplexSigmaPoints<...>::thing`

created_at: 2025-03-18T19:35:14Z
author_association: COLLABORATOR
diff_hunk: @@ -0,0 +1,616 @@
+#pragma once
+
+#include <../vendor/eigen/Eigen/Dense>
+#include <functional>
+#include <tuple>
+
+#include "../core/include/utils/math/numerical/numerical_integration.h"
+
+// Forward declare the sigma points class, it is at the bottom of this file.
+template <int STATES> class ScaledSphericalSimplexSigmaPoints;
+
+// Forward declare the Unscented Transform function, it is after the SRUKF class itself.
+template <int COV_DIM, int STATES>
+std::tuple<Eigen::Vector<double, COV_DIM>, Eigen::Matrix<double, COV_DIM, COV_DIM>> square_root_ut(
+  const Eigen::Matrix<double, COV_DIM, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wm, const Eigen::Vector<double, STATES + 2> &Wc,
+  const std::function<Eigen::Vector<double, COV_DIM>(const Eigen::Matrix<double, COV_DIM, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func,
+  const std::function<Eigen::Vector<double, COV_DIM>(const Eigen::Vector<double, COV_DIM> &, const Eigen::Vector<double, COV_DIM> &)> &residual_func,
+  const Eigen::Matrix<double, COV_DIM, COV_DIM> &square_root_R
+);
+
+/**
+ * Kalman filters combine predictions from a model and measurements to estimate
+ * a system's true state.
+ *
+ * The Unscented Kalman Filter is a nonlinear estimator, meaning that the model
+ * used to predict how the state changes over time can be nonlinear. The model
+ * that determines the expected measurement given the current state can also be
+ * nonlinear.
+ *
+ * At each timestep, sigma points are generated close to the mean, they are all
+ * propagated forward in time according to the nonlinear model. The Unscented
+ * Transform uses the propagated sigma points to compute the prior state and
+ * covariance.
+ *
+ * When correcting the state and covariance with a measurement, sigma points are
+ * again generated, but are transformed into the measurement space using the
+ * measurement function. A Kalman gain matrix K is then computed, and used to
+ * update the state and covariance.
+ *
+ * To read more about Kalman filters and the standard UKF read:
+ * https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
+ *
+ * This implementation is somewhat non-standard. The square-root form of the UKF
+ * (SR-UKF) is used, and the way the sigma points are generated is different than
+ * most implementations. The square-root form is used to ensure that the
+ * covariance matrix remains positive definite.
+ *
+ * To learn more about the SR-UKF, and see the exact formulation that most of this
+ * implementation follows, read:
+ * https://www.researchgate.net/publication/3908304
+ *
+ * The sigma points are not generated symmetrically around the mean, instead they
+ * are generated as vertices of a simplex. Using N = # of states, this method uses
+ * N + 2 sigma points instead of the standard 2N + 1 sigma points. This reduces
+ * computation up to 50%. To learn more about this method, read:
+ * https://www.sciencedirect.com/science/article/pii/S0888327020308190
+ *
+ * @tparam STATES Dimension of the state vector.
+ * @tparam INPUTS Dimension of the control input vector.
+ * @tparam OUTPUTS Dimension of the measurement vector.
+ */
+template <int STATES, int INPUTS, int OUTPUTS> class SquareRootUnscentedKalmanFilter {
+  public:
+    using StateVector = Eigen::Vector<double, STATES>;
+    using InputVector = Eigen::Vector<double, INPUTS>;
+    using OutputVector = Eigen::Vector<double, OUTPUTS>;
+
+    using StateMatrix = Eigen::Matrix<double, STATES, STATES>;
+
+    using WithInputIntegrator = std::function<Eigen::Vector<double, STATES>(
+      const WithInputDerivative<STATES, INPUTS> &f, const Eigen::Vector<double, STATES> &x, const Eigen::Vector<double, INPUTS> &u, const double &h
+    )>;
+
+    /**
+     * Constructs an Unscented Kalman Filter.
+     *
+     * @param f A vector valued function of x and u that returns the derivative of
+     * the state vector with respect to time.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param integrator A function from "numerical_integration.h" that integrates
+     * a differential equation of the form f(x, u).
+     * @param state_stddevs Standard deviations of the states in the model.
+     * @param measurement_stddevs Standard deviations of the measurements.
+     */
+    SquareRootUnscentedKalmanFilter(
+      const std::function<StateVector(const StateVector &, const InputVector &)> &f,
+      const std::function<OutputVector(const StateVector &, const InputVector &)> &h, const WithInputIntegrator &integrator, const StateVector &state_stddevs,
+      const OutputVector &measurement_stddevs
+    )
+        : m_f(f), m_h(h), m_integrator(integrator) {
+        m_sqrt_Q = state_stddevs.asDiagonal();
+        m_measurement_stddevs = measurement_stddevs;
+        m_mean_func_X = [](const Eigen::Matrix<double, STATES, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wm) -> StateVector {
+            return sigmas * Wm;
+        };
+
+        m_mean_func_Y = [](const Eigen::Matrix<double, OUTPUTS, STATES + 2> &sigmas, const Eigen::Vector<double, STATES + 2> &Wc) -> OutputVector {
+            return sigmas * Wc;
+        };
+        m_residual_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a - b; };
+        m_residual_func_Y = [](const OutputVector &a, const OutputVector &b) -> OutputVector { return a - b; };
+        m_add_func_X = [](const StateVector &a, const StateVector &b) -> StateVector { return a + b; };
+
+        reset();
+    }
+
+    /**
+     * Constructs an Unscented Kalman Filter with custom mean, residual, and
+     * addition functions. The most common use for these functions is when you
+     * are estimating angles whose arithmetic operations need to be wrapped.
+     *
+     * @param f A vector valued function of x and u that returns the derivative of
+     * the state vector with respect to time.
+     * @param h A vector valued function of x and u that returns the expected
+     * measurement at the given state.
+     * @param integrator A function from "numerical_integration.h" that integrates
+     * a differential equation of the form f(x, u).
+     * @param state_stddevs Standard deviations of the states in the model.
+     * @param measurement_stddevs Standard deviations of the measurements.
+     * @param mean_func_X A function that computes the mean of a matrix
+     * containing STATES + 2 state sigma points with a set of weights for each.
+     * @param mean_func_Y A function that computes the mean of a matrix
+     * containing STATES + 2 measurement sigma points with a set of weights for each.
+     * @param residual_func_X A function that computes the residual of two state
+     * vectors, usually by simple subtraction.
+     * @param residual_func_Y A function that computes the residual of two measurement
+     * vectors, usually by simple subtraction.
+     * @param add_funx_X A function that adds two state vectors.
+     */
+    SquareRootUnscentedKalmanFilter(
+      const std::function<StateVector(const StateVector &, const InputVector &)> &f,
+      const std::function<OutputVector(const StateVector &, const InputVector &)> &h, const WithInputIntegrator &integrator, const StateVector &state_stddevs,
+      const OutputVector &measurement_stddevs,
+      const std::function<StateVector(const Eigen::Matrix<double, STATES, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func_X,
+      const std::function<OutputVector(const Eigen::Matrix<double, OUTPUTS, STATES + 2> &, const Eigen::Vector<double, STATES + 2> &)> &mean_func_Y,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &residual_func_X,
+      const std::function<OutputVector(const OutputVector &, const OutputVector &)> &residual_func_Y,
+      const std::function<StateVector(const StateVector &, const StateVector &)> &add_func_X
+    )
+        : m_f(f), m_h(h), m_integrator(integrator), m_mean_func_X(mean_func_X), m_mean_func_Y(mean_func_Y), m_residual_func_X(residual_func_X),
+          m_residual_func_Y(residual_func_Y), m_add_func_X(add_func_X) {
+        m_sqrt_Q = state_stddevs.asDiagonal();
+        m_measurement_stddevs = measurement_stddevs;
+
+        reset();
+    }
+
+    /**
+     * Returns the square-root covariance matrix S.
+     *
+     * @return the square-root covariance matrix S.
+     */
+    StateMatrix S() const { return m_S; }
+
+    /**
+     * Returns one element of the square-root covariance matrix S.
+     *
+     * @param i Row of S.
+     * @param j Column of S.
+     *
+     * @return one element of the square-root covariance matrix S.
+     */
+    double S(int i, int j) const { return m_S(i, j); }
+
+    /**
+     * Set the current square-root covariance matrix S.
+     *
+     * @param S The new square-root covariance matrix S.
+     */
+    void set_S(const StateMatrix &S) { m_S = S; }
+
+    /**
+     * Returns the reconstructed covariance matrix P.
+     *
+     * @return the reconstructed covariance matrix P.
+     */
+    StateMatrix P() const { return m_S * m_S.transpose(); }
+
+    /**
+     * Set the current square-root covariance matrix S to the square-root of P.
+     *
+     * @param P The covariance matrix P.
+     */
+    void set_P(const StateMatrix &P) { m_S = P.llt().matrixL(); }
+
+    /**
+     * Returns the current state estimate x-hat.
+     *
+     * @return the current state estimate x-hat.
+     */
+    StateVector xhat() const { return m_xhat; }
+
+    /**
+     * Returns one element of the current state estimate x-hat.
+     *
+     * @param i Row of x-hat.
+     *
+     * @return one element of the current state estimate x-hat.
+     */
+    double xhat(int i) const { return m_xhat(i); }
+
+    /**
+     * Set the current state estimate x-hat.
+     */
+    void set_xhat(const StateVector &xhat) { m_xhat = xhat; }
+
+    /**
+     * Set one element of the current state estimate x-hat.
+     *
+     * @param i Row of x-hat.
+     */
+    void set_xhat(int i, double value) { m_xhat(i) = value; }
+
+    /**
+     * Resets the filter.
+     * Make sure to explicitly set S after calling this.
+     */
+    void reset() {
+        m_xhat.setZero();
+        m_S.setZero();
+        m_sigmas_F.setZero();
+    }
+
+    /**
+     * Projects the state into the future by dt seconds with control input u.
+     *
+     * @param u The control input.
+     * @param dt The timestep in seconds.
+     */
+    void predict(const InputVector &u, double dt) {
+        // Generate sigma points around the state mean
+        //
+        // equation (17)
+        Eigen::Matrix<double, STATES, STATES + 2> sigmas = m_pts.square_root_sigma_points(m_xhat, m_S);
+
+        // Project each sigma point forward in time according to the
+        // dynamics f(x, u)
+        //
+        //   sigmas  = 𝒳ₖ₋₁
path: include/utils/math/estimator/srukf.h
user: login: PascalSkylake
body: https://www.unicodeit.net/ my goat
created_at: 2025-03-18T19:51:02Z
author_association: CONTRIBUTOR
path: include/utils/math/estimator/srukf.h
user: login: cowsed
body: I <3 EVec
created_at: 2025-03-19T00:58:23Z
author_association: COLLABORATOR

PR comments:
user: login: cowsed
created_at: 2025-03-18T19:37:22Z
body: Whens the vex::motor subclass with filtering built in coming :eyes:
author_association: COLLABORATOR
user: login: cowsed
created_at: 2025-03-19T00:21:18Z
body: wHERES THE NOTEBOKO ENTRY ON n+2
author_association: COLLABORATOR