plottingCPLT.m

function EKF

    % Close all windows and reset workspace
    clear all;
    close all;

    % Robot & Environment configuration
    filename = 'simulated_data.txt';    % better not to edit the filename for the simulated data
    baseline = 0.5;                     % distance between the contact points of the robot wheels [m]
    focal = 600;                        % focal length of the camera [pixel]
    camera_height = 10;                 % how far is the camera w.r.t. the world plane [m]
    pTpDistance = 0.6;                  % distance between the two observed points on the robot [m]

    % Read & parse data from the input file
    DATA = dlmread(filename,';');
    odometry  = [DATA(:,3),DATA(:,4)];  % sdx, ssx  
    camera_readings = [DATA(:,11),DATA(:,12),DATA(:,13),DATA(:,14)]; % u1, v1, u2, v2

    steps = size(odometry,1); 
    kalmanstate = zeros(steps,3);       % memory for the 3 state variables, for each timestamp
    kalmanstatecov = zeros(3,3,steps);  % memory for the covariance of the 3 state variables, for each timestamp
    prediction = zeros(steps,3);        % memory for the 3 predicted state variables, for each timestamp
    predictedcov = zeros(3,3,steps);    % memory for the covariance of the 3 predicted state variables, for each timestamp

    % Initializations 
    kalmanstate(1,:) = [0, 0, 0];       % initial state estimate
    kalmanstatecov(:,:,1) = eye(3) * 0.05;      % initial covariance set to identity matrix

    % configuration of uncertainties
    R = eye(3) * 0.01;   
    Q = eye(4) * 0.01;   

    % Compute symbolic jacobians once and for all!
    H = calculateSymbolicH; 

    % Create a figure for animation
    figure(3);
    hold on;
    axis equal;
    xlabel('X Position [m]');
    ylabel('Y Position [m]');
    title('Robot Motion Animation');
    
    % Initialize the plot handles
    h_robot = plot(kalmanstate(1,1), kalmanstate(1,2), 'g+');
    h_pred = plot(prediction(1,1), prediction(1,2), 'c+');
    h_cam = plot(camera_readings(1,1)*camera_height/focal, -camera_readings(1,2)*camera_height/focal, 'm*');
    h_cov = plot_gaussian_ellipsoid(kalmanstate(1,1:2), kalmanstatecov(1:2,1:2,1), 3.0);

    % Batch-simulate in this loop the evolution of the system and your KF estimate
    for i = 1:steps-1
        fprintf('Filtering step %d/%d; Current uncertainty (x,y,ang): %0.6f %0.6f %0.6f\n', i, steps, kalmanstatecov(1,1,i), kalmanstatecov(2,2,i), kalmanstatecov(3,3,i)); 
        
        syms symb_baseline symb_sdx symb_ssx
        g = forward_stato;
        G = calculateSymbolicG(g); 

        [kalmanstate(i+1,:), kalmanstatecov(:,:,i+1), prediction(i+1,:), predictedcov(:,:,i+1)] = execute_kalman_step(kalmanstate(i,:), kalmanstatecov(:,:,i), odometry(i,:), camera_readings(i,:), baseline, R, Q, focal, camera_height, pTpDistance, G, H, g);

        % Update the plot handles
        set(h_robot, 'XData', kalmanstate(1:i+1,1), 'YData', kalmanstate(1:i+1,2));
        set(h_pred, 'XData', prediction(1:i+1,1), 'YData', prediction(1:i+1,2));
        set(h_cam, 'XData', camera_readings(1:i+1,1)*camera_height/focal, 'YData', -camera_readings(1:i+1,2)*camera_height/focal);

        % Update the covariance ellipses
        delete(h_cov);
        h_cov = plot_gaussian_ellipsoid(kalmanstate(i+1,1:2), kalmanstatecov(1:2,1:2,i+1), 3.0);

        pause(1); % Pause to create animation effect
    end

    hold off; 
end


function [filtered_state, filtered_sigma, predicted_state, predicted_sigma] = execute_kalman_step(current_state, current_state_sigma, odometry, camera_readings, baseline, R, Q, focal, camera_height, pTpDistance, G, H, g)

    filtered_state = zeros(1,3);       % memory for the 3 state variables, for each timestamp
    filtered_sigma = zeros(3,3);  % memory for the covariance of the 3 state variables, for each timestamp
    predicted_state = zeros(1,3);        % memory for the 3 predicted state variables, for each timestamp
    expected_camera_readings = zeros(1,4);  % memory for the expected camera readings
    

    syms symb_focal symb_pTpDistance symb_camera_height symb_x symb_y symb_theta
    H = eval(subs(H, [symb_focal, symb_pTpDistance, symb_camera_height, symb_x, symb_y, symb_theta], {focal, pTpDistance, camera_height, current_state(1), current_state(2), current_state(3)}));

    %predicted_state = current_state + [odometry(1)*cos(current_state(3)), odometry(2)*sin(current_state(3)), odometry(1)/baseline - odometry(2)/baseline];
   
    % Generate the expected new pose, based on the previous pose and the controls
    syms symb_x symb_y symb_theta symb_baseline symb_sdx symb_ssx
    predicted_state(1,:) = eval(subs(g, [symb_x,symb_y,symb_theta,symb_baseline,symb_sdx,symb_ssx],[current_state(1), current_state(2), ...
        current_state(3), baseline, odometry(1),odometry(2)]));


    syms symb_x symb_y symb_theta symb_baseline symb_sdx symb_ssx        
    G = eval(subs(G,[symb_x,symb_y,symb_theta,symb_baseline,symb_sdx,symb_ssx],[current_state(1), current_state(2), ...
        current_state(3), baseline, odometry(1),odometry(2)]));

    predicted_sigma = G * current_state_sigma * G' + R;

    % then integrate the expected new pose with the measurements
    K = predicted_sigma * H' / (H * predicted_sigma * H' + Q);
    expected_camera_readings(1,1) = focal * (current_state(1) + pTpDistance/2) / camera_height;
    expected_camera_readings(1,2) = focal * (current_state(2) + pTpDistance/2) / camera_height;
    expected_camera_readings(1,3) = focal * (current_state(1) - pTpDistance/2) / camera_height;
    expected_camera_readings(1,4) = focal * (current_state(2) - pTpDistance/2) / camera_height;
    filtered_state = predicted_state' + K * (camera_readings' - expected_camera_readings');
    filtered_sigma = (eye(size(K,1)) - K * H) * predicted_sigma;
end


function H = calculateSymbolicH
    % *** H MATRIX STEP, CALCULATION OF DERIVATIVES aka JACOBIANS ***
    % Please write here the measurement equation(s)
    syms symb_focal symb_x symb_y symb_theta symb_pTpDistance symb_camera_height
    u1 = symb_focal * symb_x / symb_camera_height;
    v1 = symb_focal * symb_y / symb_camera_height;
    u2 = (symb_focal * (symb_x + symb_pTpDistance * cos(symb_theta))) / symb_camera_height;
    v2 = (symb_focal * (symb_y + symb_pTpDistance * sin(symb_theta))) / symb_camera_height;
    % derivatives in Matlab can be calculted in this compact way.
    H = jacobian([u1; v1; u2; v2], [symb_x, symb_y, symb_theta]);
end


function nuovo_stato = forward_stato
    syms symb_ssx symb_sdx symb_baseline symb_x symb_y symb_theta %ricorda symb_theta è l'alfa dello stato del robot, non ri quanto ruota wrt il cir!
    theta_cir = (symb_ssx + symb_sdx)/symb_baseline;

    xt = (symb_baseline/2 + (symb_ssx+symb_baseline)/(symb_ssx-symb_sdx) * sin(theta_cir) + symb_x * cos(theta_cir) + symb_y * sin(theta_cir));
    yt = (symb_baseline/2 + (symb_ssx+symb_baseline)/(symb_ssx-symb_sdx) * (cos(theta_cir)-1) - symb_x * sin(theta_cir) + symb_y * cos(theta_cir));
    thetat = atan2(sin(symb_theta) - theta_cir, cos(theta_cir - symb_theta));
    nuovo_stato = [xt; yt; thetat];
end


function G = calculateSymbolicG(symb_state)

    syms symb_ssx symb_sdx symb_baseline symb_x symb_y symb_theta
    
    G = jacobian(symb_state,[symb_x,symb_y,symb_theta]);
end