Robot.py

import numpy as np 

class Robot:
    def __init__(self, alpha1, alpha2, alpha3, alpha4, 
                 wheel_base=0.5, wheel_radius=0.1, ticks_per_rev=360, 
                 dt=0.5, initial_pose=(0, 0, 0)):
        
        self.x, self.y, self.theta = initial_pose 
        
        self.x_est, self.y_est, self.theta_est = initial_pose 

        self.B = wheel_base      
        self.R = wheel_radius    
        self.Et = ticks_per_rev  
        self.dt = dt             

        self.alpha1, self.alpha2, self.alpha3, self.alpha4 = alpha1, alpha2, alpha3, alpha4 
        
        self.ground_truth = [[self.x, self.y, self.theta]] 
        self.noisy = [[self.x_est, self.y_est, self.theta_est]] 

    def step(self, v, omega):
        theta_past = self.theta 
        self.x += np.cos(theta_past) * self.dt * v 
        self.y += np.sin(theta_past) * self.dt * v 
        self.theta = self.wrap(self.theta + omega * self.dt) 
        self.ground_truth.append([self.x, self.y, self.theta]) 

        w_l_ideal = (v - (omega * self.B / 2.0)) / self.R 
        w_r_ideal = (v + (omega * self.B / 2.0)) / self.R 

        sigma_l = self.alpha1 * abs(w_l_ideal) + self.alpha2 * abs(omega) 
        sigma_r = self.alpha3 * abs(w_r_ideal) + self.alpha4 * abs(omega) 
        w_l_noisy = w_l_ideal + np.random.normal(0, sigma_l) 
        w_r_noisy = w_r_ideal + np.random.normal(0, sigma_r) 

        E_L = (w_l_noisy * self.Et * self.dt) / (2 * np.pi) 
        E_R = (w_r_noisy * self.Et * self.dt) / (2 * np.pi) 

        w_t_L = (2 * np.pi * E_L) / (self.Et * self.dt) 
        w_t_R = (2 * np.pi * E_R) / (self.Et * self.dt) 
        
        v_t = (w_t_R * self.R + w_t_L * self.R) / 2.0 
        w_t = (w_t_R * self.R - w_t_L * self.R) / self.B 

        theta_est_past = self.theta_est 
        self.x_est += v_t * self.dt * np.cos(theta_est_past) 
        self.y_est += v_t * self.dt * np.sin(theta_est_past) 
        self.theta_est = self.wrap(self.theta_est + w_t * self.dt) 
        self.noisy.append([self.x_est, self.y_est, self.theta_est]) 

        return E_L, E_R

    def wrap(self, angle): 
        return (angle + np.pi) % (2 * np.pi) - np.pi