simple-autonomous-car 0.1.2__py3-none-any.whl

simple_autonomous_car/filters/particle_filter.py

@@ -0,0 +1,162 @@
+"""Particle filter for state estimation."""
+
+import numpy as np
+
+from simple_autonomous_car.filters.base_filter import BaseFilter
+
+
+class ParticleFilter(BaseFilter):
+    """
+    Particle filter for state estimation.
+
+    Uses a set of particles to represent the probability distribution
+    of the state. Good for non-linear systems and multi-modal distributions.
+
+    Parameters
+    ----------
+    initial_state : np.ndarray
+        Initial state vector (e.g., [x, y]).
+    initial_covariance : np.ndarray
+        Initial covariance matrix (used to initialize particles).
+    num_particles : int, default=100
+        Number of particles.
+    process_noise : np.ndarray
+        Process noise covariance matrix.
+    measurement_noise : np.ndarray
+        Measurement noise covariance matrix.
+    name : str, default="particle_filter"
+        Filter name.
+    """
+
+    def __init__(
+        self,
+        initial_state: np.ndarray,
+        initial_covariance: np.ndarray,
+        num_particles: int = 100,
+        process_noise: np.ndarray | None = None,
+        measurement_noise: np.ndarray | None = None,
+        name: str = "particle_filter",
+    ):
+        super().__init__(name=name)
+        self.state_dim = len(initial_state)
+        self.num_particles = num_particles
+        self.process_noise = (
+            np.asarray(process_noise, dtype=np.float64)
+            if process_noise is not None
+            else np.eye(self.state_dim) * 0.1
+        )
+        self.measurement_noise = (
+            np.asarray(measurement_noise, dtype=np.float64)
+            if measurement_noise is not None
+            else np.eye(self.state_dim) * 0.1
+        )
+
+        # Initialize particles
+        self.particles = np.random.multivariate_normal(
+            initial_state, initial_covariance, num_particles
+        )
+        self.weights = np.ones(num_particles) / num_particles
+
+    def predict(self, dt: float, control: dict[str, float] | None = None) -> None:
+        """
+        Predict state forward in time.
+
+        Parameters
+        ----------
+        dt : float
+            Time step in seconds.
+        control : dict, optional
+            Control inputs (e.g., {"velocity": 5.0, "heading": 0.5}).
+        """
+        if not self.enabled:
+            return
+
+        # Add process noise to particles
+        noise = np.random.multivariate_normal(
+            np.zeros(self.state_dim), self.process_noise * dt, self.num_particles
+        )
+        self.particles += noise
+
+        # Apply control if provided (simple constant velocity model)
+        if control is not None:
+            if "velocity" in control and "heading" in control:
+                v = control["velocity"]
+                h = control["heading"]
+                self.particles[:, 0] += v * np.cos(h) * dt  # x
+                self.particles[:, 1] += v * np.sin(h) * dt  # y
+
+    def update(
+        self, measurement: np.ndarray, measurement_covariance: np.ndarray | None = None
+    ) -> None:
+        """
+        Update state estimate with measurement.
+
+        Parameters
+        ----------
+        measurement : np.ndarray
+            Measurement vector.
+        measurement_covariance : np.ndarray, optional
+            Measurement noise covariance (uses default if not provided).
+        """
+        if not self.enabled:
+            return
+
+        measurement = np.asarray(measurement, dtype=np.float64)
+        r_matrix = (
+            measurement_covariance if measurement_covariance is not None else self.measurement_noise
+        )
+
+        # Update weights based on measurement likelihood
+        for i in range(self.num_particles):
+            # Compute likelihood: p(measurement | particle)
+            residual = measurement - self.particles[i, : len(measurement)]
+            likelihood = np.exp(-0.5 * residual.T @ np.linalg.inv(r_matrix) @ residual)
+            self.weights[i] *= likelihood
+
+        # Normalize weights
+        self.weights /= np.sum(self.weights)
+
+        # Resample if effective number of particles is too low
+        effective_particles = 1.0 / np.sum(self.weights**2)
+        if effective_particles < self.num_particles / 2:
+            self._resample()
+
+    def _resample(self) -> None:
+        """Resample particles based on weights."""
+        # Systematic resampling
+        cumulative_weights = np.cumsum(self.weights)
+        step = 1.0 / self.num_particles
+        u = np.random.uniform(0, step)
+
+        new_particles = np.zeros_like(self.particles)
+        j = 0
+        for i in range(self.num_particles):
+            while u > cumulative_weights[j]:
+                j += 1
+            new_particles[i] = self.particles[j]
+            u += step
+
+        self.particles = new_particles
+        self.weights = np.ones(self.num_particles) / self.num_particles
+
+    def get_state(self) -> np.ndarray:
+        """Get current state estimate (weighted mean of particles)."""
+        result = np.average(self.particles, axis=0, weights=self.weights)
+        return np.asarray(result, dtype=np.float64)
+
+    def get_covariance(self) -> np.ndarray:
+        """Get current state covariance."""
+        state = self.get_state()
+        diff = self.particles - state
+        return np.cov(diff.T, aweights=self.weights)
+
+    def reset(
+        self, initial_state: np.ndarray, initial_covariance: np.ndarray | None = None
+    ) -> None:
+        """Reset filter to initial state."""
+        if initial_covariance is None:
+            initial_covariance = np.eye(self.state_dim) * 0.1
+        self.particles = np.random.multivariate_normal(
+            initial_state, initial_covariance, self.num_particles
+        )
+        self.weights = np.ones(self.num_particles) / self.num_particles

simple_autonomous_car/footprint/__init__.py

@@ -0,0 +1,7 @@
+"""Footprint module for vehicle shape representation."""
+
+from simple_autonomous_car.footprint.base_footprint import BaseFootprint
+from simple_autonomous_car.footprint.circular_footprint import CircularFootprint
+from simple_autonomous_car.footprint.rectangular_footprint import RectangularFootprint
+
+__all__ = ["BaseFootprint", "RectangularFootprint", "CircularFootprint"]

simple_autonomous_car/footprint/base_footprint.py

@@ -0,0 +1,128 @@
+"""Base footprint class for vehicle shape representation."""
+
+from abc import ABC, abstractmethod
+
+import numpy as np
+
+
+class BaseFootprint(ABC):
+    """
+    Base class for vehicle footprints.
+
+    A footprint represents the shape of the vehicle in 2D space.
+    It's used to ensure the entire vehicle (not just a point) avoids obstacles.
+
+    Attributes
+    ----------
+    name : str
+        Footprint name/identifier.
+    """
+
+    def __init__(self, name: str = "footprint"):
+        """
+        Initialize base footprint.
+
+        Parameters
+        ----------
+        name : str, default="footprint"
+            Footprint name/identifier.
+        """
+        self.name = name
+
+    @abstractmethod
+    def get_vertices(self, position: np.ndarray, heading: float) -> np.ndarray:
+        """
+        Get footprint vertices in world frame.
+
+        Parameters
+        ----------
+        position : np.ndarray
+            Vehicle position [x, y] in world frame.
+        heading : float
+            Vehicle heading angle in radians.
+
+        Returns
+        -------
+        np.ndarray
+            Array of shape (N, 2) with vertex positions [[x1, y1], [x2, y2], ...].
+        """
+        pass
+
+    @abstractmethod
+    def get_bounding_radius(self) -> float:
+        """
+        Get bounding radius (distance from center to farthest point).
+
+        This is used for efficient collision checking and costmap inflation.
+
+        Returns
+        -------
+        float
+            Bounding radius in meters.
+        """
+        pass
+
+    @abstractmethod
+    def contains_point(self, point: np.ndarray, position: np.ndarray, heading: float) -> bool:
+        """
+        Check if a point is inside the footprint.
+
+        Parameters
+        ----------
+        point : np.ndarray
+            Point [x, y] in world frame.
+        position : np.ndarray
+            Vehicle position [x, y] in world frame.
+        heading : float
+            Vehicle heading angle in radians.
+
+        Returns
+        -------
+        bool
+            True if point is inside footprint, False otherwise.
+        """
+        pass
+
+    def get_inflation_radius(self, padding: float = 0.0) -> float:
+        """
+        Get inflation radius for costmap (bounding radius + padding).
+
+        Parameters
+        ----------
+        padding : float, default=0.0
+            Additional safety padding in meters.
+
+        Returns
+        -------
+        float
+            Inflation radius in meters.
+        """
+        return self.get_bounding_radius() + padding
+
+    def check_collision(self, obstacles: np.ndarray, position: np.ndarray, heading: float) -> bool:
+        """
+        Check if footprint collides with any obstacles.
+
+        Parameters
+        ----------
+        obstacles : np.ndarray
+            Array of obstacle positions [[x1, y1], [x2, y2], ...].
+        position : np.ndarray
+            Vehicle position [x, y] in world frame.
+        heading : float
+            Vehicle heading angle in radians.
+
+        Returns
+        -------
+        bool
+            True if collision detected, False otherwise.
+        """
+        if len(obstacles) == 0:
+            return False
+
+        # Simple collision check: if any obstacle is inside footprint
+        for obstacle in obstacles:
+            if self.contains_point(obstacle, position, heading):
+                return True
+
+        return False

simple_autonomous_car/footprint/circular_footprint.py

@@ -0,0 +1,73 @@
+"""Circular footprint for vehicles."""
+
+import numpy as np
+
+from simple_autonomous_car.footprint.base_footprint import BaseFootprint
+
+
+class CircularFootprint(BaseFootprint):
+    """
+    Circular footprint for vehicles.
+
+    Represents a vehicle as a circle (simpler but less accurate for cars).
+
+    Parameters
+    ----------
+    radius : float
+        Vehicle radius in meters.
+    name : str, default="circular_footprint"
+        Footprint name.
+    """
+
+    def __init__(self, radius: float, name: str = "circular_footprint"):
+        super().__init__(name=name)
+        self.radius = radius
+
+    def get_vertices(self, position: np.ndarray, heading: float) -> np.ndarray:
+        """
+        Get circle vertices (approximated as polygon) in world frame.
+
+        Parameters
+        ----------
+        position : np.ndarray
+            Vehicle position [x, y] in world frame.
+        heading : float
+            Vehicle heading angle (not used for circle, but kept for interface compatibility).
+
+        Returns
+        -------
+        np.ndarray
+            Array of shape (N, 2) with circle vertices (approximated as 16-sided polygon).
+        """
+        # Approximate circle as 16-sided polygon
+        num_vertices = 16
+        angles = np.linspace(0, 2 * np.pi, num_vertices, endpoint=False)
+
+        vertices = position + self.radius * np.column_stack([np.cos(angles), np.sin(angles)])
+
+        return np.asarray(vertices, dtype=np.float64)
+
+    def get_bounding_radius(self) -> float:
+        """Get bounding radius (same as circle radius)."""
+        return self.radius
+
+    def contains_point(self, point: np.ndarray, position: np.ndarray, heading: float) -> bool:
+        """
+        Check if point is inside circle.
+
+        Parameters
+        ----------
+        point : np.ndarray
+            Point [x, y] in world frame.
+        position : np.ndarray
+            Vehicle position [x, y] in world frame.
+        heading : float
+            Vehicle heading angle (not used for circle).
+
+        Returns
+        -------
+        bool
+            True if point is inside circle.
+        """
+        distance = np.linalg.norm(point - position)
+        return bool(distance <= self.radius)

simple_autonomous_car/footprint/rectangular_footprint.py

@@ -0,0 +1,123 @@
+"""Rectangular footprint for vehicles."""
+
+import numpy as np
+
+from simple_autonomous_car.footprint.base_footprint import BaseFootprint
+
+
+class RectangularFootprint(BaseFootprint):
+    """
+    Rectangular footprint for vehicles.
+
+    Represents a vehicle as a rectangle (typical for cars).
+
+    Parameters
+    ----------
+    length : float
+        Vehicle length in meters (front to back).
+    width : float
+        Vehicle width in meters (left to right).
+    center_offset : np.ndarray, optional
+        Offset of footprint center from vehicle position [x, y] in vehicle frame.
+        Default: [0, 0] (center at vehicle position).
+    name : str, default="rectangular_footprint"
+        Footprint name.
+    """
+
+    def __init__(
+        self,
+        length: float,
+        width: float,
+        center_offset: np.ndarray | None = None,
+        name: str = "rectangular_footprint",
+    ):
+        super().__init__(name=name)
+        self.length = length
+        self.width = width
+        self.center_offset = center_offset if center_offset is not None else np.array([0.0, 0.0])
+
+        # Calculate bounding radius (half diagonal)
+        self._bounding_radius = np.sqrt((length / 2) ** 2 + (width / 2) ** 2)
+
+    def get_vertices(self, position: np.ndarray, heading: float) -> np.ndarray:
+        """
+        Get rectangle vertices in world frame.
+
+        Parameters
+        ----------
+        position : np.ndarray
+            Vehicle position [x, y] in world frame.
+        heading : float
+            Vehicle heading angle in radians.
+
+        Returns
+        -------
+        np.ndarray
+            Array of shape (4, 2) with rectangle corners.
+        """
+        # Define rectangle corners in vehicle frame (centered at origin)
+        half_length = self.length / 2
+        half_width = self.width / 2
+
+        corners_vehicle = np.array(
+            [
+                [half_length, half_width],  # Front-right
+                [-half_length, half_width],  # Rear-right
+                [-half_length, -half_width],  # Rear-left
+                [half_length, -half_width],  # Front-left
+            ]
+        )
+
+        # Apply center offset
+        corners_vehicle += self.center_offset
+
+        # Rotate by heading
+        cos_h = np.cos(heading)
+        sin_h = np.sin(heading)
+        rotation_matrix = np.array([[cos_h, -sin_h], [sin_h, cos_h]])
+        corners_rotated = (rotation_matrix @ corners_vehicle.T).T
+
+        # Translate to world position
+        vertices = corners_rotated + position
+
+        return np.asarray(vertices, dtype=np.float64)
+
+    def get_bounding_radius(self) -> float:
+        """Get bounding radius (half diagonal of rectangle)."""
+        return float(self._bounding_radius)
+
+    def contains_point(self, point: np.ndarray, position: np.ndarray, heading: float) -> bool:
+        """
+        Check if point is inside rectangle.
+
+        Parameters
+        ----------
+        point : np.ndarray
+            Point [x, y] in world frame.
+        position : np.ndarray
+            Vehicle position [x, y] in world frame.
+        heading : float
+            Vehicle heading angle in radians.
+
+        Returns
+        -------
+        bool
+            True if point is inside rectangle.
+        """
+        # Transform point to vehicle frame
+        point_relative = point - position
+
+        # Rotate by -heading to align with vehicle frame
+        cos_h = np.cos(-heading)
+        sin_h = np.sin(-heading)
+        rotation_matrix = np.array([[cos_h, -sin_h], [sin_h, cos_h]])
+        point_vehicle = rotation_matrix @ point_relative
+
+        # Apply center offset
+        point_vehicle -= self.center_offset
+
+        # Check if point is within rectangle bounds
+        half_length = self.length / 2
+        half_width = self.width / 2
+
+        return bool(abs(point_vehicle[0]) <= half_length and abs(point_vehicle[1]) <= half_width)

simple_autonomous_car/frames/__init__.py

@@ -0,0 +1,21 @@
+"""Frame conversion utilities for coordinate transformations."""
+
+from simple_autonomous_car.frames.frenet import (
+    FrenetFrame,
+    ego_to_frenet,
+    ego_to_sensor,
+    frenet_to_ego,
+    frenet_to_global,
+    global_to_frenet,
+    sensor_to_ego,
+)
+
+__all__ = [
+    "FrenetFrame",
+    "global_to_frenet",
+    "frenet_to_global",
+    "ego_to_frenet",
+    "frenet_to_ego",
+    "sensor_to_ego",
+    "ego_to_sensor",
+]