diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index e9a34644..f30fe905 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -189,5 +189,9 @@ object RobotContainer {
 
   fun getAutonomousCommand() = AutonomousSelector.getCommand(drivetrain)
 
+  fun drivetrainResetFieldFrameEstimator() {
+    drivetrain.resetFieldFrameEstimator(drivetrain.fieldTRobot)
+  }
+
   fun mapTunableCommands() {}
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
index 606e4021..a0536880 100644
--- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
+++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
@@ -1,5 +1,6 @@
 package com.team4099.robot2023.auto
 
+import com.team4099.robot2023.RobotContainer
 import com.team4099.robot2023.auto.mode.TestAutoPath
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import edu.wpi.first.networktables.GenericEntry
@@ -57,6 +58,7 @@ object AutonomousSelector {
 
   fun getCommand(drivetrain: Drivetrain): Command {
     val mode = AutonomousMode.TEST_AUTO_PATH
+    RobotContainer.drivetrainResetFieldFrameEstimator()
     // val mode = autonomousModeChooser.get()
     //    println("${waitTime().inSeconds} wait command")
     when (mode) {
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
index 22b8e298..390e89a8 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
@@ -147,6 +147,7 @@ private constructor(
 
   private var drivePoseSupplier: () -> Pose2d
   private var odoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees)
+  private var lastStableOdoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees)
 
   private var errorString = ""
 
@@ -214,6 +215,8 @@ private constructor(
 
   override fun initialize() {
     odoTField = drivetrain.odomTField
+    lastStableOdoTField = drivetrain.lastStableOdomTField
+
     pathTransform =
       Transform2d(
         Translation2d(waypoints.get()[0].translation),
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt
index eaca7e80..c31bc713 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt
@@ -65,6 +65,7 @@ class FollowPathPlannerPathCommand(
 
   private var drivePoseSupplier: () -> Pose2d
   private var odoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees)
+  private var lastStableOdoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees)
 
   private val atReference: Boolean
     get() =
@@ -174,6 +175,7 @@ class FollowPathPlannerPathCommand(
   override fun initialize() {
     trajStartTime = Clock.fpgaTime
     odoTField = drivetrain.odomTField
+    lastStableOdoTField = drivetrain.lastStableOdomTField
 
     currentSpeeds = drivetrain.targetedChassisSpeeds
     poseRotation = drivePoseSupplier().rotation.inRotation2ds
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
index f08c8c25..9c153497 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
@@ -181,15 +181,15 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
 
   val odomTRobot: Pose2d
     get() = Pose2d(swerveDriveOdometry.poseMeters)
-
-  // NOTE(parth): This should be expected to be noisy. Avoid using this directly if possible.
+  // NOTE (parth): This should be expected to be noisy. Avoid using this directly if possible.
   val fieldTRobot: Pose2d
     get() =
       (fieldFrameEstimator.getLatestOdometryTField().inverse() + odomTRobot.asTransform2d())
         .asPose2d()
-
   val odomTField: Transform2d
     get() = fieldFrameEstimator.getLatestOdometryTField()
+  val lastStableOdomTField: Transform2d
+    get() = fieldFrameEstimator.getLatestOdometryTField()
 
   private var undriftedPose: Pose2d
     get() = Pose2d(undriftedSwerveDriveOdometry.poseMeters)