diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 00000000..13566b81
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
diff --git a/.idea/discord.xml b/.idea/discord.xml
new file mode 100644
index 00000000..d8e95616
--- /dev/null
+++ b/.idea/discord.xml
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="DiscordProjectSettings">
+    <option name="show" value="PROJECT_FILES" />
+    <option name="description" value="" />
+  </component>
+</project>
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
new file mode 100644
index 00000000..dade4032
--- /dev/null
+++ b/.idea/modules.xml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/robomaster.iml" filepath="$PROJECT_DIR$/.idea/robomaster.iml" />
+    </modules>
+  </component>
+</project>
\ No newline at end of file
diff --git a/.idea/robomaster.iml b/.idea/robomaster.iml
new file mode 100644
index 00000000..d6ebd480
--- /dev/null
+++ b/.idea/robomaster.iml
@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="JAVA_MODULE" version="4">
+  <component name="NewModuleRootManager" inherit-compiler-output="true">
+    <exclude-output />
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 00000000..35eb1ddf
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="" vcs="Git" />
+  </component>
+</project>
\ No newline at end of file
diff --git a/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp b/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp
index c08528fd..5a3f622f 100644
--- a/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp
+++ b/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp
@@ -1,5 +1,12 @@
 #include "subsystems/odometry/observer_displacement.hpp"
 
+#include "tap/architecture/clock.hpp"
+
+/*
+ * Notes:
+ * https://gitlab.com/aruw/controls/aruw-mcb/-/tree/develop/aruw-mcb-project/src/aruwsrc/algorithms/odometry?ref_type=heads
+ * */
+
 namespace subsystems::odometry
 {
 
@@ -20,7 +27,23 @@ bool ChassisDisplacementObserver::getVelocityChassisDisplacement(
     modm::Vector3f* const velocity,
     modm::Vector3f* const displacement) const
 {
-    bmi088::Bmi088* imu = &drivers->bmi088;
+    uint32_t currTime = tap::arch::clock::getTimeMicroseconds();
+    if (prevTime != 0)
+    {
+        modm::Vector3f myv = chassis->measureVelocity();
+        velocity->set(myv.x, myv.y, myv.z);
+
+        //  velocity.set(chassisRelVel.x, chassisRelVel.y);
+
+        // displacement->move(tickDisp);
+        float deltaT = (static_cast<float>(currTime - prevTime)) / 1'000'000.0f;
+        modm::Vector3f myDisp = myv * deltaT;
+        displacement->set(myDisp.x, myDisp.y, myDisp.z);
+        // displacement += velocity * (static_cast<float>(currTime - prevTime) / 1'000'000.0f);
+    }
+
+    prevTime = currTime;
+    // bmi088::Bmi088* imu = &drivers->bmi088;
 
     // Attempt integration with Velocity Verlet
     // a(t) = last acceleration, a(t + dt) = current acceleration
@@ -29,25 +52,26 @@ bool ChassisDisplacementObserver::getVelocityChassisDisplacement(
 
     // TODO: Depending on when this subsystem gets initialized,
     //   the first time this function runs, deltaT might be large
-    auto nowTime = imu->getPrevIMUDataReceivedTime();  // Units of us
-    auto dt = (nowTime - lastTime) / 1e6f;             // Want units of s
+    // auto nowTime = imu->getPrevIMUDataReceivedTime();  // Units of us
+    // auto dt = (nowTime - lastTime) / 1e6f;             // Want units of s
 
-    // z is 0 since we're moving on the x-y plane and gravity affects z
-    Vector3f nowAcc{imu->getAx(), imu->getAy(), 0.0f};
-    Vector3f nowDisp = lastDisp + lastVel * dt + lastAcc * dt * dt / 2.0f;
-    Vector3f nowVel = lastVel + (lastAcc + nowAcc) * dt / 2.0f;
+    // // z is 0 since we're moving on the x-y plane and gravity affects z
+    // Vector3f nowAcc{imu->getAx(), imu->getAy(), 0.0f};
+    // Vector3f nowDisp = lastDisp + lastVel * dt + lastAcc * dt * dt / 2.0f;
+    // Vector3f nowVel = lastVel + (lastAcc + nowAcc) * dt / 2.0f;
 
-    // Update by copy
-    lastTime = nowTime;
-    lastAcc = nowAcc;
-    lastVel = nowVel;
-    lastDisp = nowDisp;
+    // // Update by copy
+    // lastTime = nowTime;
+    // lastAcc = nowAcc;
+    // lastVel = nowVel;
+    // lastDisp = nowDisp;
 
-    // Return
-    *velocity = nowVel;
-    *displacement = nowDisp;
+    // // Return
+    // *velocity = nowVel;
+    // *displacement = nowDisp;
 
-    return imu->getImuState() == ImuInterface::ImuState::IMU_CALIBRATED;
+    // return imu->getImuState() == ImuInterface::ImuState::IMU_CALIBRATED;
+    return true;
 }
 
 }  // namespace subsystems::odometry
\ No newline at end of file
diff --git a/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp b/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp
index 42626172..a6f1b88d 100644
--- a/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp
+++ b/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp
@@ -28,6 +28,7 @@ class ChassisDisplacementObserver : public ChassisDisplacementObserverInterface
     mutable Vector3f lastVel;   // m/s
     mutable Vector3f lastDisp;  // m
     mutable uint32_t lastTime;  // ms
+    mutable uint32_t prevTime = 0;
 };
 }  // namespace subsystems::odometry