ubc-solar-physics 1.1.0__cp39-cp39-macosx_11_0_arm64.whl → 1.7.3__cp39-cp39-macosx_11_0_arm64.whl

physics/models/battery/battery_model.py

@@ -0,0 +1,226 @@
+import numpy as np
+import physics_rs
+from typing import Callable, TypeAlias, Protocol, runtime_checkable, Optional, cast
+from numpy.typing import NDArray
+
+
+SOCDependent: TypeAlias = Callable[[float | NDArray[float]], float | NDArray[float]]
+
+
+@runtime_checkable
+class EquivalentCircuitModelConfig(Protocol):
+    """
+    A specification for a configuration object which contains the requisite data to specify
+    a `EquivalentCircuitBatteryModel`.
+    """
+
+    @property
+    def get_Uoc(self) -> SOCDependent:
+        """
+        A map from an SOC to Uoc (open-circuit voltage).
+        Should be compatible with non-vectorized and vectorized calls: float -> float or NDArray -> NDArray
+        """
+        ...
+
+    @property
+    def get_R_0(self) -> SOCDependent:
+        """
+        A map from an SOC to R_0 (internal resistance).
+        Should be compatible with non-vectorized and vectorized calls: float -> float or NDArray -> NDArray
+        """
+        ...
+
+    @property
+    def get_R_P(self) -> SOCDependent:
+        """
+        A map from an SOC to R_P (polarization resistance).
+        Should be compatible with non-vectorized and vectorized calls: float -> float or NDArray -> NDArray
+        """
+        ...
+
+    @property
+    def get_C_P(self) -> SOCDependent:
+        """
+        A map from an SOC to C_P (polarization capacitance).
+        Should be compatible with non-vectorized and vectorized calls: float -> float or NDArray -> NDArray
+        """
+        ...
+
+    @property
+    def Q_total(self) -> float:
+        """
+        The total charge capacity of the battery pack, in Coulombs.
+        """
+        ...
+
+
+class EquivalentCircuitBatteryModel:
+    """
+    A first-order Thevenin equivalent model of a lithium-ion battery pack
+    """
+
+    def __init__(self, battery_config: EquivalentCircuitModelConfig, state_of_charge: float = 1.0):
+        """
+        Constructor for the EquivalentCircuitBatteryModel class.
+
+        :param BatteryModelConfig battery_config: Configuration object containing the battery's parameters and data.
+        :param float state_of_charge: Initial state of charge of the battery (default is 1.0, fully charged).
+        """
+
+        # We initialize the active components as uncharged
+        self._U_P = 0.0  # V
+        self._U_L = 0.0  # V
+        self._state_of_charge = state_of_charge
+        self._nominal_charge_capacity = battery_config.Q_total
+
+        # Now, the config contains methods to map SOC to each respective parameter.
+        # We can't efficiently pass these functions to compiled libraries.
+        # Instead, we will pre-compute the parameters as a function of SOC
+        # to create fine lookup tables as a portable substitute for runtime computation.
+
+        # Things are going to get a tiny bit messy here, so we will go through this carefully.
+        # I'll write what each resulting map achieves below each code block.
+
+        # Firstly, we're going to discretize SOC by making a range of SOC values in the range [-0.05, 1.1], because
+        # sometimes we are marginally outside the range (0.0, 1.0].
+        # We will quantize at about 4 digits of precision, so ~10,000 values
+        self._min_soc = -0.05
+        self._max_soc = 1.1
+        self._num_indices = int((self._max_soc - self._min_soc) * 10000)
+        SOC_values = np.linspace(self._min_soc, self._max_soc, self._num_indices, dtype=float)
+        # maps: (discrete index) -> (SOC)
+
+        # Now, we're going to create a map from an arbitrary SOC, to the index of the closest SOC
+        # value in our quantized SOC range (`SOC_values`)
+        self._quantization_step: float = (self._max_soc - self._min_soc) / self._num_indices
+        self._soc_to_index = lambda _soc: int(
+            max(0, min(self._num_indices - 1, (_soc - self._min_soc) // self._quantization_step))
+        )
+        # maps: (SOC) -> (discrete index)
+
+        # Now, calculate the value of each parameter for each discrete SOC value using the injected `get_` functions
+        self._U_oc_lookup: NDArray[float] = battery_config.get_Uoc(SOC_values)
+        self._R_0_lookup: NDArray[float] = battery_config.get_R_0(SOC_values)
+        self._R_P_lookup: NDArray[float] = battery_config.get_R_P(SOC_values)
+        self._C_P_lookup: NDArray[float] = battery_config.get_C_P(SOC_values)
+        # maps: (discrete index) -> (parameter)
+
+        # Finally, combine the above maps to create a map from an arbitrary SOC to each battery parameter, using
+        # the discrete lookup tables
+        # These `cast` calls just promise to the type-checker that these will map floats to floats
+        self._U_oc = cast(Callable[[float], float], lambda SOC: self._U_oc_lookup[self._soc_to_index(SOC)])
+        self._R_0 = cast(Callable[[float], float], lambda SOC: self._R_0_lookup[self._soc_to_index(SOC)])
+        self._R_P = cast(Callable[[float], float], lambda SOC: self._R_P_lookup[self._soc_to_index(SOC)])
+        self._C_P = cast(Callable[[float], float], lambda SOC: self._C_P_lookup[self._soc_to_index(SOC)])
+        # maps: ((SOC) -> (discrete index)) -> ((discrete index) -> (parameter))  |==>  (SOC) -> (parameter)
+
+        self._tau: Callable[[float], float] = lambda soc: self._R_P(soc) * self._C_P(soc)  # Characteristic Time in s
+
+    def update_array(
+            self,
+            tick: float,
+            delta_energy_array: Optional[NDArray] = None,
+            current_array: Optional[NDArray] = None,
+            use_compiled: bool = True
+    ) -> tuple[NDArray, NDArray]:
+        """
+        Compute the battery's state of charge and terminal voltage over time in response to a
+        time series of energy/current draw from a load.
+
+        Only ONE of `current_array` or `delta_energy_array` should be provided.
+
+        Notes
+        -----
+        If both current and power are known, current should be provided.
+        The model implementation requires current for calculations, so it must be derived from power if power
+        was provided.
+        Computing current from power relies on voltage, which is a model output, and therefore
+        the derived current could be less accurate.
+
+        :param NDArray delta_energy_array: Array of energy changes (J) at each time step.
+        :param float tick: Time interval for each step (seconds).
+        :param NDArray current_array: Array of current draw (positive sign convention) in Amperes at each time step.
+        :param bool use_compiled: If `True`, use compiled binaries for calculations.
+            Disable for better debugging.
+        :return: A tuple containing arrays for state-of-charge and terminal voltage.
+        :raises ValueError: If BOTH or NEITHER of `current_array` or `delta_energy_array` are provided.
+        :rtype: tuple[NDArray, NDArray]
+        """
+        if (delta_energy_array is None) == (current_array is None):  # Enforce that only one should be provided
+            raise ValueError("Exactly one of `delta_energy_array` or `current_array` "
+                             "must be provided, not both or neither.")
+
+        energy_or_current = delta_energy_array if delta_energy_array is not None else current_array
+
+        if use_compiled:
+            return physics_rs.update_battery_state(
+                energy_or_current,
+                tick,
+                self._state_of_charge,
+                self._U_P,
+                self._R_0_lookup,
+                self._U_oc_lookup,
+                self._R_P_lookup,
+                self._C_P_lookup,
+                self._nominal_charge_capacity,
+                current_array is None,  # Pass to the library if `energy_or_current` is current or power,
+                self._quantization_step,
+                self._min_soc
+            )
+
+        else:
+            return self._update_array_py(energy_or_current, tick, current_array is None)
+
+    def _update_array_py(self, energy_or_current, tick, is_power):
+        """
+        Perform energy calculations using Python (fallback method if Rust is disabled).
+
+        :param NDArray energy_or_current: Array of energy changes (J) at each time step.
+        :param float tick: Time interval for each step (seconds).
+
+        :return: A tuple containing arrays for state-of-charge and voltage.
+        """
+        soc = np.empty_like(energy_or_current, dtype=float)
+        voltage = np.empty_like(energy_or_current, dtype=float)
+
+        for (i, value) in enumerate(energy_or_current):
+            if is_power:
+                # Use the last voltage to calculate current, or an absurdly large number if it is the first,
+                # because we don't know voltage yet.
+                # We will have a very small initial current, no matter what.
+                # We shouldn't be starting to simulate when the battery is in an active state anyway,
+                # so this should be an alright compromise.
+                last_terminal_voltage = voltage[i - 1] if i - 1 >= 0 else 10000
+
+                current: float = value / (tick * last_terminal_voltage)
+            else:
+                current = value
+
+            self._evolve(current, tick)
+            soc[i] = self._state_of_charge
+            voltage[i] = self._U_L
+
+        return soc, voltage
+
+    def _evolve(self, current: float, tick: float) -> None:
+        """
+        Update the battery state given the current and time elapsed.
+
+        :param float current: Current applied to the battery (A).
+            Positive for charging, negative for discharging.
+        :param float tick: Time interval over which the power is applied (seconds).
+        """
+        soc = self._state_of_charge  # State of Charge (dimensionless, 0 < soc < 1)
+        U_P = self._U_P  # Polarization Potential (V)
+        R_P = self._R_P(soc)  # Polarization Resistance (Ohms)
+        U_oc = self._U_oc(soc)  # Open-Circuit Potential (V)
+        R_0 = self._R_0(soc)  # Ohmic Resistance (Ohms)
+        Q = self._nominal_charge_capacity  # Nominal Charge Capacity (C)
+        tau = self._tau(soc)  # Time constant (s)
+
+        new_soc = soc + (current * tick / Q)
+        new_U_P = np.exp(-tick / tau) * U_P + current * R_P * (1 - np.exp(-tick / tau))
+
+        self._state_of_charge = new_soc
+        self._U_P = new_U_P
+        self._U_L = U_oc + new_U_P + (current * R_0)

physics/models/battery/kalman_filter.py

@@ -0,0 +1,223 @@
+import numpy as np
+from filterpy.kalman import ExtendedKalmanFilter
+from typing import Protocol, runtime_checkable, cast, Callable
+from physics.models.battery import EquivalentCircuitModelConfig
+from numpy.typing import NDArray
+
+
+@runtime_checkable
+class FilteredBatteryModelConfig(Protocol):
+    """
+    A specification for a configuration object which contains the requisite data to specify
+    a `FilteredBatteryModel`.
+    """
+    @property
+    def battery_model_config(self) -> EquivalentCircuitModelConfig:
+        """
+        Configuration of the underlying `EquivalentCircuitModel`.
+        """
+        ...
+
+    @property
+    def process_noise_matrix(self) -> NDArray[float]:
+        """
+        A 2x2 matrix containing the process noise covariance matrix where [0, 0] is the SOC evolution
+        noise and [1, 1] is the polarization potential evolution noise.
+        """
+        ...
+
+    @property
+    def state_covariance_matrix(self) -> NDArray[float]:
+        """
+        A 2x2 matrix containing the state covariance matrix where [0, 0] is the SOC covariance
+        noise and [1, 1] is the polarization potential covariance.
+        """
+        ...
+
+    @property
+    def measurement_noise_vector(self) -> NDArray[float]:
+        """
+        A 1x1 vector containing the noise expected in the terminal voltage measurement.
+        """
+        ...
+
+
+class FilteredBatteryModel:
+    """
+    `FilteredBatteryModel` is a first-order Thevenin equivalent model of a lithium-ion battery packed, wrapped
+    in a Kalman filter which uses voltage measurements with model predictions.
+    """
+    def __init__(
+            self,
+            battery_config: FilteredBatteryModelConfig,
+            initial_SOC: float = 1.0,
+            initial_Uc: float = 0.0,
+            alpha: float = 0.9
+    ):
+        """
+        :param FilteredBatteryModelConfig battery_config: Contains Kalman filter state estimation configuration and
+            underlying equivalent circuit model configuration.
+        :param float initial_SOC: Initial SOC of the battery, in the range (0, 1].
+        :param float initial_Uc: Initial polarization voltage of the battery in Volts.
+        """
+        # Initial state
+        assert 0.0 <= initial_SOC <= 1.1, "`initial_SOC` must be in (0, 1.1]!"
+
+        self._SOC = initial_SOC     # State of Charge
+        self._Uc = initial_Uc       # Polarization Voltage
+
+        # Load Config data
+        self._Q_total = battery_config.battery_model_config.Q_total
+
+        # These `cast` calls just promise to the type-checker that these will map floats to floats
+        self._U_oc = cast(Callable[[float], float], battery_config.battery_model_config.get_Uoc)
+        self._R_0 = cast(Callable[[float], float], battery_config.battery_model_config.get_R_0)
+        self._R_P = cast(Callable[[float], float], battery_config.battery_model_config.get_R_P)
+        self._C_P = cast(Callable[[float], float], battery_config.battery_model_config.get_C_P)
+
+        self._tau: Callable[[float], float] = lambda soc: self._R_P(soc) * self._C_P(soc)  # Characteristic Time in s
+
+        def central_difference_derivative(func, value, h=1e-6):
+            """
+            Compute the derivative of an arbitrary function `func` at `SOC` using central difference.
+
+            :param func: The function to differentiate.
+            :param value: The point at which to compute the derivative.
+            :param h: Step size for the finite difference.
+            :return: The derivative of the function at `SOC`.
+            """
+            return (func(value + h) - func(value - h)) / (2 * h)
+
+        self._dUoc_dSOC = lambda soc: central_difference_derivative(self._U_oc, np.minimum(1.0, soc))  # dUOC wrt to SOC
+        self._dR0_dSOC = lambda soc: central_difference_derivative(self._R_0, np.minimum(1.0, soc))    # dR0 wrt to SOC
+
+        # Initializing the EKF object
+        self._ekf = ExtendedKalmanFilter(dim_x=2, dim_z=1)
+
+        # State Vector
+        self._ekf.x = np.array([
+            self._SOC,
+            self._Uc]
+        )
+
+        self._ekf.P = battery_config.state_covariance_matrix
+        self._ekf.Q = battery_config.process_noise_matrix
+        self._ekf.R = battery_config.measurement_noise_vector
+
+        assert 0 <= alpha <= 1, "`alpha` should be between 0 and 1!"
+        self._alpha = alpha
+
+        self._filtered_I = 0
+        self._predicted_measurement = 0
+
+    @property
+    def SOC(self) -> float:
+        """
+        Return the current SOC of the battery.
+
+        :return: The current state of charge.
+        """
+        return self._SOC
+
+    @property
+    def Uc(self) -> float:
+        """
+        Return the polarization voltage of the battery.
+
+        :return: The current polarization voltage.
+        """
+        return self._Uc
+
+    @property
+    def Ut(self) -> float:
+        """
+        Return the predicted terminal voltage for the last prediction step.
+
+        :return: The predicted terminal voltage.
+        """
+        return self._predicted_measurement
+
+    def update_filter(self, measured_Ut, current):
+        """
+        Update the filter based on a new measurement and the predicted state.
+        This function should be called after `predict_state` in a typical predict-update workflow.
+
+        :param float measured_Ut: The actual voltage across the terminals of the battery.
+        :param float current: The current being sourced by the battery.
+        """
+        # Simple low-pass filter to current
+        self._filtered_I = self._alpha * self._filtered_I + (1 - self._alpha) * current
+
+        self._ekf.update(z=measured_Ut, HJacobian=self._measurement_jacobian, Hx=self._measurement_function)
+
+        self._SOC, self._Uc = self._ekf.x
+        self._SOC = np.clip(self._SOC, 0.0, 1.1)
+
+    def predict_state(self, current, time_step):
+        """
+        Predict the next evolution of the state vector (SOC, Uc).
+        This function should be called before updating the filter in a typical predict-update workflow.
+
+        :param float current: The current being sourced by the battery.
+            Sign convention is that positive indicates current being drawn.
+        :param float time_step: Time elapsed between this prediction and the last updated state of the filter (seconds).
+        """
+        # Control matrix B (for input current I_k)
+        self._ekf.B = np.array([
+            -time_step / self._Q_total,
+            self._R_P(self._SOC) * (1 - np.exp(-time_step / (self._tau(self._SOC)))),
+        ])
+        self._ekf.F = self._state_jacobian(time_step)
+
+        self._ekf.predict(u=current)
+        self._SOC, self._Uc = self._ekf.x
+
+    def predict_then_update(self, measured_Ut: float, current: float, time_step: float):
+        """
+        Predict the next evolution of the state vector (SOC, Uc), then update the filter
+        based on this prediction and a measurement. Abstracts the full predict-update workflow of the EKF.
+
+        :param float measured_Ut: The actual voltage across the terminals of the battery.
+        :param float current: The current being sourced by the battery. Positive indicates current being drawn.
+        :param float time_step: Time elapsed between this prediction and the last updated state of the filter (seconds).
+        """
+        self.predict_state(current, time_step)
+        self.update_filter(measured_Ut, current)
+
+    def _state_jacobian(self, time_step):
+        """
+        Return the state Jacobian matrix for the current time step.
+
+        :param float time_step: Time elapsed between this prediction and the last updated state of the filter (seconds).
+        :return: The state Jacobian matrix.
+        :rtype: np.ndarray
+        """
+        return np.array([[1, 0], [0, np.exp(-time_step / self._tau(self._SOC))]])
+
+    def _measurement_jacobian(self, x):
+        """
+        Return the measurement Jacobian matrix for the current state vector.
+
+        :param list[float, float] x: The state vector [SOC, Uc].
+        :return: The measurement Jacobian matrix.
+        :rtype: np.ndarray
+        """
+        SOC = x[0]
+        dUoc_dSOC = self._dUoc_dSOC(SOC)
+        dR0_dSOC = self._dR0_dSOC(SOC)
+
+        return np.array([[dUoc_dSOC - dR0_dSOC * self._filtered_I, -1]])
+
+    def _measurement_function(self, x) -> float:
+        """
+        Return the measurement function relating terminal voltage to SOC and polarization voltage.
+
+        :param list[float, float] x: The state vector [SOC, Uc].
+        :return: The predicted terminal voltage.
+        """
+        SOC, Uc = x
+        Uoc = self._U_oc(SOC)
+        R0 = self._R_0(SOC)
+        self._predicted_measurement = Uoc - Uc - R0 * self._filtered_I
+
+        return self._predicted_measurement

physics/models/battery.rs

@@ -1 +1 @@
-mod battery;
+pub mod battery;

physics/models/motor/__init__.py

@@ -1,7 +1,9 @@
 from .base_motor import BaseMotor
 from .basic_motor import BasicMotor
+from .advanced_motor import AdvancedMotor
 
 __all__ = [
     "BaseMotor",
-    "BasicMotor"
+    "BasicMotor",
+    "AdvancedMotor",
 ]

physics/models/motor/advanced_motor.py

@@ -0,0 +1,196 @@
+import numpy as np
+from haversine import haversine, Unit
+from numpy.typing import NDArray
+from physics.models.motor import BasicMotor
+
+
+class AdvancedMotor(BasicMotor):
+    def __init__(self, **kwargs):
+        super().__init__(**kwargs)
+        self.cornering_coefficient = 15  # tuned to Day 1 and 3 FSGP data
+
+    def calculate_energy_in(self, required_speed_kmh, gradients, wind_speeds, tick, coords):
+        """
+        A function which takes in array of elevation, array of wind speed, required
+            speed, returns the consumed energy.
+
+        :param np.ndarray required_speed_kmh: (float[N]) required speed array in km/h
+        :param np.ndarray gradients: (float[N]) gradient at parts of the road
+        :param np.ndarray wind_speeds: (float[N]) speeds of wind in m/s, where > 0 means against the direction of the vehicle
+        :param float tick: length of 1 update cycle in seconds
+        :param np.ndarray coords: ([float[N,2]) The lat,lon coordinate  of the car at each tick
+        :returns: (float[N]) energy expended by the motor at every tick
+        :rtype: np.ndarray
+
+        """
+        net_force, required_angular_speed_rads = self.calculate_net_force(required_speed_kmh, wind_speeds, gradients)
+
+        cornering_work = self.calculate_cornering_losses(required_speed_kmh, coords, tick)
+
+        motor_output_energies = required_angular_speed_rads * net_force * self.tire_radius * tick + cornering_work
+        motor_output_energies = np.clip(motor_output_energies, a_min=0, a_max=None)
+
+        e_m = self.calculate_motor_efficiency(required_angular_speed_rads, motor_output_energies, tick)
+        e_mc = self.calculate_motor_controller_efficiency(required_angular_speed_rads, motor_output_energies, tick)
+
+        motor_controller_input_energies = motor_output_energies / (e_m * e_mc)
+
+        # Filter out and replace negative energy consumption as 0
+        motor_controller_input_energies = np.where(motor_controller_input_energies > 0,
+                                                   motor_controller_input_energies, 0)
+
+        return motor_controller_input_energies
+
+    def calculate_cornering_losses(self, required_speed_kmh, coords, tick):
+        """
+        Calculate the energy losses due to cornering based on vehicle speed and trajectory.
+
+        :param np.ndarray required_speed_kmh: (float[N]) Required speed array in km/h
+        :param np.ndarray coords: (float[N, 2]) Array containing latitude and longitude coordinates of the car at each tick
+        :param float tick: Length of one update cycle in seconds
+        :returns: (float[N]) Energy loss due to cornering at each tick
+        :rtype: np.ndarray
+        """
+        required_speed_ms = required_speed_kmh / 3.6
+        cornering_radii = self.calculate_radii(coords)
+
+        centripetal_lateral_force = self.vehicle_mass * (required_speed_ms ** 2) / cornering_radii
+        centripetal_lateral_force = np.clip(centripetal_lateral_force, a_min=0, a_max=10000)
+
+        slip_angles_degrees = self.get_slip_angle_for_tire_force(centripetal_lateral_force)
+        slip_angles_radians = np.radians(slip_angles_degrees)
+        slip_distances = np.tan(slip_angles_radians) * required_speed_ms * tick
+
+        return slip_distances * centripetal_lateral_force * self.cornering_coefficient
+
+    def calculate_radii(self, coords):
+        """
+        Calculate the cornering radii for a given set of waypoints.
+
+        :param np.ndarray coords: (float[N, 2]) Array containing latitude and longitude coordinates of the car's path
+        :returns: (float[N]) Array of cornering radii at each waypoint
+        :rtype: np.ndarray
+        """
+
+        # pop off last coordinate if first and last coordinate are the same
+        repeated_last_coordinate = False
+        if np.array_equal(coords[0], coords[len(coords) - 1]):
+            coords = coords[:-1]
+            repeated_last_coordinate = True
+
+        cornering_radii = np.empty(len(coords))
+        for i in range(len(coords)):
+            # if the next point or previous point is out of bounds, wrap the index around the array
+            i2 = (i - 1) % len(coords)
+            i3 = (i + 1) % len(coords)
+            current_point = coords[i]
+            previous_point = coords[i2]
+            next_point = coords[i3]
+
+            x1 = 0
+            y1 = 0
+            x2, y2 = self.calculate_meter_distance(current_point, previous_point)
+            x3, y3 = self.calculate_meter_distance(current_point, next_point)
+            cornering_radii[i] = self.radius_of_curvature(x1, y1, x2, y2, x3, y3)
+
+        # If the last coordinate was removed, duplicate the first radius value to the end of the array
+        if repeated_last_coordinate:
+            cornering_radii = np.append(cornering_radii, cornering_radii[0])
+
+        # ensure that super large radii are bounded by a large number, like 10000
+        cornering_radii = np.where(np.isnan(cornering_radii), 10000, cornering_radii)
+        cornering_radii = np.where(cornering_radii > 10000, 10000, cornering_radii)
+
+        return cornering_radii
+
+    def generate_slip_angle_lookup(self, min_degrees, max_degrees, num_elements):
+        """
+        Generate a lookup table of slip angles and corresponding tire forces using Pacejka's Magic Formula.
+
+        https://www.edy.es/dev/docs/pacejka-94-parameters-explained-a-comprehensive-guide/
+
+        :param float min_degrees: Minimum slip angle in degrees
+        :param float max_degrees: Maximum slip angle in degrees
+        :param int num_elements: Number of discrete elements in the lookup table
+        :returns: (float[num_elements], float[num_elements]) Arrays of slip angles (degrees) and corresponding tire forces (Newtons)
+        :rtype: tuple[np.ndarray, np.ndarray]
+        """
+
+        b = .25  # Stiffness
+        c = 2.2  # Shape
+        d = 2.75  # Peak
+        e = 1.0  # Curvature
+
+        fz = self.vehicle_mass * 9.81  # Newtons
+
+        slip_angles = np.linspace(min_degrees, max_degrees, num_elements)
+        tire_forces = fz * d * np.sin(
+            c * np.arctan(b * slip_angles - e * (b * slip_angles - np.arctan(b * slip_angles))))
+
+        return slip_angles, tire_forces
+
+    def get_slip_angle_for_tire_force(self, desired_tire_force):
+        slip_angles, tire_forces = self.generate_slip_angle_lookup(0, 70, 100000)
+
+        # Use the numpy interpolation function to find slip angle for the given tire force
+        estimated_slip_angle = np.interp(desired_tire_force, tire_forces, slip_angles)
+
+        return estimated_slip_angle
+
+    @staticmethod
+    def calculate_meter_distance(coord1: NDArray, coord2: NDArray):
+        """
+        Calculate the x and y distance in meters between two latitude-longitude coordinates.
+
+        :param tuple coord1: (float[2]) The (latitude, longitude) coordinates of the first point
+        :param tuple coord2: (float[2]) The (latitude, longitude) coordinates of the second point
+        :returns: (float[2]) The x (longitude) and y (latitude) distances in meters
+        :rtype: tuple
+        """
+        lat1, lon1 = coord1
+        lat2, lon2 = coord2
+
+        # Base coordinate
+        coord_base = (lat1, lon1)
+        # Coordinate for latitude difference (keep longitude the same)
+        coord_lat = (lat2, lon1)
+        # Coordinate for longitude difference (keep latitude the same)
+        coord_long = (lat1, lon2)
+
+        # Calculate y distance (latitude difference)
+        y_distance = haversine(coord_base, coord_lat, unit=Unit.METERS)
+        # Calculate x distance (longitude difference)
+        x_distance = haversine(coord_base, coord_long, unit=Unit.METERS)
+
+        if lat2 < lat1:
+            y_distance = -y_distance
+        if lon2 < lon1:
+            x_distance = -x_distance
+
+        return x_distance, y_distance
+
+    @staticmethod
+    def radius_of_curvature(x1, y1, x2, y2, x3, y3):
+        """
+        Uses the circumcircle the radius of curvature of a circle passing through three points.
+
+        :param float x1: X-coordinate of the first point
+        :param float y1: Y-coordinate of the first point
+        :param float x2: X-coordinate of the second point
+        :param float y2: Y-coordinate of the second point
+        :param float x3: X-coordinate of the third point
+        :param float y3: Y-coordinate of the third point
+        :returns: Radius of curvature of the circle passing through the three points
+        :rtype: float
+        """
+        numerator = np.sqrt(
+            ((x3 - x2) ** 2 + (y3 - y2) ** 2) *
+            ((x1 - x3) ** 2 + (y1 - y3) ** 2) *
+            ((x2 - x1) ** 2 + (y2 - y1) ** 2)
+        )
+
+        denominator = 2 * abs(
+            ((x2 - x1) * (y1 - y3) - (x1 - x3) * (y2 - y1))
+        )
+
+        return numerator / denominator

physics/models/motor/base_motor.py

@@ -4,3 +4,5 @@ from abc import ABC
 class BaseMotor(ABC):
     def __init__(self):
         super().__init__()
+
+