ubc-solar-physics 1.7.3__cp39-cp39-macosx_10_12_x86_64.whl

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

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

physics/models/constants.py

@@ -0,0 +1,23 @@
+# Radius of the Earth (m)
+EARTH_RADIUS = 6371009
+
+# Acceleration caused by gravity (m/s^2)
+ACCELERATION_G = 9.81
+
+# Density of Air at 15C and 101kPa (kg/m^3)
+AIR_DENSITY = 1.225
+
+# Maximum number of waypoints that can be given to generate route data
+MAX_WAYPOINTS = 10
+
+# Solar Irradiance (W/m^2)
+SOLAR_IRRADIANCE = 1353
+
+# As we currently have a limited number of API calls(60) every minute with the
+# current Weather API, we must shrink the dataset significantly. As the
+# OpenWeatherAPI models have a resolution of between 2.5 - 70 km, we will
+# go for a resolution of 25km. Assuming we travel at 100km/h for 12 hours,
+# 1200 kilometres/25 = 48 API calls
+# As the Google Maps API has a resolution of around 40m between points,
+# for ASC, we must cull at 625:1 (because 25,000m / 40m = 625)
+REDUCTION_FACTOR = 625

physics/models/lvs/__init__.py

@@ -0,0 +1,7 @@
+from .base_lvs import BaseLVS
+from .basic_lvs import BasicLVS
+
+__all__ = [
+    "BaseLVS",
+    "BasicLVS"
+]

physics/models/lvs/base_lvs.py

@@ -0,0 +1,6 @@
+from abc import ABC
+
+
+class BaseLVS(ABC):
+    def __init__(self, consumed_energy):
+        super().__init__()

physics/models/lvs/basic_lvs.py

@@ -0,0 +1,18 @@
+from physics.models.lvs.base_lvs import BaseLVS
+
+
+class BasicLVS(BaseLVS):
+
+    def __init__(self, consumed_energy, lvs_current, lvs_voltage):
+        super().__init__(consumed_energy)
+        self.lvs_current = lvs_current
+        self.lvs_voltage = lvs_voltage
+
+    def get_consumed_energy(self, tick):
+        """
+            Get the energy consumption of the Low Voltage System (current * voltage * time)
+
+            :param tick - (int) tick time passed
+            :returns: consumed_energy - (number) value of energy consumed
+        """
+        return self.lvs_current * self.lvs_voltage * tick

physics/models/lvs.rs

@@ -0,0 +1 @@
+mod lvs;

physics/models/motor/__init__.py

@@ -0,0 +1,9 @@
+from .base_motor import BaseMotor
+from .basic_motor import BasicMotor
+from .advanced_motor import AdvancedMotor
+
+__all__ = [
+    "BaseMotor",
+    "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

@@ -0,0 +1,8 @@
+from abc import ABC
+
+
+class BaseMotor(ABC):
+    def __init__(self):
+        super().__init__()
+
+