Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Replace DiscretizeAQTaylor() with DiscretizeAQ() (#5344)

Browse Source 
Until #5339 is fixed, we have to use the slower, more accurate version.
This commit is contained in:
Tyler Veness
2023-05-18 07:13:20 -07:00
committed by GitHub
parent c976f40364
commit eed1e6e3cb
6 changed files with 9 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
View File

@@ -138,7 +138,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
NumericalJacobian.numericalJacobianX(
outputs, states, h, m_xHat, new Matrix<>(inputs, Nat.N1()));
final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
final var discPair = Discretization.discretizeAQ(contA, m_contQ, dtSeconds);
final var discA = discPair.getFirst();
final var discQ = discPair.getSecond();
@@ -259,7 +259,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u);
// Find discrete A and Q
final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
final var discPair = Discretization.discretizeAQ(contA, m_contQ, dtSeconds);
final var discA = discPair.getFirst();
final var discQ = discPair.getSecond();

2
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
View File

@@ -65,7 +65,7 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
var pair = Discretization.discretizeAQTaylor(plant.getA(), contQ, dtSeconds);
var pair = Discretization.discretizeAQ(plant.getA(), contQ, dtSeconds);
var discA = pair.getFirst();
var discQ = pair.getSecond();

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

@@ -331,7 +331,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
// Discretize Q before projecting mean and covariance forward
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 discQ = Discretization.discretizeAQ(contA, m_contQ, dtSeconds).getSecond();
var squareRootDiscQ = discQ.lltDecompose(true);
var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);