Particle_2.py

import numpy as np

class Landmark:
    def __init__(self, mu, covariance, counter, landmark_id):
        self.mu = mu
        self.covariance = covariance
        self.counter = counter
        self.landmark_id = landmark_id

class Particle:
    def __init__(self, initial_pose = None):
        if initial_pose is None:
            self.pose = np.zeros(3) 
        else:
            self.pose = np.array(initial_pose, dtype=float)
        self.landmarks = []
        self.weight = 1 
        self.n_landmarks = 0

    def predict_pose(self, u, dt):
        v, w = u 
        theta = self.pose[2]

        if abs(w) < 1e-6: 
            dx = v * dt * np.cos(theta)
            dy = v * dt * np.sin(theta)
            dtheta = 0
        else:
            v_w = v / w
            dx = v_w * (np.sin(theta + w*dt) - np.sin(theta))
            dy = v_w * (np.cos(theta) - np.cos(theta + w*dt))
            dtheta = w * dt

        x_hat = self.pose + np.array([dx, dy, dtheta])
        x_hat[2] = self.wrap(x_hat[2])

        return x_hat

    def predict_measurement(self, landmark: Landmark, robot_pose):
       
        if robot_pose is None:
            robot_pose = self.pose

        rx, ry, rtheta = robot_pose 
        lx, ly = landmark.mu

        dx = lx - rx
        dy = ly - ry
        r = np.sqrt(dx*dx + dy*dy)

        phi = np.arctan2(dy, dx) - rtheta
        phi = self.wrap(phi)

        return np.array([r, phi])
    
    def measurement_jacobian_pose(self, landmark: Landmark, robot_pose = None):

        if robot_pose is None:
            robot_pose = self.pose

        rx, ry, rtheta = robot_pose
        lx, ly = landmark.mu

        dx = lx - rx
        dy = ly - ry
        q = dx*dx + dy*dy
        r = np.sqrt(q)

        H_x = np.array([
            [-dx/r, -dy/r, 0],
            [dy/q, -dx/q, -1]
        ])

        return H_x
    
    def measurement_jacobian_landmark(self, landmark: Landmark, robot_pose = None):
        
        if robot_pose is None:
            robot_pose = self.pose

        rx, ry, rtheta = robot_pose
        lx, ly = landmark.mu

        dx = lx - rx
        dy = ly - ry
        q = dx*dx + dy*dy
        r = np.sqrt(q)

        H_m = np.array([
            [dx/r, dy/r],
            [-dy/q, dx/q]
        ])

        return H_m

    def add_landmark(self, landmark: Landmark):
        self.landmarks.append(landmark)
        self.n_landmarks += 1
    
    def incremental_proposal_update(self, z, landmark, x_hat, mu_current, Sigma_current, Q, R_inv):

        z_bar = self.predict_measurement(landmark, x_hat)
        
        H_x = self.measurement_jacobian_pose(landmark, x_hat)
        H_m = self.measurement_jacobian_landmark(landmark, x_hat)
        
        H_m_cov = H_m @ landmark.covariance
        Q_j = Q + H_m_cov @ H_m.T
        Q_j_inv = np.linalg.inv(Q_j)

        Sigma_inv_current = np.linalg.inv(Sigma_current)
        H_x_Q = H_x.T @ Q_j_inv
        Sigma_new = np.linalg.inv(H_x_Q @ H_x + Sigma_inv_current)
        
        nu = z - z_bar
        nu[1] = self.wrap(nu[1])
        
        mu_new = mu_current + Sigma_new @ H_x_Q @ nu
        
        return mu_new, Sigma_new, Q_j, Q_j_inv

    def initialize_landmark(self, z, Q, landmark_id):
        r, phi = z
        rx, ry, rtheta = self.pose

        angle = phi + rtheta
        cos_angle = np.cos(angle)
        sin_angle = np.sin(angle)
        
        lx = rx + r * cos_angle
        ly = ry + r * sin_angle
        mu = np.array([lx, ly])

        H_inv = np.array([
            [cos_angle, -r * sin_angle],
            [sin_angle, r * cos_angle]
        ])

        covariance = H_inv @ Q @ H_inv.T

        landmark = Landmark(mu, covariance, counter=1, landmark_id=landmark_id)
        self.add_landmark(landmark)

        return landmark
    
    def get_landmark(self, landmark_id):
        for l in self.landmarks:
            if l.landmark_id == landmark_id:
                return l
        return None
    
    def update_landmark(self, landmark_id, z, Q, Q_inv=None):
        landmark = self.get_landmark(landmark_id)

        if landmark is None:
            print(f"Landmark {landmark_id} not found!")
            return
        
        z_hat = self.predict_measurement(landmark, self.pose)
        H_m = self.measurement_jacobian_landmark(landmark, self.pose)
        Q_j = Q + H_m @ landmark.covariance @ H_m.T

        K = landmark.covariance @ H_m.T @ np.linalg.inv(Q_j)

        nu = z - z_hat
        nu[1] = self.wrap(nu[1])
        landmark.mu += K @ nu

        I_minus_KH = np.eye(2) - K @ H_m
        landmark.covariance = I_minus_KH @ landmark.covariance

        landmark.counter += 1

    def importance_weight(self, z, landmark, Q, R):
        z_hat = self.predict_measurement(landmark, self.pose)
        H_x = self.measurement_jacobian_pose(landmark, self.pose)
        H_m = self.measurement_jacobian_landmark(landmark, self.pose)

        L = H_x @ R @ H_x.T + H_m @ landmark.covariance @ H_m.T + Q

        nu = z - z_hat
        nu[1] = self.wrap(nu[1])

        det = np.linalg.det(2 * np.pi * L)
        L_inv = np.linalg.inv(L)
        exponent = -0.5 * (nu @ L_inv @ nu)

        w = np.exp(exponent) / np.sqrt(det)
        return w
    
    def update_landmark_counters(self, observed_landmarks_ids, sensor_range):
        remove = []
        observed_set = set(observed_landmarks_ids)

        for i, landmark in enumerate(self.landmarks):
            if landmark.landmark_id in observed_set:
                continue

            dx = landmark.mu[0] - self.pose[0]
            dy = landmark.mu[1] - self.pose[1]
            r = np.sqrt(dx*dx + dy*dy)

            if r <= sensor_range:
                landmark.counter -= 1
                if landmark.counter < 0:
                    remove.append(i)

        for i in reversed(remove):
            del self.landmarks[i]
            self.n_landmarks -= 1

    @staticmethod
    def wrap(angle):
        return (angle + np.pi) % (2*np.pi) - np.pi