Code_Problem_Set_9_Math.py

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def bayesian_tracking_sim(L, p_real, p_est, e_real, e_est, init_type='uniform', title_str='Simulation'):
    
    # --- Configuration ---
    T = 100         # Number of simulation steps
    N = 100         # Number of discrete steps around circle
    
    W = np.zeros((T + 1, N))
    

    if init_type == 'uniform':
        W[0, :] = 1.0 / N
    elif init_type == 'perfect':
        
        start_idx = int(round(N / 4))
        W[0, start_idx] = 1.0
    elif init_type == 'upper_half':
        
        angles = 2 * np.pi * np.arange(N) / N
       
        upper_indices = np.where(np.sin(angles) > 0)[0]
        W[0, upper_indices] = 1.0 / len(upper_indices)
    else:
        
        W[0, :] = 1.0 / N

    
    loc = np.zeros(T + 1, dtype=int)
    loc[0] = int(round(N / 4)) # Initial position 
    
    
    angles_vec = 2 * np.pi * np.arange(N) / N
    x_loc_hypo = np.cos(angles_vec)
    y_loc_hypo = np.sin(angles_vec)
    
   
    for t in range(T):
     
        if np.random.rand() < p_real:
            loc[t+1] = (loc[t] + 1) % N
        else:
            loc[t+1] = (loc[t] - 1) % N
     
        #Cartetian coordinates of actual position
        angle_actual = 2 * np.pi * loc[t+1] / N
        x_actual = np.cos(angle_actual)
        y_actual = np.sin(angle_actual)
        
        # Distance to sensor
        dist_true = np.sqrt((L - x_actual)**2 + y_actual**2)
        
        
        noise = e_real * 2 * (np.random.rand() - 0.5)
        dist_measured = dist_true + noise
        
        # ---------------------------
        #Updated Belief State

        #Moves right prob
        w_right = np.roll(W[t, :], 1)  
        
        #Moves left prob 
        w_left = np.roll(W[t, :], -1) 
        
        #prediction 
        predict_W = p_est * w_right + (1 - p_est) * w_left
        
        #Measurement Update
        dist_hypo_all = np.sqrt((L - x_loc_hypo)**2 + y_loc_hypo**2)
        
       
        diff = np.abs(dist_measured - dist_hypo_all)
        
        likelihood = np.zeros(N)
        mask = diff <= e_est
        likelihood[mask] = 1.0 / (2 * e_est)
        
        # Combine prediction and measurement
        W[t+1, :] = likelihood * predict_W
        
        #  Normalization
        norm_const = np.sum(W[t+1, :])
        
        if norm_const > 1e-10:
            W[t+1, :] /= norm_const
        else:
            #If prob equal zero something fail 
            print(f"Warning: Model inconsistency at step {t+1}. Resetting to uniform.")
            W[t+1, :] = 1.0 / N

    # --- Graph ---
    fig = plt.figure(figsize=(10, 6))
    ax = fig.add_subplot(111, projection='3d')
    
    x_steps = np.arange(N)
    t_steps = np.arange(T + 1)
    X, Y = np.meshgrid(x_steps, t_steps)
    
    # probability surface
    surf = ax.plot_surface(X, Y, W, cmap='viridis', edgecolor='none', alpha=0.9)
    
    z_lift = np.max(W) * 1.05
    ax.plot(loc, t_steps, np.ones_like(loc) * z_lift, color='red', linewidth=2, label='Actual Position')
    
    ax.set_xlabel('Position $x_k$ (Index)')
    ax.set_ylabel('Time Step $k$')
    ax.set_zlabel('Probability $P(x_k | y_{1:k})$')
    ax.set_title(title_str)
    
    # Angles
    ax.view_init(elev=40, azim=-30)
    plt.legend()
    plt.tight_layout()
    plt.show()


if __name__ == "__main__":
    
    print("Running simulations...")
    
    # Common parameters
    N = 100
    x0 = N / 4
    e_default = 0.5
    
    # For a) 
    
    # 1) L=2, p=0.95
    #bayesian_tracking_sim(L=2.0, p_real=0.95, p_est=0.95, 
                        #   e_real=0.5, e_est=0.5, init_type='uniform',
                        #   title_str='1a): L=2, p=0.95')

    # 2) L=0.4, p=0.35
    #bayesian_tracking_sim(L=0.4, p_real=0.35, p_est=0.35, 
    #                       e_real=0.5, e_est=0.5, init_type='uniform',
    #                       title_str='2a: L=0.4, p=0.35 ')

    # 3) L=0, p=0.5
    # Note: With L=0 (sensor at center), distance is constant. Sensor provides NO info.
    #bayesian_tracking_sim(L=0.0, p_real=0.5, p_est=0.5, 
    #                       e_real=0.5, e_est=0.5, init_type='uniform',
    #                       title_str='3a): L=0, p=0.5')
    
    
    # b) 
    # Real parameters for part b
    L_real_b = 2.0
    p_real_b = 0.55
    e_real_b = 0.5
    
    #a) Estimator: p_hat = 0.1, e_hat = 0.8
   # bayesian_tracking_sim(L=L_real_b, p_real=p_real_b, p_est=0.1, 
    #                       e_real=e_real_b, e_est=0.8, init_type='uniform',
    #                       title_str='b1): Wrong p (0.1 vs 0.55), Loose e')

    #Estimator: p_hat = 0.85, e_hat = 0.5
    #bayesian_tracking_sim(L=L_real_b, p_real=p_real_b, p_est=0.85, 
    #                     e_real=e_real_b, e_est=0.5, init_type='uniform',
    #                     title_str='b2):  p (0.85 vs 0.55) ')

    
    # --- Part (c): Initial Conditions ---
    # Using L=2, p=0.55 case as base
    
    # (A) Perfect Initial Knowledge
    #bayesian_tracking_sim(L=2.0, p_real=0.55, p_est=0.55, 
    #                       e_real=0.5, e_est=0.5, init_type='perfect',
    #                       title_str='Q9.1c(A): Perfect Initial Knowledge')
                          
    # (B) Upper Half Initial Knowledge
    bayesian_tracking_sim(L=2.0, p_real=0.55, p_est=0.55, 
                           e_real=0.5, e_est=0.5, init_type='upper_half',
                           title_str='Q9.1c(B): Upper Half Prior')