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

[wpimath] Remove incorrect discretization in pose estimators (#3460)

Browse Source
This commit is contained in:
Prateek Machiraju
2021-06-24 00:57:52 -04:00
committed by GitHub
parent ea0b8f48e6
commit 646ded9123
10 changed files with 22 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,7 +4,6 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.Discretization;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
@@ -59,7 +58,7 @@ public class DifferentialDrivePoseEstimator {
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private Matrix<N3, N3> m_visionDiscreteR;
private Matrix<N3, N3> m_visionContR;
/**
* Constructs a DifferentialDrivePoseEstimator.
@@ -143,7 +142,7 @@ public class DifferentialDrivePoseEstimator {
u,
y,
(x, u1) -> new Matrix<>(x.getStorage().extractMatrix(0, 3, 0, 1)),
m_visionDiscreteR,
m_visionContR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
@@ -164,8 +163,7 @@ public class DifferentialDrivePoseEstimator {
* theta]^T, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
m_visionDiscreteR = Discretization.discretizeR(visionContR, m_nominalDt);
m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
}
@SuppressWarnings({"ParameterName", "MethodName"})

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

@@ -4,7 +4,6 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.Discretization;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.StateSpaceUtil;
@@ -54,7 +53,7 @@ public class MecanumDrivePoseEstimator {
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private Matrix<N3, N3> m_visionDiscreteR;
private Matrix<N3, N3> m_visionContR;
/**
* Constructs a MecanumDrivePoseEstimator.
@@ -144,7 +143,7 @@ public class MecanumDrivePoseEstimator {
u,
y,
(x, u1) -> x,
m_visionDiscreteR,
m_visionContR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
@@ -165,8 +164,7 @@ public class MecanumDrivePoseEstimator {
* theta]^T, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
m_visionDiscreteR = Discretization.discretizeR(visionContR, m_nominalDt);
m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
}
/**

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

@@ -4,7 +4,6 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.Discretization;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.StateSpaceUtil;
@@ -54,7 +53,7 @@ public class SwerveDrivePoseEstimator {
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private Matrix<N3, N3> m_visionDiscreteR;
private Matrix<N3, N3> m_visionContR;
/**
* Constructs a SwerveDrivePoseEstimator.
@@ -144,7 +143,7 @@ public class SwerveDrivePoseEstimator {
u,
y,
(x, u1) -> x,
m_visionDiscreteR,
m_visionContR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
@@ -165,8 +164,7 @@ public class SwerveDrivePoseEstimator {
* theta]^T, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
m_visionDiscreteR = Discretization.discretizeR(visionContR, m_nominalDt);
m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
}
/**

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

@@ -334,7 +334,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
* @param u Same control input used in the predict step.
* @param y Measurement vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param R Measurement noise covariance matrix.
* @param R Measurement noise covariance matrix (continuous-time).
*/
@SuppressWarnings({"ParameterName", "LambdaParameterName", "LocalVariableName"})
public <R extends Num> void correct(
@@ -364,7 +364,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
* @param u Same control input used in the predict step.
* @param y Measurement vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param R Measurement noise covariance matrix.
* @param R Measurement noise covariance matrix (continuous-time).
* @param meanFuncY A function that computes the mean of 2 * States + 1 measurement vectors using
* a given set of weights.
* @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it