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

[wpimath] Replace UKF implementation with square root form (#4168)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Connor Worley
2022-06-08 22:19:01 -07:00
committed by GitHub
parent 45b7fc445b
commit a99c11c14c
22 changed files with 494 additions and 297 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.github/workflows/gradle.yml vendored
View File

@@ -55,7 +55,7 @@ jobs:
artifact-name: Win64Debug
architecture: x64
task: "build"
build-options: "-PciDebugOnly"
build-options: "-PciDebugOnly --max-workers 1"
- os: windows-2019
artifact-name: Win64Release
architecture: x64

41
wpimath/src/main/java/edu/wpi/first/math/Matrix.java
View File

@@ -323,6 +323,10 @@ public class Matrix<R extends Num, C extends Num> {
* <p>The matrix equation could also be written as x = A<sup>-1</sup>b. Where the pseudo inverse
* is used if A is not square.
*
* <p>Note that this method does not support solving using a QR decomposition with full-pivoting,
* as only column-pivoting is supported. For full-pivoting, use {@link
* #solveFullPivHouseholderQr}.
*
* @param <C2> Columns in b.
* @param b The right-hand side of the equation to solve.
* @return The solution to the linear system.
@@ -332,6 +336,29 @@ public class Matrix<R extends Num, C extends Num> {
return new Matrix<>(this.m_storage.solve(Objects.requireNonNull(b).m_storage));
}
/**
* Solves the least-squares problem Ax=B using a QR decomposition with full pivoting, where this
* matrix is A.
*
* @param <R2> Number of rows in B.
* @param <C2> Number of columns in B.
* @param other The B matrix.
* @return The solution matrix.
*/
public final <R2 extends Num, C2 extends Num> Matrix<C, C2> solveFullPivHouseholderQr(
Matrix<R2, C2> other) {
Matrix<C, C2> solution = new Matrix<>(new SimpleMatrix(this.getNumCols(), other.getNumCols()));
WPIMathJNI.solveFullPivHouseholderQr(
this.getData(),
this.getNumRows(),
this.getNumCols(),
other.getData(),
other.getNumRows(),
other.getNumCols(),
solution.getData());
return solution;
}
/**
* Computes the matrix exponential using Eigen's solver. This method only works for square
* matrices, and will otherwise throw an {@link MatrixDimensionException}.
@@ -677,6 +704,20 @@ public class Matrix<R extends Num, C extends Num> {
this.m_storage.getDDRM(), other.m_storage.getDDRM(), tolerance);
}
/**
* Performs an inplace Cholesky rank update (or downdate).
*
* <p>If this matrix contains L where A = LL<sup>&top;</sup> before the update, it will contain L
* where LL<sup>&top;</sup> = A + &sigma;vv<sup>&top;</sup> after the update.
*
* @param v Vector to use for the update.
* @param sigma Sigma to use for the update.
* @param lowerTriangular Whether or not this matrix is lower triangular.
*/
public void rankUpdate(Matrix<R, N1> v, double sigma, boolean lowerTriangular) {
WPIMathJNI.rankUpdate(this.getData(), this.getNumRows(), v.getData(), sigma, lowerTriangular);
}
@Override
public String toString() {
return m_storage.toString();

27
wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
View File

@@ -125,6 +125,19 @@ public final class WPIMathJNI {
*/
public static native String serializeTrajectory(double[] elements);
/**
* Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
* matrix.
*
* @param mat Array of elements of the matrix to be updated.
* @param lowerTriangular Whether or not mat is lower triangular.
* @param rows How many rows there are.
* @param vec Vector to use for the rank update.
* @param sigma Sigma value to use for the rank update.
*/
public static native void rankUpdate(
double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
@@ -136,4 +149,18 @@ public final class WPIMathJNI {
extractOnStaticLoad.set(load);
}
}
/**
* Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
*
* @param A Array of elements of the A matrix.
* @param Arows Number of rows of the A matrix.
* @param Acols Number of rows of the A matrix.
* @param B Array of elements of the B matrix.
* @param Brows Number of rows of the B matrix.
* @param Bcols Number of rows of the B matrix.
* @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
*/
public static native void solveFullPivHouseholderQr(
double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
}

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

@@ -71,16 +71,16 @@ public class MerweScaledSigmaPoints<S extends Num> {
* of the filter.
*
* @param x An array of the means.
* @param P Covariance of the filter.
* @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}, Xi_{n+1..2n}.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public Matrix<S, ?> sigmaPoints(Matrix<S, N1> x, Matrix<S, S> P) {
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());
var intermediate = P.times(lambda + m_states.getNum());
var U = intermediate.lltDecompose(true); // Lower triangular
Matrix<S, S> U = s.times(eta);
// 2 * states + 1 by states
Matrix<S, ?> sigmas =

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

@@ -14,6 +14,7 @@ import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.NumericalIntegration;
import edu.wpi.first.math.system.NumericalJacobian;
import java.util.function.BiFunction;
import org.ejml.dense.row.decomposition.qr.QRDecompositionHouseholder_DDRM;
import org.ejml.simple.SimpleMatrix;
/**
@@ -33,6 +34,9 @@ import org.ejml.simple.SimpleMatrix;
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
* theory".
*
* <p>This class implements a square-root-form unscented Kalman filter (SR-UKF). For more
* information about the SR-UKF, see https://www.researchgate.net/publication/3908304.
*/
@SuppressWarnings({"MemberName", "ClassTypeParameterName"})
public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
@@ -50,7 +54,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
private BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> m_addFuncX;
private Matrix<States, N1> m_xHat;
private Matrix<States, States> m_P;
private Matrix<States, States> m_S;
private final Matrix<States, States> m_contQ;
private final Matrix<Outputs, Outputs> m_contR;
private Matrix<States, ?> m_sigmasF;
@@ -152,14 +156,16 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
}
@SuppressWarnings({"ParameterName", "LocalVariableName"})
static <S extends Num, C extends Num> Pair<Matrix<C, N1>, Matrix<C, C>> unscentedTransform(
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) {
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) {
throw new IllegalArgumentException(
"Sigmas must be covDim by 2 * states + 1! Got "
@@ -184,28 +190,64 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
// k=1
Matrix<C, N1> x = meanFunc.apply(sigmas, Wm);
// New covariance is the sum of the outer product of the residuals times the
// weights
Matrix<C, ?> y = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + 1));
for (int i = 0; i < 2 * s.getNum() + 1; i++) {
// y[:, i] = sigmas[:, i] - x
y.setColumn(i, residualFunc.apply(sigmas.extractColumnVector(i), x));
Matrix<C, ?> Sbar = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + dim.getNum()));
for (int i = 0; i < 2 * s.getNum(); i++) {
Sbar.setColumn(
i,
residualFunc.apply(sigmas.extractColumnVector(1 + i), x).times(Math.sqrt(Wc.get(1, 0))));
}
Matrix<C, C> P =
y.times(Matrix.changeBoundsUnchecked(Wc.diag()))
.times(Matrix.changeBoundsUnchecked(y.transpose()));
Sbar.assignBlock(0, 2 * s.getNum(), squareRootR);
return new Pair<>(x, P);
QRDecompositionHouseholder_DDRM qr = new QRDecompositionHouseholder_DDRM();
var qrStorage = Sbar.transpose().getStorage();
if (!qr.decompose(qrStorage.getDDRM())) {
throw new RuntimeException("QR decomposition failed! Input matrix:\n" + qrStorage.toString());
}
Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)));
newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), false);
return new Pair<>(x, newS);
}
/**
* Returns the error covariance matrix P.
* Returns the square-root error covariance matrix S.
*
* @return the square-root error covariance matrix S.
*/
public Matrix<States, States> getS() {
return m_S;
}
/**
* Returns an element of the square-root error covariance matrix S.
*
* @param row Row of S.
* @param col Column of S.
* @return the value of the square-root error covariance matrix S at (i, j).
*/
public double getS(int row, int col) {
return m_S.get(row, col);
}
/**
* Sets the entire square-root error covariance matrix S.
*
* @param newS The new value of S to use.
*/
public void setS(Matrix<States, States> newS) {
m_S = newS;
}
/**
* Returns the reconstructed error covariance matrix P.
*
* @return the error covariance matrix P.
*/
@Override
public Matrix<States, States> getP() {
return m_P;
return m_S.transpose().times(m_S);
}
/**
@@ -214,10 +256,12 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
* @param row Row of P.
* @param col Column of P.
* @return the value of the error covariance matrix P at (i, j).
* @throws UnsupportedOperationException indexing into the reconstructed P matrix is not supported
*/
@Override
public double getP(int row, int col) {
return m_P.get(row, col);
throw new UnsupportedOperationException(
"indexing into the reconstructed P matrix is not supported");
}
/**
@@ -227,7 +271,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
*/
@Override
public void setP(Matrix<States, States> newP) {
m_P = newP;
m_S = newP.lltDecompose(false);
}
/**
@@ -277,7 +321,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
@Override
public void reset() {
m_xHat = new Matrix<>(m_states, Nat.N1());
m_P = new Matrix<>(m_states, m_states);
m_S = new Matrix<>(m_states, m_states);
m_sigmasF = new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
}
@@ -294,8 +338,9 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
Matrix<States, States> contA =
NumericalJacobian.numericalJacobianX(m_states, m_states, m_f, m_xHat, u);
var discQ = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds).getSecond();
var squareRootDiscQ = discQ.lltDecompose(true);
var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
Matrix<States, N1> x = sigmas.extractColumnVector(i);
@@ -304,17 +349,18 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
}
var ret =
unscentedTransform(
squareRootUnscentedTransform(
m_states,
m_states,
m_sigmasF,
m_pts.getWm(),
m_pts.getWc(),
m_meanFuncX,
m_residualFuncX);
m_residualFuncX,
squareRootDiscQ);
m_xHat = ret.getFirst();
m_P = ret.getSecond().plus(discQ);
m_S = ret.getSecond();
m_dtSeconds = dtSeconds;
}
@@ -394,10 +440,11 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
final var discR = Discretization.discretizeR(R, m_dtSeconds);
final var squareRootDiscR = discR.lltDecompose(true);
// Transform sigma points into measurement space
Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), 2 * m_states.getNum() + 1));
var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
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);
sigmasH.setColumn(i, hRet);
@@ -405,10 +452,17 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
// Mean and covariance of prediction passed through unscented transform
var transRet =
unscentedTransform(
m_states, rows, sigmasH, m_pts.getWm(), m_pts.getWc(), meanFuncY, residualFuncY);
squareRootUnscentedTransform(
m_states,
rows,
sigmasH,
m_pts.getWm(),
m_pts.getWc(),
meanFuncY,
residualFuncY,
squareRootDiscR);
var yHat = transRet.getFirst();
var Py = transRet.getSecond().plus(discR);
var Sy = transRet.getSecond();
// Compute cross covariance of the state and the measurements
Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
@@ -420,17 +474,20 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i)));
}
// K = P_{xy} P_y⁻¹
// Kᵀ = P_yᵀ⁻¹ P_{xy}ᵀ
// P_yᵀKᵀ = P_{xy}ᵀ
// Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
// K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
Matrix<States, R> K = new Matrix<>(Py.transpose().solve(Pxy.transpose()).transpose());
// K = (P_{xy} / S_yᵀ) / S_y
// K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
// K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
Matrix<States, R> K =
Sy.transpose()
.solveFullPivHouseholderQr(Sy.solveFullPivHouseholderQr(Pxy.transpose()))
.transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));
// Pₖ₊₁⁺ = Pₖ₊₁⁻ − KP_yKᵀ
m_P = m_P.minus(K.times(Py).times(K.transpose()));
Matrix<States, R> U = K.times(Sy);
for (int i = 0; i < rows.getNum(); i++) {
m_S.rankUpdate(U.extractColumnVector(i), -1, false);
}
}
}

57
wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
View File

@@ -8,6 +8,7 @@
#include <wpi/jni_util.h>
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "Eigen/QR"
@@ -307,4 +308,60 @@ Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
}
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: rankUpdate
* Signature: ([DI[DDZ)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
(JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
jdouble sigma, jboolean lowerTriangular)
{
jdouble* matBody = env->GetDoubleArrayElements(mat, nullptr);
jdouble* vecBody = env->GetDoubleArrayElements(vec, nullptr);
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
L{matBody, rows, rows};
Eigen::Map<Eigen::Vector<double, Eigen::Dynamic>> v{vecBody, rows};
if (lowerTriangular == JNI_TRUE) {
Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(L, v, sigma);
} else {
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(L, v, sigma);
}
env->ReleaseDoubleArrayElements(mat, matBody, 0);
env->ReleaseDoubleArrayElements(vec, vecBody, 0);
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: solveFullPivHouseholderQr
* Signature: ([DII[DII[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
(JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
jint Brows, jint Bcols, jdoubleArray dst)
{
jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Amat{nativeA, Arows, Acols};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
Bmat{nativeB, Brows, Bcols};
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Xmat =
Amat.fullPivHouseholderQr().solve(Bmat);
env->ReleaseDoubleArrayElements(A, nativeA, 0);
env->ReleaseDoubleArrayElements(B, nativeB, 0);
env->SetDoubleArrayRegion(dst, 0, Brows * Bcols, Xmat.data());
}
} // extern "C"

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

@@ -90,11 +90,15 @@ template <int CovDim, int States>
Vectord<CovDim> AngleMean(const Matrixd<CovDim, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm,
int angleStatesIdx) {
double sumSin = sigmas.row(angleStatesIdx)
.unaryExpr([](auto it) { return std::sin(it); })
double sumSin = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
return std::sin(it);
}) *
Wm)
.sum();
double sumCos = sigmas.row(angleStatesIdx)
.unaryExpr([](auto it) { return std::cos(it); })
double sumCos = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
return std::cos(it);
}) *
Wm)
.sum();
Vectord<CovDim> ret = sigmas * Wm;

6
wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
View File

@@ -21,14 +21,14 @@ class KalmanFilterLatencyCompensator {
public:
struct ObserverSnapshot {
Vectord<States> xHat;
Matrixd<States, States> errorCovariances;
Matrixd<States, States> squareRootErrorCovariances;
Vectord<Inputs> inputs;
Vectord<Outputs> localMeasurements;
ObserverSnapshot(const KalmanFilterType& observer, const Vectord<Inputs>& u,
const Vectord<Outputs>& localY)
: xHat(observer.Xhat()),
errorCovariances(observer.P()),
squareRootErrorCovariances(observer.S()),
inputs(u),
localMeasurements(localY) {}
};
@@ -135,7 +135,7 @@ class KalmanFilterLatencyCompensator {
auto& [key, snapshot] = m_pastObserverSnapshots[i];
if (i == indexOfClosestEntry) {
observer->SetP(snapshot.errorCovariances);
observer->SetS(snapshot.squareRootErrorCovariances);
observer->SetXhat(snapshot.xHat);
}

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

@@ -6,7 +6,6 @@
#include <cmath>
#include "Eigen/Cholesky"
#include "frc/EigenCore.h"
namespace frc {
@@ -52,20 +51,21 @@ class MerweScaledSigmaPoints {
/**
* Computes the sigma points for an unscented Kalman filter given the mean
* (x) and covariance(P) of the filter.
* (x) and square-root covariance(S) of the filter.
*
* @param x An array of the means.
* @param P Covariance of the filter.
* @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}, Xi_{n+1..2n}.
*
*/
Matrixd<States, 2 * States + 1> SigmaPoints(
const Vectord<States>& x, const Matrixd<States, States>& P) {
Matrixd<States, 2 * States + 1> SquareRootSigmaPoints(
const Vectord<States>& x, const Matrixd<States, States>& S) {
double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
Matrixd<States, States> U = ((lambda + States) * P).llt().matrixL();
double eta = std::sqrt(lambda + States);
Matrixd<States, States> U = eta * S;
Matrixd<States, 2 * States + 1> sigmas;
sigmas.template block<States, 1>(0, 0) = x;

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

@@ -35,6 +35,10 @@ namespace frc {
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
* "Stochastic control theory".
*
* <p> This class implements a square-root-form unscented Kalman filter
* (SR-UKF). For more information about the SR-UKF, see
* https://www.researchgate.net/publication/3908304.
*
* @tparam States The number of states.
* @tparam Inputs The number of inputs.
* @tparam Outputs The number of outputs.
@@ -111,24 +115,37 @@ class UnscentedKalmanFilter {
units::second_t dt);
/**
* Returns the error covariance matrix P.
* Returns the square-root error covariance matrix S.
*/
const StateMatrix& P() const { return m_P; }
const StateMatrix& S() const { return m_S; }
/**
* Returns an element of the error covariance matrix P.
* Returns an element of the square-root error covariance matrix S.
*
* @param i Row of P.
* @param j Column of P.
* @param i Row of S.
* @param j Column of S.
*/
double P(int i, int j) const { return m_P(i, j); }
double S(int i, int j) const { return m_S(i, j); }
/**
* Set the current error covariance matrix P.
* Set the current square-root error covariance matrix S.
*
* @param S The square-root error covariance matrix S.
*/
void SetS(const StateMatrix& S) { m_S = S; }
/**
* Returns the reconstructed error covariance matrix P.
*/
StateMatrix P() const { return m_S.transpose() * m_S; }
/**
* Set the current square-root error covariance matrix S by taking the square
* root of P.
*
* @param P The error covariance matrix P.
*/
void SetP(const StateMatrix& P) { m_P = P; }
void SetP(const StateMatrix& P) { m_S = P.llt().matrixU(); }
/**
* Returns the state estimate x-hat.
@@ -162,7 +179,7 @@ class UnscentedKalmanFilter {
*/
void Reset() {
m_xHat.setZero();
m_P.setZero();
m_S.setZero();
m_sigmasF.setZero();
}
@@ -254,7 +271,7 @@ class UnscentedKalmanFilter {
m_residualFuncY;
std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
StateVector m_xHat;
StateMatrix m_P;
StateMatrix m_S;
StateMatrix m_contQ;
Matrixd<Outputs, Outputs> m_contR;
Matrixd<States, 2 * States + 1> m_sigmasF;

49
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
View File

@@ -76,21 +76,22 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
DiscretizeAQTaylor<States>(contA, m_contQ, m_dt, &discA, &discQ);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
Matrixd<States, 2 * States + 1> sigmas = m_pts.SigmaPoints(m_xHat, m_P);
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.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);
}
auto ret = UnscentedTransform<States, States>(
m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX);
m_xHat = std::get<0>(ret);
m_P = std::get<1>(ret);
m_P += discQ;
auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
discQ.template triangularView<Eigen::Lower>());
m_xHat = xHat;
m_S = S;
}
template <int States, int Inputs, int Outputs>
@@ -123,20 +124,22 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
residualFuncX,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX) {
const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);
// Transform sigma points into measurement space
Matrixd<Rows, 2 * States + 1> sigmasH;
Matrixd<States, 2 * States + 1> sigmas = m_pts.SigmaPoints(m_xHat, m_P);
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
sigmasH.template block<Rows, 1>(0, i) =
h(sigmas.template block<States, 1>(0, i), u);
}
// Mean and covariance of prediction passed through UT
auto [yHat, Py] = UnscentedTransform<Rows, States>(
sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY);
Py += discR;
auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
discR.template triangularView<Eigen::Lower>());
// Compute cross covariance of the state and the measurements
Matrixd<States, Rows> Pxy;
@@ -149,19 +152,23 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
.transpose();
}
// K = P_{xy} P_y⁻¹
// Kᵀ = P_yᵀ⁻¹ P_{xy}ᵀ
// P_yᵀKᵀ = P_{xy}ᵀ
// Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
// K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
// K = (P_{xy} / S_yᵀ) / S_y
// K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
// K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
Matrixd<States, Rows> K =
Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
Sy.transpose()
.fullPivHouseholderQr()
.solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
.transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
// Pₖ₊₁⁺ = Pₖ₊₁⁻ − KP_yKᵀ
m_P -= K * Py * K.transpose();
Matrixd<States, Rows> U = K * Sy;
for (int i = 0; i < Rows; i++) {
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
m_S, U.template block<States, 1>(0, i), -1);
}
}
} // namespace frc

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

@@ -6,15 +6,17 @@
#include <tuple>
#include "Eigen/QR"
#include "frc/EigenCore.h"
namespace frc {
/**
* Computes unscented transform of a set of sigma points and weights. CovDim
* returns the mean and covariance in a tuple.
* returns the mean and square-root covariance of the sigma points in a tuple.
*
* This works in conjunction with the UnscentedKalmanFilter class.
* This works in conjunction with the UnscentedKalmanFilter class. For use with
* square-root form UKFs.
*
* @tparam CovDim Dimension of covariance of sigma points after passing
* through the transform.
@@ -26,12 +28,14 @@ namespace frc {
* vectors using a given set of weights.
* @param residualFunc A function that computes the residual of two state
* vectors (i.e. it subtracts them.)
* @param squareRootR Square-root of the noise covaraince of the sigma points.
*
* @return Tuple of x, mean of sigma points; P, covariance of sigma points after
* passing through the transform.
* @return Tuple of x, mean of sigma points; S, square-root covariance of
* sigmas.
*/
template <int CovDim, int States>
std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>> UnscentedTransform(
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>&,
@@ -39,25 +43,33 @@ std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>> UnscentedTransform(
meanFunc,
std::function<Vectord<CovDim>(const Vectord<CovDim>&,
const Vectord<CovDim>&)>
residualFunc) {
residualFunc,
const Matrixd<CovDim, CovDim>& squareRootR) {
// New mean is usually just the sum of the sigmas * weight:
// n
// dot = Σ W[k] Xᵢ[k]
// k=1
Vectord<CovDim> x = meanFunc(sigmas, Wm);
// New covariance is the sum of the outer product of the residuals times the
// weights
Matrixd<CovDim, 2 * States + 1> y;
for (int i = 0; i < 2 * States + 1; ++i) {
// y[:, i] = sigmas[:, i] - x
y.template block<CovDim, 1>(0, i) =
residualFunc(sigmas.template block<CovDim, 1>(0, i), x);
Matrixd<CovDim, States * 2 + CovDim> Sbar;
for (int i = 0; i < States * 2; i++) {
Sbar.template block<CovDim, 1>(0, i) =
std::sqrt(Wc[1]) *
residualFunc(sigmas.template block<CovDim, 1>(0, 1 + i), x);
}
Matrixd<CovDim, CovDim> P =
y * Eigen::DiagonalMatrix<double, 2 * States + 1>(Wc) * y.transpose();
Sbar.template block<CovDim, CovDim>(0, States * 2) = squareRootR;
return std::make_tuple(x, P);
// Merwe defines the QR decomposition as Aᵀ = QR
Matrixd<CovDim, CovDim> S = Sbar.transpose()
.householderQr()
.matrixQR()
.template block<CovDim, CovDim>(0, 0)
.template triangularView<Eigen::Upper>();
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
S, residualFunc(sigmas.template block<CovDim, 1>(0, 0), x), Wc[0]);
return std::make_tuple(x, S);
}
} // namespace frc

4
wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
View File

@@ -110,7 +110,7 @@ class DifferentialDrivePoseEstimatorTest {
t += dt;
}
assertEquals(0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.035, "Incorrect mean error");
assertEquals(0.0, maxError, 0.055, "Incorrect max error");
assertEquals(0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
assertEquals(0.0, maxError, 0.1, "Incorrect max error");
}
}

4
wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
View File

@@ -111,7 +111,7 @@ class MecanumDrivePoseEstimatorTest {
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
assertEquals(0.0, maxError, 0.42, "Incorrect max error");
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
assertEquals(0.0, maxError, 0.1, "Incorrect max error");
}
}

6
wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
View File

@@ -16,7 +16,7 @@ class MerweScaledSigmaPointsTest {
void testZeroMeanPoints() {
var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
var points =
merweScaledSigmaPoints.sigmaPoints(
merweScaledSigmaPoints.squareRootSigmaPoints(
VecBuilder.fill(0, 0), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1));
assertTrue(
@@ -31,8 +31,8 @@ class MerweScaledSigmaPointsTest {
void testNonzeroMeanPoints() {
var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
var points =
merweScaledSigmaPoints.sigmaPoints(
VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 10));
merweScaledSigmaPoints.squareRootSigmaPoints(
VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, Math.sqrt(10)));
assertTrue(
points.isEqual(

4
wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
View File

@@ -110,7 +110,7 @@ class SwerveDrivePoseEstimatorTest {
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
assertEquals(0.0, maxError, 0.42, "Incorrect max error");
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
assertEquals(0.0, maxError, 0.1, "Incorrect max error");
}
}

278
wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
View File

@@ -16,8 +16,8 @@ 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.N4;
import edu.wpi.first.math.numbers.N6;
import edu.wpi.first.math.numbers.N3;
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;
@@ -31,91 +31,111 @@ import org.junit.jupiter.api.Test;
class UnscentedKalmanFilterTest {
@SuppressWarnings({"LocalVariableName", "ParameterName"})
private static Matrix<N6, N1> getDynamics(Matrix<N6, N1> x, Matrix<N2, N1> u) {
private static Matrix<N5, N1> getDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
var motors = DCMotor.getCIM(2);
var gHigh = 7.08;
var rb = 0.8382 / 2.0;
var r = 0.0746125;
var m = 63.503;
var J = 5.6;
// 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.KtNMPerAmp
/ (motors.KvRadPerSecPerVolt * motors.rOhms * r * r);
var C2 = gHigh * motors.KtNMPerAmp / (motors.rOhms * r);
var k1 = 1.0 / m + Math.pow(rb, 2) / J;
var k2 = 1.0 / m - Math.pow(rb, 2) / J;
var c = x.get(2, 0);
var s = x.get(3, 0);
var vl = x.get(4, 0);
var vr = x.get(5, 0);
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 k1 = 1.0 / m + rb * rb / J;
var k2 = 1.0 / m - rb * rb / J;
var xvel = (vl + vr) / 2;
var w = (vr - vl) / (2.0 * rb);
var v = 0.5 * (vl + vr);
return VecBuilder.fill(
xvel * c,
xvel * s,
-s * w,
c * w,
k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)),
k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)));
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", "ParameterName"})
private static Matrix<N4, N1> getLocalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0), x.get(5, 0));
private static Matrix<N3, N1> getLocalMeasurementModel(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", "ParameterName"})
private static Matrix<N6, N1> getGlobalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
private static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
return x.copy();
}
@Test
@SuppressWarnings("LocalVariableName")
void testInit() {
var dtSeconds = 0.005;
assertDoesNotThrow(
() -> {
UnscentedKalmanFilter<N6, N2, N4> observer =
UnscentedKalmanFilter<N5, N2, N3> observer =
new UnscentedKalmanFilter<>(
Nat.N6(),
Nat.N4(),
Nat.N5(),
Nat.N3(),
UnscentedKalmanFilterTest::getDynamics,
UnscentedKalmanFilterTest::getLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
0.00505);
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),
dtSeconds);
var u = VecBuilder.fill(12.0, 12.0);
observer.predict(u, 0.00505);
observer.predict(u, dtSeconds);
var localY = getLocalMeasurementModel(observer.getXhat(), u);
observer.correct(u, localY);
var globalY = getGlobalMeasurementModel(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,
UnscentedKalmanFilterTest::getGlobalMeasurementModel,
R,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2));
});
}
@SuppressWarnings("LocalVariableName")
@Test
void testConvergence() {
double dtSeconds = 0.00505;
double dtSeconds = 0.005;
double rbMeters = 0.8382 / 2.0; // Robot radius
UnscentedKalmanFilter<N6, N2, N4> observer =
UnscentedKalmanFilter<N5, N2, N3> observer =
new UnscentedKalmanFilter<>(
Nat.N6(),
Nat.N4(),
Nat.N5(),
Nat.N3(),
UnscentedKalmanFilterTest::getDynamics,
UnscentedKalmanFilterTest::getLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
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),
dtSeconds);
List<Pose2d> waypoints =
@@ -125,56 +145,52 @@ class UnscentedKalmanFilterTest {
var trajectory =
TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
Matrix<N6, N1> nextR;
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.N6(),
Nat.N5(),
Nat.N2(),
UnscentedKalmanFilterTest::getDynamics,
new Matrix<>(Nat.N6(), Nat.N1()),
u);
new Matrix<>(Nat.N5(), Nat.N1()),
new Matrix<>(Nat.N2(), Nat.N1()));
observer.setXhat(VecBuilder.fill(2.75, 22.521, 1.0, 0.0, 0.0, 0.0)); // TODO not hard code this
var ref = trajectory.sample(0.0);
Matrix<N6, N1> r =
observer.setXhat(
VecBuilder.fill(
ref.poseMeters.getTranslation().getX(),
ref.poseMeters.getTranslation().getY(),
ref.poseMeters.getRotation().getCos(),
ref.poseMeters.getRotation().getSin(),
ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)),
ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters)));
nextR = r.copy();
trajectory.getInitialPose().getTranslation().getX(),
trajectory.getInitialPose().getTranslation().getY(),
trajectory.getInitialPose().getRotation().getRadians(),
0.0,
0.0));
var trueXhat = observer.getXhat();
double totalTime = trajectory.getTotalTimeSeconds();
for (int i = 0; i < (totalTime / dtSeconds); i++) {
ref = trajectory.sample(dtSeconds * i);
var ref = trajectory.sample(dtSeconds * i);
double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
nextR.set(2, 0, ref.poseMeters.getRotation().getCos());
nextR.set(3, 0, ref.poseMeters.getRotation().getSin());
nextR.set(4, 0, vl);
nextR.set(5, 0, vr);
var nextR =
VecBuilder.fill(
ref.poseMeters.getTranslation().getX(),
ref.poseMeters.getTranslation().getY(),
ref.poseMeters.getRotation().getRadians(),
vl,
vr);
Matrix<N4, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
var noiseStdDev = VecBuilder.fill(0.001, 0.001, 0.5, 0.5);
Matrix<N3, N1> localY = getLocalMeasurementModel(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(dtSeconds);
u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
r = nextR;
observer.predict(u, dtSeconds);
r = nextR;
trueXhat =
NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
}
@@ -183,25 +199,28 @@ class UnscentedKalmanFilterTest {
observer.correct(u, localY);
var globalY = getGlobalMeasurementModel(trueXhat, u);
var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5));
var R =
StateSpaceUtil.makeCovarianceMatrix(
Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
observer.correct(
Nat.N6(),
Nat.N5(),
u,
globalY,
UnscentedKalmanFilterTest::getGlobalMeasurementModel,
R,
(sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
Matrix::minus,
Matrix::minus,
Matrix::plus);
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2));
final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.25);
assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.25);
assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
assertEquals(0.0, observer.getXhat(3), 1.0);
assertEquals(0.0, observer.getXhat(4), 1.0);
assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.055);
assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.15);
assertEquals(
finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 0.000005);
assertEquals(0.0, observer.getXhat(3), 0.1);
assertEquals(0.0, observer.getXhat(4), 0.1);
}
@Test
@@ -235,95 +254,24 @@ class UnscentedKalmanFilterTest {
assertEquals(ref.get(0, 0), observer.getXhat(0), 5);
}
@SuppressWarnings("LocalVariableName")
@Test
void testUnscentedTransform() {
// From FilterPy
var ret =
UnscentedKalmanFilter.unscentedTransform(
Nat.N4(),
Nat.N4(),
Matrix.mat(Nat.N4(), Nat.N9())
.fill(
-0.9,
-0.822540333075852,
-0.8922540333075852,
-0.9,
-0.9,
-0.9774596669241481,
-0.9077459666924148,
-0.9,
-0.9,
1.0,
1.0,
1.077459666924148,
1.0,
1.0,
1.0,
0.9225403330758519,
1.0,
1.0,
-0.9,
-0.9,
-0.9,
-0.822540333075852,
-0.8922540333075852,
-0.9,
-0.9,
-0.9774596669241481,
-0.9077459666924148,
1.0,
1.0,
1.0,
1.0,
1.077459666924148,
1.0,
1.0,
1.0,
0.9225403330758519),
VecBuilder.fill(
-132.33333333,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667),
VecBuilder.fill(
-129.34333333,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667),
(sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
Matrix::minus);
void testRoundTripP() {
var dtSeconds = 0.005;
assertTrue(VecBuilder.fill(-0.9, 1, -0.9, 1).isEqual(ret.getFirst(), 1E-5));
var observer =
new UnscentedKalmanFilter<>(
Nat.N2(),
Nat.N2(),
(x, u) -> x,
(x, u) -> x,
VecBuilder.fill(0.0, 0.0),
VecBuilder.fill(0.0, 0.0),
dtSeconds);
assertTrue(
Matrix.mat(Nat.N4(), Nat.N4())
.fill(
2.02000002e-01,
2.00000500e-02,
-2.69044710e-29,
-4.59511477e-29,
2.00000500e-02,
2.00001000e-01,
-2.98781068e-29,
-5.12759588e-29,
-2.73372625e-29,
-3.09882635e-29,
2.02000002e-01,
2.00000500e-02,
-4.67065917e-29,
-5.10705197e-29,
2.00000500e-02,
2.00001000e-01)
.isEqual(ret.getSecond(), 1E-5));
var P = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 1.0, 2.0);
observer.setP(P);
assertTrue(observer.getP().isEqual(P, 1e-9));
}
}

4
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
View File

@@ -93,6 +93,6 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
}
EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().value() / dt.value()),
0.2);
EXPECT_NEAR(0.0, maxError, 0.4);
0.05);
EXPECT_NEAR(0.0, maxError, 0.1);
}

4
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
View File

@@ -84,6 +84,6 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
t += dt;
}
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
EXPECT_LT(maxError, 0.4);
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
EXPECT_LT(maxError, 0.1);
}

7
wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
View File

@@ -10,7 +10,7 @@ namespace drake::math {
namespace {
TEST(MerweScaledSigmaPointsTest, ZeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.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(
@@ -21,8 +21,9 @@ TEST(MerweScaledSigmaPointsTest, ZeroMean) {
TEST(MerweScaledSigmaPointsTest, NonzeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SigmaPoints(
frc::Vectord<2>{1.0, 2.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 10.0}});
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, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},

4
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -84,6 +84,6 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
t += dt;
}
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
EXPECT_LT(maxError, 0.4);
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
EXPECT_LT(maxError, 0.1);
}

44
wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
View File

@@ -23,7 +23,7 @@ namespace {
frc::Vectord<5> Dynamics(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 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
@@ -71,6 +71,11 @@ TEST(UnscentedKalmanFilterTest, Init) {
LocalMeasurementModel,
{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,6 +98,11 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
LocalMeasurementModel,
{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 =
@@ -150,12 +160,28 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
);
auto finalPosition = trajectory.Sample(trajectory.TotalTime());
ASSERT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
1.0);
ASSERT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
1.0);
ASSERT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
1.0);
ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
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(UnscentedKalmanFilterTest, RoundTripP) {
constexpr auto dt = 5_ms;
frc::UnscentedKalmanFilter<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));
}