Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix S3UKF tests (#8097)

Browse Source 
They weren't actually instantiating S3UKF.
This commit is contained in:
Tyler Veness
2025-07-20 22:18:31 -07:00
committed by GitHub
parent 5905a3ba27
commit 946ab9e98f
6 changed files with 57 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
View File

@@ -82,15 +82,14 @@ AngleAdd(int angleStateIdx) {
*
* @tparam CovDim Dimension of covariance of sigma points after passing through
* the transform.
* @tparam States Number of states.
* @tparam NumSigmas Number of sigma points.
* @param sigmas Sigma points.
* @param Wm Weights for the mean.
* @param angleStatesIdx The row containing the angles.
*/
template <int CovDim, int States>
Vectord<CovDim> AngleMean(const Matrixd<CovDim, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm,
int angleStatesIdx) {
template <int CovDim, int NumSigmas>
Vectord<CovDim> AngleMean(const Matrixd<CovDim, NumSigmas>& sigmas,
const Vectord<NumSigmas>& Wm, int angleStatesIdx) {
double sumSin = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
return std::sin(it);
}) *
@@ -113,15 +112,15 @@ Vectord<CovDim> AngleMean(const Matrixd<CovDim, 2 * States + 1>& sigmas,
*
* @tparam CovDim Dimension of covariance of sigma points after passing through
* the transform.
* @tparam States Number of states.
* @tparam NumSigmas Number of sigma points.
* @param angleStateIdx The row containing the angles.
*/
template <int CovDim, int States>
std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
template <int CovDim, int NumSigmas>
std::function<Vectord<CovDim>(const Matrixd<CovDim, NumSigmas>&,
const Vectord<NumSigmas>&)>
AngleMean(int angleStateIdx) {
return [=](auto sigmas, auto Wm) {
return AngleMean<CovDim, States>(sigmas, Wm, angleStateIdx);
return AngleMean<CovDim, NumSigmas>(sigmas, Wm, angleStateIdx);
};
}

16
wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
View File

@@ -58,13 +58,13 @@ class MerweScaledSigmaPoints {
* Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
*
*/
Matrixd<States, 2 * States + 1> SquareRootSigmaPoints(
Matrixd<States, NumSigmas> SquareRootSigmaPoints(
const Vectord<States>& x, const Matrixd<States, States>& S) {
double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
double eta = std::sqrt(lambda + States);
Matrixd<States, States> U = eta * S;
Matrixd<States, 2 * States + 1> sigmas;
Matrixd<States, NumSigmas> sigmas;
// equation (17)
sigmas.template block<States, 1>(0, 0) = x;
@@ -81,7 +81,7 @@ class MerweScaledSigmaPoints {
/**
* Returns the weight for each sigma point for the mean.
*/
const Vectord<2 * States + 1>& Wm() const { return m_Wm; }
const Vectord<NumSigmas>& Wm() const { return m_Wm; }
/**
* Returns an element of the weight for each sigma point for the mean.
@@ -93,7 +93,7 @@ class MerweScaledSigmaPoints {
/**
* Returns the weight for each sigma point for the covariance.
*/
const Vectord<2 * States + 1>& Wc() const { return m_Wc; }
const Vectord<NumSigmas>& Wc() const { return m_Wc; }
/**
* Returns an element of the weight for each sigma point for the covariance.
@@ -103,8 +103,8 @@ class MerweScaledSigmaPoints {
double Wc(int i) const { return m_Wc(i, 0); }
private:
Vectord<2 * States + 1> m_Wm;
Vectord<2 * States + 1> m_Wc;
Vectord<NumSigmas> m_Wm;
Vectord<NumSigmas> m_Wc;
double m_alpha;
int m_kappa;
@@ -117,8 +117,8 @@ class MerweScaledSigmaPoints {
double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
double c = 0.5 / (States + lambda);
m_Wm = Vectord<2 * States + 1>::Constant(c);
m_Wc = Vectord<2 * States + 1>::Constant(c);
m_Wm = Vectord<NumSigmas>::Constant(c);
m_Wc = Vectord<NumSigmas>::Constant(c);
m_Wm(0) = lambda / (States + lambda);
m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta);

2
wpimath/src/main/native/include/frc/estimator/S3SigmaPoints.h
View File

@@ -58,7 +58,7 @@ class S3SigmaPoints {
Matrixd<States, NumSigmas> SquareRootSigmaPoints(
const Vectord<States>& x, const Matrixd<States, States>& S) const {
// table (1), equation (12)
wpi::array<double, States> q(wpi::empty_array);
wpi::array<double, States> q{wpi::empty_array};
for (size_t t = 1; t <= States; ++t) {
q[t - 1] = m_alpha * std::sqrt(static_cast<double>(t * (States + 1)) /
static_cast<double>(t + 1));