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

[wpimath] Reset error covariance in pose estimator ResetPosition() (#3418)

Browse Source 
This also fixes a member function name inconsistency between languages
and adds missing documentation to C++'s KalmanFilterLatencyCompensator.

Fixes #3229.
This commit is contained in:
Tyler Veness
2021-06-08 21:20:43 -07:00
committed by GitHub
parent 0773f4033e
commit f9c3d54bd1
8 changed files with 76 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -216,9 +216,14 @@ public class DifferentialDrivePoseEstimator {
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_previousAngle = poseMeters.getRotation();
// Reset state estimate and error covariance
m_observer.reset();
m_latencyCompensator.reset();
m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0));
m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
m_previousAngle = poseMeters.getRotation();
}
/**

19
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
View File

@@ -22,6 +22,11 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
m_pastObserverSnapshots = new ArrayList<>();
}
/** Clears the observer snapshot buffer. */
public void reset() {
m_pastObserverSnapshots.clear();
}
/**
* Add past observer states to the observer snapshots list.
*
@@ -51,18 +56,18 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
* @param rows The rows in the global measurement vector.
* @param observer The observer to apply the past global measurement.
* @param nominalDtSeconds The nominal timestep.
* @param globalMeasurement The measurement.
* @param y The measurement.
* @param globalMeasurementCorrect The function take calls correct() on the observer.
* @param globalMeasurementTimestampSeconds The timestamp of the measurement.
* @param timestampSeconds The timestamp of the measurement.
*/
@SuppressWarnings({"ParameterName", "PMD.AvoidInstantiatingObjectsInLoops"})
public <R extends Num> void applyPastGlobalMeasurement(
Nat<R> rows,
KalmanTypeFilter<S, I, O> observer,
double nominalDtSeconds,
Matrix<R, N1> globalMeasurement,
Matrix<R, N1> y,
BiConsumer<Matrix<I, N1>, Matrix<R, N1>> globalMeasurementCorrect,
double globalMeasurementTimestampSeconds) {
double timestampSeconds) {
if (m_pastObserverSnapshots.isEmpty()) {
// State map was empty, which means that we got a past measurement right at startup. The only
// thing we can really do is ignore the measurement.
@@ -77,7 +82,7 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
while (low != high) {
int mid = (low + high) / 2;
if (m_pastObserverSnapshots.get(mid).getKey() < globalMeasurementTimestampSeconds) {
if (m_pastObserverSnapshots.get(mid).getKey() < timestampSeconds) {
// This index and everything under it are less than the requested timestamp. Therefore, we
// can discard them.
low = mid + 1;
@@ -91,7 +96,7 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
// We are simply assigning this index to a new variable to avoid confusion
// with variable names.
int index = low;
double timestamp = globalMeasurementTimestampSeconds;
double timestamp = timestampSeconds;
int indexOfClosestEntry =
Math.abs(timestamp - m_pastObserverSnapshots.get(index - 1).getKey())
<= Math.abs(
@@ -124,7 +129,7 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
// timestep for which we called predict.
// This makes the assumption that the dt is small enough that the difference between the
// measurement time and the time that the inputs were captured at is very small.
globalMeasurementCorrect.accept(snapshot.inputs, globalMeasurement);
globalMeasurementCorrect.accept(snapshot.inputs, y);
}
lastTimestamp = key;

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

@@ -181,9 +181,14 @@ public class MecanumDrivePoseEstimator {
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_previousAngle = poseMeters.getRotation();
// Reset state estimate and error covariance
m_observer.reset();
m_latencyCompensator.reset();
m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
m_previousAngle = poseMeters.getRotation();
}
/**

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

@@ -181,9 +181,14 @@ public class SwerveDrivePoseEstimator {
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_previousAngle = poseMeters.getRotation();
// Reset state estimate and error covariance
m_observer.reset();
m_latencyCompensator.reset();
m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
m_previousAngle = poseMeters.getRotation();
}
/**

13
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -55,9 +55,14 @@ void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
void DifferentialDrivePoseEstimator::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle) {
m_previousAngle = pose.Rotation();
// Reset state estimate and error covariance
m_observer.Reset();
m_latencyCompensator.Reset();
m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle;
m_previousAngle = pose.Rotation();
}
Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
@@ -68,9 +73,9 @@ Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
void DifferentialDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt,
PoseTo3dVector(visionRobotPose),
m_visionCorrect, timestamp);
m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
&m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
m_visionCorrect, timestamp);
}
Pose2d DifferentialDrivePoseEstimator::Update(

12
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -60,10 +60,12 @@ void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
void frc::MecanumDrivePoseEstimator::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle) {
// Set observer state.
// Reset state estimate and error covariance
m_observer.Reset();
m_latencyCompensator.Reset();
m_observer.SetXhat(PoseTo3dVector(pose));
// Calculate offsets.
m_gyroOffset = pose.Rotation() - gyroAngle;
m_previousAngle = pose.Rotation();
}
@@ -75,9 +77,9 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt,
PoseTo3dVector(visionRobotPose),
m_visionCorrect, timestamp);
m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
&m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
m_visionCorrect, timestamp);
}
Pose2d frc::MecanumDrivePoseEstimator::Update(

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

@@ -34,6 +34,19 @@ class KalmanFilterLatencyCompensator {
localMeasurements(localY) {}
};
/**
* Clears the observer snapshot buffer.
*/
void Reset() { m_pastObserverSnapshots.clear(); }
/**
* Add past observer states to the observer snapshots list.
*
* @param observer The observer.
* @param u The input at the timestamp.
* @param localY The local output at the timestamp
* @param timestamp The timesnap of the state.
*/
void AddObserverState(const KalmanFilterType& observer,
Eigen::Matrix<double, Inputs, 1> u,
Eigen::Matrix<double, Outputs, 1> localY,
@@ -48,8 +61,19 @@ class KalmanFilterLatencyCompensator {
}
}
/**
* Add past global measurements (such as from vision)to the estimator.
*
* @param observer The observer to apply the past global
* measurement.
* @param nominalDt The nominal timestep.
* @param y The measurement.
* @param globalMeasurementCorrect The function take calls correct() on the
* observer.
* @param timestamp The timestamp of the measurement.
*/
template <int Rows>
void ApplyPastMeasurement(
void ApplyPastGlobalMeasurement(
KalmanFilterType* observer, units::second_t nominalDt,
Eigen::Matrix<double, Rows, 1> y,
std::function<void(const Eigen::Matrix<double, Inputs, 1>& u,

8
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -125,10 +125,12 @@ class SwerveDrivePoseEstimator {
* @param gyroAngle The angle reported by the gyroscope.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
// Set observer state.
// Reset state estimate and error covariance
m_observer.Reset();
m_latencyCompensator.Reset();
m_observer.SetXhat(PoseTo3dVector(pose));
// Calculate offsets.
m_gyroOffset = pose.Rotation() - gyroAngle;
m_previousAngle = pose.Rotation();
}
@@ -184,7 +186,7 @@ class SwerveDrivePoseEstimator {
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
m_latencyCompensator.ApplyPastMeasurement<3>(
m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
&m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
m_visionCorrect, timestamp);
}