Otonom Sistemler

# Genişletilmiş Kalman Filtresi ile Sensör Füzyon
import numpy as np
from filterpy.kalman import ExtendedKalmanFilter as EKF

class VehicleEKF(EKF):
    def __init__(self, dt):
        super().__init__(dim_x=4, dim_z=2)
        self.dt = dt
        
        # Durum geçiş matrisi
        self.F = np.array([[1, 0, self.dt, 0],
                          [0, 1, 0, self.dt],
                          [0, 0, 1, 0],
                          [0, 0, 0, 1]])
        
        # Ölçüm matrisi
        self.H = np.array([[1, 0, 0, 0],
                          [0, 1, 0, 0]])
        
        # Gürültü matrisleri
        self.Q *= 0.1  # Süreç gürültüsü
        self.R = np.eye(2) * 0.1  # Ölçüm gürültüsü
        
    def predict(self):
        # EKF prediction step
        self.x = np.dot(self.F, self.x)
        self.P = np.dot(self.F, np.dot(self.P, self.F.T)) + self.Q
        
    def update(self, gps_data, imu_data):
        # GPS ve IMU verilerini birleştir
        z = np.array([gps_data['x'], gps_data['y']])
        
        # EKF update step
        y = z - np.dot(self.H, self.x)
        S = np.dot(self.H, np.dot(self.P, self.H.T)) + self.R
        K = np.dot(self.P, np.dot(self.H.T, np.linalg.inv(S)))
        
        self.x = self.x + np.dot(K, y)
        self.P = self.P - np.dot(K, np.dot(self.H, self.P))

# D* Lite Algoritması - Dinamik Yol Planlama
import heapq
from dataclasses import dataclass

@dataclass
class Node:
    x: int
    y: int
    g: float = float('inf')
    rhs: float = float('inf')
    
    def __lt__(self, other):
        return False

class DStarLite:
    def __init__(self, start, goal, grid):
        self.start = start
        self.goal = goal
        self.grid = grid
        self.U = []  # Priority queue
        self.nodes = {}
        self.km = 0
    
    def heuristic(self, a, b):
        return abs(a.x - b.x) + abs(a.y - b.y)
    
    def get_node(self, x, y):
        if (x, y) not in self.nodes:
            self.nodes[(x, y)] = Node(x, y)
        return self.nodes[(x, y)]
    
    def calculate_key(self, s):
        return [
            min(s.g, s.rhs) + self.heuristic(self.start, s) + self.km,
            min(s.g, s.rhs)
        ]
    
    def update_vertex(self, u):
        if u != self.goal:
            u.rhs = min([
                self.get_node(x, y).g + 1
                for (x, y) in self.neighbors(u)
                if self.is_free(x, y)
            ])
        
        # Priority queue'dan çıkar
        self.U = [item for item in self.U if item[1] != u]
        
        if u.g != u.rhs:
            heapq.heappush(self.U, (self.calculate_key(u), u))
    
    def compute_shortest_path(self):
        while self.U and (
            self.U[0][0] < self.calculate_key(self.start) or
            self.start.rhs != self.start.g
        ):
            k_old, u = heapq.heappop(self.U)
            
            if k_old > self.calculate_key(u):
                heapq.heappush(self.U, (self.calculate_key(u), u))
            elif u.g > u.rhs:
                u.g = u.rhs
                for (x, y) in self.neighbors(u):
                    self.update_vertex(self.get_node(x, y))
            else:
                u.g = float('inf')
                self.update_vertex(u)
                for (x, y) in self.neighbors(u):
                    self.update_vertex(self.get_node(x, y))

# Pure Pursuit Algoritması ve Ackermann Kinematik
import math
from dataclasses import dataclass

@dataclass
class VehicleParams:
    L: float  # Dingil mesafesi (m)
    track: float  # İz genişliği (m)
    max_steer: float  # Max direksiyon açısı (rad)

def pure_pursuit_steer(px, py, yaw, gx, gy, params, Ld):
    """
    Pure Pursuit yönlendirme açısı hesaplama
    (px,py,yaw): araç pozisyonu ve yaw açısı  
    (gx,gy): hedef nokta
    Ld: look-ahead mesafesi
    """
    # Araca göre hedef nokta
    dx, dy = gx - px, gy - py
    
    # Gövde çerçevesine döndür
    x_l = math.cos(yaw) * dx + math.sin(yaw) * dy
    y_l = -math.sin(yaw) * dx + math.cos(yaw) * dy
    
    # Alpha açısı (hedef noktaya olan açı)
    alpha = math.atan2(y_l, x_l)
    
    # Pure Pursuit formülü
    delta = math.atan2(2.0 * params.L * math.sin(alpha), 
                      max(1e-6, Ld))
    
    # Direk