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

[wpimath] Move DiscretizeR() in EKF and UKF from Predict() to Correct() (#2753)

Browse Source 
By storing the previous dt, it can be moved into Correct() where it is
actually used. This lets us take the continuous R as an argument in the
user-provided R overload.
This commit is contained in:
Tyler Veness
2020-10-07 21:54:38 -07:00
committed by GitHub
parent bf26656547
commit 32f429a819
4 changed files with 37 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java
View File

@@ -39,13 +39,13 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
@SuppressWarnings("MemberName")
private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> m_h;
private final Matrix<States, States> m_contQ;
private Matrix<Outputs, Outputs> m_discR;
private final Matrix<States, States> m_initP;
private final Matrix<Outputs, Outputs> m_contR;
@SuppressWarnings("MemberName")
private Matrix<States, N1> m_xHat;
@SuppressWarnings("MemberName")
private Matrix<States, States> m_P;
private double m_dtSeconds;
/**
* Constructs an extended Kalman filter.
@@ -78,10 +78,11 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
m_f = f;
m_h = h;
reset();
m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
this.m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
m_dtSeconds = dtSeconds;
reset();
final var contA = NumericalJacobian
.numericalJacobianX(states, states, f, m_xHat, new Matrix<>(inputs, Nat.N1()));
@@ -92,13 +93,13 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
final var discA = discPair.getFirst();
final var discQ = discPair.getSecond();
m_discR = Discretization.discretizeR(m_contR, dtSeconds);
final var discR = Discretization.discretizeR(m_contR, dtSeconds);
// IsStabilizable(A^T, C^T) will tell us if the system is observable.
boolean isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
if (isObservable && outputs.getNum() <= states.getNum()) {
m_initP = Drake.discreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, m_discR) ;
discA.transpose(), C.transpose(), discQ, discR) ;
} else {
m_initP = new Matrix<>(states, states);
}
@@ -223,7 +224,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
m_xHat = RungeKutta.rungeKutta(f, m_xHat, u, dtSeconds);
m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
m_discR = Discretization.discretizeR(m_contR, dtSeconds);
m_dtSeconds = dtSeconds;
}
/**
@@ -235,7 +236,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
@SuppressWarnings("ParameterName")
@Override
public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
correct(m_outputs, u, y, m_h, m_discR);
correct(m_outputs, u, y, m_h, m_contR);
}
/**
@@ -261,8 +262,9 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
Matrix<Rows, Rows> R
) {
final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u);
final var discR = Discretization.discretizeR(R, m_dtSeconds);
final var S = C.times(m_P).times(C.transpose()).plus(R);
final var S = C.times(m_P).times(C.transpose()).plus(discR);
// We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
// efficiently.

16
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java
View File

@@ -45,8 +45,8 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
private Matrix<States, States> m_P;
private final Matrix<States, States> m_contQ;
private final Matrix<Outputs, Outputs> m_contR;
private Matrix<Outputs, Outputs> m_discR;
private Matrix<States, ?> m_sigmasF;
private double m_dtSeconds;
private final MerweScaledSigmaPoints<States> m_pts;
@@ -61,7 +61,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
* the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param nominalDtSeconds Nominal discretization timestep.
* @param dtSeconds Nominal discretization timestep.
*/
@SuppressWarnings("ParameterName")
public UnscentedKalmanFilter(Nat<States> states, Nat<Outputs> outputs,
@@ -71,7 +71,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
Matrix<Outputs, N1>> h,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
double nominalDtSeconds) {
double dtSeconds) {
this.m_states = states;
this.m_outputs = outputs;
@@ -81,7 +81,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
m_discR = Discretization.discretizeR(m_contR, nominalDtSeconds);
m_dtSeconds = dtSeconds;
m_pts = new MerweScaledSigmaPoints<>(states);
@@ -238,7 +238,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
m_xHat = ret.getFirst();
m_P = ret.getSecond().plus(discQ);
m_discR = Discretization.discretizeR(m_contR, dtSeconds);
m_dtSeconds = dtSeconds;
}
/**
@@ -250,7 +250,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
@SuppressWarnings("ParameterName")
@Override
public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
correct(m_outputs, u, y, m_h, m_discR);
correct(m_outputs, u, y, m_h, m_contR);
}
/**
@@ -272,6 +272,8 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
Matrix<R, N1> y,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<R, N1>> h,
Matrix<R, R> R) {
final var discR = Discretization.discretizeR(R, m_dtSeconds);
// Transform sigma points into measurement space
Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(
rows.getNum(), 2 * m_states.getNum() + 1));
@@ -287,7 +289,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
// Mean and covariance of prediction passed through unscented transform
var transRet = unscentedTransform(m_states, rows, sigmasH, m_pts.getWm(), m_pts.getWc());
var yHat = transRet.getFirst();
var Py = transRet.getSecond().plus(R);
var Py = transRet.getSecond().plus(discR);
// Compute cross covariance of the state and the measurements
Matrix<States, R> Pxy = new Matrix<>(m_states, rows);

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

@@ -49,6 +49,7 @@ class ExtendedKalmanFilter {
: m_f(f), m_h(h) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
Reset();
@@ -63,14 +64,15 @@ class ExtendedKalmanFilter {
Eigen::Matrix<double, States, States> discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
m_discR = DiscretizeR<Outputs>(m_contR, dt);
Eigen::Matrix<double, Outputs, Outputs> discR =
DiscretizeR<Outputs>(m_contR, dt);
// IsStabilizable(A^T, C^T) will tell us if the system is observable.
bool isObservable =
IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
if (isObservable && Outputs <= States) {
m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, m_discR);
discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = Eigen::Matrix<double, States, States>::Zero();
}
@@ -139,6 +141,8 @@ class ExtendedKalmanFilter {
* @param dt Timestep for prediction.
*/
void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
m_dt = dt;
// Find continuous A
Eigen::Matrix<double, States, States> contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
@@ -150,7 +154,6 @@ class ExtendedKalmanFilter {
m_xHat = RungeKutta(m_f, m_xHat, u, dt);
m_P = discA * m_P * discA.transpose() + discQ;
m_discR = DiscretizeR<Outputs>(m_contR, dt);
}
/**
@@ -161,7 +164,7 @@ class ExtendedKalmanFilter {
*/
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
const Eigen::Matrix<double, Outputs, 1>& y) {
Correct<Outputs>(u, y, m_h, m_discR);
Correct<Outputs>(u, y, m_h, m_contR);
}
/**
@@ -187,8 +190,9 @@ class ExtendedKalmanFilter {
const Eigen::Matrix<double, Rows, Rows>& R) {
const Eigen::Matrix<double, Rows, States> C =
NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + R;
Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + discR;
// We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
// efficiently.
@@ -222,7 +226,7 @@ class ExtendedKalmanFilter {
Eigen::Matrix<double, States, States> m_P;
Eigen::Matrix<double, States, States> m_contQ;
Eigen::Matrix<double, Outputs, Outputs> m_contR;
Eigen::Matrix<double, Outputs, Outputs> m_discR;
units::second_t m_dt;
Eigen::Matrix<double, States, States> m_initP;
};

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

@@ -50,8 +50,7 @@ class UnscentedKalmanFilter {
: m_f(f), m_h(h) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_discR = DiscretizeR<Outputs>(m_contR, dt);
m_dt = dt;
Reset();
}
@@ -119,6 +118,8 @@ class UnscentedKalmanFilter {
* @param dt Timestep for prediction.
*/
void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
m_dt = dt;
// Discretize Q before projecting mean and covariance forward
Eigen::Matrix<double, States, States> contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
@@ -141,7 +142,6 @@ class UnscentedKalmanFilter {
m_P = std::get<1>(ret);
m_P += discQ;
m_discR = DiscretizeR<Outputs>(m_contR, dt);
}
/**
@@ -152,7 +152,7 @@ class UnscentedKalmanFilter {
*/
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
const Eigen::Matrix<double, Outputs, 1>& y) {
Correct<Outputs>(u, y, m_h, m_discR);
Correct<Outputs>(u, y, m_h, m_contR);
}
/**
@@ -176,6 +176,8 @@ class UnscentedKalmanFilter {
const Eigen::Matrix<double, Inputs, 1>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R) {
const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
// Transform sigma points into measurement space
Eigen::Matrix<double, Rows, 2 * States + 1> sigmasH;
Eigen::Matrix<double, States, 2 * States + 1> sigmas =
@@ -188,7 +190,7 @@ class UnscentedKalmanFilter {
// Mean and covariance of prediction passed through UT
auto [yHat, Py] =
UnscentedTransform<States, Rows>(sigmasH, m_pts.Wm(), m_pts.Wc());
Py += R;
Py += discR;
// Compute cross covariance of the state and the measurements
Eigen::Matrix<double, States, Rows> Pxy;
@@ -224,8 +226,8 @@ class UnscentedKalmanFilter {
Eigen::Matrix<double, States, States> m_P;
Eigen::Matrix<double, States, States> m_contQ;
Eigen::Matrix<double, Outputs, Outputs> m_contR;
Eigen::Matrix<double, Outputs, Outputs> m_discR;
Eigen::Matrix<double, States, 2 * States + 1> m_sigmasF;
units::second_t m_dt;
MerweScaledSigmaPoints<States> m_pts;
};