Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Implement Scaled Spherical Simplex Filter (S3F) (#8091)

Browse Source 
Adds S3SigmaPoints based on MerweScaledSigmaPoints. In addition, restructures UnscentedKalmanFilter to support different sigma point generators and provides MerweUKF and S3UKF for convenience when working with either kind of filter.

S3UKFTest is copied from MerweUKFTest (which is a rename of UnscentedKalmanFilterTest). Curiously, however, in Java the original tolerance used in MerweUKFTest.testDriveConvergence() for the final rotation was too low for S3UKFTest, so the tolerance is increased from 0.000005 (5e-6) radians to 0.00015 (1.5e-4) radians. However, the C++ version still uses the original tolerance. (This difference is probably because Java uses a final rotation of 5.846 degrees while C++ uses a final rotation of 5.846 radians)

Closes #8072.

Breaking changes:

- (C++) UnscentedKalmanFilter has a new template parameter for the sigma point generator type.
- (Java) UnscentedKalmanFilter has an additional parameter to every constructor providing an instance of a sigma point generator.
- (C++) int MerweScaledSigmaPoints.NumSigmas() has been replaced with constexpr int MerweScaledSigmaPoints::NumSigmas.
- (C++) The second parameter of SquareRootUnscentedTransform has been changed from States to NumSigmas.
This commit is contained in:
Joseph Eng
2025-07-15 21:17:25 -07:00
committed by GitHub
parent f03df5388e
commit 1530fccbd0
22 changed files with 1694 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
View File

@@ -15,7 +15,8 @@ import org.ejml.simple.SimpleMatrix;
* UnscentedKalmanFilter class.
*
* <p>It parametrizes the sigma points using alpha, beta, kappa terms, and is the version seen in
* most publications. Unless you know better, this should be your default choice.
* most publications. S3SigmaPoints is generally preferred due to its greater performance with
* nearly identical accuracy.
*
* <p>States is the dimensionality of the state. 2*States+1 weights will be generated.
*
@@ -24,7 +25,7 @@ import org.ejml.simple.SimpleMatrix;
*
* @param <S> The dimensionality of the state. 2 * States + 1 weights will be generated.
*/
public class MerweScaledSigmaPoints<S extends Num> {
public class MerweScaledSigmaPoints<S extends Num> implements SigmaPoints<S> {
private final double m_alpha;
private final int m_kappa;
private final Nat<S> m_states;
@@ -64,6 +65,7 @@ public class MerweScaledSigmaPoints<S extends Num> {
*
* @return The number of sigma points for each variable in the state x.
*/
@Override
public int getNumSigmas() {
return 2 * m_states.getNum() + 1;
}
@@ -77,6 +79,7 @@ public class MerweScaledSigmaPoints<S extends Num> {
* @return Two-dimensional array of sigma points. Each column contains all the sigmas for one
* dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
*/
@Override
public Matrix<S, ?> squareRootSigmaPoints(Matrix<S, N1> x, Matrix<S, S> s) {
double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
double eta = Math.sqrt(lambda + m_states.getNum());
@@ -125,6 +128,7 @@ public class MerweScaledSigmaPoints<S extends Num> {
*
* @return the weight for each sigma point for the mean.
*/
@Override
public Matrix<?, N1> getWm() {
return m_wm;
}
@@ -135,6 +139,7 @@ public class MerweScaledSigmaPoints<S extends Num> {
* @param element Element of vector to return.
* @return the element i's weight for the mean.
*/
@Override
public double getWm(int element) {
return m_wm.get(element, 0);
}
@@ -144,6 +149,7 @@ public class MerweScaledSigmaPoints<S extends Num> {
*
* @return the weight for each sigma point for the covariance.
*/
@Override
public Matrix<?, N1> getWc() {
return m_wc;
}
@@ -154,6 +160,7 @@ public class MerweScaledSigmaPoints<S extends Num> {
* @param element Element of vector to return.
* @return The element I's weight for the covariance.
*/
@Override
public double getWc(int element) {
return m_wc.get(element, 0);
}

117
wpimath/src/main/java/edu/wpi/first/math/estimator/MerweUKF.java Normal file
View File

@@ -0,0 +1,117 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;
import java.util.function.BiFunction;
/**
* An Unscented Kalman Filter using sigma points and weights from Van der Merwe's 2004 dissertation.
* S3UKF is generally preferred due to its greater performance with nearly identical accuracy.
*
* @param <States> Number of states.
* @param <Inputs> Number of inputs.
* @param <Outputs> Number of outputs.
*/
public class MerweUKF<States extends Num, Inputs extends Num, Outputs extends Num>
extends UnscentedKalmanFilter<States, Inputs, Outputs> {
/**
* Constructs a Merwe Unscented Kalman Filter.
*
* <p>See <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
* for how to select the standard deviations.
*
* @param states A Nat representing the number of states.
* @param outputs A Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns the derivative of the state vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param nominalDt Nominal discretization timestep in seconds.
*/
public MerweUKF(
Nat<States> states,
Nat<Outputs> outputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
double nominalDt) {
super(
new MerweScaledSigmaPoints<>(states),
states,
outputs,
f,
h,
stateStdDevs,
measurementStdDevs,
(sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)),
(sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)),
Matrix::minus,
Matrix::minus,
Matrix::plus,
nominalDt);
}
/**
* Constructs a Merwe Unscented Kalman filter with custom mean, residual, and addition functions.
* Using custom functions for arithmetic can be useful if you have angles in the state or
* measurements, because they allow you to correctly account for the modular nature of angle
* arithmetic.
*
* <p>See <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
* for how to select the standard deviations.
*
* @param states A Nat representing the number of states.
* @param outputs A Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns the derivative of the state vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param meanFuncX A function that computes the mean of NumSigmas state vectors using a given set
* of weights.
* @param meanFuncY A function that computes the mean of NumSigmas measurement vectors using a
* given set of weights.
* @param residualFuncX A function that computes the residual of two state vectors (i.e. it
* subtracts them.)
* @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
* subtracts them.)
* @param addFuncX A function that adds two state vectors.
* @param nominalDt Nominal discretization timestep in seconds.
*/
public MerweUKF(
Nat<States> states,
Nat<Outputs> outputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
BiFunction<Matrix<States, ?>, Matrix<?, N1>, Matrix<States, N1>> meanFuncX,
BiFunction<Matrix<Outputs, ?>, Matrix<?, N1>, Matrix<Outputs, N1>> meanFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> residualFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX,
double nominalDt) {
super(
new MerweScaledSigmaPoints<>(states),
states,
outputs,
f,
h,
stateStdDevs,
measurementStdDevs,
meanFuncX,
meanFuncY,
residualFuncX,
residualFuncY,
addFuncX,
nominalDt);
}
}

172
wpimath/src/main/java/edu/wpi/first/math/estimator/S3SigmaPoints.java Normal file
View File

@@ -0,0 +1,172 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;
import org.ejml.simple.SimpleMatrix;
/**
* Generates sigma points and weights according to Papakonstantinou's paper[1] for the
* UnscentedKalmanFilter class.
*
* <p>It parameterizes the sigma points using alpha and beta terms. Unless you know better, this
* should be your default choice due to its high accuracy and performance.
*
* <p>[1] K. Papakonstantinou "A Scaled Spherical Simplex Filter (S3F) with a decreased n + 2 sigma
* points set size and equivalent 2n + 1 Unscented Kalman Filter (UKF) accuracy"
*
* @param <States> The dimenstionality of the state. States + 2 weights will be generated.
*/
public class S3SigmaPoints<States extends Num> implements SigmaPoints<States> {
private final Nat<States> m_states;
private final double m_alpha;
private Matrix<?, N1> m_wm;
private Matrix<?, N1> m_wc;
/**
* Constructs a generator for Papakonstantinou sigma points.
*
* @param states an instance of Num that represents the number of states.
* @param alpha Determines the spread of the sigma points around the mean. Usually a small
* positive value (1e-3).
* @param beta Incorporates prior knowledge of the distribution of the mean. For Gaussian
* distributions, beta = 2 is optimal.
*/
@SuppressWarnings("this-escape")
public S3SigmaPoints(Nat<States> states, double alpha, double beta) {
m_states = states;
m_alpha = alpha;
computeWeights(beta);
}
/**
* Constructs a generator for Papakonstantinou sigma points with default values for alpha, beta,
* and kappa.
*
* @param states an instance of Num that represents the number of states.
*/
public S3SigmaPoints(Nat<States> states) {
this(states, 1e-3, 2);
}
/**
* Returns number of sigma points for each variable in the state x.
*
* @return The number of sigma points for each variable in the state x.
*/
@Override
public int getNumSigmas() {
return m_states.getNum() + 2;
}
/**
* Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root
* covariance (s) of the filter.
*
* @param x An array of the means.
* @param s Square-root covariance of the filter.
* @return Two-dimensional array of sigma points. Each column contains all the sigmas for one
* dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
*/
@Override
public Matrix<States, ?> squareRootSigmaPoints(Matrix<States, N1> x, Matrix<States, States> s) {
// table (1), equation (12)
double[] q = new double[m_states.getNum()];
for (int t = 1; t <= m_states.getNum(); ++t) {
q[t - 1] = m_alpha * Math.sqrt(t * (m_states.getNum() + 1) / (double) (t + 1));
}
Matrix<States, ?> C = new Matrix<>(new SimpleMatrix(m_states.getNum(), getNumSigmas()));
for (int row = 0; row < m_states.getNum(); ++row) {
C.set(row, 0, 0.0);
}
for (int col = 1; col < getNumSigmas(); ++col) {
for (int row = 0; row < m_states.getNum(); ++row) {
if (row < col - 2) {
C.set(row, col, 0.0);
} else if (row == col - 2) {
C.set(row, col, q[row]);
} else {
C.set(row, col, -q[row] / (row + 1));
}
}
}
Matrix<States, ?> sigmas = new Matrix<>(new SimpleMatrix(m_states.getNum(), getNumSigmas()));
for (int col = 0; col < getNumSigmas(); ++col) {
sigmas.setColumn(col, x.plus(s.times(C.extractColumnVector(col))));
}
return sigmas;
}
/**
* Computes the weights for the scaled unscented Kalman filter.
*
* @param beta Incorporates prior knowledge of the distribution of the mean.
*/
private void computeWeights(double beta) {
double alpha_sq = m_alpha * m_alpha;
double c = 1.0 / (alpha_sq * (m_states.getNum() + 1));
Matrix<?, N1> wM = new Matrix<>(new SimpleMatrix(getNumSigmas(), 1));
Matrix<?, N1> wC = new Matrix<>(new SimpleMatrix(getNumSigmas(), 1));
wM.fill(c);
wC.fill(c);
wM.set(0, 0, 1.0 - 1.0 / alpha_sq);
wC.set(0, 0, 1.0 - 1.0 / alpha_sq + (1 - alpha_sq + beta));
this.m_wm = wM;
this.m_wc = wC;
}
/**
* Returns the weight for each sigma point for the mean.
*
* @return the weight for each sigma point for the mean.
*/
@Override
public Matrix<?, N1> getWm() {
return m_wm;
}
/**
* Returns an element of the weight for each sigma point for the mean.
*
* @param element Element of vector to return.
* @return the element i's weight for the mean.
*/
@Override
public double getWm(int element) {
return m_wm.get(element, 0);
}
/**
* Returns the weight for each sigma point for the covariance.
*
* @return the weight for each sigma point for the covariance.
*/
@Override
public Matrix<?, N1> getWc() {
return m_wc;
}
/**
* Returns an element of the weight for each sigma point for the covariance.
*
* @param element Element of vector to return.
* @return The element I's weight for the covariance.
*/
@Override
public double getWc(int element) {
return m_wc.get(element, 0);
}
}

117
wpimath/src/main/java/edu/wpi/first/math/estimator/S3UKF.java Normal file
View File

@@ -0,0 +1,117 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;
import java.util.function.BiFunction;
/**
* An Unscented Kalman Filter using sigma points and weights from Papakonstantinou's paper. This is
* generally preferred over other UKF variants due to its high accuracy and performance.
*
* @param <States> Number of states.
* @param <Inputs> Number of inputs.
* @param <Outputs> Number of outputs.
*/
public class S3UKF<States extends Num, Inputs extends Num, Outputs extends Num>
extends UnscentedKalmanFilter<States, Inputs, Outputs> {
/**
* Constructs a Scaled Spherical Simplex (S3) Unscented Kalman Filter.
*
* <p>See <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
* for how to select the standard deviations.
*
* @param states A Nat representing the number of states.
* @param outputs A Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns the derivative of the state vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param nominalDt Nominal discretization timestep in seconds.
*/
public S3UKF(
Nat<States> states,
Nat<Outputs> outputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
double nominalDt) {
super(
new S3SigmaPoints<>(states),
states,
outputs,
f,
h,
stateStdDevs,
measurementStdDevs,
(sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)),
(sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)),
Matrix::minus,
Matrix::minus,
Matrix::plus,
nominalDt);
}
/**
* Constructs a Scaled Spherical Simplex (S3) Unscented Kalman filter with custom mean, residual,
* and addition functions. Using custom functions for arithmetic can be useful if you have angles
* in the state or measurements, because they allow you to correctly account for the modular
* nature of angle arithmetic.
*
* <p>See <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
* for how to select the standard deviations.
*
* @param states A Nat representing the number of states.
* @param outputs A Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns the derivative of the state vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param meanFuncX A function that computes the mean of NumSigmas state vectors using a given set
* of weights.
* @param meanFuncY A function that computes the mean of NumSigmas measurement vectors using a
* given set of weights.
* @param residualFuncX A function that computes the residual of two state vectors (i.e. it
* subtracts them.)
* @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
* subtracts them.)
* @param addFuncX A function that adds two state vectors.
* @param nominalDt Nominal discretization timestep in seconds.
*/
public S3UKF(
Nat<States> states,
Nat<Outputs> outputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
BiFunction<Matrix<States, ?>, Matrix<?, N1>, Matrix<States, N1>> meanFuncX,
BiFunction<Matrix<Outputs, ?>, Matrix<?, N1>, Matrix<Outputs, N1>> meanFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> residualFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX,
double nominalDt) {
super(
new S3SigmaPoints<>(states),
states,
outputs,
f,
h,
stateStdDevs,
measurementStdDevs,
meanFuncX,
meanFuncY,
residualFuncX,
residualFuncY,
addFuncX,
nominalDt);
}
}

64
wpimath/src/main/java/edu/wpi/first/math/estimator/SigmaPoints.java Normal file
View File

@@ -0,0 +1,64 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;
/**
* A sigma points generator for the UnscentedKalmanFilter class.
*
* @param <States> The dimensionality of the state.
*/
public interface SigmaPoints<States extends Num> {
/**
* Returns number of sigma points for each variable in the state x.
*
* @return The number of sigma points for each variable in the state x.
*/
int getNumSigmas();
/**
* Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root
* covariance (s) of the filter.
*
* @param x An array of the means.
* @param s Square-root covariance of the filter.
* @return Two-dimensional array of sigma points. Each column contains all the sigmas for one
* dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
*/
Matrix<States, ?> squareRootSigmaPoints(Matrix<States, N1> x, Matrix<States, States> s);
/**
* Returns the weight for each sigma point for the mean.
*
* @return the weight for each sigma point for the mean.
*/
Matrix<?, N1> getWm();
/**
* Returns an element of the weight for each sigma point for the mean.
*
* @param element Element of vector to return.
* @return the element i's weight for the mean.
*/
double getWm(int element);
/**
* Returns the weight for each sigma point for the covariance.
*
* @return the weight for each sigma point for the covariance.
*/
Matrix<?, N1> getWc();
/**
* Returns an element of the weight for each sigma point for the covariance.
*
* @param element Element of vector to return.
* @return The element I's weight for the covariance.
*/
double getWc(int element);
}

74
wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
View File

@@ -22,6 +22,11 @@ import org.ejml.simple.SimpleMatrix;
* true system state. This is useful because many states cannot be measured directly as a result of
* sensor noise, or because the state is "hidden".
*
* <p>This class's constructors require a SigmaPoints generator. For convenience, {@link S3UKF} and
* {@link MerweUKF} subclasses are provided to create a suitable generator for you. S3UKF is
* generally preferred over MerweUKF because of its greater performance while maintaining nearly
* identical accuracy.
*
* <p>Kalman filters use a K gain matrix to determine whether to trust the model or measurements
* more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum
* of squares error in the state estimate. This K gain is used to correct the state estimate by some
@@ -64,7 +69,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
private Matrix<States, ?> m_sigmasF;
private double m_dt;
private final MerweScaledSigmaPoints<States> m_pts;
private final SigmaPoints<States> m_pts;
/**
* Constructs an Unscented Kalman Filter.
@@ -73,6 +78,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
* for how to select the standard deviations.
*
* @param pts A sigma points and weights generator.
* @param states A Nat representing the number of states.
* @param outputs A Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns the derivative of the state vector.
@@ -82,6 +88,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
* @param nominalDt Nominal discretization timestep in seconds.
*/
public UnscentedKalmanFilter(
SigmaPoints<States> pts,
Nat<States> states,
Nat<Outputs> outputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
@@ -90,6 +97,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
Matrix<Outputs, N1> measurementStdDevs,
double nominalDt) {
this(
pts,
states,
outputs,
f,
@@ -113,16 +121,17 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a>
* for how to select the standard deviations.
*
* @param pts A sigma points and weights generator.
* @param states A Nat representing the number of states.
* @param outputs A Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns the derivative of the state vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param meanFuncX A function that computes the mean of 2 * States + 1 state vectors using a
* @param meanFuncX A function that computes the mean of NumSigmas state vectors using a given set
* of weights.
* @param meanFuncY A function that computes the mean of NumSigmas measurement vectors using a
* given set of weights.
* @param meanFuncY A function that computes the mean of 2 * States + 1 measurement vectors using
* a given set of weights.
* @param residualFuncX A function that computes the residual of two state vectors (i.e. it
* subtracts them.)
* @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
@@ -131,6 +140,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
* @param nominalDt Nominal discretization timestep in seconds.
*/
public UnscentedKalmanFilter(
SigmaPoints<States> pts,
Nat<States> states,
Nat<Outputs> outputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
@@ -160,37 +170,36 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
m_pts = new MerweScaledSigmaPoints<>(states);
m_pts = pts;
reset();
}
static <S extends Num, C extends Num>
Pair<Matrix<C, N1>, Matrix<C, C>> squareRootUnscentedTransform(
Nat<S> s,
Nat<C> dim,
Matrix<C, ?> sigmas,
Matrix<?, N1> Wm,
Matrix<?, N1> Wc,
BiFunction<Matrix<C, ?>, Matrix<?, N1>, Matrix<C, N1>> meanFunc,
BiFunction<Matrix<C, N1>, Matrix<C, N1>, Matrix<C, N1>> residualFunc,
Matrix<C, C> squareRootR) {
if (sigmas.getNumRows() != dim.getNum() || sigmas.getNumCols() != 2 * s.getNum() + 1) {
static <C extends Num> Pair<Matrix<C, N1>, Matrix<C, C>> squareRootUnscentedTransform(
Nat<C> covdim,
int numSigmas,
Matrix<C, ?> sigmas,
Matrix<?, N1> Wm,
Matrix<?, N1> Wc,
BiFunction<Matrix<C, ?>, Matrix<?, N1>, Matrix<C, N1>> meanFunc,
BiFunction<Matrix<C, N1>, Matrix<C, N1>, Matrix<C, N1>> residualFunc,
Matrix<C, C> squareRootR) {
if (sigmas.getNumRows() != covdim.getNum() || sigmas.getNumCols() != numSigmas) {
throw new IllegalArgumentException(
"Sigmas must be covDim by 2 * states + 1! Got "
"Sigmas must be covDim by numSigmas! Got "
+ sigmas.getNumRows()
+ " by "
+ sigmas.getNumCols());
}
if (Wm.getNumRows() != 2 * s.getNum() + 1 || Wm.getNumCols() != 1) {
if (Wm.getNumRows() != numSigmas || Wm.getNumCols() != 1) {
throw new IllegalArgumentException(
"Wm must be 2 * states + 1 by 1! Got " + Wm.getNumRows() + " by " + Wm.getNumCols());
"Wm must be numSigmas by 1! Got " + Wm.getNumRows() + " by " + Wm.getNumCols());
}
if (Wc.getNumRows() != 2 * s.getNum() + 1 || Wc.getNumCols() != 1) {
if (Wc.getNumRows() != numSigmas || Wc.getNumCols() != 1) {
throw new IllegalArgumentException(
"Wc must be 2 * states + 1 by 1! Got " + Wc.getNumRows() + " by " + Wc.getNumCols());
"Wc must be numSigmas by 1! Got " + Wc.getNumRows() + " by " + Wc.getNumCols());
}
// New mean is usually just the sum of the sigmas * weights:
@@ -208,13 +217,18 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
// [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}]
//
// the part of equations (20) and (24) within the "qr{}"
Matrix<C, ?> Sbar = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + dim.getNum()));
for (int i = 0; i < 2 * s.getNum(); i++) {
//
// Note that we allow a custom function instead of the difference to allow
// angle wrapping. Furthermore, we allow an arbitrary number of sigma points to
// support similar methods such as the Scaled Spherical Simplex Filter (S3F).
Matrix<C, ?> Sbar =
new Matrix<>(new SimpleMatrix(covdim.getNum(), numSigmas - 1 + covdim.getNum()));
for (int i = 0; i < numSigmas - 1; i++) {
Sbar.setColumn(
i,
residualFunc.apply(sigmas.extractColumnVector(1 + i), x).times(Math.sqrt(Wc.get(1, 0))));
}
Sbar.assignBlock(0, 2 * s.getNum(), squareRootR);
Sbar.assignBlock(0, numSigmas - 1, squareRootR);
QRDecompositionHouseholder_DDRM qr = new QRDecompositionHouseholder_DDRM();
var qrStorage = Sbar.transpose().getStorage();
@@ -355,7 +369,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
public final void reset() {
m_xHat = new Matrix<>(m_states, Nat.N1());
m_S = new Matrix<>(m_states, m_states);
m_sigmasF = new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
m_sigmasF = new Matrix<>(new SimpleMatrix(m_states.getNum(), m_pts.getNumSigmas()));
}
/**
@@ -397,7 +411,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
var ret =
squareRootUnscentedTransform(
m_states,
m_states,
m_pts.getNumSigmas(),
m_sigmasF,
m_pts.getWm(),
m_pts.getWc(),
@@ -477,8 +491,8 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
* @param y Measurement vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param R Continuous measurement noise covariance matrix.
* @param meanFuncY A function that computes the mean of 2 * States + 1 measurement vectors using
* a given set of weights.
* @param meanFuncY A function that computes the mean of NumSigmas measurement vectors using a
* given set of weights.
* @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
* subtracts them.)
* @param residualFuncX A function that computes the residual of two state vectors (i.e. it
@@ -507,7 +521,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
// This differs from equation (22) which uses
// the prior sigma points, regenerating them allows
// multiple measurement updates per time update
Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), 2 * m_states.getNum() + 1));
Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), m_pts.getNumSigmas()));
var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.getNumSigmas(); i++) {
Matrix<R, N1> hRet = h.apply(sigmas.extractColumnVector(i), u);
@@ -521,8 +535,8 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
// equations (23) (24) and (25)
var transRet =
squareRootUnscentedTransform(
m_states,
rows,
m_pts.getNumSigmas(),
sigmasH,
m_pts.getWm(),
m_pts.getWc(),

14
wpimath/src/main/native/cpp/estimator/MerweUKF.cpp Normal file
View File

@@ -0,0 +1,14 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/estimator/MerweUKF.h"
namespace frc {
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<3, 3, 1, MerweScaledSigmaPoints<3>>;
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<5, 3, 3, MerweScaledSigmaPoints<5>>;
} // namespace frc

6
wpimath/src/main/native/cpp/estimator/UnscentedKalmanFilter.cpp → wpimath/src/main/native/cpp/estimator/S3UKF.cpp
View File

@@ -2,13 +2,13 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/estimator/S3UKF.h"
namespace frc {
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<3, 3, 1>;
UnscentedKalmanFilter<3, 3, 1, S3SigmaPoints<3>>;
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<5, 3, 3>;
UnscentedKalmanFilter<5, 3, 3, S3SigmaPoints<5>>;
} // namespace frc

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

@@ -15,8 +15,8 @@ namespace frc {
* dissertation[1] for the UnscentedKalmanFilter class.
*
* It parametrizes the sigma points using alpha, beta, kappa terms, and is the
* version seen in most publications. Unless you know better, this should be
* your default choice.
* version seen in most publications. S3SigmaPoints is generally preferred due
* to its greater performance with nearly identical accuracy.
*
* [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilistic
* Inference in Dynamic State-Space Models" (Doctoral dissertation)
@@ -27,6 +27,8 @@ namespace frc {
template <int States>
class MerweScaledSigmaPoints {
public:
static constexpr int NumSigmas = 2 * States + 1;
/**
* Constructs a generator for Van der Merwe scaled sigma points.
*
@@ -44,11 +46,6 @@ class MerweScaledSigmaPoints {
ComputeWeights(beta);
}
/**
* Returns number of sigma points for each variable in the state x.
*/
int NumSigmas() { return 2 * States + 1; }
/**
* Computes the sigma points for an unscented Kalman filter given the mean
* (x) and square-root covariance (S) of the filter.

25
wpimath/src/main/native/include/frc/estimator/MerweUKF.h Normal file
View File

@@ -0,0 +1,25 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <wpi/SymbolExports.h>
#include "frc/estimator/MerweScaledSigmaPoints.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
namespace frc {
template <int States, int Inputs, int Outputs>
using MerweUKF = UnscentedKalmanFilter<States, Inputs, Outputs,
MerweScaledSigmaPoints<States>>;
// Because MerweUKF is a type alias and not a class, we have to use
// UnscentedKalmanFilter instead
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<3, 3, 1, MerweScaledSigmaPoints<3>>;
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<5, 3, 3, MerweScaledSigmaPoints<5>>;
} // namespace frc

135
wpimath/src/main/native/include/frc/estimator/S3SigmaPoints.h Normal file
View File

@@ -0,0 +1,135 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <wpi/array.h>
#include "frc/EigenCore.h"
#include "frc/estimator/SigmaPoints.h"
namespace frc {
/**
* Generates sigma points and weights according to Papakonstantinou's paper[1]
* for the UnscentedKalmanFilter class.
*
* It parameterizes the sigma points using alpha and beta terms. Unless you know
* better, this should be your default choice due to its high accuracy and
* performance.
*
* [1] K. Papakonstantinou "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 dimenstionality of the state. States + 2 weights will be
* generated.
*/
template <int States>
class S3SigmaPoints {
public:
static constexpr int NumSigmas = States + 2;
/**
* Constructs a generator for Papakonstantinou sigma points.
*
* @param alpha Determines the spread of the sigma points around the mean.
* Usually a small positive value (1e-3).
* @param beta Incorporates prior knowledge of the distribution of the mean.
* For Gaussian distributions, beta = 2 is optimal.
*/
explicit S3SigmaPoints(double alpha = 1e-3, double beta = 2)
: m_alpha{alpha} {
ComputeWeights(beta);
}
/**
* Computes the sigma points for an unscented Kalman filter given the mean (x)
* and square-root covariance (S) of the filter.
*
* @param x An array of the means.
* @param S Square-root covariance of the filter.
*
* @return Two dimensional array of sigma points. Each column contains all of
* the sigmas for one dimension in the problem space. Ordered by Xi_0,
* Xi_{1..n+1}.
*/
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);
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));
}
Matrixd<States, NumSigmas> C;
C.template block<States, 1>(0, 0) = Vectord<States>::Constant(0.0);
for (int col = 1; col < NumSigmas; ++col) {
for (int row = 0; row < States; ++row) {
if (row < col - 2) {
C(row, col) = 0.0;
} else if (row == col - 2) {
C(row, col) = q[row];
} else {
C(row, col) = -q[row] / (row + 1);
}
}
}
Matrixd<States, NumSigmas> sigmas;
for (int col = 0; col < NumSigmas; ++col) {
sigmas.col(col) = x + S * C.col(col);
}
return sigmas;
}
/**
* Returns the weight for each sigma point for the mean.
*/
const Vectord<NumSigmas>& Wm() const { return m_Wm; }
/**
* Returns an element of the weight for each sigma point for the mean.
*
* @param i Element of vector to return.
*/
double Wm(int i) const { return m_Wm(i, 0); }
/**
* Returns the weight for each sigma point for the covariance.
*/
const Vectord<NumSigmas>& Wc() const { return m_Wc; }
/**
* Returns an element of the weight for each sigma point for the covariance.
*
* @param i Element of vector to return.
*/
double Wc(int i) const { return m_Wc(i, 0); }
private:
Vectord<NumSigmas> m_Wm;
Vectord<NumSigmas> m_Wc;
double m_alpha;
/**
* Computes the weights for the scaled unscented Kalman filter.
*
* @param beta Incorporates prior knowledge of the distribution of the mean.
*/
void ComputeWeights(double beta) {
double alpha_sq = m_alpha * m_alpha;
double c = 1.0 / (alpha_sq * (States + 1));
m_Wm = Vectord<NumSigmas>::Constant(c);
m_Wc = Vectord<NumSigmas>::Constant(c);
m_Wm(0) = 1.0 - 1.0 / alpha_sq;
m_Wc(0) = 1.0 - 1.0 / alpha_sq + (1 - alpha_sq + beta);
}
};
} // namespace frc

25
wpimath/src/main/native/include/frc/estimator/S3UKF.h Normal file
View File

@@ -0,0 +1,25 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <wpi/SymbolExports.h>
#include "frc/estimator/S3SigmaPoints.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
namespace frc {
template <int States, int Inputs, int Outputs>
using S3UKF =
UnscentedKalmanFilter<States, Inputs, Outputs, S3SigmaPoints<States>>;
// Because S3UKF is a type alias and not a class, we have to use
// UnscentedKalmanFilter instead
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<3, 3, 1, S3SigmaPoints<3>>;
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<5, 3, 3, S3SigmaPoints<5>>;
} // namespace frc

26
wpimath/src/main/native/include/frc/estimator/SigmaPoints.h Normal file
View File

@@ -0,0 +1,26 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <concepts>
#include "frc/EigenCore.h"
namespace frc {
template <typename T, int States>
concept SigmaPoints =
requires(T t, Vectord<States> x, Matrixd<States, States> S, int i) {
{ T::NumSigmas } -> std::convertible_to<const int>;
{
t.SquareRootSigmaPoints(x, S)
} -> std::same_as<Matrixd<States, T::NumSigmas>>;
{ t.Wm() } -> std::convertible_to<Vectord<T::NumSigmas>>;
{ t.Wm(i) } -> std::same_as<double>;
{ t.Wc() } -> std::convertible_to<Vectord<T::NumSigmas>>;
{ t.Wc(i) } -> std::same_as<double>;
} && std::default_initializable<T>;
} // namespace frc

71
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -13,7 +13,7 @@
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/MerweScaledSigmaPoints.h"
#include "frc/estimator/SigmaPoints.h"
#include "frc/estimator/UnscentedTransform.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
@@ -28,6 +28,11 @@ namespace frc {
* be measured directly as a result of sensor noise, or because the state is
* "hidden".
*
* This class requires a SigmaPoints template parameter. For convenience, S3UKF
* and MerweUKF type aliases are provided to specify a suitable generator for
* you. S3UKF is generally preferred over MerweUKF because of its greater
* performance while maintaining nearly identical accuracy.
*
* Kalman filters use a K gain matrix to determine whether to trust the model or
* measurements more. Kalman filter theory uses statistics to compute an optimal
* K gain which minimizes the sum of squares error in the state estimate. This K
@@ -51,10 +56,13 @@ namespace frc {
* @tparam States Number of states.
* @tparam Inputs Number of inputs.
* @tparam Outputs Number of outputs.
* @tparam SigmaPoints Type used to generate sigma points and weights.
*/
template <int States, int Inputs, int Outputs>
template <int States, int Inputs, int Outputs, SigmaPoints<States> SigmaPoints>
class UnscentedKalmanFilter {
public:
static constexpr int NumSigmas = SigmaPoints::NumSigmas;
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
@@ -87,12 +95,12 @@ class UnscentedKalmanFilter {
: m_f(std::move(f)), m_h(std::move(h)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_meanFuncX = [](const Matrixd<States, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm) -> StateVector {
m_meanFuncX = [](const Matrixd<States, NumSigmas>& sigmas,
const Vectord<NumSigmas>& Wm) -> StateVector {
return sigmas * Wm;
};
m_meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wc) -> OutputVector {
m_meanFuncY = [](const Matrixd<Outputs, NumSigmas>& sigmas,
const Vectord<NumSigmas>& Wc) -> OutputVector {
return sigmas * Wc;
};
m_residualFuncX = [](const StateVector& a,
@@ -141,11 +149,11 @@ class UnscentedKalmanFilter {
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
std::function<StateVector(const Matrixd<States, NumSigmas>&,
const Vectord<NumSigmas>&)>
meanFuncX,
std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
std::function<OutputVector(const Matrixd<Outputs, NumSigmas>&,
const Vectord<NumSigmas>&)>
meanFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
@@ -257,7 +265,7 @@ class UnscentedKalmanFilter {
// Generate sigma points around the state mean
//
// equation (17)
Matrixd<States, 2 * States + 1> sigmas =
Matrixd<States, NumSigmas> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
// Project each sigma point forward in time according to the
@@ -267,7 +275,7 @@ class UnscentedKalmanFilter {
// sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability
//
// equation (18)
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
for (int i = 0; i < NumSigmas; ++i) {
StateVector x = sigmas.template block<States, 1>(0, i);
m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
}
@@ -276,7 +284,7 @@ class UnscentedKalmanFilter {
// to compute the prior state mean and covariance
//
// equations (18) (19) and (20)
auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
auto [xHat, S] = SquareRootUnscentedTransform<States, NumSigmas>(
m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
discQ.template triangularView<Eigen::Lower>());
m_xHat = xHat;
@@ -327,8 +335,8 @@ class UnscentedKalmanFilter {
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R) {
auto meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wc) -> Vectord<Rows> {
auto meanFuncY = [](const Matrixd<Outputs, NumSigmas>& sigmas,
const Vectord<NumSigmas>& Wc) -> Vectord<Rows> {
return sigmas * Wc;
};
auto residualFuncX = [](const StateVector& a,
@@ -358,7 +366,7 @@ class UnscentedKalmanFilter {
* @param h A vector-valued function of x and u that returns the
* measurement vector.
* @param R Continuous measurement noise covariance matrix.
* @param meanFuncY A function that computes the mean of 2 * States + 1
* @param meanFuncY A function that computes the mean of NumSigmas
* measurement vectors using a given set of weights.
* @param residualFuncY A function that computes the residual of two
* measurement vectors (i.e. it subtracts them.)
@@ -371,8 +379,8 @@ class UnscentedKalmanFilter {
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R,
std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
std::function<Vectord<Rows>(const Matrixd<Rows, NumSigmas>&,
const Vectord<NumSigmas>&)>
meanFuncY,
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
@@ -392,10 +400,10 @@ class UnscentedKalmanFilter {
// This differs from equation (22) which uses
// the prior sigma points, regenerating them allows
// multiple measurement updates per time update
Matrixd<Rows, 2 * States + 1> sigmasH;
Matrixd<States, 2 * States + 1> sigmas =
Matrixd<Rows, NumSigmas> sigmasH;
Matrixd<States, NumSigmas> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
for (int i = 0; i < NumSigmas; ++i) {
sigmasH.template block<Rows, 1>(0, i) =
h(sigmas.template block<States, 1>(0, i), u);
}
@@ -405,7 +413,7 @@ class UnscentedKalmanFilter {
// covariance.
//
// equations (23) (24) and (25)
auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, NumSigmas>(
sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
discR.template triangularView<Eigen::Lower>());
@@ -419,7 +427,7 @@ class UnscentedKalmanFilter {
// equation (26)
Matrixd<States, Rows> Pxy;
Pxy.setZero();
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
for (int i = 0; i < NumSigmas; ++i) {
Pxy +=
m_pts.Wc(i) *
(residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
@@ -467,11 +475,11 @@ class UnscentedKalmanFilter {
private:
std::function<StateVector(const StateVector&, const InputVector&)> m_f;
std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
std::function<StateVector(const Matrixd<States, NumSigmas>&,
const Vectord<NumSigmas>&)>
m_meanFuncX;
std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
std::function<OutputVector(const Matrixd<Outputs, NumSigmas>&,
const Vectord<NumSigmas>&)>
m_meanFuncY;
std::function<StateVector(const StateVector&, const StateVector&)>
m_residualFuncX;
@@ -482,15 +490,10 @@ class UnscentedKalmanFilter {
StateMatrix m_S;
StateMatrix m_contQ;
Matrixd<Outputs, Outputs> m_contR;
Matrixd<States, 2 * States + 1> m_sigmasF;
Matrixd<States, NumSigmas> m_sigmasF;
units::second_t m_dt;
MerweScaledSigmaPoints<States> m_pts;
SigmaPoints m_pts;
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<3, 3, 1>;
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<5, 3, 3>;
} // namespace frc

25
wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
View File

@@ -22,11 +22,11 @@ namespace frc {
*
* @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 List of sigma points.
* @param Wm Weights for the mean.
* @param Wc Weights for the covariance.
* @param meanFunc A function that computes the mean of 2 * States + 1 state
* @param meanFunc A function that computes the mean of NumSigmas state
* vectors using a given set of weights.
* @param residualFunc A function that computes the residual of two state
* vectors (i.e. it subtracts them.)
@@ -35,13 +35,13 @@ namespace frc {
* @return Tuple of x, mean of sigma points; S, square-root covariance of
* sigmas.
*/
template <int CovDim, int States>
template <int CovDim, int NumSigmas>
std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>>
SquareRootUnscentedTransform(
const Matrixd<CovDim, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm, const Vectord<2 * States + 1>& Wc,
std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
const Matrixd<CovDim, NumSigmas>& sigmas, const Vectord<NumSigmas>& Wm,
const Vectord<NumSigmas>& Wc,
std::function<Vectord<CovDim>(const Matrixd<CovDim, NumSigmas>&,
const Vectord<NumSigmas>&)>
meanFunc,
std::function<Vectord<CovDim>(const Vectord<CovDim>&,
const Vectord<CovDim>&)>
@@ -62,13 +62,18 @@ SquareRootUnscentedTransform(
// [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}]
//
// the part of equations (20) and (24) within the "qr{}"
Matrixd<CovDim, States * 2 + CovDim> Sbar;
for (int i = 0; i < States * 2; i++) {
//
// Note that we allow a custom function instead of the difference to allow
// angle wrapping. Furthermore, we allow an arbitrary number of sigma points
// to support similar methods such as the Scaled Spherical Simplex Filter
// (S3F).
Matrixd<CovDim, NumSigmas - 1 + CovDim> Sbar;
for (int i = 0; i < NumSigmas - 1; i++) {
Sbar.template block<CovDim, 1>(0, i) =
std::sqrt(Wc[1]) *
residualFunc(sigmas.template block<CovDim, 1>(0, 1 + i), x);
}
Sbar.template block<CovDim, CovDim>(0, States * 2) = squareRootR;
Sbar.template block<CovDim, CovDim>(0, NumSigmas - 1) = squareRootR;
// Compute the square-root covariance of the sigma points.
//

2
wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
View File

@@ -114,7 +114,7 @@ class ExtendedKalmanFilterTest {
List<Pose2d> waypoints =
List.of(
new Pose2d(2.75, 22.521, Rotation2d.kZero),
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
new Pose2d(24.73, 19.68, Rotation2d.fromRadians(5.846)));
var trajectory =
TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));

39
wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java → wpimath/src/test/java/edu/wpi/first/math/estimator/MerweUKFTest.java
View File

@@ -33,7 +33,7 @@ import java.util.Collections;
import java.util.List;
import org.junit.jupiter.api.Test;
class UnscentedKalmanFilterTest {
class MerweUKFTest {
private static Matrix<N5, N1> driveDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
var motors = DCMotor.getCIM(2);
@@ -78,12 +78,12 @@ class UnscentedKalmanFilterTest {
var dt = 0.005;
assertDoesNotThrow(
() -> {
UnscentedKalmanFilter<N5, N2, N3> observer =
new UnscentedKalmanFilter<>(
MerweUKF<N5, N2, N3> observer =
new MerweUKF<>(
Nat.N5(),
Nat.N3(),
UnscentedKalmanFilterTest::driveDynamics,
UnscentedKalmanFilterTest::driveLocalMeasurementModel,
MerweUKFTest::driveDynamics,
MerweUKFTest::driveLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.0001, 0.01, 0.01),
AngleStatistics.angleMean(2),
@@ -107,7 +107,7 @@ class UnscentedKalmanFilterTest {
Nat.N5(),
u,
globalY,
UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
MerweUKFTest::driveGlobalMeasurementModel,
R,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
@@ -121,12 +121,12 @@ class UnscentedKalmanFilterTest {
final double dt = 0.005;
final double rb = 0.8382 / 2.0; // Robot radius
UnscentedKalmanFilter<N5, N2, N3> observer =
new UnscentedKalmanFilter<>(
MerweUKF<N5, N2, N3> observer =
new MerweUKF<>(
Nat.N5(),
Nat.N3(),
UnscentedKalmanFilterTest::driveDynamics,
UnscentedKalmanFilterTest::driveLocalMeasurementModel,
MerweUKFTest::driveDynamics,
MerweUKFTest::driveLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.0001, 0.5, 0.5),
AngleStatistics.angleMean(2),
@@ -139,7 +139,7 @@ class UnscentedKalmanFilterTest {
List<Pose2d> waypoints =
List.of(
new Pose2d(2.75, 22.521, Rotation2d.kZero),
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
new Pose2d(24.73, 19.68, Rotation2d.fromRadians(5.846)));
var trajectory =
TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
@@ -150,7 +150,7 @@ class UnscentedKalmanFilterTest {
NumericalJacobian.numericalJacobianU(
Nat.N5(),
Nat.N2(),
UnscentedKalmanFilterTest::driveDynamics,
MerweUKFTest::driveDynamics,
new Matrix<>(Nat.N5(), Nat.N1()),
new Matrix<>(Nat.N2(), Nat.N1()));
@@ -190,8 +190,7 @@ class UnscentedKalmanFilterTest {
observer.predict(u, dt);
r = nextR;
trueXhat =
NumericalIntegration.rk4(UnscentedKalmanFilterTest::driveDynamics, trueXhat, u, dt);
trueXhat = NumericalIntegration.rk4(MerweUKFTest::driveDynamics, trueXhat, u, dt);
}
var localY = driveLocalMeasurementModel(trueXhat, u);
@@ -205,7 +204,7 @@ class UnscentedKalmanFilterTest {
Nat.N5(),
u,
globalY,
UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
MerweUKFTest::driveGlobalMeasurementModel,
R,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
@@ -226,7 +225,7 @@ class UnscentedKalmanFilterTest {
var dt = 0.020;
var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
var observer =
new UnscentedKalmanFilter<>(
new MerweUKF<>(
Nat.N1(),
Nat.N1(),
(x, u) -> plant.getA().times(x).plus(plant.getB().times(u)),
@@ -256,7 +255,7 @@ class UnscentedKalmanFilterTest {
var dt = 0.005;
var observer =
new UnscentedKalmanFilter<>(
new MerweUKF<>(
Nat.N2(),
Nat.N2(),
(x, u) -> x,
@@ -371,11 +370,11 @@ class UnscentedKalmanFilterTest {
0.0, 0.0, 10);
var observer =
new UnscentedKalmanFilter<N4, N1, N3>(
new MerweUKF<N4, N1, N3>(
Nat.N4(),
Nat.N3(),
UnscentedKalmanFilterTest::motorDynamics,
UnscentedKalmanFilterTest::motorMeasurementModel,
MerweUKFTest::motorDynamics,
MerweUKFTest::motorMeasurementModel,
VecBuilder.fill(0.1, 1.0, 1e-10, 1e-10),
VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev),
dt);

86
wpimath/src/test/java/edu/wpi/first/math/estimator/S3SigmaPointsTest.java Normal file
View File

@@ -0,0 +1,86 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.numbers.N2;
import org.junit.jupiter.api.Test;
class S3SigmaPointsTest {
@Test
void testSimplex() {
double alpha = 1e-3;
double beta = 2;
Nat<N2> N = Nat.N2();
var sigmaPoints = new S3SigmaPoints<>(N, alpha, beta);
var points = sigmaPoints.squareRootSigmaPoints(new Vector<>(N), Matrix.eye(N));
var v1 = new Vector<>(points.extractColumnVector(1));
var v2 = new Vector<>(points.extractColumnVector(2));
var v3 = new Vector<>(points.extractColumnVector(3));
assertAll(
() -> assertEquals(alpha * Math.sqrt(N.getNum()), v1.norm(), 1e-15),
() -> assertEquals(alpha * Math.sqrt(N.getNum()), v2.norm(), 1e-15),
() -> assertEquals(alpha * Math.sqrt(N.getNum()), v3.norm(), 1e-15),
() -> assertEquals(v1.minus(v2).norm(), v1.minus(v3).norm(), 1e-15),
() -> assertEquals(v1.minus(v2).norm(), v2.minus(v3).norm(), 1e-15));
}
@Test
void testZeroMeanPoints() {
var sigmaPoints = new S3SigmaPoints<>(Nat.N2());
var points =
sigmaPoints.squareRootSigmaPoints(
VecBuilder.fill(0, 0), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1));
assertTrue(
points.isEqual(
MatBuilder.fill(
Nat.N2(),
Nat.N4(),
0.0,
-0.00122474,
0.00122474,
0.0,
0.0,
-0.00070711,
-0.00070711,
0.00141421),
1E-6));
}
@Test
void testNonzeroMeanPoints() {
var sigmaPoints = new S3SigmaPoints<>(Nat.N2());
var points =
sigmaPoints.squareRootSigmaPoints(
VecBuilder.fill(1, 2), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, Math.sqrt(10)));
assertTrue(
points.isEqual(
MatBuilder.fill(
Nat.N2(),
Nat.N4(),
1.0,
0.99877526,
1.00122474,
1.0,
2.0,
1.99776393,
1.99776393,
2.00447214),
1E-6));
}
}

393
wpimath/src/test/java/edu/wpi/first/math/estimator/S3UKFTest.java Normal file
View File

@@ -0,0 +1,393 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.StateSpaceUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N4;
import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.NumericalIntegration;
import edu.wpi.first.math.system.NumericalJacobian;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import org.junit.jupiter.api.Test;
class S3UKFTest {
private static Matrix<N5, N1> driveDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
var motors = DCMotor.getCIM(2);
// var gLow = 15.32; // Low gear ratio
var gHigh = 7.08; // High gear ratio
var rb = 0.8382 / 2.0; // Robot radius
var r = 0.0746125; // Wheel radius
var m = 63.503; // Robot mass
var J = 5.6; // Robot moment of inertia
var C1 = -Math.pow(gHigh, 2) * motors.Kt / (motors.Kv * motors.R * r * r);
var C2 = gHigh * motors.Kt / (motors.R * r);
var k1 = 1.0 / m + Math.pow(rb, 2) / J;
var k2 = 1.0 / m - Math.pow(rb, 2) / J;
var vl = x.get(3, 0);
var vr = x.get(4, 0);
var Vl = u.get(0, 0);
var Vr = u.get(1, 0);
var v = 0.5 * (vl + vr);
return VecBuilder.fill(
v * Math.cos(x.get(2, 0)),
v * Math.sin(x.get(2, 0)),
(vr - vl) / (2.0 * rb),
k1 * (C1 * vl + C2 * Vl) + k2 * (C1 * vr + C2 * Vr),
k2 * (C1 * vl + C2 * Vl) + k1 * (C1 * vr + C2 * Vr));
}
@SuppressWarnings("PMD.UnusedFormalParameter")
private static Matrix<N3, N1> driveLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
}
@SuppressWarnings("PMD.UnusedFormalParameter")
private static Matrix<N5, N1> driveGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
return x.copy();
}
@Test
void testDriveInit() {
var dt = 0.005;
assertDoesNotThrow(
() -> {
S3UKF<N5, N2, N3> observer =
new S3UKF<>(
Nat.N5(),
Nat.N3(),
S3UKFTest::driveDynamics,
S3UKFTest::driveLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.0001, 0.01, 0.01),
AngleStatistics.angleMean(2),
AngleStatistics.angleMean(0),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(0),
AngleStatistics.angleAdd(2),
dt);
var u = VecBuilder.fill(12.0, 12.0);
observer.predict(u, dt);
var localY = driveLocalMeasurementModel(observer.getXhat(), u);
observer.correct(u, localY);
var globalY = driveGlobalMeasurementModel(observer.getXhat(), u);
var R =
StateSpaceUtil.makeCovarianceMatrix(
Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01));
observer.correct(
Nat.N5(),
u,
globalY,
S3UKFTest::driveGlobalMeasurementModel,
R,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2));
});
}
@Test
void testDriveConvergence() {
final double dt = 0.005;
final double rb = 0.8382 / 2.0; // Robot radius
S3UKF<N5, N2, N3> observer =
new S3UKF<>(
Nat.N5(),
Nat.N3(),
S3UKFTest::driveDynamics,
S3UKFTest::driveLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.0001, 0.5, 0.5),
AngleStatistics.angleMean(2),
AngleStatistics.angleMean(0),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(0),
AngleStatistics.angleAdd(2),
dt);
List<Pose2d> waypoints =
List.of(
new Pose2d(2.75, 22.521, Rotation2d.kZero),
new Pose2d(24.73, 19.68, Rotation2d.fromRadians(5.846)));
var trajectory =
TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
Matrix<N5, N1> r = new Matrix<>(Nat.N5(), Nat.N1());
Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
var B =
NumericalJacobian.numericalJacobianU(
Nat.N5(),
Nat.N2(),
S3UKFTest::driveDynamics,
new Matrix<>(Nat.N5(), Nat.N1()),
new Matrix<>(Nat.N2(), Nat.N1()));
observer.setXhat(
VecBuilder.fill(
trajectory.getInitialPose().getTranslation().getX(),
trajectory.getInitialPose().getTranslation().getY(),
trajectory.getInitialPose().getRotation().getRadians(),
0.0,
0.0));
var trueXhat = observer.getXhat();
double totalTime = trajectory.getTotalTime();
for (int i = 0; i < (totalTime / dt); ++i) {
var ref = trajectory.sample(dt * i);
double vl = ref.velocity * (1 - (ref.curvature * rb));
double vr = ref.velocity * (1 + (ref.curvature * rb));
var nextR =
VecBuilder.fill(
ref.pose.getTranslation().getX(),
ref.pose.getTranslation().getY(),
ref.pose.getRotation().getRadians(),
vl,
vr);
Matrix<N3, N1> localY =
driveLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5);
observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
var rdot = nextR.minus(r).div(dt);
u = new Matrix<>(B.solve(rdot.minus(driveDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
observer.predict(u, dt);
r = nextR;
trueXhat = NumericalIntegration.rk4(S3UKFTest::driveDynamics, trueXhat, u, dt);
}
var localY = driveLocalMeasurementModel(trueXhat, u);
observer.correct(u, localY);
var globalY = driveGlobalMeasurementModel(trueXhat, u);
var R =
StateSpaceUtil.makeCovarianceMatrix(
Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
observer.correct(
Nat.N5(),
u,
globalY,
S3UKFTest::driveGlobalMeasurementModel,
R,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2));
final var finalPosition = trajectory.sample(trajectory.getTotalTime());
assertEquals(finalPosition.pose.getTranslation().getX(), observer.getXhat(0), 0.055);
assertEquals(finalPosition.pose.getTranslation().getY(), observer.getXhat(1), 0.15);
assertEquals(finalPosition.pose.getRotation().getRadians(), observer.getXhat(2), 0.00015);
assertEquals(0.0, observer.getXhat(3), 0.1);
assertEquals(0.0, observer.getXhat(4), 0.1);
}
@Test
void testLinearUKF() {
var dt = 0.020;
var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
var observer =
new S3UKF<>(
Nat.N1(),
Nat.N1(),
(x, u) -> plant.getA().times(x).plus(plant.getB().times(u)),
plant::calculateY,
VecBuilder.fill(0.05),
VecBuilder.fill(1.0),
dt);
var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dt);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
Matrix<N1, N1> ref = VecBuilder.fill(100);
Matrix<N1, N1> u = VecBuilder.fill(0);
for (int i = 0; i < (2.0 / dt); ++i) {
observer.predict(u, dt);
u = discB.solve(ref.minus(discA.times(ref)));
}
assertEquals(ref.get(0, 0), observer.getXhat(0), 5);
}
@Test
void testRoundTripP() {
var dt = 0.005;
var observer =
new S3UKF<>(
Nat.N2(),
Nat.N2(),
(x, u) -> x,
(x, u) -> x,
VecBuilder.fill(0.0, 0.0),
VecBuilder.fill(0.0, 0.0),
dt);
var P = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 1.0, 2.0);
observer.setP(P);
assertTrue(observer.getP().isEqual(P, 1e-9));
}
// Second system, single motor feedforward estimator
private static Matrix<N4, N1> motorDynamics(Matrix<N4, N1> x, Matrix<N1, N1> u) {
double v = x.get(1, 0);
double kV = x.get(2, 0);
double kA = x.get(3, 0);
double V = u.get(0, 0);
double a = -kV / kA * v + 1.0 / kA * V;
return MatBuilder.fill(Nat.N4(), Nat.N1(), v, a, 0, 0);
}
private static Matrix<N3, N1> motorMeasurementModel(Matrix<N4, N1> x, Matrix<N1, N1> u) {
double p = x.get(0, 0);
double v = x.get(1, 0);
double kV = x.get(2, 0);
double kA = x.get(3, 0);
double V = u.get(0, 0);
double a = -kV / kA * v + 1.0 / kA * V;
return MatBuilder.fill(Nat.N3(), Nat.N1(), p, v, a);
}
private static Matrix<N1, N1> motorControlInput(double t) {
return MatBuilder.fill(
Nat.N1(),
Nat.N1(),
MathUtil.clamp(
8 * Math.sin(Math.PI * Math.sqrt(2.0) * t)
+ 6 * Math.sin(Math.PI * Math.sqrt(3.0) * t)
+ 4 * Math.sin(Math.PI * Math.sqrt(5.0) * t),
-12.0,
12.0));
}
@Test
void testMotorConvergence() {
final double dt = 0.01;
final int steps = 500;
final double true_kV = 3;
final double true_kA = 0.2;
final double pos_stddev = 0.02;
final double vel_stddev = 0.1;
final double accel_stddev = 0.1;
var states =
new ArrayList<>(
Collections.nCopies(
steps + 1, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 0.0, 0.0)));
var inputs =
new ArrayList<>(Collections.nCopies(steps, MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0)));
var measurements =
new ArrayList<>(
Collections.nCopies(steps + 1, MatBuilder.fill(Nat.N3(), Nat.N1(), 0.0, 0.0, 0.0)));
states.set(0, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, true_kV, true_kA));
var A =
MatBuilder.fill(
Nat.N4(),
Nat.N4(),
0.0,
1.0,
0.0,
0.0,
0.0,
-true_kV / true_kA,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0);
var B = MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 1.0 / true_kA, 0.0, 0.0);
var discABPair = Discretization.discretizeAB(A, B, dt);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
for (int i = 0; i < steps; ++i) {
inputs.set(i, motorControlInput(i * dt));
states.set(i + 1, discA.times(states.get(i)).plus(discB.times(inputs.get(i))));
measurements.set(
i,
motorMeasurementModel(states.get(i + 1), inputs.get(i))
.plus(
StateSpaceUtil.makeWhiteNoiseVector(
VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev))));
}
var P0 =
MatBuilder.fill(
Nat.N4(), Nat.N4(), 0.001, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 10, 0.0, 0.0,
0.0, 0.0, 10);
var observer =
new S3UKF<N4, N1, N3>(
Nat.N4(),
Nat.N3(),
S3UKFTest::motorDynamics,
S3UKFTest::motorMeasurementModel,
VecBuilder.fill(0.1, 1.0, 1e-10, 1e-10),
VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev),
dt);
observer.setXhat(MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 2.0, 2.0));
observer.setP(P0);
for (int i = 0; i < steps; ++i) {
observer.predict(inputs.get(i), dt);
observer.correct(inputs.get(i), measurements.get(i));
}
assertEquals(true_kV, observer.getXhat(2), true_kV * 0.5);
assertEquals(true_kA, observer.getXhat(3), true_kA * 0.5);
}
}

58
wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp → wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp
View File

@@ -13,7 +13,7 @@
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/estimator/MerweUKF.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
@@ -67,19 +67,19 @@ frc::Vectord<5> DriveGlobalMeasurementModel(
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
TEST(UnscentedKalmanFilterTest, DriveInit) {
TEST(MerweUKFTest, DriveInit) {
constexpr auto dt = 5_ms;
frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
frc::AngleMean<5, 5>(2),
frc::AngleMean<3, 5>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
dt};
frc::MerweUKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
frc::AngleMean<5, 5>(2),
frc::AngleMean<3, 5>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
dt};
frc::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
@@ -93,20 +93,20 @@ TEST(UnscentedKalmanFilterTest, DriveInit) {
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
}
TEST(UnscentedKalmanFilterTest, DriveConvergence) {
TEST(MerweUKFTest, DriveConvergence) {
constexpr auto dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
frc::AngleMean<5, 5>(2),
frc::AngleMean<3, 5>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
dt};
frc::MerweUKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
frc::AngleMean<5, 5>(2),
frc::AngleMean<3, 5>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
dt};
auto waypoints =
std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
@@ -174,11 +174,11 @@ TEST(UnscentedKalmanFilterTest, DriveConvergence) {
EXPECT_NEAR(0.0, observer.Xhat(4), 0.1);
}
TEST(UnscentedKalmanFilterTest, LinearUKF) {
TEST(MerweUKFTest, LinearUKF) {
constexpr units::second_t dt = 20_ms;
auto plant = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
frc::UnscentedKalmanFilter<1, 1, 1> observer{
frc::MerweUKF<1, 1, 1> observer{
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return plant.A() * x + plant.B() * u;
},
@@ -205,10 +205,10 @@ TEST(UnscentedKalmanFilterTest, LinearUKF) {
EXPECT_NEAR(ref(0, 0), observer.Xhat(0), 5);
}
TEST(UnscentedKalmanFilterTest, RoundTripP) {
TEST(MerweUKFTest, RoundTripP) {
constexpr auto dt = 5_ms;
frc::UnscentedKalmanFilter<2, 2, 2> observer{
frc::MerweUKF<2, 2, 2> observer{
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
{0.0, 0.0},
@@ -255,7 +255,7 @@ frc::Vectord<1> MotorControlInput(double t) {
-12.0, 12.0)};
}
TEST(UnscentedKalmanFilterTest, MotorConvergence) {
TEST(MerweUKFTest, MotorConvergence) {
constexpr units::second_t dt = 10_ms;
constexpr int steps = 500;
constexpr double true_kV = 3;
@@ -290,7 +290,7 @@ TEST(UnscentedKalmanFilterTest, MotorConvergence) {
frc::Vectord<4> P0{0.001, 0.001, 10, 10};
frc::UnscentedKalmanFilter<4, 1, 3> observer{
frc::MerweUKF<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::array{0.1, 1.0, 1e-10, 1e-10},
wpi::array{pos_stddev, vel_stddev, accel_stddev}, dt};

50
wpimath/src/test/native/cpp/estimator/S3SigmaPointsTest.cpp Normal file
View File

@@ -0,0 +1,50 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <gtest/gtest.h>
#include "frc/estimator/S3SigmaPoints.h"
TEST(S3SigmaPointsTest, Simplex) {
constexpr double alpha = 1e-3;
constexpr double beta = 2;
constexpr size_t N = 2;
frc::S3SigmaPoints<N> sigmaPoints{alpha, beta};
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<N>::Zero(), frc::Matrixd<N, N>::Identity());
auto v1 = points.template block<2, 1>(0, 1);
auto v2 = points.template block<2, 1>(0, 2);
auto v3 = points.template block<2, 1>(0, 3);
EXPECT_DOUBLE_EQ(alpha * std::sqrt(N), v1.norm());
EXPECT_DOUBLE_EQ(alpha * std::sqrt(N), v2.norm());
EXPECT_DOUBLE_EQ(alpha * std::sqrt(N), v3.norm());
EXPECT_DOUBLE_EQ((v1 - v2).norm(), (v1 - v3).norm());
EXPECT_DOUBLE_EQ((v1 - v2).norm(), (v2 - v3).norm());
}
TEST(S3SigmaPointsTest, ZeroMean) {
frc::S3SigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
(points - frc::Matrixd<2, 4>{{0.0, -0.00122474, 0.00122474, 0.0},
{0.0, -0.00070711, -0.00070711, 0.00141421}})
.norm() < 1e-7);
}
TEST(S3SigmaPointsTest, NonzeroMean) {
frc::S3SigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{1.0, 2.0},
frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
EXPECT_TRUE(
(points - frc::Matrixd<2, 4>{{1.0, 0.99877526, 1.00122474, 1.0},
{2.0, 1.99776393, 1.99776393, 2.00447214}})
.norm() < 1e-7);
}

309
wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp Normal file
View File

@@ -0,0 +1,309 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <algorithm>
#include <cmath>
#include <numbers>
#include <vector>
#include <Eigen/QR>
#include <gtest/gtest.h>
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/MerweUKF.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/system/plant/LinearSystemId.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "units/moment_of_inertia.h"
namespace {
// First test system, differential drive
frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
constexpr double Ghigh = 7.08; // High gear ratio
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
constexpr auto r = 0.0746125_m; // Wheel radius
constexpr auto m = 63.503_kg; // Robot mass
constexpr auto J = 5.6_kg_sq_m; // Robot moment of inertia
auto C1 = -std::pow(Ghigh, 2) * motors.Kt /
(motors.Kv * motors.R * units::math::pow<2>(r));
auto C2 = Ghigh * motors.Kt / (motors.R * r);
auto k1 = (1 / m + units::math::pow<2>(rb) / J);
auto k2 = (1 / m - units::math::pow<2>(rb) / J);
units::meters_per_second_t vl{x(3)};
units::meters_per_second_t vr{x(4)};
units::volt_t Vl{u(0)};
units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
return frc::Vectord<5>{
v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).value(),
k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
k2.value() * ((C1 * vr).value() + (C2 * Vr).value()),
k2.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
frc::Vectord<3> DriveLocalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<3>{x(2), x(3), x(4)};
}
frc::Vectord<5> DriveGlobalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
TEST(MerweUKFTest, DriveInit) {
constexpr auto dt = 5_ms;
frc::MerweUKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
frc::AngleMean<5, 5>(2),
frc::AngleMean<3, 5>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
dt};
frc::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = DriveLocalMeasurementModel(observer.Xhat(), u);
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
}
TEST(MerweUKFTest, DriveConvergence) {
constexpr auto dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::MerweUKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
frc::AngleMean<5, 5>(2),
frc::AngleMean<3, 5>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
dt};
auto waypoints =
std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = frc::Vectord<2>::Zero();
auto B = frc::NumericalJacobianU<5, 5, 2>(
DriveDynamics, frc::Vectord<5>::Zero(), frc::Vectord<2>::Zero());
observer.SetXhat(frc::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
auto trueXhat = observer.Xhat();
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
ref.velocity * (1 - (ref.curvature * rb).value());
units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
frc::Vectord<5> nextR{
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY = DriveLocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
frc::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
DriveDynamics(r, frc::Vectord<2>::Zero()));
observer.Predict(u, dt);
r = nextR;
trueXhat = frc::RK4(DriveDynamics, trueXhat, u, dt);
}
auto localY = DriveLocalMeasurementModel(trueXhat, u);
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)
);
auto finalPosition = trajectory.Sample(trajectory.TotalTime());
EXPECT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
0.055);
EXPECT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
0.15);
EXPECT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
0.000005);
EXPECT_NEAR(0.0, observer.Xhat(3), 0.1);
EXPECT_NEAR(0.0, observer.Xhat(4), 0.1);
}
TEST(MerweUKFTest, LinearUKF) {
constexpr units::second_t dt = 20_ms;
auto plant = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
frc::MerweUKF<1, 1, 1> observer{
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return plant.A() * x + plant.B() * u;
},
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return plant.CalculateY(x, u);
},
{0.05},
{1.0},
dt};
frc::Matrixd<1, 1> discA;
frc::Matrixd<1, 1> discB;
frc::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
frc::Vectord<1> ref{100.0};
frc::Vectord<1> u{0.0};
for (int i = 0; i < 2.0 / dt.value(); ++i) {
observer.Predict(u, dt);
u = discB.householderQr().solve(ref - discA * ref);
}
EXPECT_NEAR(ref(0, 0), observer.Xhat(0), 5);
}
TEST(MerweUKFTest, RoundTripP) {
constexpr auto dt = 5_ms;
frc::MerweUKF<2, 2, 2> observer{
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
[](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
{0.0, 0.0},
{0.0, 0.0},
dt};
frc::Matrixd<2, 2> P({{2, 1}, {1, 2}});
observer.SetP(P);
ASSERT_TRUE(observer.P().isApprox(P));
}
// Second system, single motor feedforward estimator
frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
double v = x(1);
double kV = x(2);
double kA = x(3);
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<4>{v, a, 0.0, 0.0};
}
frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
double p = x(0);
double v = x(1);
double kV = x(2);
double kA = x(3);
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<3>{p, v, a};
}
frc::Vectord<1> MotorControlInput(double t) {
return frc::Vectord<1>{
std::clamp(8 * std::sin(std::numbers::pi * std::sqrt(2.0) * t) +
6 * std::sin(std::numbers::pi * std::sqrt(3.0) * t) +
4 * std::sin(std::numbers::pi * std::sqrt(5.0) * t),
-12.0, 12.0)};
}
TEST(MerweUKFTest, MotorConvergence) {
constexpr units::second_t dt = 10_ms;
constexpr int steps = 500;
constexpr double true_kV = 3;
constexpr double true_kA = 0.2;
constexpr double pos_stddev = 0.02;
constexpr double vel_stddev = 0.1;
constexpr double accel_stddev = 0.1;
std::vector<frc::Vectord<4>> states(steps + 1);
std::vector<frc::Vectord<1>> inputs(steps);
std::vector<frc::Vectord<3>> measurements(steps);
states[0] = frc::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
constexpr frc::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
{0.0, -true_kV / true_kA, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0}};
constexpr frc::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
frc::Matrixd<4, 4> discA;
frc::Matrixd<4, 1> discB;
frc::DiscretizeAB(A, B, dt, &discA, &discB);
for (int i = 0; i < steps; ++i) {
inputs[i] = MotorControlInput(i * dt.value());
states[i + 1] = discA * states[i] + discB * inputs[i];
measurements[i] =
MotorMeasurementModel(states[i + 1], inputs[i]) +
frc::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
}
frc::Vectord<4> P0{0.001, 0.001, 10, 10};
frc::MerweUKF<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::array{0.1, 1.0, 1e-10, 1e-10},
wpi::array{pos_stddev, vel_stddev, accel_stddev}, dt};
observer.SetXhat(frc::Vectord<4>{0.0, 0.0, 2.0, 2.0});
observer.SetP(P0.asDiagonal());
for (int i = 0; i < steps; ++i) {
observer.Predict(inputs[i], dt);
observer.Correct(inputs[i], measurements[i]);
}
EXPECT_NEAR(true_kV, observer.Xhat(2), true_kV * 0.5);
EXPECT_NEAR(true_kA, observer.Xhat(3), true_kA * 0.5);
}
} // namespace