Use gyro angle instead of robot angle for odometry (#2081)

The odometry classes previously took in the robot angle as an argument, meaning that users had to take care of offsetting the gyro themselves to accurately report the robot angle. This change will make it so that users will not have to worry about resetting gyros and adding offsets themselves, as this will be handled by the odometry classes.
2026-06-22 01:11:42 +00:00 · 2019-11-15 20:34:10 -05:00
parent 1b66ead49d
commit 841ef91c0f
23 changed files with 258 additions and 77 deletions
--- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -10,18 +10,22 @@
 using namespace frc;

 DifferentialDriveOdometry::DifferentialDriveOdometry(
-    DifferentialDriveKinematics kinematics, const Pose2d& initialPose)
+    DifferentialDriveKinematics kinematics, const Rotation2d& gyroAngle,
+    const Pose2d& initialPose)
    : m_kinematics(kinematics), m_pose(initialPose) {
  m_previousAngle = m_pose.Rotation();
+  m_gyroOffset = m_pose.Rotation() - gyroAngle;
 }

 const Pose2d& DifferentialDriveOdometry::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& angle,
+    units::second_t currentTime, const Rotation2d& gyroAngle,
    const DifferentialDriveWheelSpeeds& wheelSpeeds) {
  units::second_t deltaTime =
      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
  m_previousTime = currentTime;

+  auto angle = gyroAngle + m_gyroOffset;
+
  auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
  static_cast<void>(dtheta);

--- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -10,18 +10,22 @@
 using namespace frc;

 MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
+                                           const Rotation2d& gyroAngle,
                                           const Pose2d& initialPose)
    : m_kinematics(kinematics), m_pose(initialPose) {
  m_previousAngle = m_pose.Rotation();
+  m_gyroOffset = m_pose.Rotation() - gyroAngle;
 }

 const Pose2d& MecanumDriveOdometry::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& angle,
+    units::second_t currentTime, const Rotation2d& gyroAngle,
    MecanumDriveWheelSpeeds wheelSpeeds) {
  units::second_t deltaTime =
      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
  m_previousTime = currentTime;

+  auto angle = gyroAngle + m_gyroOffset;
+
  auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
  static_cast<void>(dtheta);

--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -33,19 +33,26 @@ class DifferentialDriveOdometry {
   * Constructs a DifferentialDriveOdometry object.
   *
   * @param kinematics The differential drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
   * @param initialPose The starting position of the robot on the field.
   */
  explicit DifferentialDriveOdometry(DifferentialDriveKinematics kinematics,
+                                     const Rotation2d& gyroAngle,
                                     const Pose2d& initialPose = Pose2d());

  /**
   * Resets the robot's position on the field.
   *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
   * @param pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
   */
-  void ResetPosition(const Pose2d& pose) {
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
    m_pose = pose;
    m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
  }

  /**
@@ -63,13 +70,13 @@ class DifferentialDriveOdometry {
   * angular rate that is calculated from forward kinematics.
   *
   * @param currentTime The current time.
-   * @param angle The angle of the robot.
+   * @param gyroAngle The angle reported by the gyroscope.
   * @param wheelSpeeds The current wheel speeds.
   *
   * @return The new pose of the robot.
   */
  const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& angle,
+                               const Rotation2d& gyroAngle,
                               const DifferentialDriveWheelSpeeds& wheelSpeeds);

  /**
@@ -80,14 +87,15 @@ class DifferentialDriveOdometry {
   * This also takes in an angle parameter which is used instead of the
   * angular rate that is calculated from forward kinematics.
   *
-   * @param angle The angle of the robot.
+   * @param gyroAngle The angle reported by the gyroscope.
   * @param wheelSpeeds     The current wheel speeds.
   *
   * @return The new pose of the robot.
   */
-  const Pose2d& Update(const Rotation2d& angle,
+  const Pose2d& Update(const Rotation2d& gyroAngle,
                       const DifferentialDriveWheelSpeeds& wheelSpeeds) {
-    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
+    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
+                          wheelSpeeds);
  }

 private:
@@ -95,6 +103,7 @@ class DifferentialDriveOdometry {
  Pose2d m_pose;

  units::second_t m_previousTime = -1_s;
+  Rotation2d m_gyroOffset;
  Rotation2d m_previousAngle;
 };
 }  // namespace frc
--- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
@@ -31,19 +31,26 @@ class MecanumDriveOdometry {
   * Constructs a MecanumDriveOdometry object.
   *
   * @param kinematics The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
   * @param initialPose The starting position of the robot on the field.
   */
  explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
+                                const Rotation2d& gyroAngle,
                                const Pose2d& initialPose = Pose2d());

  /**
   * Resets the robot's position on the field.
   *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
   * @param pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
   */
-  void ResetPosition(const Pose2d& pose) {
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
    m_pose = pose;
    m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
  }

  /**
@@ -61,13 +68,13 @@ class MecanumDriveOdometry {
   * angular rate that is calculated from forward kinematics.
   *
   * @param currentTime The current time.
-   * @param angle The angle of the robot.
+   * @param gyroAngle The angle reported by the gyroscope.
   * @param wheelSpeeds The current wheel speeds.
   *
   * @return The new pose of the robot.
   */
  const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& angle,
+                               const Rotation2d& gyroAngle,
                               MecanumDriveWheelSpeeds wheelSpeeds);

  /**
@@ -78,14 +85,15 @@ class MecanumDriveOdometry {
   * This also takes in an angle parameter which is used instead of the
   * angular rate that is calculated from forward kinematics.
   *
-   * @param angle The angle of the robot.
+   * @param gyroAngle The angle reported by the gyroscope.
   * @param wheelSpeeds The current wheel speeds.
   *
   * @return The new pose of the robot.
   */
-  const Pose2d& Update(const Rotation2d& angle,
+  const Pose2d& Update(const Rotation2d& gyroAngle,
                       MecanumDriveWheelSpeeds wheelSpeeds) {
-    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
+    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
+                          wheelSpeeds);
  }

 private:
@@ -94,6 +102,7 @@ class MecanumDriveOdometry {

  units::second_t m_previousTime = -1_s;
  Rotation2d m_previousAngle;
+  Rotation2d m_gyroOffset;
 };

 }  // namespace frc
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
@@ -36,19 +36,26 @@ class SwerveDriveOdometry {
   * Constructs a SwerveDriveOdometry object.
   *
   * @param kinematics The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
   * @param initialPose The starting position of the robot on the field.
   */
  SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
+                      const Rotation2d& gyroAngle,
                      const Pose2d& initialPose = Pose2d());

  /**
   * Resets the robot's position on the field.
   *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
   * @param pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
   */
-  void ResetPosition(const Pose2d& pose) {
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
    m_pose = pose;
    m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
  }

  /**
@@ -66,7 +73,7 @@ class SwerveDriveOdometry {
   * angular rate that is calculated from forward kinematics.
   *
   * @param currentTime The current time.
-   * @param angle The angle of the robot.
+   * @param gyroAngle The angle reported by the gyroscope.
   * @param moduleStates The current state of all swerve modules. Please provide
   *                     the states in the same order in which you instantiated
   *                     your SwerveDriveKinematics.
@@ -75,7 +82,7 @@ class SwerveDriveOdometry {
   */
  template <typename... ModuleStates>
  const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& angle,
+                               const Rotation2d& gyroAngle,
                               ModuleStates&&... moduleStates);

  /**
@@ -86,7 +93,7 @@ class SwerveDriveOdometry {
   * This also takes in an angle parameter which is used instead of the
   * angular rate that is calculated from forward kinematics.
   *
-   * @param angle The angle of the robot.
+   * @param gyroAngle The angle reported by the gyroscope.
   * @param moduleStates The current state of all swerve modules. Please provide
   *                     the states in the same order in which you instantiated
   *                     your SwerveDriveKinematics.
@@ -94,9 +101,9 @@ class SwerveDriveOdometry {
   * @return The new pose of the robot.
   */
  template <typename... ModuleStates>
-  const Pose2d& Update(const Rotation2d& angle,
+  const Pose2d& Update(const Rotation2d& gyroAngle,
                       ModuleStates&&... moduleStates) {
-    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle,
+    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
                          moduleStates...);
  }

@@ -106,6 +113,7 @@ class SwerveDriveOdometry {

  units::second_t m_previousTime = -1_s;
  Rotation2d m_previousAngle;
+  Rotation2d m_gyroOffset;
 };

 }  // namespace frc
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -10,20 +10,24 @@
 namespace frc {
 template <size_t NumModules>
 SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
-    SwerveDriveKinematics<NumModules> kinematics, const Pose2d& initialPose)
+    SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
+    const Pose2d& initialPose)
    : m_kinematics(kinematics), m_pose(initialPose) {
  m_previousAngle = m_pose.Rotation();
+  m_gyroOffset = m_pose.Rotation() - gyroAngle;
 }

 template <size_t NumModules>
 template <typename... ModuleStates>
 const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& angle,
+    units::second_t currentTime, const Rotation2d& gyroAngle,
    ModuleStates&&... moduleStates) {
  units::second_t deltaTime =
      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
  m_previousTime = currentTime;

+  auto angle = gyroAngle + m_gyroOffset;
+
  auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
  static_cast<void>(dtheta);

--- a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
+++ b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
@@ -17,9 +17,9 @@ using namespace frc;

 TEST(DifferentialDriveOdometry, OneIteration) {
  DifferentialDriveKinematics kinematics{0.381_m * 2};
-  DifferentialDriveOdometry odometry{kinematics};
+  DifferentialDriveOdometry odometry{kinematics, 0_rad};

-  odometry.ResetPosition(Pose2d());
+  odometry.ResetPosition(Pose2d(), 0_rad);
  DifferentialDriveWheelSpeeds wheelSpeeds{0.02_mps, 0.02_mps};
  odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
  const auto& pose = odometry.UpdateWithTime(1_s, Rotation2d(), wheelSpeeds);
@@ -31,9 +31,9 @@ TEST(DifferentialDriveOdometry, OneIteration) {

 TEST(DifferentialDriveOdometry, QuarterCircle) {
  DifferentialDriveKinematics kinematics{0.381_m * 2};
-  DifferentialDriveOdometry odometry{kinematics};
+  DifferentialDriveOdometry odometry{kinematics, 0_rad};

-  odometry.ResetPosition(Pose2d());
+  odometry.ResetPosition(Pose2d(), 0_rad);
  DifferentialDriveWheelSpeeds wheelSpeeds{
      0.0_mps, units::meters_per_second_t(5 * wpi::math::pi)};
  odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
@@ -44,3 +44,16 @@ TEST(DifferentialDriveOdometry, QuarterCircle) {
  EXPECT_NEAR(pose.Translation().Y().to<double>(), 5.0, kEpsilon);
  EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
 }
+
+TEST(DifferentialDriveWheelSpeeds, GyroAngleReset) {
+  DifferentialDriveKinematics kinematics{0.381_m * 2};
+  DifferentialDriveOdometry odometry{kinematics, Rotation2d(90_deg)};
+
+  odometry.UpdateWithTime(0_s, Rotation2d(90_deg), {});
+  const auto& pose =
+      odometry.UpdateWithTime(1_s, Rotation2d(90_deg), {1_mps, 1_mps});
+
+  EXPECT_NEAR(pose.Translation().X().to<double>(), 1.0, kEpsilon);
+  EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+}
--- a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
+++ b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
@@ -18,11 +18,11 @@ class MecanumDriveOdometryTest : public ::testing::Test {
  Translation2d m_br{-12_m, -12_m};

  MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
-  MecanumDriveOdometry odometry{kinematics};
+  MecanumDriveOdometry odometry{kinematics, 0_rad};
 };

 TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
-  odometry.ResetPosition(Pose2d());
+  odometry.ResetPosition(Pose2d(), 0_rad);
  MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
                                      3.536_mps};

@@ -35,7 +35,7 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
 }

 TEST_F(MecanumDriveOdometryTest, TwoIterations) {
-  odometry.ResetPosition(Pose2d());
+  odometry.ResetPosition(Pose2d(), 0_rad);
  MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};

  odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
@@ -47,7 +47,7 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) {
 }

 TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
-  odometry.ResetPosition(Pose2d());
+  odometry.ResetPosition(Pose2d(), 0_rad);
  MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
                                 39.986_mps};
  odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
@@ -57,3 +57,16 @@ TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
  EXPECT_NEAR(pose.Translation().Y().to<double>(), 12, 0.01);
  EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
 }
+
+TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
+  odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+
+  MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+
+  odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
+  auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
+
+  EXPECT_NEAR(pose.Translation().X().to<double>(), 0.5, 0.01);
+  EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, 0.01);
+  EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
+}
--- a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
+++ b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
@@ -21,13 +21,13 @@ class SwerveDriveOdometryTest : public ::testing::Test {
  Translation2d m_br{-12_m, -12_m};

  SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
-  SwerveDriveOdometry<4> m_odometry{m_kinematics};
+  SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad};
 };

 TEST_F(SwerveDriveOdometryTest, TwoIterations) {
  SwerveModuleState state{5_mps, Rotation2d()};

-  m_odometry.ResetPosition(Pose2d());
+  m_odometry.ResetPosition(Pose2d(), 0_rad);
  m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(),
                            SwerveModuleState(), SwerveModuleState(),
                            SwerveModuleState());
@@ -47,7 +47,7 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {

  SwerveModuleState zero{0_mps, Rotation2d()};

-  m_odometry.ResetPosition(Pose2d());
+  m_odometry.ResetPosition(Pose2d(), 0_rad);
  m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero);
  auto pose =
      m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
@@ -56,3 +56,19 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
  EXPECT_NEAR(12.0, pose.Translation().Y().to<double>(), kEpsilon);
  EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
 }
+
+TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
+  m_odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+
+  SwerveModuleState state{5_mps, Rotation2d()};
+
+  m_odometry.UpdateWithTime(0_s, Rotation2d(90_deg), SwerveModuleState(),
+                            SwerveModuleState(), SwerveModuleState(),
+                            SwerveModuleState());
+  auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
+                                        state, state);
+
+  EXPECT_NEAR(0.5, pose.Translation().X().to<double>(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Translation().Y().to<double>(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+}
--- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
@@ -74,5 +74,5 @@ class Drivetrain {
  frc::AnalogGyro m_gyro{0};

  frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
-  frc::DifferentialDriveOdometry m_odometry{m_kinematics};
+  frc::DifferentialDriveOdometry m_odometry{m_kinematics, GetAngle()};
 };
--- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
@@ -71,5 +71,5 @@ class Drivetrain {
      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
      m_backRightLocation};

-  frc::MecanumDriveOdometry m_odometry{m_kinematics};
+  frc::MecanumDriveOdometry m_odometry{m_kinematics, GetAngle()};
 };
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -19,7 +19,8 @@ DriveSubsystem::DriveSubsystem()
      m_right2{kRightMotor2Port},
      m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
      m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
-      m_odometry{kDriveKinematics, frc::Pose2d()} {
+      m_odometry{kDriveKinematics,
+                 frc::Rotation2d(units::degree_t(GetHeading()))} {
  // Set the distance per pulse for the encoders
  m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
  m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
@@ -70,5 +71,6 @@ double DriveSubsystem::GetTurnRate() {
 frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }

 void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
-  m_odometry.ResetPosition(pose);
+  m_odometry.ResetPosition(pose,
+                           frc::Rotation2d(units::degree_t(GetHeading())));
 }
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
@@ -57,5 +57,5 @@ class Drivetrain {
      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
      m_backRightLocation};

-  frc::SwerveDriveOdometry<4> m_odometry{m_kinematics};
+  frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, GetAngle()};
 };
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
@@ -30,18 +30,21 @@ public class DifferentialDriveOdometry {
  private Pose2d m_poseMeters;
  private double m_prevTimeSeconds = -1;

+  private Rotation2d m_gyroOffset;
  private Rotation2d m_previousAngle;

  /**
   * Constructs a DifferentialDriveOdometry object.
   *
   * @param kinematics        The differential drive kinematics for your drivetrain.
+   * @param gyroAngle         The angle reported by the gyroscope.
   * @param initialPoseMeters The starting position of the robot on the field.
   */
-  public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics,
+  public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics, Rotation2d gyroAngle,
                                   Pose2d initialPoseMeters) {
    m_kinematics = kinematics;
    m_poseMeters = initialPoseMeters;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
    m_previousAngle = initialPoseMeters.getRotation();
  }

@@ -49,24 +52,31 @@ public class DifferentialDriveOdometry {
   * Constructs a DifferentialDriveOdometry object with the default pose at the origin.
   *
   * @param kinematics The differential drive kinematics for your drivetrain.
+   * @param gyroAngle  The angle reported by the gyroscope.
   */
-  public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics) {
-    this(kinematics, new Pose2d());
+  public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics, Rotation2d gyroAngle) {
+    this(kinematics, gyroAngle, new Pose2d());
  }

  /**
   * Resets the robot's position on the field. Do NOT zero your encoders if you
   * call this function at any other time except initialization.
   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
   * @param poseMeters The position on the field that your robot is at.
+   * @param gyroAngle  The angle reported by the gyroscope.
   */
-  public void resetPosition(Pose2d poseMeters) {
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
    m_poseMeters = poseMeters;
    m_previousAngle = poseMeters.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
  }

  /**
   * Returns the position of the robot on the field.
+   *
   * @return The pose of the robot (x and y are in meters).
   */
  public Pose2d getPoseMeters() {
@@ -82,15 +92,17 @@ public class DifferentialDriveOdometry {
   * angular rate that is calculated from forward kinematics.
   *
   * @param currentTimeSeconds The current time in seconds.
-   * @param angle              The current robot angle.
+   * @param gyroAngle          The angle reported by the gyroscope.
   * @param wheelSpeeds        The current wheel speeds.
   * @return The new pose of the robot.
   */
-  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle,
+  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
                               DifferentialDriveWheelSpeeds wheelSpeeds) {
    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
    m_prevTimeSeconds = currentTimeSeconds;

+    var angle = gyroAngle.plus(m_gyroOffset);
+
    var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
    var newPose = m_poseMeters.exp(
        new Twist2d(chassisState.vxMetersPerSecond * period,
@@ -111,13 +123,13 @@ public class DifferentialDriveOdometry {
   * also takes in an angle parameter which is used instead of the
   * angular rate that is calculated from forward kinematics.
   *
-   * @param angle       The angle of the robot.
+   * @param gyroAngle   The angle reported by the gyroscope.
   * @param wheelSpeeds The current wheel speeds.
   * @return The new pose of the robot.
   */
-  public Pose2d update(Rotation2d angle,
+  public Pose2d update(Rotation2d gyroAngle,
                       DifferentialDriveWheelSpeeds wheelSpeeds) {
    return updateWithTime(Timer.getFPGATimestamp(),
-        angle, wheelSpeeds);
+        gyroAngle, wheelSpeeds);
  }
 }
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
@@ -26,17 +26,21 @@ public class MecanumDriveOdometry {
  private Pose2d m_poseMeters;
  private double m_prevTimeSeconds = -1;

+  private Rotation2d m_gyroOffset;
  private Rotation2d m_previousAngle;

  /**
   * Constructs a MecanumDriveOdometry object.
   *
   * @param kinematics        The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle         The angle reported by the gyroscope.
   * @param initialPoseMeters The starting position of the robot on the field.
   */
-  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Pose2d initialPoseMeters) {
+  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle,
+                              Pose2d initialPoseMeters) {
    m_kinematics = kinematics;
    m_poseMeters = initialPoseMeters;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
    m_previousAngle = initialPoseMeters.getRotation();
  }

@@ -44,23 +48,30 @@ public class MecanumDriveOdometry {
   * Constructs a MecanumDriveOdometry object with the default pose at the origin.
   *
   * @param kinematics The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle  The angle reported by the gyroscope.
   */
-  public MecanumDriveOdometry(MecanumDriveKinematics kinematics) {
-    this(kinematics, new Pose2d());
+  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
+    this(kinematics, gyroAngle, new Pose2d());
  }

  /**
   * Resets the robot's position on the field.
   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
   * @param poseMeters The position on the field that your robot is at.
+   * @param gyroAngle  The angle reported by the gyroscope.
   */
-  public void resetPosition(Pose2d poseMeters) {
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
    m_poseMeters = poseMeters;
    m_previousAngle = poseMeters.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
  }

  /**
   * Returns the position of the robot on the field.
+   *
   * @return The pose of the robot (x and y are in meters).
   */
  public Pose2d getPoseMeters() {
@@ -76,15 +87,17 @@ public class MecanumDriveOdometry {
   * angular rate that is calculated from forward kinematics.
   *
   * @param currentTimeSeconds The current time in seconds.
-   * @param angle              The angle of the robot.
+   * @param gyroAngle          The angle reported by the gyroscope.
   * @param wheelSpeeds        The current wheel speeds.
   * @return The new pose of the robot.
   */
-  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle,
+  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
                               MecanumDriveWheelSpeeds wheelSpeeds) {
    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
    m_prevTimeSeconds = currentTimeSeconds;

+    var angle = gyroAngle.plus(m_gyroOffset);
+
    var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
    var newPose = m_poseMeters.exp(
        new Twist2d(chassisState.vxMetersPerSecond * period,
@@ -104,13 +117,13 @@ public class MecanumDriveOdometry {
   * also takes in an angle parameter which is used instead of the
   * angular rate that is calculated from forward kinematics.
   *
-   * @param angle       The angle of the robot.
+   * @param gyroAngle   The angle reported by the gyroscope.
   * @param wheelSpeeds The current wheel speeds.
   * @return The new pose of the robot.
   */
-  public Pose2d update(Rotation2d angle,
+  public Pose2d update(Rotation2d gyroAngle,
                       MecanumDriveWheelSpeeds wheelSpeeds) {
-    return updateWithTime(Timer.getFPGATimestamp(), angle,
+    return updateWithTime(Timer.getFPGATimestamp(), gyroAngle,
        wheelSpeeds);
  }
 }
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
@@ -26,17 +26,21 @@ public class SwerveDriveOdometry {
  private Pose2d m_poseMeters;
  private double m_prevTimeSeconds = -1;

+  private Rotation2d m_gyroOffset;
  private Rotation2d m_previousAngle;

  /**
   * Constructs a SwerveDriveOdometry object.
   *
   * @param kinematics  The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle   The angle reported by the gyroscope.
   * @param initialPose The starting position of the robot on the field.
   */
-  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Pose2d initialPose) {
+  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle,
+                             Pose2d initialPose) {
    m_kinematics = kinematics;
    m_poseMeters = initialPose;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
    m_previousAngle = initialPose.getRotation();
  }

@@ -44,23 +48,30 @@ public class SwerveDriveOdometry {
   * Constructs a SwerveDriveOdometry object with the default pose at the origin.
   *
   * @param kinematics The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle  The angle reported by the gyroscope.
   */
-  public SwerveDriveOdometry(SwerveDriveKinematics kinematics) {
-    this(kinematics, new Pose2d());
+  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
+    this(kinematics, gyroAngle, new Pose2d());
  }

  /**
   * Resets the robot's position on the field.
   *
-   * @param pose The position on the field that your robot is at.
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param pose      The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
   */
-  public void resetPosition(Pose2d pose) {
+  public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
    m_poseMeters = pose;
    m_previousAngle = pose.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
  }

  /**
   * Returns the position of the robot on the field.
+   *
   * @return The pose of the robot (x and y are in meters).
   */
  public Pose2d getPoseMeters() {
@@ -76,17 +87,19 @@ public class SwerveDriveOdometry {
   * angular rate that is calculated from forward kinematics.
   *
   * @param currentTimeSeconds The current time in seconds.
-   * @param angle              The angle of the robot.
+   * @param gyroAngle          The angle reported by the gyroscope.
   * @param moduleStates       The current state of all swerve modules. Please provide
   *                           the states in the same order in which you instantiated your
   *                           SwerveDriveKinematics.
   * @return The new pose of the robot.
   */
-  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle,
+  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
                               SwerveModuleState... moduleStates) {
    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
    m_prevTimeSeconds = currentTimeSeconds;

+    var angle = gyroAngle.plus(m_gyroOffset);
+
    var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
    var newPose = m_poseMeters.exp(
        new Twist2d(chassisState.vxMetersPerSecond * period,
@@ -107,13 +120,13 @@ public class SwerveDriveOdometry {
   * also takes in an angle parameter which is used instead of the angular
   * rate that is calculated from forward kinematics.
   *
-   * @param angle        The angle of the robot.
+   * @param gyroAngle    The angle reported by the gyroscope.
   * @param moduleStates The current state of all swerve modules. Please provide
   *                     the states in the same order in which you instantiated your
   *                     SwerveDriveKinematics.
   * @return The new pose of the robot.
   */
-  public Pose2d update(Rotation2d angle, SwerveModuleState... moduleStates) {
-    return updateWithTime(Timer.getFPGATimestamp(), angle, moduleStates);
+  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
+    return updateWithTime(Timer.getFPGATimestamp(), gyroAngle, moduleStates);
  }
 }
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
@@ -11,6 +11,7 @@ import org.junit.jupiter.api.Test;

 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;

 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
@@ -19,11 +20,12 @@ class DifferentialDriveOdometryTest {
  private static final double kEpsilon = 1E-9;
  private final DifferentialDriveKinematics m_kinematics
      = new DifferentialDriveKinematics(0.381 * 2);
-  private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(m_kinematics);
+  private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(m_kinematics,
+      new Rotation2d());

  @Test
  void testOneIteration() {
-    m_odometry.resetPosition(new Pose2d());
+    m_odometry.resetPosition(new Pose2d(), new Rotation2d());
    var speeds = new DifferentialDriveWheelSpeeds(0.02, 0.02);
    m_odometry.updateWithTime(0.0, new Rotation2d(), new DifferentialDriveWheelSpeeds());
    var pose = m_odometry.updateWithTime(1.0, new Rotation2d(), speeds);
@@ -37,7 +39,7 @@ class DifferentialDriveOdometryTest {

  @Test
  void testQuarterCircle() {
-    m_odometry.resetPosition(new Pose2d());
+    m_odometry.resetPosition(new Pose2d(), new Rotation2d());
    var speeds = new DifferentialDriveWheelSpeeds(0.0, 5 * Math.PI);
    m_odometry.updateWithTime(0.0, new Rotation2d(), new DifferentialDriveWheelSpeeds());
    var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), speeds);
@@ -48,4 +50,21 @@ class DifferentialDriveOdometryTest {
        () -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon)
    );
  }
+
+  @Test
+  void testGyroAngleReset() {
+    var gyro = Rotation2d.fromDegrees(90.0);
+    var fieldAngle = Rotation2d.fromDegrees(0.0);
+    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
+    var speeds = new DifferentialDriveWheelSpeeds(1.0, 1.0);
+    m_odometry.updateWithTime(0.0, gyro, new DifferentialDriveWheelSpeeds());
+    var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
+
+    assertAll(
+        () -> assertEquals(1.0, pose.getTranslation().getX(), kEpsilon),
+        () -> assertEquals(0.0, pose.getTranslation().getY(), kEpsilon),
+        () -> assertEquals(0.0, pose.getRotation().getRadians(), kEpsilon)
+    );
+  }
+
 }
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
@@ -9,6 +9,7 @@ package edu.wpi.first.wpilibj.kinematics;

 import org.junit.jupiter.api.Test;

+import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Translation2d;

@@ -21,10 +22,12 @@ class MecanumDriveOdometryTest {
  private final Translation2d m_bl = new Translation2d(-12, 12);
  private final Translation2d m_br = new Translation2d(-12, -12);

+
  private final MecanumDriveKinematics m_kinematics =
      new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);

-  private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics);
+  private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
+      new Rotation2d());

  @Test
  void testMultipleConsecutiveUpdates() {
@@ -71,4 +74,21 @@ class MecanumDriveOdometryTest {
    );
  }

+  @Test
+  void testGyroAngleReset() {
+    var gyro = Rotation2d.fromDegrees(90.0);
+    var fieldAngle = Rotation2d.fromDegrees(0.0);
+    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
+    var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536,
+        3.536, 3.536);
+    m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
+    var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
+
+    assertAll(
+        () -> assertEquals(5.0, pose.getTranslation().getX(), 0.1),
+        () -> assertEquals(0.00, pose.getTranslation().getY(), 0.1),
+        () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
+    );
+  }
+
 }
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
@@ -9,6 +9,7 @@ package edu.wpi.first.wpilibj.kinematics;

 import org.junit.jupiter.api.Test;

+import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Translation2d;

@@ -24,7 +25,8 @@ class SwerveDriveOdometryTest {
  private final SwerveDriveKinematics m_kinematics =
      new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);

-  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics);
+  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
+      new Rotation2d());

  @Test
  void testTwoIterations() {
@@ -73,4 +75,22 @@ class SwerveDriveOdometryTest {
        () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
    );
  }
+
+  @Test
+  void testGyroAngleReset() {
+    var gyro = Rotation2d.fromDegrees(90.0);
+    var fieldAngle = Rotation2d.fromDegrees(0.0);
+    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
+    var state = new SwerveModuleState();
+    m_odometry.updateWithTime(0.0, gyro, state, state, state, state);
+    state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0));
+    var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state);
+
+    assertAll(
+        () -> assertEquals(1.0, pose.getTranslation().getX(), 0.1),
+        () -> assertEquals(0.00, pose.getTranslation().getY(), 0.1),
+        () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
+    );
+  }
+
 }
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
@@ -51,7 +51,7 @@ public class Drivetrain {
      = new DifferentialDriveKinematics(kTrackWidth);

  private final DifferentialDriveOdometry m_odometry
-      = new DifferentialDriveOdometry(m_kinematics);
+      = new DifferentialDriveOdometry(m_kinematics, getAngle());

  /**
   * Constructs a differential drive object.
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
@@ -52,7 +52,8 @@ public class Drivetrain {
      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
  );

-  private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics);
+  private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
+      getAngle());

  /**
   * Constructs a MecanumDrive and resets the gyro.
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
@@ -57,7 +57,8 @@ public class DriveSubsystem extends SubsystemBase {
  private final Gyro m_gyro = new ADXRS450_Gyro();

  // Odometry class for tracking robot pose
-  DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(kDriveKinematics);
+  DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(kDriveKinematics,
+      Rotation2d.fromDegrees(getHeading()));

  /**
   * Creates a new DriveSubsystem.
@@ -93,7 +94,7 @@ public class DriveSubsystem extends SubsystemBase {
   * @param pose The pose to which to set the odometry.
   */
  public void resetOdometry(Pose2d pose) {
-    m_odometry.resetPosition(pose);
+    m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
  }

  /**
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
@@ -37,7 +37,7 @@ public class Drivetrain {
      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
  );

-  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics);
+  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics, getAngle());

  public Drivetrain() {
    m_gyro.reset();