diff --git a/firmware/hexray/VC/CMakeLists.txt b/firmware/hexray/VC/CMakeLists.txt
index e853f5f232..95231ab917 100644
--- a/firmware/hexray/VC/CMakeLists.txt
+++ b/firmware/hexray/VC/CMakeLists.txt
@@ -18,7 +18,9 @@ set(SYSTEM_INCLUDE_DIRS
 file(GLOB_RECURSE APP_SRCS CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/src/app/*.cpp")
 list(APPEND APP_SRCS
         "${SHARED_APP_INCLUDE_DIR_CPP}/app_pid.cpp"
+        "${SHARED_APP_INCLUDE_DIR_CPP}/state_estimation/app_kalman_filter.hpp"
 )
+
 set(APP_INCLUDE_DIRS
         "${SHARED_APP_INCLUDE_DIR}" "${SHARED_APP_INCLUDE_DIR_CPP}" "${CMAKE_CURRENT_SOURCE_DIR}/src/app/"
 )
@@ -130,7 +132,14 @@ if ("${TARGET}" STREQUAL "binary")
             "${CMAKE_CURRENT_BINARY_DIR}/app"
     )
 
-    target_link_libraries("hexray_VC_app.elf" PRIVATE "hexray_VC_stm32" "hexray_VC_commit_info" "hexray_VC_jsoncan" "m" "autodiff_interface" "sbg_ecom_${ARM_CORE}")
+    target_link_libraries("hexray_VC_app.elf" PRIVATE 
+        "hexray_VC_stm32" 
+        "hexray_VC_commit_info" 
+        "hexray_VC_jsoncan" 
+        "m" 
+        "sbg_ecom_${ARM_CORE}" 
+        "eigen_interface" 
+        "autodiff_interface")
     target_compile_definitions("hexray_VC_app.elf" PRIVATE VC)
 
     add_chimera_stm32h7("hexray_VC_chimera" "${CHIMERA_SRCS}" "${CHIMERA_INCLUDE_DIRS}" "hexray_chimera_v2_proto_cm7")
@@ -166,7 +175,11 @@ elseif ("${TARGET}" STREQUAL "test")
             "${CMAKE_CURRENT_BINARY_DIR}/app"
     )
 
-    target_link_libraries("hexray_VC_test" PRIVATE "hexray_VC_commit_info" "hexray_VC_jsoncan" "autodiff_interface")
+    target_link_libraries("hexray_VC_test" PRIVATE 
+        "hexray_VC_commit_info" 
+        "hexray_VC_jsoncan"
+        "eigen_interface" 
+        "autodiff_interface")
     target_compile_definitions("hexray_VC_test" PRIVATE STM32H733xx)
 
 
@@ -177,8 +190,17 @@ elseif ("${TARGET}" STREQUAL "test")
     )
     add_library("hexray_VC_torque_vectoring" STATIC ${TV_SRCS})
     target_include_directories("hexray_VC_torque_vectoring" PUBLIC "${INCLUDE_DIRS}")
-    target_link_libraries("hexray_VC_torque_vectoring" PUBLIC "autodiff_interface" "eigen_interface" "m")
-    target_compile_options("hexray_VC_torque_vectoring" PRIVATE /MT)
+    target_link_libraries("hexray_VC_torque_vectoring" PUBLIC
+            "autodiff_interface"
+            "eigen_interface"
+            "hexray_VC_jsoncan"
+            "m")
+    target_compile_options("hexray_VC_torque_vectoring" PRIVATE
+            $<$<CXX_COMPILER_ID:MSVC>:/MT>
+            $<$<CXX_COMPILER_ID:MSVC>:/bigobj>)
+    target_compile_definitions("hexray_VC_torque_vectoring" PRIVATE
+            DISABLE_GPS_UPDATE
+            $<$<CXX_COMPILER_ID:MSVC>:NOMINMAX>)
 
 
     # MATPLOTLIB TESTING
@@ -195,4 +217,4 @@ elseif ("${TARGET}" STREQUAL "test")
     target_link_libraries(test_controls PRIVATE "matplotlib_cpp" "autodiff_interface")
     target_compile_options(test_controls PRIVATE /Zi)
     target_link_options(test_controls PRIVATE /DEBUG /INCREMENTAL:NO)
-endif ()
\ No newline at end of file
+endif ()
diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/steering_model.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/steering_model.cpp
index 391f9b3126..b521a33057 100644
--- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/steering_model.cpp
+++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/steering_model.cpp
@@ -60,4 +60,7 @@ template <Decimal T> [[nodiscard]] wheel_set<T> wheel_steer_angles(const T steer
 
     return wheel_ang_rad;
 }
+
+template wheel_set<float>  wheel_steer_angles(float steer_ang_rad);
+template wheel_set<double> wheel_steer_angles(double steer_ang_rad);
 } // namespace app::tv::estimators::steering
diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp
index f8aa56c21a..b118eb46ee 100644
--- a/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp
+++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/vehicle_state_estimator.cpp
@@ -1,3 +1,4 @@
+#if 0
 #include "vehicle_state_estimator.hpp"
 
 #include <array>
@@ -237,3 +238,4 @@ namespace VehicleStateEstimator
     }
 } // namespace VehicleStateEstimator
 } // namespace app::tv::estimation
+#endif
diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/velocity_estimator.cpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/velocity_estimator.cpp
new file mode 100644
index 0000000000..66286e18b6
--- /dev/null
+++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/velocity_estimator.cpp
@@ -0,0 +1,205 @@
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+#include <tuple>
+
+#include "app_canUtils.hpp"
+#include "app_sbgEllipse.hpp"
+
+#include "util_units.hpp"
+
+#include "velocity_estimator.hpp"
+#include "torque_vectoring/shared_datatypes/constants.hpp"
+
+using namespace app::tv::shared_datatypes::vd_constants;
+
+namespace app::tv::estimators::velocity_estimator
+{
+static constexpr float time_step  = 0.01f;
+static constexpr float SLIP_THRES = 0.0117f; // TODO: tune this
+static constexpr float VX_MIN_MPS = 0.5f;    // TODO: we should probably make a global threshold in constants.hpp
+
+// System/Process Model
+autodiff::dual velocity_state_x(const EkfStateInp<float> &x)
+{
+    const autodiff::dual &vx  = x(0);
+    const autodiff::dual &vy  = x(1);
+    const autodiff::dual &ax  = x(2);
+    const autodiff::dual &yaw = x(4);
+    return vx + time_step * (ax - vy * yaw);
+}
+
+autodiff::dual velocity_state_y(const EkfStateInp<float> &x)
+{
+    const autodiff::dual &vx  = x(0);
+    const autodiff::dual &vy  = x(1);
+    const autodiff::dual &ay  = x(3);
+    const autodiff::dual &yaw = x(4);
+    return vy + time_step * (ay + vx * yaw);
+}
+
+/**
+ * Measurement Model
+ *
+ * Note:
+ *
+ * Wheel speed measurements are converted into body speed then passed into the EKF
+ * so the measurement model is trivial
+ *
+ * GPS measurements by default are in body velocity so its measurement model is also
+ * trivial
+ */
+autodiff::dual velocity_meas_x(const EkfState<float> &x)
+{
+    return x(0);
+}
+
+autodiff::dual velocity_meas_y(const EkfState<float> &x)
+{
+    return x(1);
+}
+
+// Velocity Estimator EKF instantiation
+template <Decimal T>
+VelocityEstimator<T> velocity_estimator(
+    typename VelocityEstimator<T>::PredictStep{ { velocity_state_x, velocity_state_y } },
+    ProcessNoiseCov<T>::Identity() * static_cast<T>(0.001),
+    typename VelocityEstimator<T>::UpdateSteps{
+        WsUpdateStep<T>{
+            .h = { { velocity_meas_x, velocity_meas_y } },
+            .R = WsNoiseCov<T>::Identity() * static_cast<T>(0.01),
+        },
+        GpsUpdateStep<T>{
+            .h = { { velocity_meas_x, velocity_meas_y } },
+            .R = GpsNoiseCov<T>::Identity() * static_cast<T>(0.1),
+        },
+    });
+
+// Helper functions for converting wheel velocity into body velocity
+namespace detail
+{
+    template <Decimal T> wheel_set<T> motorRpmToWheelSpeedMps(const wheel_set<T> &rpm)
+    {
+        const T rpm_to_mps = static_cast<T>(MOTOR_RPM_TO_MPS(1.0f));
+        return {
+            .fl = rpm.fl * rpm_to_mps,
+            .fr = rpm.fr * rpm_to_mps,
+            .rl = rpm.rl * rpm_to_mps,
+            .rr = rpm.rr * rpm_to_mps,
+        };
+    }
+
+    template <Decimal T>
+    wheel_set<Pair<T>> computeWheelVelocities(
+        const wheel_set<T> &wheel_speeds_mps,
+        const wheel_set<T> &steer_angles_rad,
+        const T             yaw_rate_radps)
+    {
+        return {
+            .fl = { wheel_speeds_mps.fl * std::cos(steer_angles_rad.fl) - yaw_rate_radps * HALF_TRACK_m,
+                    yaw_rate_radps * DIST_FRONT_AXLE_CG_m - wheel_speeds_mps.fl * std::sin(steer_angles_rad.fl) },
+            .fr = { wheel_speeds_mps.fr * std::cos(steer_angles_rad.fr) + yaw_rate_radps * HALF_TRACK_m,
+                    yaw_rate_radps * DIST_FRONT_AXLE_CG_m - wheel_speeds_mps.fr * std::sin(steer_angles_rad.fr) },
+            .rl = { wheel_speeds_mps.rl - yaw_rate_radps * HALF_TRACK_m, -yaw_rate_radps * DIST_REAR_AXLE_CG_m },
+            .rr = { wheel_speeds_mps.rr + yaw_rate_radps * HALF_TRACK_m, -yaw_rate_radps * DIST_REAR_AXLE_CG_m },
+        };
+    }
+
+    template <Decimal T> T computeSlipRatio(T vx_wheel, T vx_state)
+    {
+        return (vx_wheel - vx_state) / (std::max)(static_cast<T>(VX_MIN_MPS), std::abs(vx_state));
+    }
+
+    template <Decimal T>
+    std::optional<WsMeasurement<T>> averageValidWheels(const wheel_set<Pair<T>> &wheel_vels_mps, const T vx_state)
+    {
+        T        sum_vx      = T{ 0 };
+        T        sum_vy      = T{ 0 };
+        uint32_t valid_count = 0;
+
+        const auto sum_if_valid = [&](const Pair<T> &wheel_vel_mps)
+        {
+            if (std::abs(computeSlipRatio(wheel_vel_mps.x, vx_state)) >= static_cast<T>(SLIP_THRES))
+                return;
+
+            sum_vx += wheel_vel_mps.x;
+            sum_vy += wheel_vel_mps.y;
+            valid_count++;
+        };
+
+        sum_if_valid(wheel_vels_mps.fl);
+        sum_if_valid(wheel_vels_mps.fr);
+        sum_if_valid(wheel_vels_mps.rl);
+        sum_if_valid(wheel_vels_mps.rr);
+
+        if (valid_count == 0)
+            return std::nullopt;
+
+        WsMeasurement<T> z;
+        z << sum_vx / static_cast<T>(valid_count), sum_vy / static_cast<T>(valid_count);
+        return z;
+    }
+} // namespace detail
+
+// Wheel speed measurement handling
+template <Decimal T>
+static std::optional<WsMeasurement<T>> wheelSpeedToBodyVelocity(const VelocityEstimatorInputs<T> &inputs)
+{
+    const wheel_set<T> wheel_speeds_mps = detail::motorRpmToWheelSpeedMps(inputs.rpm);
+    const T speed_sum_mps = wheel_speeds_mps.fl + wheel_speeds_mps.fr + wheel_speeds_mps.rl + wheel_speeds_mps.rr;
+
+    if (speed_sum_mps <= T{ 0 })
+        return WsMeasurement<T>::Zero();
+
+    const wheel_set<Pair<T>> wheel_vels_mps =
+        detail::computeWheelVelocities(wheel_speeds_mps, inputs.steer_angles_rad, inputs.control.r_rads);
+    const T vx_state = velocity_estimator<T>.state()(0);
+
+    return detail::averageValidWheels(wheel_vels_mps, vx_state);
+}
+
+// GPS measurement handling
+template <Decimal T> static std::optional<GpsMeasurement<T>> gpsMeasurement(const VelocityEstimatorInputs<T> &inputs)
+{
+    if (app::sbgEllipse::getEkfSolutionMode() != app::can_utils::VcEkfStatus::POSITION)
+        return std::nullopt;
+
+    GpsMeasurement<T> z;
+    z << inputs.v_body_gps_mps.x, inputs.v_body_gps_mps.y;
+    return z;
+}
+
+// Estimation entry point
+template <Decimal T> Pair<T> estimate_body_velocity(const VelocityEstimatorInputs<T> &inputs)
+{
+    const auto ws_meas = wheelSpeedToBodyVelocity(inputs);
+#ifdef DISABLE_GPS_UPDATE
+    const auto gps_meas = std::nullopt;
+#else
+    const auto gps_meas = gpsMeasurement(inputs);
+#endif
+
+    typename VelocityEstimator<T>::Measurements measurements = std::make_tuple(ws_meas, gps_meas);
+
+    Input<T> u;
+    u << inputs.control.a_body_mps2.x, inputs.control.a_body_mps2.y, inputs.control.r_rads;
+
+    const auto velocity = velocity_estimator<T>.estimated_states(u, measurements);
+
+    return Pair<T>{ velocity(0), velocity(1) };
+}
+
+template Pair<float>  estimate_body_velocity(const VelocityEstimatorInputs<float> &inputs);
+template Pair<double> estimate_body_velocity(const VelocityEstimatorInputs<double> &inputs);
+
+template wheel_set<float>  detail::motorRpmToWheelSpeedMps(const wheel_set<float> &rpm);
+template wheel_set<double> detail::motorRpmToWheelSpeedMps(const wheel_set<double> &rpm);
+template wheel_set<Pair<float>>
+    detail::computeWheelVelocities(const wheel_set<float> &, const wheel_set<float> &, float);
+template wheel_set<Pair<double>>
+                detail::computeWheelVelocities(const wheel_set<double> &, const wheel_set<double> &, double);
+template float  detail::computeSlipRatio(float vx_wheel, float vx_state);
+template double detail::computeSlipRatio(double vx_wheel, double vx_state);
+template std::optional<WsMeasurement<float>>  detail::averageValidWheels(const wheel_set<Pair<float>> &, float);
+template std::optional<WsMeasurement<double>> detail::averageValidWheels(const wheel_set<Pair<double>> &, double);
+} // namespace app::tv::estimators::velocity_estimator
diff --git a/firmware/hexray/VC/src/app/torque_vectoring/estimation/velocity_estimator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/estimation/velocity_estimator.hpp
new file mode 100644
index 0000000000..4e8dd38703
--- /dev/null
+++ b/firmware/hexray/VC/src/app/torque_vectoring/estimation/velocity_estimator.hpp
@@ -0,0 +1,65 @@
+#pragma once
+
+#include <cstddef>
+#include <optional>
+
+#include "state_estimation/app_kalman_filter.hpp"
+#include "torque_vectoring/shared_datatypes/pair.hpp"
+#include "torque_vectoring/shared_datatypes/wheel_set.hpp"
+
+namespace app::tv::estimators::velocity_estimator
+{
+using app::tv::shared_datatypes::Pair;
+using app::tv::shared_datatypes::wheel_set;
+
+template <Decimal T> struct VelocityEstimatorInputs
+{
+    struct
+    {
+        Pair<T> a_body_mps2{};
+        T       r_rads = T{ 0 };
+    } control;
+    wheel_set<T> rpm{};
+    wheel_set<T> steer_angles_rad{};
+    Pair<T>      v_body_gps_mps{};
+};
+
+static constexpr std::size_t NUM_STATES           = 2;
+static constexpr std::size_t NUM_INPUTS           = 3;
+static constexpr std::size_t NUM_WS_MEASUREMENTS  = 2;
+static constexpr std::size_t NUM_GPS_MEASUREMENTS = 2;
+
+template <Decimal T>
+using VelocityEstimator =
+    app::state_estimation::ekf<T, NUM_STATES, NUM_INPUTS, NUM_WS_MEASUREMENTS, NUM_GPS_MEASUREMENTS>;
+
+template <Decimal T> using Input         = typename VelocityEstimator<T>::U_1;
+template <Decimal T> using WsUpdateStep  = typename VelocityEstimator<T>::template UpdateStep<NUM_WS_MEASUREMENTS>;
+template <Decimal T> using GpsUpdateStep = typename VelocityEstimator<T>::template UpdateStep<NUM_GPS_MEASUREMENTS>;
+
+template <Decimal T> using WsMeasurement  = Eigen::Matrix<T, NUM_WS_MEASUREMENTS, 1>;
+template <Decimal T> using GpsMeasurement = Eigen::Matrix<T, NUM_GPS_MEASUREMENTS, 1>;
+
+template <Decimal T> using EkfStateInp = typename VelocityEstimator<T>::state_inp_mtx;
+template <Decimal T> using EkfState    = typename VelocityEstimator<T>::state_mtx;
+template <Decimal T> using Velocity    = typename VelocityEstimator<T>::N_1;
+
+template <Decimal T> using ProcessNoiseCov = Eigen::Matrix<T, NUM_STATES, NUM_STATES>;
+template <Decimal T> using WsNoiseCov      = Eigen::Matrix<T, NUM_WS_MEASUREMENTS, NUM_WS_MEASUREMENTS>;
+template <Decimal T> using GpsNoiseCov     = Eigen::Matrix<T, NUM_GPS_MEASUREMENTS, NUM_GPS_MEASUREMENTS>;
+
+namespace detail
+{
+    template <Decimal T> wheel_set<T> motorRpmToWheelSpeedMps(const wheel_set<T> &rpm);
+    template <Decimal T>
+    wheel_set<Pair<T>> computeWheelVelocities(
+        const wheel_set<T> &wheel_speeds_mps,
+        const wheel_set<T> &steer_angles_rad,
+        T                   yaw_rate_radps);
+    template <Decimal T> T computeSlipRatio(T vx_wheel, T vx_state);
+    template <Decimal T>
+    std::optional<WsMeasurement<T>> averageValidWheels(const wheel_set<Pair<T>> &wheel_velocities_mps, T vx_state);
+} // namespace detail
+
+template <Decimal T> Pair<T> estimate_body_velocity(const VelocityEstimatorInputs<T> &inputs);
+} // namespace app::tv::estimators::velocity_estimator
diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp
index a9b6b2e6be..98d03c7114 100644
--- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp
+++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/constants.hpp
@@ -27,8 +27,8 @@ inline constexpr float WHEELBASE_m  = WHEELBASE_mm * MM_TO_M;
 
 inline constexpr float TRACK_WIDTH_mm = 1100.0f;
 inline constexpr float TRACK_WIDTH_m  = TRACK_WIDTH_mm * MM_TO_M;
-inline constexpr float HALF_TRACK_M   = TRACK_WIDTH_m * 0.5f;
-inline constexpr float WHEEL_RADIUS_M = WHEEL_DIAMETER_IN * IN_TO_M / 2.0f;
+inline constexpr float HALF_TRACK_m   = TRACK_WIDTH_m * 0.5f;
+inline constexpr float WHEEL_RADIUS_m = WHEEL_DIAMETER_IN * IN_TO_M / 2.0f;
 
 // =============================================================================
 // VEHICLE MASS & CENTER OF GRAVITY
diff --git a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp
index e515da0366..bbb6898784 100644
--- a/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp
+++ b/firmware/hexray/VC/src/app/torque_vectoring/shared_datatypes/vehicle_state_estimator.hpp
@@ -2,18 +2,18 @@
 #include "torque_vectoring/shared_datatypes/wheel_set.hpp"
 #include "torque_vectoring/shared_datatypes/constants.hpp"
 #include "torque_vectoring/shared_datatypes/decimal_dual.hpp"
+#include "torque_vectoring/shared_datatypes/pair.hpp"
 
 namespace app::tv::shared_datatypes
 {
 template <Decimal T> struct VehicleState
 {
     // state variables
-    T            v_x_mps        = 0.0f;
-    T            v_y_mps        = 0.0f;
-    T            yaw_rate_radps = 0.0f;
-    T            a_x_mps2       = 0.0f;
-    T            a_y_mps2       = 0.0f;
-    T            apps           = 0.0f;
+
+    Pair<T>      v_body_mps;
+    T            yaw_rate_rads = 0.0f;
+    Pair<T>      a_body_mps2;
+    T            apps = 0.0f;
     wheel_set<T> delta{};
 
     /**
@@ -23,20 +23,20 @@ template <Decimal T> struct VehicleState
     {
         wheel_set<Pair<T>> v = {
             {
-                v_x_mps - yaw_rate_radps * vd_constants::HALF_TRACK_M,
-                v_y_mps + yaw_rate_radps * vd_constants::DIST_FRONT_AXLE_CG_m,
+                v_body_mps.x - yaw_rate_rads * vd_constants::HALF_TRACK_m,
+                v_body_mps.y + yaw_rate_rads * vd_constants::DIST_FRONT_AXLE_CG_m,
             },
             {
-                v_x_mps - yaw_rate_radps * -vd_constants::HALF_TRACK_M,
-                v_y_mps + yaw_rate_radps * vd_constants::DIST_FRONT_AXLE_CG_m,
+                v_body_mps.x - yaw_rate_rads * -vd_constants::HALF_TRACK_m,
+                v_body_mps.y + yaw_rate_rads * vd_constants::DIST_FRONT_AXLE_CG_m,
             },
             {
-                v_x_mps - yaw_rate_radps * vd_constants::HALF_TRACK_M,
-                v_y_mps + yaw_rate_radps * -vd_constants::DIST_FRONT_AXLE_CG_m,
+                v_body_mps.x - yaw_rate_rads * vd_constants::HALF_TRACK_m,
+                v_body_mps.y + yaw_rate_rads * -vd_constants::DIST_FRONT_AXLE_CG_m,
             },
             {
-                v_x_mps - yaw_rate_radps * -vd_constants::HALF_TRACK_M,
-                v_y_mps + yaw_rate_radps * -vd_constants::DIST_FRONT_AXLE_CG_m,
+                v_body_mps.x - yaw_rate_rads * -vd_constants::HALF_TRACK_m,
+                v_body_mps.y + yaw_rate_rads * -vd_constants::DIST_FRONT_AXLE_CG_m,
             },
         };
         v.rotate(delta);
@@ -64,12 +64,12 @@ template <Decimal T> struct VehicleState
     [[nodiscard]] constexpr T est_dragFx_N() const
     {
         return 0.5f * vd_constants::AIR_DENSITY_KGPM3 * vd_constants::FRONTAL_AREA_M2 * vd_constants::DRAG_COEFF *
-               v_x_mps * v_x_mps;
+               v_body_mps.x * v_body_mps.x;
     }
     [[nodiscard]] constexpr T est_downforceFz_N() const
     {
         return 0.5f * vd_constants::AIR_DENSITY_KGPM3 * vd_constants::FRONTAL_AREA_M2 * vd_constants::LIFT_COEFF *
-               v_x_mps * v_x_mps;
+               v_body_mps.x * v_body_mps.x;
     }
 
     // =============================================================================
@@ -84,7 +84,7 @@ template <Decimal T> struct VehicleState
      */
     [[nodiscard]] constexpr T LONG_ACCEL_TERM_VERTICAL_FORCE() const
     {
-        return (vd_constants::CAR_MASS_AT_CG_KG * a_x_mps2 * vd_constants::DIST_HEIGHT_CG_m) /
+        return (vd_constants::CAR_MASS_AT_CG_KG * a_body_mps2.x * vd_constants::DIST_HEIGHT_CG_m) /
                vd_constants::WHEELBASE_m;
     }
     /**
@@ -94,7 +94,7 @@ template <Decimal T> struct VehicleState
      */
     [[nodiscard]] constexpr T LAT_ACCEL_TERM_VERTICAL_FORCE() const
     {
-        return (vd_constants::CAR_MASS_AT_CG_KG * a_y_mps2 * vd_constants::DIST_HEIGHT_CG_m) /
+        return (vd_constants::CAR_MASS_AT_CG_KG * a_body_mps2.y * vd_constants::DIST_HEIGHT_CG_m) /
                (2.0f * vd_constants::TRACK_WIDTH_m);
     }
 
@@ -137,7 +137,7 @@ template <Decimal T> struct VehicleState
      * Get body slip
      * @return
      */
-    [[nodiscard]] T est_beta_rad() const { return std::atan2(v_y_mps, safe_vx(v_x_mps)); }
+    [[nodiscard]] T est_beta_rad() const { return std::atan2(v_body_mps.y, safe_vx(v_body_mps.x)); }
 
     /**
      * @param tires_F_N tire forces
@@ -148,13 +148,13 @@ template <Decimal T> struct VehicleState
         // TODO aligning moment contributions to the yaw moment equation
         tires_F_N.rotate(delta);
         const F fl_moment =
-            (vd_constants::DIST_FRONT_AXLE_CG_m * tires_F_N.fl.y) - (vd_constants::HALF_TRACK_M * tires_F_N.fl.x);
+            (vd_constants::DIST_FRONT_AXLE_CG_m * tires_F_N.fl.y) - (vd_constants::HALF_TRACK_m * tires_F_N.fl.x);
         const F fr_moment =
-            (vd_constants::DIST_FRONT_AXLE_CG_m * tires_F_N.fr.y) + (vd_constants::HALF_TRACK_M * tires_F_N.fr.x);
+            (vd_constants::DIST_FRONT_AXLE_CG_m * tires_F_N.fr.y) + (vd_constants::HALF_TRACK_m * tires_F_N.fr.x);
         const F rl_moment =
-            (-vd_constants::DIST_REAR_AXLE_CG_m * tires_F_N.rl.y) - (vd_constants::HALF_TRACK_M * tires_F_N.rl.x);
+            (-vd_constants::DIST_REAR_AXLE_CG_m * tires_F_N.rl.y) - (vd_constants::HALF_TRACK_m * tires_F_N.rl.x);
         const F rr_moment =
-            (-vd_constants::DIST_REAR_AXLE_CG_m * tires_F_N.rr.y) + (vd_constants::HALF_TRACK_M * tires_F_N.rr.x);
+            (-vd_constants::DIST_REAR_AXLE_CG_m * tires_F_N.rr.y) + (vd_constants::HALF_TRACK_m * tires_F_N.rr.x);
         return fl_moment + fr_moment + rl_moment + rr_moment;
     }
 
@@ -165,7 +165,8 @@ template <Decimal T> struct VehicleState
      */
     [[nodiscard]] constexpr T ACCELERATION_TERM_KMZ() const
     {
-        return vd_constants::DIST_FRONT_AXLE_CG_m + (a_x_mps2 * vd_constants::DIST_HEIGHT_CG_m) / vd_constants::GRAVITY;
+        return vd_constants::DIST_FRONT_AXLE_CG_m +
+               (a_body_mps2.x * vd_constants::DIST_HEIGHT_CG_m) / vd_constants::GRAVITY;
     }
     [[nodiscard]] constexpr T KMZ() const
     {
diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp
index f804b4deb1..f24467c194 100644
--- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp
+++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.cpp
@@ -4,18 +4,69 @@
 #include "shared_datatypes/vehicle_state_estimator.hpp"
 #include "torque_vectoring/controllers/controllers_dyrc.hpp"
 #include "torque_vectoring/controllers/torque_allocator.hpp"
+#include "torque_vectoring/estimation/steering_model.hpp"
 #include "torque_vectoring/shared_datatypes/constants.hpp"
+#include "torque_vectoring/estimation/velocity_estimator.hpp"
+// #include "torque_vectoring/estimation/vehicle_state_estimator.hpp"
 
 using namespace app::tv::shared_datatypes;
+using namespace app::tv::estimators;
 using namespace vd_constants;
 
+// TODO: should we input the actual sensor measurements or should we just grab them all in the function?
+template <Decimal T>
+VehicleState<T> estimate(
+    const T            apps,
+    const T            steer_ang_rad,
+    const T            a_x_mps2,
+    const T            a_y_mps2,
+    const T            yaw_rate_rads,
+    const wheel_set<T> rpm)
+{
+    VehicleState<T> state{};
+
+    state.apps = apps;
+
+    // TODO: replace these with actual sensor measurements
+    state.a_body_mps2.x = a_x_mps2;
+    state.a_body_mps2.y = a_y_mps2;
+    state.yaw_rate_rads = yaw_rate_rads;
+
+    // TODO: Replace with steering model once merged in
+    state.delta = steering::wheel_steer_angles(steer_ang_rad);
+
+    velocity_estimator::VelocityEstimatorInputs<T> inputs{};
+    inputs.control.a_body_mps2 = state.a_body_mps2;
+    inputs.control.r_rads      = state.yaw_rate_rads;
+    inputs.rpm                 = rpm;
+    inputs.steer_angles_rad    = state.delta;
+    inputs.v_body_gps_mps      = Pair<T>{};
+    state.v_body_mps           = velocity_estimator::estimate_body_velocity(inputs);
+
+    return state;
+}
+template VehicleState<float> estimate(
+    float            apps,
+    float            steer_ang_rad,
+    float            a_x_mps2,
+    float            a_y_mps2,
+    float            yaw_rate_rads,
+    wheel_set<float> rpm);
+template VehicleState<double> estimate(
+    double            apps,
+    double            steer_ang_rad,
+    double            a_x_mps2,
+    double            a_y_mps2,
+    double            yaw_rate_rads,
+    wheel_set<double> rpm);
+
 template <Decimal T> ControlOutput<T> update(const VehicleState<T> &state)
 {
     //------------------------------------- HIGH LEVEL CONTROLLER ----------------------------//
     const T ax_mps2_setpoint = MAX_AX_MPS2 * state.apps;
     // Direct yaw rate control: corrective yaw moment
     const T omegadot_radps2_setpoint = app::tv::controllers::dyrc::computeYawMoment(
-        state.yaw_rate_radps, (state.delta.fl + state.delta.fr) / 2, state.v_x_mps);
+        state.yaw_rate_rads, (state.delta.fl + state.delta.fr) / 2, state.v_body_mps.x);
 
     //------------------------------------- LOW LEVEL CONTROLLER -----------------------------//
 
@@ -50,18 +101,16 @@ extern "C" void update_matlab(
     double       torque_max[4],
     double       torque_min[4])
 {
-    const VehicleState state = { .v_x_mps        = v_x,
-                                 .v_y_mps        = v_y,
-                                 .yaw_rate_radps = yaw_rate,
-                                 .a_x_mps2       = a_x,
-                                 .a_y_mps2       = a_y,
-                                 .apps           = apps,
-                                 .delta          = {
-                                              .fl = delta_fl,
-                                              .fr = delta_fr,
-                                              .rl = 0.0f,
-                                              .rr = 0.0f,
-                                 } };
+    const VehicleState<double> state = { .v_body_mps    = { .x = v_x, .y = v_y },
+                                         .yaw_rate_rads = yaw_rate,
+                                         .a_body_mps2   = { .x = a_x, .y = a_y },
+                                         .apps          = apps,
+                                         .delta         = {
+                                                     .fl = delta_fl,
+                                                     .fr = delta_fr,
+                                                     .rl = 0.0,
+                                                     .rr = 0.0,
+                                         } };
     // bring it in
     const auto [k_kappas, k_torque_max, k_torque_min] = update(state);
     // std::cout << "DIH" << std::endl;
@@ -80,6 +129,27 @@ extern "C" void update_matlab(
     torque_min[3] = k_torque_min.rr;
 }
 
+extern "C" void estimate_matlab(
+    const double apps,
+    const double steering_angle,
+    const double a_x,
+    const double a_y,
+    const double yaw_rate,
+    double       rpm[4],
+    double       v_body_mps[2],
+    double       delta[4])
+{
+    const VehicleState<double> state =
+        estimate(apps, steering_angle, a_x, a_y, yaw_rate, wheel_set<double>{ rpm[0], rpm[1], rpm[2], rpm[3] });
+
+    v_body_mps[0] = state.v_body_mps.x;
+    v_body_mps[1] = state.v_body_mps.y;
+    delta[0]      = state.delta.fl;
+    delta[1]      = state.delta.fr;
+    delta[2]      = state.delta.rl;
+    delta[3]      = state.delta.rr;
+}
+
 template <Decimal T> ControlOutputAutonomous<T> update_autonomous(const VehicleState<T> &state)
 {
     (void)state;
diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp
index 5b1734fab6..33534a2d07 100644
--- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp
+++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring.hpp
@@ -1,4 +1,6 @@
 #pragma once
+#include <algorithm>
+
 #include "shared_datatypes/constants.hpp"
 #include "shared_datatypes/vehicle_state_estimator.hpp"
 #include "shared_datatypes/wheel_set.hpp"
@@ -19,6 +21,20 @@ template <Decimal T> struct ControlOutputAutonomous
     const T                                 delta = 0;
 };
 
+/**
+ * @brief Generate vehicle state to provide to the control algorithm
+ * @param apps, steer_ang_rad: pedal percent [0, 1] and steering wheel angle in radians
+ * @return
+ */
+template <Decimal T>
+app::tv::shared_datatypes::VehicleState<T> estimate(
+    const T                                       apps,
+    const T                                       steer_ang_rad,
+    const T                                       a_x_mps2,
+    const T                                       a_y_mps2,
+    const T                                       yaw_rate_rads,
+    const app::tv::shared_datatypes::wheel_set<T> rpm);
+
 /**
  * This is the main entrypoint into the low level vehicle controls algorithm
  * @param state The current measured vehicle state, note that intent is in here as well
@@ -48,16 +64,16 @@ app::tv::shared_datatypes::wheel_set<T>
     // std::cout << "Vx: " << v_x_mps << std::endl;
     // std::cout << "Kappas: " << kappas.fl << kappas.fr << kappas.rl << kappas.rr << std::endl;
 
-    T v_x_mps_capped = std::max(v_x_mps, static_cast<T>(1));
+    T v_x_mps_capped = (std::max)(v_x_mps, static_cast<T>(1));
 
     return {
         .fl = GEAR_RATIO * (static_cast<T>(1) + kappas.fl) *
-              (v_x_mps_capped / static_cast<T>(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)),
+              (v_x_mps_capped / static_cast<T>(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_m)),
         .fr = GEAR_RATIO * (static_cast<T>(1) + kappas.fr) *
-              (v_x_mps_capped / static_cast<T>(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)),
+              (v_x_mps_capped / static_cast<T>(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_m)),
         .rl = GEAR_RATIO * (static_cast<T>(1) + kappas.rl) *
-              (v_x_mps_capped / static_cast<T>(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)),
+              (v_x_mps_capped / static_cast<T>(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_m)),
         .rr = GEAR_RATIO * (static_cast<T>(1) + kappas.rr) *
-              (v_x_mps_capped / static_cast<T>(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_M)),
+              (v_x_mps_capped / static_cast<T>(app::tv::shared_datatypes::vd_constants::WHEEL_RADIUS_m)),
     };
 }
diff --git a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h
index 000b31942f..03de95011c 100644
--- a/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h
+++ b/firmware/hexray/VC/src/app/torque_vectoring/torque_vectoring_matlab.h
@@ -22,4 +22,17 @@ extern "C"
      * Matlab wrapper for kappa_update
      */
     void kappa_update_matlab(double kappas[4], double v_x, double oemgas[4]);
-}
\ No newline at end of file
+
+    /**
+     * Matlab wrapper for estimate
+     */
+    void estimate_matlab(
+        const double apps,
+        const double steering_angle,
+        const double a_x,
+        const double a_y,
+        const double yaw_rate,
+        double       rpm[4],
+        double       v_body_mps[2],
+        double       delta[4]);
+}
diff --git a/firmware/hexray/VC/test/test_velocityEstimator.cpp b/firmware/hexray/VC/test/test_velocityEstimator.cpp
new file mode 100644
index 0000000000..5790609206
--- /dev/null
+++ b/firmware/hexray/VC/test/test_velocityEstimator.cpp
@@ -0,0 +1,404 @@
+#include <gtest/gtest.h>
+#include "test/test_VCBase.hpp"
+
+#include "vc_fakes.hpp"
+#include "util_errorCodes.hpp"
+#include "util_units.hpp"
+
+#include "app_sbgEllipse.hpp"
+#include "app_canUtils.hpp"
+#include "torque_vectoring/estimators/velocity_estimator.hpp"
+#include "torque_vectoring/datatypes/datatypes_vd_constants.hpp"
+
+using namespace app::tv::estimators::velocity_estimator;
+using namespace app::tv::estimators::velocity_estimator::detail;
+using namespace app::tv::datatypes::vd_constants;
+
+static constexpr float kSlipThres = 0.0117f;
+static constexpr float kVxMin     = 0.5f;
+static constexpr float kTol       = 1e-5f;
+
+namespace app::tv::estimators::velocity_estimator
+{
+extern ProcessNoiseCov                Q;
+extern VelocityEstimator::PredictStep system_model;
+extern VelocityEstimator::UpdateSteps update_steps;
+extern VelocityEstimator              velocity_estimator;
+} // namespace app::tv::estimators::velocity_estimator
+
+namespace io::sbgEllipse
+{
+void setEkfSolutionMode(uint32_t ekf_sol_mode_);
+} // namespace io::sbgEllipse
+
+namespace
+{
+VelocityEstimator makeEstimator(float vx0 = 0.0f, float vy0 = 0.0f)
+{
+    Velocity x0 = Velocity::Zero();
+    x0 << vx0, vy0;
+    return VelocityEstimator(system_model, Q, update_steps, x0, ProcessNoiseCov::Zero());
+}
+
+void resetEstimator(float vx0 = 0.0f, float vy0 = 0.0f)
+{
+    velocity_estimator = makeEstimator(vx0, vy0);
+}
+
+void setGpsMode(const app::can_utils::VcEkfStatus mode)
+{
+    io::sbgEllipse::setEkfSolutionMode(static_cast<uint32_t>(mode));
+    app::sbgEllipse::broadcast();
+}
+
+struct VehicleBodyMotion
+{
+    float vx_mps     = 0.0f;
+    float vy_mps     = 0.0f;
+    float yaw_rads   = 0.0f;
+    float ax_mps2    = 0.0f;
+    float ay_mps2    = 0.0f;
+    float ang_fr_rad = 0.0f;
+    float ang_fl_rad = 0.0f;
+    float rr_ws_mps  = 0.0f;
+    float rl_ws_mps  = 0.0f;
+    float fr_ws_mps  = 0.0f;
+    float fl_ws_mps  = 0.0f;
+};
+
+VehicleBodyMotion makeSteadyStateBodyMotion(float vx_mps, float vy_mps, float yaw_rads)
+{
+    VehicleBodyMotion motion{};
+    motion.vx_mps   = vx_mps;
+    motion.vy_mps   = vy_mps;
+    motion.yaw_rads = yaw_rads;
+
+    // Keep the body-frame velocity constant under the estimator process model:
+    // vx_dot = ax - vy * r, vy_dot = ay + vx * r.
+    motion.ax_mps2 = vy_mps * yaw_rads;
+    motion.ay_mps2 = -vx_mps * yaw_rads;
+
+    const float rr_vx_body    = vx_mps - yaw_rads * HALF_TRACK_WIDTH_m;
+    const float rl_vx_body    = vx_mps + yaw_rads * HALF_TRACK_WIDTH_m;
+    const float fr_vx_body    = vx_mps - yaw_rads * HALF_TRACK_WIDTH_m;
+    const float fl_vx_body    = vx_mps + yaw_rads * HALF_TRACK_WIDTH_m;
+    const float front_vy_term = yaw_rads * DIST_FRONT_AXLE_CG_m - vy_mps;
+
+    motion.rr_ws_mps  = rr_vx_body;
+    motion.rl_ws_mps  = rl_vx_body;
+    motion.ang_fr_rad = std::atan2(front_vy_term, fr_vx_body);
+    motion.ang_fl_rad = std::atan2(front_vy_term, fl_vx_body);
+    motion.fr_ws_mps  = std::hypot(fr_vx_body, front_vy_term);
+    motion.fl_ws_mps  = std::hypot(fl_vx_body, front_vy_term);
+
+    return motion;
+}
+
+VelocityEstimatorInputs makeEstimatorInputs(const VehicleBodyMotion &motion)
+{
+    VelocityEstimatorInputs inputs{};
+    inputs.control.ax_mps2   = motion.ax_mps2;
+    inputs.control.ay_mps2   = motion.ay_mps2;
+    inputs.control.r_rads    = motion.yaw_rads;
+    inputs.wheels.rr_rpm     = static_cast<float>(MOTOR_KMH_TO_RPM(MPS_TO_KMH(motion.rr_ws_mps)));
+    inputs.wheels.rl_rpm     = static_cast<float>(MOTOR_KMH_TO_RPM(MPS_TO_KMH(motion.rl_ws_mps)));
+    inputs.wheels.fr_rpm     = static_cast<float>(MOTOR_KMH_TO_RPM(MPS_TO_KMH(motion.fr_ws_mps)));
+    inputs.wheels.fl_rpm     = static_cast<float>(MOTOR_KMH_TO_RPM(MPS_TO_KMH(motion.fl_ws_mps)));
+    inputs.wheels.ang_fr_rad = motion.ang_fr_rad;
+    inputs.wheels.ang_fl_rad = motion.ang_fl_rad;
+    inputs.gps.body_vx_mps   = motion.vx_mps;
+    inputs.gps.body_vy_mps   = motion.vy_mps;
+    return inputs;
+}
+
+Velocity
+    runEstimatorLoop(const VelocityEstimatorInputs &inputs, const app::can_utils::VcEkfStatus gps_mode, uint32_t steps)
+{
+    setGpsMode(gps_mode);
+
+    Velocity estimate = Velocity::Zero();
+    for (uint32_t i = 0; i < steps; i++)
+    {
+        estimate = estimate_body_velocity(inputs);
+    }
+
+    return estimate;
+}
+
+} // namespace
+
+class VCVelocityEstimatorTest : public VCBaseTest
+{
+};
+
+// ==================== computeSlipRatio ====================
+
+TEST(VCVelocityEstimatorTest, ZeroSlipWhenWheelMatchesState)
+{
+    EXPECT_NEAR(computeSlipRatio(2.0f, 2.0f), 0.0f, kTol);
+}
+
+TEST(VCVelocityEstimatorTest, PositiveSlipWhenWheelFaster)
+{
+    const float slip = computeSlipRatio(3.0f, 2.0f);
+    EXPECT_GT(slip, 0.0f);
+    EXPECT_NEAR(slip, 1.0f / 2.0f, kTol);
+}
+
+TEST(VCVelocityEstimatorTest, NegativeSlipWhenWheelSlower)
+{
+    const float slip = computeSlipRatio(1.0f, 2.0f);
+    EXPECT_LT(slip, 0.0f);
+}
+
+TEST(VCVelocityEstimatorTest, ClampsDenominatorToVxMin)
+{
+    // vx_state = 0 should clamp denominator to VX_MIN_MPS
+    const float slip = computeSlipRatio(1.0f, 0.0f);
+    EXPECT_NEAR(slip, 1.0f / kVxMin, kTol);
+}
+
+TEST(VCVelocityEstimatorTest, ClampsDenominatorWhenStateBelowMin)
+{
+    // vx_state = 0.1 < VX_MIN_MPS, denominator should be VX_MIN_MPS
+    const float slip = computeSlipRatio(1.0f, 0.1f);
+    EXPECT_NEAR(slip, (1.0f - 0.1f) / kVxMin, kTol);
+}
+
+// ==================== computeWheelVx ====================
+
+TEST(VCVelocityEstimatorTest, StraightLineNoYaw)
+{
+    // With zero yaw, vx_wheel = ws_mps * cos(delta)
+    Eigen::Matrix<float, 4, 1> ws;
+    ws << 5.0f, 5.0f, 5.0f, 5.0f;
+
+    const float cos_fr = 1.0f; // delta = 0
+    const float cos_fl = 1.0f;
+    const float yaw    = 0.0f;
+
+    const auto vx = computeWheelVx(ws, cos_fr, cos_fl, yaw);
+
+    // All wheels should report same speed with zero steering and yaw
+    EXPECT_NEAR(vx[0], 5.0f, kTol); // rr
+    EXPECT_NEAR(vx[1], 5.0f, kTol); // rl
+    EXPECT_NEAR(vx[2], 5.0f, kTol); // fr
+    EXPECT_NEAR(vx[3], 5.0f, kTol); // fl
+}
+
+TEST(VCVelocityEstimatorTest, YawCouplingAddsCorrectly)
+{
+    Eigen::Matrix<float, 4, 1> ws;
+    ws << 5.0f, 5.0f, 5.0f, 5.0f;
+
+    const float yaw = 1.0f; // rad/s
+
+    const auto vx = computeWheelVx(ws, 1.0f, 1.0f, yaw);
+
+    // rr: ws + yaw * half_track, rl: ws - yaw * half_track
+    EXPECT_NEAR(vx[0], 5.0f + yaw * HALF_TRACK_WIDTH_m, kTol);
+    EXPECT_NEAR(vx[1], 5.0f - yaw * HALF_TRACK_WIDTH_m, kTol);
+    EXPECT_NEAR(vx[2], 5.0f + yaw * HALF_TRACK_WIDTH_m, kTol); // cos = 1
+    EXPECT_NEAR(vx[3], 5.0f - yaw * HALF_TRACK_WIDTH_m, kTol); // cos = 1
+}
+
+TEST(VCVelocityEstimatorTest, SteeringAngleScalesFrontWheels)
+{
+    Eigen::Matrix<float, 4, 1> ws;
+    ws << 5.0f, 5.0f, 5.0f, 5.0f;
+
+    const float delta  = 0.2f; // rad
+    const float cos_fr = std::cosf(delta);
+    const float cos_fl = std::cosf(delta);
+
+    const auto vx = computeWheelVx(ws, cos_fr, cos_fl, 0.0f);
+
+    // Rear wheels unaffected by steering
+    EXPECT_NEAR(vx[0], 5.0f, kTol);
+    EXPECT_NEAR(vx[1], 5.0f, kTol);
+    // Front wheels scaled by cos(delta)
+    EXPECT_NEAR(vx[2], 5.0f * cos_fr, kTol);
+    EXPECT_NEAR(vx[3], 5.0f * cos_fl, kTol);
+}
+
+// ==================== computeWheelVy ====================
+
+TEST(VCVelocityEstimatorTest, ZeroYawZeroVy)
+{
+    Eigen::Matrix<float, 4, 1> ws;
+    ws << 5.0f, 5.0f, 5.0f, 5.0f;
+
+    const auto vy = computeWheelVy(ws, 0.0f, 0.0f, 0.0f);
+
+    for (int i = 0; i < 4; i++)
+        EXPECT_NEAR(vy[i], 0.0f, kTol);
+}
+
+TEST(VCVelocityEstimatorTest, YawCouplingCorrect)
+{
+    Eigen::Matrix<float, 4, 1> ws;
+    ws << 5.0f, 5.0f, 5.0f, 5.0f;
+
+    const float yaw = 1.0f;
+    const auto  vy  = computeWheelVy(ws, 0.0f, 0.0f, yaw); // sin_delta = 0
+
+    EXPECT_NEAR(vy[0], -yaw * DIST_REAR_AXLE_CG_m, kTol); // rr
+    EXPECT_NEAR(vy[1], -yaw * DIST_REAR_AXLE_CG_m, kTol); // rl
+    EXPECT_NEAR(vy[2], yaw * DIST_FRONT_AXLE_CG_m, kTol); // fr (sin=0 so no ws contribution)
+    EXPECT_NEAR(vy[3], yaw * DIST_FRONT_AXLE_CG_m, kTol); // fl
+}
+
+TEST(VCVelocityEstimatorTest, SteeringAngleSubtractsFrontWheelVy)
+{
+    Eigen::Matrix<float, 4, 1> ws;
+    ws << 0.0f, 0.0f, 5.0f, 5.0f;
+
+    const float delta  = 0.2f;
+    const float sin_fr = std::sinf(delta);
+    const float sin_fl = std::sinf(delta);
+
+    const auto vy = computeWheelVy(ws, sin_fr, sin_fl, 0.0f);
+
+    EXPECT_NEAR(vy[0], 0.0f, kTol);
+    EXPECT_NEAR(vy[1], 0.0f, kTol);
+    EXPECT_NEAR(vy[2], -5.0f * sin_fr, kTol);
+    EXPECT_NEAR(vy[3], -5.0f * sin_fl, kTol);
+}
+
+// ==================== averageValidWheels ====================
+
+TEST(VCVelocityEstimatorTest, AllValidWheelsAveraged)
+{
+    Eigen::Matrix<float, 4, 1> vx_wheels;
+    vx_wheels << 2.0f, 2.0f, 2.0f, 2.0f;
+
+    Eigen::Matrix<float, 4, 1> vy_wheels;
+    vy_wheels << 0.1f, 0.1f, 0.1f, 0.1f;
+
+    const auto result = averageValidWheels(vx_wheels, vy_wheels, 2.0f);
+
+    ASSERT_TRUE(result.has_value());
+    EXPECT_NEAR(result->operator()(0), 2.0f, kTol);
+    EXPECT_NEAR(result->operator()(1), 0.1f, kTol);
+}
+
+TEST(VCVelocityEstimatorTest, ReturnsNulloptWhenAllWheelsSlipping)
+{
+    Eigen::Matrix<float, 4, 1> vx_wheels;
+    vx_wheels << 10.0f, 10.0f, 10.0f, 10.0f; // all far from vx_state
+
+    Eigen::Matrix<float, 4, 1> vy_wheels;
+    vy_wheels << 0.0f, 0.0f, 0.0f, 0.0f;
+
+    const auto result = averageValidWheels(vx_wheels, vy_wheels, 2.0f);
+
+    EXPECT_FALSE(result.has_value());
+}
+
+TEST(VCVelocityEstimatorTest, ExcludesSlippingWheelFromAverage)
+{
+    // Three good wheels at 2.0, one slipping at 10.0
+    Eigen::Matrix<float, 4, 1> vx_wheels;
+    vx_wheels << 2.0f, 2.0f, 2.0f, 10.0f;
+
+    Eigen::Matrix<float, 4, 1> vy_wheels;
+    vy_wheels << 0.0f, 0.0f, 0.0f, 0.0f;
+
+    const auto result = averageValidWheels(vx_wheels, vy_wheels, 2.0f);
+
+    ASSERT_TRUE(result.has_value());
+    // Should average only the three valid wheels
+    EXPECT_NEAR(result->operator()(0), 2.0f, kTol);
+}
+
+TEST(VCVelocityEstimatorTest, ZeroVxStateUsesVxMinAsDenominator)
+{
+    // With vx_state = 0, denominator clamps to VX_MIN_MPS
+    // wheels at 0 + small offset: slip = offset / VX_MIN_MPS
+    // to stay under threshold: offset < SLIP_THRES * VX_MIN_MPS
+    const float safe_offset = kSlipThres * kVxMin * 0.5f;
+
+    Eigen::Matrix<float, 4, 1> vx_wheels;
+    vx_wheels << safe_offset, safe_offset, safe_offset, safe_offset;
+
+    Eigen::Matrix<float, 4, 1> vy_wheels;
+    vy_wheels << 0.0f, 0.0f, 0.0f, 0.0f;
+
+    const auto result = averageValidWheels(vx_wheels, vy_wheels, 0.0f);
+
+    ASSERT_TRUE(result.has_value());
+}
+
+TEST(VCVelocityEstimatorTest, MixedValidInvalidWheelsAveragesCorrectly)
+{
+    // Two valid at 3.0, two slipping at 10.0
+    Eigen::Matrix<float, 4, 1> vx_wheels;
+    vx_wheels << 3.0f, 3.0f, 10.0f, 10.0f;
+
+    Eigen::Matrix<float, 4, 1> vy_wheels;
+    vy_wheels << 1.0f, 1.0f, 5.0f, 5.0f;
+
+    const auto result = averageValidWheels(vx_wheels, vy_wheels, 3.0f);
+
+    ASSERT_TRUE(result.has_value());
+    EXPECT_NEAR(result->operator()(0), 3.0f, kTol);
+    EXPECT_NEAR(result->operator()(1), 1.0f, kTol);
+}
+
+TEST(VCVelocityEstimatorTest, ConvergesToStraightLineBodyVelocityFromWheelSpeeds)
+{
+    const auto motion = makeSteadyStateBodyMotion(8.0f, 0.0f, 0.0f);
+    const auto inputs = makeEstimatorInputs(motion);
+    resetEstimator(motion.vx_mps + 0.05f, 0.02f);
+
+    const auto estimate = runEstimatorLoop(inputs, app::can_utils::VcEkfStatus::UNINITIALIZED, 150);
+
+    EXPECT_NEAR(estimate(0), motion.vx_mps, 0.02f);
+    EXPECT_NEAR(estimate(1), motion.vy_mps, 0.02f);
+}
+
+TEST(VCVelocityEstimatorTest, ConvergesToSteadyStateCorneringMotionFromWheelAndGps)
+{
+    resetEstimator();
+
+    const auto motion = makeSteadyStateBodyMotion(10.0f, -0.35f, 0.55f);
+    const auto inputs = makeEstimatorInputs(motion);
+
+    ASSERT_GT(motion.ang_fr_rad, 0.0f);
+    ASSERT_GT(motion.ang_fl_rad, 0.0f);
+
+    const auto estimate = runEstimatorLoop(inputs, app::can_utils::VcEkfStatus::POSITION, 200);
+
+    EXPECT_NEAR(estimate(0), motion.vx_mps, 0.02f);
+    EXPECT_NEAR(estimate(1), motion.vy_mps, 0.02f);
+}
+
+TEST(VCVelocityEstimatorTest, GpsMeasurementIsIgnoredOutsidePositionMode)
+{
+    const auto motion = makeSteadyStateBodyMotion(6.0f, -0.25f, 0.45f);
+    auto       inputs = makeEstimatorInputs(motion);
+    resetEstimator(motion.vx_mps + 0.04f, motion.vy_mps - 0.03f);
+    inputs.gps.body_vx_mps = 20.0f;
+    inputs.gps.body_vy_mps = 3.0f;
+
+    const auto estimate = runEstimatorLoop(inputs, app::can_utils::VcEkfStatus::UNINITIALIZED, 100);
+
+    EXPECT_NEAR(estimate(0), motion.vx_mps, 0.02f);
+    EXPECT_NEAR(estimate(1), motion.vy_mps, 0.04f);
+}
+
+TEST(VCVelocityEstimatorTest, RejectsSingleSlippingWheelAndStillConverges)
+{
+    resetEstimator();
+
+    const auto motion = makeSteadyStateBodyMotion(9.0f, -0.2f, 0.4f);
+    auto       inputs = makeEstimatorInputs(motion);
+
+    // Inject a large front-left slip so that wheel should be excluded.
+    inputs.wheels.fl_rpm = static_cast<float>(MOTOR_KMH_TO_RPM(MPS_TO_KMH(motion.fl_ws_mps * 1.3f)));
+
+    const auto estimate = runEstimatorLoop(inputs, app::can_utils::VcEkfStatus::POSITION, 300);
+
+    EXPECT_NEAR(estimate(0), motion.vx_mps, 0.05f);
+    EXPECT_NEAR(estimate(1), motion.vy_mps, 0.06f);
+}
diff --git a/firmware/hexray/VC/test/vc_fakes.cpp b/firmware/hexray/VC/test/vc_fakes.cpp
index b6b11f4d45..427f66bdf2 100644
--- a/firmware/hexray/VC/test/vc_fakes.cpp
+++ b/firmware/hexray/VC/test/vc_fakes.cpp
@@ -1,5 +1,6 @@
 #include "vc_fakes.hpp"
 #include "app_canTx.hpp"
+#include "app_canUtils.hpp"
 #include "io_imus.hpp"
 #include "io_pumpControl.hpp"
 #include "io_efuses.hpp"
@@ -59,9 +60,14 @@ namespace sbgEllipse
     {
         return Attitude{ 0.0f, 0.0f, 0.0f };
     }
-    uint32_t getEkfSolutionMode()
+    static uint32_t ekf_sol_mode = 0;
+    uint32_t        getEkfSolutionMode()
     {
-        return 0;
+        return ekf_sol_mode;
+    }
+    void setEkfSolutionMode(uint32_t ekf_sol_mode_)
+    {
+        ekf_sol_mode = ekf_sol_mode_;
     }
     const VelocityData getEkfNavVelocityData()
     {
diff --git a/firmware/hexray/VC/test/vc_fakes.hpp b/firmware/hexray/VC/test/vc_fakes.hpp
index c618bd4ce7..05770018c1 100644
--- a/firmware/hexray/VC/test/vc_fakes.hpp
+++ b/firmware/hexray/VC/test/vc_fakes.hpp
@@ -1,4 +1,5 @@
 #pragma once
+
 #include "util_errorCodes.hpp"
 #include "io_imu.hpp"
 
@@ -22,7 +23,7 @@ namespace pumpController
     void pumps_enabled(bool enabled);
 } // namespace pumpController
 
-namespace sbgEllipses
+namespace sbgEllipse
 {
     struct Attitude
     {
@@ -40,11 +41,11 @@ namespace sbgEllipses
         float    east_std_dev;
         float    down_std_dev;
     };
-
     void setAttitude(float roll, float pitch, float yaw);
     void setAngularVelocity(float roll_rate, float pitch_rate, float yaw_rate);
     void setLinearAcceleration(float x_accel, float y_accel, float z_accel);
-} // namespace sbgEllipses
+    void setEkfSolutionMode(uint32_t ekf_sol_mode_);
+} // namespace sbgEllipse
 
 namespace shdnLoop
 {
diff --git a/firmware/shared/srcpp/app/state_estimation/app_kalman_filter.hpp b/firmware/shared/srcpp/app/state_estimation/app_kalman_filter.hpp
index 0b323de7d8..413c733546 100644
--- a/firmware/shared/srcpp/app/state_estimation/app_kalman_filter.hpp
+++ b/firmware/shared/srcpp/app/state_estimation/app_kalman_filter.hpp
@@ -3,6 +3,14 @@
 #include <Eigen/Dense>
 #include <dual.hpp>
 #include <gradient.hpp>
+
+#include "util_utils.hpp"
+
+constexpr Eigen::Index idx(auto i)
+{
+    return static_cast<Eigen::Index>(i);
+}
+
 namespace app::state_estimation
 {
 namespace detail
@@ -79,12 +87,14 @@ namespace detail
      * - Q symmetric and PSD
      * - R symmetric and PD (required by LLT solve in update step)
      */
+    // TODO: can we assert at compile time?
     template <typename StateCov, typename MeasurementCov>
-    void assert_covariance_constraints(const StateCov &P0, const StateCov &Q, const MeasurementCov &R)
+    void assert_covariance_constraints(const StateCov &P, const StateCov &Q, const MeasurementCov &R)
     {
-        assert(is_symmetric(P0) && "Kalman filter constructor: P0 must be symmetric.");
+#ifndef NDEBUG
+        assert(is_symmetric(P) && "Kalman filter constructor: P0 must be symmetric.");
         assert(
-            is_positive_semidefinite(P0) &&
+            is_positive_semidefinite(P) &&
             "Kalman filter constructor: P0 must be positive semidefinite (all eigenvalues must be >= 0).");
         assert(is_symmetric(Q) && "Kalman filter constructor: Q must be symmetric.");
         assert(
@@ -94,6 +104,11 @@ namespace detail
         assert(
             is_positive_definite(R) && "Kalman filter constructor: R must be positive definite because update() uses "
                                        "LLT for solving innovation covariance.");
+#else
+        UNUSED(P);
+        UNUSED(Q);
+        UNUSED(R);
+#endif
     }
 } // namespace detail
 
@@ -195,7 +210,7 @@ template <typename T, std::size_t STATES, std::size_t MEASUREMENTS, std::size_t
  * Template parameters:
  * - T: scalar type (float or double, must support autodiff dual numbers)
  * - STATES: number of states
- * - MEASUREMENTS: number of measurements
+ * - MEASUREMENT_DIMS: number of measurements per update step
  *
  * State transition function f: R^STATES -> R^STATES (nonlinear)
  * Measurement function h: R^STATES -> R^MEASUREMENTS (nonlinear)
@@ -204,16 +219,12 @@ template <typename T, std::size_t STATES, std::size_t MEASUREMENTS, std::size_t
  * - f[i]: computes x_dot[i] = f[i](x) where x is the full state vector
  * - h[i]: computes z_predicted[i] = h[i](x) where x is the full state vector
  */
-template <typename T, std::size_t STATES, std::size_t MEASUREMENTS, std::size_t INPUTS> class ekf
+template <typename T, std::size_t STATES, std::size_t INPUTS, std::size_t... MEASUREMENT_DIMS> class ekf
 {
   public:
     using N_N = Eigen::Matrix<T, STATES, STATES>;
     using N_1 = Eigen::Matrix<T, STATES, 1>;
-    using M_1 = Eigen::Matrix<T, MEASUREMENTS, 1>;
     using U_1 = Eigen::Matrix<T, INPUTS, 1>;
-    using M_M = Eigen::Matrix<T, MEASUREMENTS, MEASUREMENTS>;
-    using M_N = Eigen::Matrix<T, MEASUREMENTS, STATES>;
-    using N_M = Eigen::Matrix<T, STATES, MEASUREMENTS>;
     using N_U = Eigen::Matrix<T, STATES, INPUTS>;
 
     /*
@@ -225,36 +236,58 @@ template <typename T, std::size_t STATES, std::size_t MEASUREMENTS, std::size_t
     */
     using state_mtx           = Eigen::Matrix<autodiff::dual, STATES, 1>;
     using state_inp_mtx       = Eigen::Matrix<autodiff::dual, STATES + INPUTS, 1>;
-    using measurement_arr     = Eigen::Matrix<autodiff::dual, MEASUREMENTS, 1>;
     using StateFunction       = autodiff::dual (*)(const state_inp_mtx &);
     using MeasurementFunction = autodiff::dual (*)(const state_mtx &);
 
+    using PredictStep = std::array<StateFunction, STATES>;
+
+    template <std::size_t MEASUREMENTS> struct UpdateStep
+    {
+        static constexpr std::size_t                  dim = MEASUREMENTS;
+        std::array<MeasurementFunction, MEASUREMENTS> h; // measurement functions for this update step
+        Eigen::Matrix<T, MEASUREMENTS, MEASUREMENTS>  R; // measurement noise covariance
+    };
+
+    using UpdateSteps  = std::tuple<UpdateStep<MEASUREMENT_DIMS>...>;
+    using Measurements = std::tuple<std::optional<Eigen::Matrix<T, MEASUREMENT_DIMS, 1>>...>;
+
     /**
      * @brief Constructor for EKF
      *
      * @param f Array of STATES functions -- for every state we have one function
-     * @param h Array of MEASUREMENTS functions -- for every measurement we have one function
      * @param Q Process noise covariance (NxN) - how much we trust the model
-     * @param R Measurement noise covariance (MxM) - how much we trust measurements
+     * @param update_steps All update steps the EKF must run. Consists of:
+     *      @param h Measurement model
+     *      @param R Measurement noise covariance
+     *      @param enable_outlier_rejection enables outlier rejection
      * @param x0 Initial state estimate (Nx1)
      * @param P0 Initial state covariance estimate (NxN)
      */
-    ekf(const std::array<StateFunction, STATES>             &f,
-        const std::array<MeasurementFunction, MEASUREMENTS> &h,
-        const N_N                                           &Q,
-        const M_M                                           &R,
-        const N_1                                           &x0,
-        const N_N                                           &P0)
-      : f_(f), h_(h), Q_(Q), P_(P0), F_(N_N::Identity()), R_(R), H_(M_N::Zero()), x_(x0), y_(M_1::Zero())
+    explicit ekf(
+        const std::array<StateFunction, STATES> &f,
+        const N_N                               &Q,
+        const UpdateSteps                       &update_steps,
+        const N_1                               &x0,
+        const N_N                               &P0)
+      : f_(f), Q_(Q), P_(P0), F_(N_N::Identity()), x_(x0), update_steps_(update_steps)
     {
-        detail::assert_covariance_constraints(P0, Q, R);
+        std::apply(
+            [&](const auto &...step) { (detail::assert_covariance_constraints(P_, Q, step.R), ...); }, update_steps_);
+    }
+
+    // Same constructor but with zero initialized state and covariance
+    explicit ekf(const std::array<StateFunction, STATES> &f, const N_N &Q, const UpdateSteps &update_steps)
+      : f_(f), Q_(Q), P_(N_N::Zero()), F_(N_N::Identity()), x_(N_1::Zero()), update_steps_(update_steps)
+    {
+        std::apply(
+            [&](const auto &...step) { (detail::assert_covariance_constraints(P_, Q, step.R), ...); }, update_steps_);
     }
 
     const N_1 &state() const { return x_; }
     const N_N &covariance() const { return P_; }
 
     // estimate functions
-    N_1 estimated_states(const U_1 &inputs, const M_1 &measurements)
+    const N_1 estimated_states(const U_1 &inputs, const Measurements &measurements)
     {
         predict(inputs);
         update(measurements);
@@ -262,13 +295,11 @@ template <typename T, std::size_t STATES, std::size_t MEASUREMENTS, std::size_t
     }
 
   private:
-    std::array<StateFunction, STATES>             f_;
-    std::array<MeasurementFunction, MEASUREMENTS> h_;
-    N_N                                           Q_, P_, F_;
-    M_M                                           R_;
-    M_N                                           H_;
-    N_1                                           x_;
-    M_1                                           y_;
+    std::array<StateFunction, STATES> f_;
+    N_N                               Q_, P_, F_;
+    N_1                               x_;
+    UpdateSteps                       update_steps_;
+    static constexpr std::size_t      NUM_UPDATES = sizeof...(MEASUREMENT_DIMS);
 
     /**
      * @brief Compute Jacobian H = dh/dx using automatic differentiation
@@ -276,7 +307,6 @@ template <typename T, std::size_t STATES, std::size_t MEASUREMENTS, std::size_t
      * For each measurement function h_i, computes the i-th row of the Jacobian matrix.
      * H(i,j) = dh_i/dx_j using autodiff's built-in jacobian function.
      */
-
     void compute_F_eval_states(const U_1 &u)
     {
         state_inp_mtx base;
@@ -310,8 +340,14 @@ template <typename T, std::size_t STATES, std::size_t MEASUREMENTS, std::size_t
             x_(static_cast<Eigen::Index>(i)) = static_cast<T>(autodiff::val(x_eval_dual(static_cast<Eigen::Index>(i))));
     }
 
-    void compute_H_eval_y(const M_1 &z)
+    template <std::size_t MEASUREMENTS>
+    std::pair<Eigen::Matrix<T, MEASUREMENTS, STATES>, Eigen::Matrix<T, MEASUREMENTS, 1>>
+        compute_H_eval_y(const Eigen::Matrix<T, MEASUREMENTS, 1> &z, const UpdateStep<MEASUREMENTS> &update_step)
     {
+        using measurement_arr = Eigen::Matrix<autodiff::dual, MEASUREMENTS, 1>;
+        using M_1             = Eigen::Matrix<T, MEASUREMENTS, 1>;
+        using M_N             = Eigen::Matrix<T, MEASUREMENTS, STATES>;
+
         state_mtx base;
         for (std::size_t j = 0; j < STATES; ++j)
             base(static_cast<Eigen::Index>(j)) = autodiff::dual(x_(static_cast<Eigen::Index>(j)));
@@ -325,17 +361,23 @@ template <typename T, std::size_t STATES, std::size_t MEASUREMENTS, std::size_t
         {
             measurement_arr out;
             for (std::size_t i = 0; i < MEASUREMENTS; ++i)
-                out(static_cast<Eigen::Index>(i)) = h_[i](p);
+                out(static_cast<Eigen::Index>(i)) = update_step.h[i](p);
             return out;
         };
 
         state_mtx       p = base;
         measurement_arr z_eval_dual;
-        autodiff::jacobian(h_vec, autodiff::wrt(p), autodiff::at(p), z_eval_dual, H_);
+        M_N             H;
+
+        autodiff::jacobian(h_vec, autodiff::wrt(p), autodiff::at(p), z_eval_dual, H);
+
+        M_1 y;
 
         for (std::size_t i = 0; i < MEASUREMENTS; ++i)
-            y_(static_cast<Eigen::Index>(i)) = z(static_cast<Eigen::Index>(i)) -
-                                               static_cast<T>(autodiff::val(z_eval_dual(static_cast<Eigen::Index>(i))));
+            y(static_cast<Eigen::Index>(i)) = z(static_cast<Eigen::Index>(i)) -
+                                              static_cast<T>(autodiff::val(z_eval_dual(static_cast<Eigen::Index>(i))));
+
+        return { H, y };
     }
 
     /**
@@ -365,19 +407,43 @@ template <typename T, std::size_t STATES, std::size_t MEASUREMENTS, std::size_t
      *
      * @param z Sensor Measurement vector (Mx1)
      */
-    void update(const M_1 &z)
+    void update(const Measurements &z)
+    {
+        // For each update step,compile a seperate update function
+        [&]<std::size_t... I>(std::index_sequence<I...>)
+        { (update_<I>(z), ...); }(std::make_index_sequence<NUM_UPDATES>());
+    }
+
+    // TODO: Add outlier detection (either chi square or update shutoff)
+    template <std::size_t I> void update_(const Measurements &z)
     {
+        const auto           &step         = std::get<I>(update_steps_);
+        constexpr std::size_t MEASUREMENTS = step.dim;
+
+        using M_1 = Eigen::Matrix<T, MEASUREMENTS, 1>;
+        using M_M = Eigen::Matrix<T, MEASUREMENTS, MEASUREMENTS>;
+        using N_M = Eigen::Matrix<T, STATES, MEASUREMENTS>;
+
+        auto measurement_opt = std::get<I>(z);
+
+        // If measurement is invalid do an early return and do not update the state and covariance
+        if (not measurement_opt.has_value())
+            return;
+
+        M_1 measurement = measurement_opt.value();
+
         // the jacobian function works by providing you the jacobian (partial derrivatives of a function with respect to
         // each variable) and at the same time evaluating
-        compute_H_eval_y(z);
-        const M_M S =
-            H_ * P_ * H_.transpose() +
-            R_; // find the evolution covariance using linearized measurement evolution functions and measurement noise
-        const N_M K = P_ * H_.transpose() *
-                      S.llt().solve(M_M::Identity()); // cholesky decomp to avoid inversing computation overhead
-
-        x_ = x_ + K * y_;
-        P_ = (N_N::Identity() - K * H_) * P_;
+        auto [H, y] = compute_H_eval_y<MEASUREMENTS>(measurement, step);
+        const M_M S = H * P_ * H.transpose() + step.R; // find the evolution covariance using linearized measurement
+                                                       // evolution functions and measurement noise
+        const N_M K =
+            P_ * H.transpose() *
+            S.llt().solve(Eigen::Matrix<T, MEASUREMENTS, MEASUREMENTS>::Identity()); // cholesky decomp to avoid
+                                                                                     // inversing computation overhead
+
+        x_ = x_ + K * y;
+        P_ = (N_N::Identity() - K * H) * P_;
     }
 };
 } // namespace app::state_estimation
diff --git a/firmware/shared/srcpp/util/util_units.hpp b/firmware/shared/srcpp/util/util_units.hpp
index fb872565b0..0b630a2b60 100644
--- a/firmware/shared/srcpp/util/util_units.hpp
+++ b/firmware/shared/srcpp/util/util_units.hpp
@@ -74,12 +74,30 @@ inline constexpr float GEAR_RATIO        = 14.3f; // Verified by Noah
 // VELOCITY CONVERSIONS
 // =============================================================================
 
+// Convert meters per second to kilometers per hour
+[[nodiscard]] inline constexpr float MPS_TO_KMH(const float mps)
+{
+    return mps * 3.6f;
+}
+
+// Convert kilometers per hour to meters per second
+[[nodiscard]] inline constexpr float KMH_TO_MPS(const float kmh)
+{
+    return kmh / 3.6f;
+}
+
 // Convert motor RPM to vehicle speed (km/h)
 [[nodiscard]] inline constexpr float MOTOR_RPM_TO_KMH(const float rpm)
 {
     return rpm * WHEEL_DIAMETER_IN * M_PI_F * INCH_TO_KM * MIN_TO_HOUR / GEAR_RATIO;
 }
 
+// Convert motor RPM to vehicle speed (mps)
+[[nodiscard]] inline constexpr float MOTOR_RPM_TO_MPS(const float rpm)
+{
+    return KMH_TO_MPS(MOTOR_RPM_TO_KMH(rpm));
+}
+
 // Convert vehicle speed (km/h) to motor RPM
 [[nodiscard]] inline constexpr int MOTOR_KMH_TO_RPM(const float kmh)
 {
@@ -90,16 +108,4 @@ inline constexpr float GEAR_RATIO        = 14.3f; // Verified by Noah
 [[nodiscard]] inline constexpr int WHEEL_KMH_TO_RPM(const float kmh)
 {
     return static_cast<int>(kmh / (WHEEL_DIAMETER_IN * M_PI_F * INCH_TO_KM * MIN_TO_HOUR));
-}
-
-// Convert meters per second to kilometers per hour
-[[nodiscard]] inline constexpr float MPS_TO_KMH(const float mps)
-{
-    return mps * 3.6f;
-}
-
-// Convert kilometers per hour to meters per second
-[[nodiscard]] inline constexpr float KMH_TO_MPS(const float kmh)
-{
-    return kmh / 3.6f;
 }
\ No newline at end of file
diff --git a/firmware/shared/test_cpp/test_kalmanfilter.cpp b/firmware/shared/test_cpp/test_kalmanfilter.cpp
index adedb18e5b..0e09fa9e68 100644
--- a/firmware/shared/test_cpp/test_kalmanfilter.cpp
+++ b/firmware/shared/test_cpp/test_kalmanfilter.cpp
@@ -45,10 +45,10 @@ using KfU1       = Eigen::Matrix<float, 1, 1>;
 using KfM1       = Eigen::Matrix<float, 1, 1>;
 using KfH        = Eigen::Matrix<float, 1, 2>;
 
-using EkfVelocity = ekf<float, 2, 2, 3>;
+using EkfVelocity = ekf<float, 2, 3, 2>;
 using EkfN2       = Eigen::Matrix<float, 2, 2>;
 using EkfV2       = Eigen::Matrix<float, 2, 1>;
-using EkfU3       = Eigen::Matrix<float, 3, 1>;
+using EkfU3       = EkfVelocity::U_1;
 using EkfStateInp = EkfVelocity::state_inp_mtx;
 using EkfState    = EkfVelocity::state_mtx;
 
@@ -95,7 +95,7 @@ auto velocity_model_F(const EkfU3 &u) -> EkfN2
     return F;
 }
 
-auto create_velocity_state_functions() -> std::array<EkfVelocity::StateFunction, 2>
+auto create_velocity_state_functions() -> EkfVelocity::PredictStep
 {
     return { {
         velocity_state_x,
@@ -103,12 +103,9 @@ auto create_velocity_state_functions() -> std::array<EkfVelocity::StateFunction,
     } };
 }
 
-auto create_velocity_measurement_functions() -> std::array<EkfVelocity::MeasurementFunction, 2>
+auto create_velocity_update_steps(auto &R) -> EkfVelocity::UpdateSteps
 {
-    return { {
-        velocity_meas_x,
-        velocity_meas_y,
-    } };
+    return std::make_tuple(EkfVelocity::UpdateStep<2>{ .h = { { velocity_meas_x, velocity_meas_y } }, .R = R });
 }
 } // namespace
 
@@ -335,9 +332,6 @@ TEST(KalmanFilterTest, ConstructorRejectsNonPsdP0)
  */
 TEST(ExtendedKalmanFilterTest, OneStepMatchesLinearizedReference)
 {
-    const auto f = create_velocity_state_functions();
-    const auto h = create_velocity_measurement_functions();
-
     EkfV2 x0;
     x0 << 1.0f, 2.0f;
 
@@ -350,13 +344,18 @@ TEST(ExtendedKalmanFilterTest, OneStepMatchesLinearizedReference)
     EkfN2 R;
     R << 0.04f, 0.0f, 0.0f, 0.09f;
 
-    EkfVelocity filter(f, h, Q, R, x0, P0);
-
     EkfU3 u;
     u << 1.0f, 2.0f, 3.0f;
 
     EkfV2 z;
-    z << 1.5f, 2.5f;
+    z << 1.5f, 1.5f;
+
+    const auto f            = create_velocity_state_functions();
+    const auto update_steps = create_velocity_update_steps(R);
+
+    EkfVelocity filter(f, Q, update_steps, x0, P0);
+
+    EkfVelocity::Measurements meas = std::make_tuple(z);
 
     // Manual nonlinear predict and analytic Jacobian linearization.
     const EkfV2 x_pred     = velocity_model_step(x0, u);
@@ -368,7 +367,7 @@ TEST(ExtendedKalmanFilterTest, OneStepMatchesLinearizedReference)
     const EkfV2 x_expected = x_pred + K * y;
     const EkfN2 P_expected = (EkfN2::Identity() - K) * P_pred;
 
-    const EkfV2 x_actual = filter.estimated_states(u, z);
+    const EkfV2 x_actual = filter.estimated_states(u, meas);
     const EkfN2 P_actual = filter.covariance();
 
     for (int i = 0; i < 2; ++i)
@@ -398,9 +397,6 @@ TEST(ExtendedKalmanFilterTest, OneStepMatchesLinearizedReference)
  */
 TEST(ExtendedKalmanFilterTest, VeryLowMeasurementNoiseSnapsTowardMeasurement)
 {
-    const auto f = create_velocity_state_functions();
-    const auto h = create_velocity_measurement_functions();
-
     EkfV2 x0;
     x0 << 1.0f, 2.0f;
 
@@ -412,15 +408,20 @@ TEST(ExtendedKalmanFilterTest, VeryLowMeasurementNoiseSnapsTowardMeasurement)
     EkfN2 R;
     R << 1e-8f, 0.0f, 0.0f, 1e-8f;
 
-    EkfVelocity filter(f, h, Q, R, x0, P0);
-
     EkfU3 u;
     u << 1.0f, 2.0f, 3.0f;
 
     EkfV2 z;
     z << 1.5f, 2.5f;
 
-    const EkfV2 x_actual = filter.estimated_states(u, z);
+    const auto f            = create_velocity_state_functions();
+    const auto update_steps = create_velocity_update_steps(R);
+
+    EkfVelocity filter(f, Q, update_steps, x0, P0);
+
+    EkfVelocity::Measurements meas = std::make_tuple(z);
+
+    const EkfV2 x_actual = filter.estimated_states(u, meas);
     const EkfN2 P_actual = filter.covariance();
 
     for (int i = 0; i < 2; ++i)
@@ -447,9 +448,6 @@ TEST(ExtendedKalmanFilterTest, VeryLowMeasurementNoiseSnapsTowardMeasurement)
  */
 TEST(ExtendedKalmanFilterTest, SilLoopRemainsStableAndTracksState)
 {
-    const auto f = create_velocity_state_functions();
-    const auto h = create_velocity_measurement_functions();
-
     EkfV2 x0;
     x0 << 0.0f, 0.0f;
 
@@ -462,7 +460,10 @@ TEST(ExtendedKalmanFilterTest, SilLoopRemainsStableAndTracksState)
     EkfN2 R;
     R << 0.04f, 0.0f, 0.0f, 0.04f;
 
-    EkfVelocity filter(f, h, Q, R, x0, P0);
+    const auto f            = create_velocity_state_functions();
+    const auto update_steps = create_velocity_update_steps(R);
+
+    EkfVelocity filter(f, Q, update_steps, x0, P0);
 
     std::mt19937                    rng(21);
     std::normal_distribution<float> meas_noise(0.0f, 0.12f);
@@ -484,7 +485,9 @@ TEST(ExtendedKalmanFilterTest, SilLoopRemainsStableAndTracksState)
         EkfV2 z;
         z << x_true(0) + meas_noise(rng), x_true(1) + meas_noise(rng);
 
-        const EkfV2 x_est = filter.estimated_states(u, z);
+        EkfVelocity::Measurements meas = std::make_tuple(z);
+
+        const EkfV2 x_est = filter.estimated_states(u, meas);
         const EkfN2 P     = filter.covariance();
 
         EXPECT_TRUE(x_est.allFinite());
@@ -513,9 +516,6 @@ TEST(ExtendedKalmanFilterTest, SilLoopRemainsStableAndTracksState)
  */
 TEST(ExtendedKalmanFilterTest, ConstructorRejectsNonPdR)
 {
-    const auto f = create_velocity_state_functions();
-    const auto h = create_velocity_measurement_functions();
-
     EkfV2 x0;
     x0 << 0.0f, 0.0f;
 
@@ -525,9 +525,12 @@ TEST(ExtendedKalmanFilterTest, ConstructorRejectsNonPdR)
     const EkfN2 Q     = EkfN2::Zero();
     const EkfN2 bad_R = EkfN2::Zero();
 
+    const auto f            = create_velocity_state_functions();
+    const auto update_steps = create_velocity_update_steps(bad_R);
+
     EXPECT_DEATH(
         {
-            EkfVelocity invalid_filter(f, h, Q, bad_R, x0, P0);
+            EkfVelocity invalid_filter(f, Q, update_steps, x0, P0);
             (void)invalid_filter;
         },
         "R must be positive definite");