FastSLAM_1.py

import numpy as np 
from FastSLAM.Particle_1 import Particle, Landmark

class FastSLAM:

    def __init__(self, initial_pose, n_particles, R, Q, sensor_range, p0=0.01): 
        self.initial_pose = initial_pose
        self.M = n_particles
        self.R = R
        self.Q = Q
        self.sensor_range = sensor_range
        self.p0 = p0
        self.particles =[Particle(self.initial_pose) for _ in range(n_particles)]
        self.next_landmark_id = 0


    def update(self, z, u, dt):
        Y = []
        weights = []

        for p in range(self.M):
            particle = self.particles[p]
            observed_landmarks = []

            if len(z) == 0:
                x = particle.predict_pose(u, dt)
                noise = np.random.multivariate_normal(np.zeros(3), self.R)
                particle.pose = x + noise
                particle.pose[2] = particle.wrap(particle.pose[2])
                particle.weight = 1.0

            else:
                best_likelihood = -np.inf
                best_measurement = 0
                best_landmark = None
                best_proposal_params = None
            
                for i, z_i in enumerate(z):
                    likelihoods = []
                    proposal_params = []

                    for j, landmark in enumerate(particle.landmarks):
                        mu, Sigma = particle.proposal_distribution(z_i, landmark, u, dt, self.R, self.Q)
                        proposal_params.append((mu, Sigma))

                        previous_pose = particle.pose.copy() 
                        particle.sample_proposal(mu, Sigma)
                        pi_j = particle.correspondence_likelihood(z_i, landmark, self.Q)
                        likelihoods.append(pi_j)
                        particle.pose = previous_pose

                    likelihoods.append(self.p0)
                    c_hat = np.argmax(likelihoods)
                    max_likelihood = likelihoods[c_hat]

                    if max_likelihood > best_likelihood:
                        best_likelihood = max_likelihood
                        best_measurement = i
                        best_landmark = c_hat
                        if c_hat < len(particle.landmarks):
                            best_proposal_params = proposal_params[c_hat]

                z_best = z[best_measurement]
                c_best = best_landmark

                if c_best == len(particle.landmarks):
                    x = particle.predict_pose(u, dt)
                    noise = np.random.multivariate_normal(np.zeros(3), self.R)
                    particle.pose = x + noise
                    particle.pose[2] = particle.wrap(particle.pose[2])

                    particle.initialize_landmark(z_best, self.Q, self.next_landmark_id)
                    observed_landmarks.append(self.next_landmark_id)
                    self.next_landmark_id += 1
                    particle.weight = self.p0

                else:
                    best_landmark = particle.landmarks[c_best]
                    mu, Sigma = best_proposal_params
                    particle.sample_proposal(mu, Sigma)
                    particle.update_landmark(best_landmark.landmark_id, z_best, self.Q)
                    observed_landmarks.append(best_landmark.landmark_id)
                    particle.weight = particle.importance_weight(z_best, best_landmark, self.Q, self.R)

                for i, z_i in enumerate(z):

                    if i == best_measurement:
                        continue

                    likelihoods = []

                    for j, landmark in enumerate(particle.landmarks):
                        pi_j = particle.correspondence_likelihood(z_i, landmark, self.Q)
                        likelihoods.append(pi_j)

                    likelihoods.append(self.p0)

                    c_hat = np.argmax(likelihoods)

                    if c_hat == len(particle.landmarks):

                        particle.initialize_landmark(z_i, self.Q, self.next_landmark_id)
                        observed_landmarks.append(self.next_landmark_id)
                        self.next_landmark_id += 1

                        particle.weight *= self.p0

                    else:
                        landmark = particle.landmarks[c_hat]
                        particle.update_landmark(landmark.landmark_id, z_i, self.Q)
                        observed_landmarks.append(landmark.landmark_id)

                        w_meas = particle.measurement_likelihood(z_i, landmark, self.Q)
                        particle.weight *= w_meas

            particle.update_landmark_counters(observed_landmarks, self.sensor_range) 
            Y.append(particle)
            weights.append(particle.weight)

        weights = np.array(weights)
        if weights.sum() > 0:
            weights = weights / weights.sum()
        else:
            weights = np.ones(self.M) / self.M

        new_particles = []
        indices = np.random.choice(self.M, size=self.M, p=weights, replace=True)

        for i in indices:
            new_particle = self.copy_particle(Y[i])
            new_particle.weight = 1.0
            new_particles.append(new_particle)

        self.particles = new_particles

    def copy_particle(self, particle):

        new_particle = Particle(particle.pose.copy())
        new_particle.weight = particle.weight
        new_particle.n_landmarks = particle.n_landmarks

        for landmark in particle.landmarks:

            new_landmark = Landmark(
                mu=landmark.mu.copy(),
                covariance=landmark.covariance.copy(), 
                counter=landmark.counter,
                landmark_id=landmark.landmark_id
            )
            new_particle.landmarks.append(new_landmark)

        return new_particle
    
    def get_map_estimate(self):

        poses = np.array([p.pose for p in self.particles])

        x_est = np.mean(poses[:, 0])
        y_est = np.mean(poses[:, 1])
        
        sin_theta = np.mean(np.sin(poses[:, 2]))
        cos_theta = np.mean(np.cos(poses[:, 2]))
        theta_est = np.arctan2(sin_theta, cos_theta)
        
        pose = np.array([x_est, y_est, theta_est])

        landmark_dict = {}
        
        for particle in self.particles:
            for lm in particle.landmarks:
                if lm.landmark_id not in landmark_dict:
                    landmark_dict[lm.landmark_id] = []
                landmark_dict[lm.landmark_id].append((lm.mu, lm.covariance, lm.counter))
        
        landmarks_list = []
        for lm_id, observations in landmark_dict.items():
            n_obs = len(observations)
            
            if n_obs > 0:
                mu_avg = sum(obs[0] for obs in observations) / n_obs
                cov_avg = sum(obs[1] for obs in observations) / n_obs
                counter_avg = int(sum(obs[2] for obs in observations) / n_obs)
                
                landmarks_list.append({
                    'id': lm_id,
                    'mu': mu_avg,
                    'covariance': cov_avg,
                    'counter': counter_avg
                })
    
        landmarks_list.sort(key=lambda x: x['id'])
        landmarks = np.array([lm['mu'] for lm in landmarks_list]) if landmarks_list else np.array([])
    
        return pose, landmarks