diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java index 654a20ffb1..f7be897762 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java @@ -226,6 +226,8 @@ public class DifferentialDrivePoseEstimator { m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0)); + m_prevTimeSeconds = -1; + m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle); m_previousAngle = poseMeters.getRotation(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java index 0b82c870f8..c8dbdedbf0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java @@ -190,6 +190,8 @@ public class MecanumDrivePoseEstimator { m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters)); + m_prevTimeSeconds = -1; + m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle); m_previousAngle = poseMeters.getRotation(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java index 2588e4d54b..7c205de2e6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java @@ -188,6 +188,8 @@ public class SwerveDrivePoseEstimator { m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters)); + m_prevTimeSeconds = -1; + m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle); m_previousAngle = poseMeters.getRotation(); } diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index 7b25629861..d4115500e5 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -58,6 +58,8 @@ void DifferentialDrivePoseEstimator::ResetPosition( m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m)); + m_prevTime = -1_s; + m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle; m_previousAngle = pose.Rotation(); } diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index e411eaa879..a0a115481d 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -59,6 +59,8 @@ void frc::MecanumDrivePoseEstimator::ResetPosition( m_observer.SetXhat(PoseTo3dVector(pose)); + m_prevTime = -1_s; + m_gyroOffset = pose.Rotation() - gyroAngle; m_previousAngle = pose.Rotation(); } diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 2fe8ddf037..ac680ed1c6 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -127,6 +127,8 @@ class SwerveDrivePoseEstimator { m_observer.SetXhat(PoseTo3dVector(pose)); + m_prevTime = -1_s; + m_gyroOffset = pose.Rotation() - gyroAngle; m_previousAngle = pose.Rotation(); }