Body Velocity Estimator (WIP)

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 ()
+endif ()

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

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

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

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

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