diff --git a/.gitattributes b/.gitattributes
new file mode 100644
index 0000000..dfe0770
--- /dev/null
+++ b/.gitattributes
@@ -0,0 +1,2 @@
+# Auto detect text files and perform LF normalization
+* text=auto
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..8448974
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,5 @@
+
+*.asv
+simulation/worlds/.desk.wbproj
+simulation/controllers/first_controller/test_plotting.m
+simulation/controllers/first_controller/manual.mat
diff --git a/simulation/controllers/first_controller/Initialize.m b/simulation/controllers/first_controller/Initialize.m
new file mode 100644
index 0000000..a4c15bd
--- /dev/null
+++ b/simulation/controllers/first_controller/Initialize.m
@@ -0,0 +1,58 @@
+wb_console_print(sprintf('Initializing...\n'), WB_STDOUT);
+
+%% LEFT, RIGHT MOTORS
+wb_console_print(sprintf('Checking Motors'), WB_STDOUT);
+motor = [];
+motor_names = [ "left_motor", "right_motor" ];
+for i = 1:2
+    motor(i) = wb_robot_get_device(convertStringsToChars(motor_names(i)));
+    wb_motor_set_position(motor(i), inf);
+    wb_motor_set_velocity(motor(i), 0.0);
+end
+wb_console_print('DONE', WB_STDOUT);
+
+%% LEFT, RIGHT ENCODERS
+wb_console_print(sprintf('Checking Encoders'), WB_STDOUT);
+ps = [];
+ps_names = [ "left_ps", "right_ps" ];
+for i = 1:2
+    ps(i) = wb_robot_get_device(convertStringsToChars(ps_names(i)));
+    wb_position_sensor_enable(ps(i), TIME_STEP);
+end
+wb_console_print('DONE', WB_STDOUT);
+
+%% RANGE FINDER (TOF)
+wb_console_print(sprintf('Checking TOF'), WB_STDOUT);
+tof_name = "tof";
+tof = wb_robot_get_device(convertStringsToChars(tof_name));
+wb_range_finder_enable(tof, TIME_STEP);
+wb_console_print('DONE', WB_STDOUT);
+
+%% IMU
+% GYRO
+wb_console_print(sprintf('Checking IMU (Gyro)'), WB_STDOUT);
+imu_gyro_name = "imu_gyro";
+imu_gyro = wb_robot_get_device(convertStringsToChars(imu_gyro_name));
+wb_gyro_enable(imu_gyro, TIME_STEP);
+wb_console_print('DONE', WB_STDOUT);
+
+% ACC
+wb_console_print(sprintf('Checking IMU (Acc)'), WB_STDOUT);
+imu_acc_name = "imu_acc";
+imu_acc = wb_robot_get_device(convertStringsToChars(imu_acc_name));
+wb_accelerometer_enable(imu_acc, TIME_STEP);
+wb_console_print('DONE', WB_STDOUT);
+
+% MAG
+wb_console_print(sprintf('Checking IMU (Mag)'), WB_STDOUT);
+imu_mag_name = "imu_mag";
+imu_mag = wb_robot_get_device(convertStringsToChars(imu_mag_name));
+wb_inertial_unit_enable(imu_mag, TIME_STEP);
+wb_console_print('DONE', WB_STDOUT);
+
+%% KEYBOARD INPUT
+wb_console_print(sprintf('Checking Keyboard'), WB_STDOUT);
+wb_keyboard_enable(TIME_STEP)
+wb_console_print('DONE', WB_STDOUT);
+
+wb_console_print(sprintf('Initialization Complete.\n'), WB_STDOUT);
\ No newline at end of file
diff --git a/simulation/controllers/first_controller/Mode_Autonomous.m b/simulation/controllers/first_controller/Mode_Autonomous.m
new file mode 100644
index 0000000..0b41c4f
--- /dev/null
+++ b/simulation/controllers/first_controller/Mode_Autonomous.m
@@ -0,0 +1,116 @@
+% MAIN LOOP
+wb_console_print('Starting Autonomous Mode!', WB_STDOUT);
+
+%% INITIALIZATION
+step = 1;
+samples = 0;
+
+% initial pose of the robot
+position = wb_supervisor_field_get_sf_vec3f(trans_field);
+orientation = wb_supervisor_field_get_sf_rotation(orien_field);
+init_pose = [position(1) position(3) orientation(4)]'; % [x z theta]
+true_pose(:, step) = init_pose;
+
+% initializing values for encoder
+pose_enc(:, step) = init_pose;
+prev_enc_left = 0;
+prev_enc_right = 0;
+
+% initial position and velocity for IMU
+pose_imu(:, step) = init_pose;
+prev_imu_acc_z = 0;
+prev_imu_vel_z = 0;
+
+while wb_robot_step(TIME_STEP) ~= -1
+    step = step + 1;
+    
+    % circular movement
+    left_speed  = 1.0 * MAX_SPEED;
+    right_speed = 2.0 * MAX_SPEED;
+    
+    for i=1:2
+        if i == 1
+            wb_motor_set_velocity(motor(i), left_speed);
+        end
+        if i == 2
+            wb_motor_set_velocity(motor(i), right_speed);
+        end
+    end
+    
+    %% ENCODER TO POSE USING KINEMATIC MODEL
+    new_enc_left = wb_position_sensor_get_value(ps(1));
+    new_enc_right = wb_position_sensor_get_value(ps(2));
+    
+    diff_enc_left = new_enc_left - prev_enc_left;
+    diff_enc_right = new_enc_right - prev_enc_right;
+    
+    W = enc2wheelvel(diff_enc_left, diff_enc_right, TIME_STEP);
+    mu = kinematicModel(pose_enc(:, step-1), W, TIME_STEP, WHEEL_RADIUS, WHEEL_FROM_CENTER);
+    
+    pose_enc(:, step) = mu;
+    prev_enc_left = new_enc_left;
+    prev_enc_right = new_enc_right;
+    
+    %% TOF
+    image = wb_range_finder_get_range_image(tof);
+    
+    %% IMU
+    % IMU, Gyro
+    roll_pitch_yaw_array = wb_gyro_get_values(imu_gyro);
+    new_imu_gyro_y = roll_pitch_yaw_array(2);
+    
+    % IMU, Acce
+    x_y_z_array = wb_accelerometer_get_values(imu_acc);
+    new_imu_acc_z = x_y_z_array(3);
+    
+    wb_console_print(sprintf('gyro: %g %g %g\n', roll_pitch_yaw_array(1), roll_pitch_yaw_array(2), roll_pitch_yaw_array(3)), WB_STDOUT);
+    wb_console_print(sprintf('acce: %g %g %g\n', x_y_z_array(1), x_y_z_array(2), x_y_z_array(3)), WB_STDOUT);
+    
+    vel_z = imu_acc2vel(new_imu_acc_z, prev_imu_vel_z, TIME_STEP);
+    
+    if abs(new_imu_gyro_y) < 0.001
+        new_imu_gyro_y = 0;
+    end
+    
+    if abs(vel_z) < 0.001
+        vel_z = 0;
+    end
+    
+    U = [vel_z new_imu_gyro_y]';
+    
+    wb_console_print(sprintf('U: %g %g\n', U(1), U(2)), WB_STDOUT);
+    
+    mu = kinematicModel_vw(pose_imu(:, step-1), U, TIME_STEP);
+    
+    pose_imu(:, step) = mu;
+    prev_imu_acc_z = new_imu_acc_z;
+    prev_imu_vel_z = vel_z;
+    
+    %% PLOTTING
+    % Getting true position
+    position = wb_supervisor_field_get_sf_vec3f(trans_field);
+    orientation = wb_supervisor_field_get_sf_rotation(orien_field);
+    
+    true_pose(1, step) = position(1);
+    true_pose(2, step) = position(3);
+    true_pose(3, step) = orientation(4);
+    
+    %% Plotting position of robot on a map (true, odometry)
+    figure(1)
+    plot(true_pose(1, step), -true_pose(2, step), 'ro');
+    hold on;
+    plot(pose_enc(1, step), -pose_enc(2, step), 'bx');
+    hold on;
+    plot(pose_imu(1, step), -pose_imu(2, step), 'g.');
+    hold on;
+    axis([-0.8 0.8 -0.6 0.6]);
+    rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT])
+    legend("True Position", "Odometry", "IMU Estimation");
+    hold off;
+    
+    %% if your code plots some graphics, it needs to flushed like this:
+    drawnow;
+end
+
+%% CLEAN UP
+% cleanup code goes here: write data to files, etc.
\ No newline at end of file
diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m
new file mode 100644
index 0000000..eba51e9
--- /dev/null
+++ b/simulation/controllers/first_controller/Mode_Manual.m
@@ -0,0 +1,213 @@
+% MAIN LOOP
+wb_console_print('Starting Manual Mode!', WB_STDOUT);
+
+%% INITIALIZATION
+step = 1;
+samples = 0;
+
+% initial pose of the robot
+position = wb_supervisor_field_get_sf_vec3f(trans_field);
+orientation = wb_supervisor_field_get_sf_rotation(orien_field);
+init_pose = [position(1) position(3) orientation(4)]'; % [x z theta]
+true_pose(:, step) = init_pose;
+
+% initializing values for encoder
+pose_enc(:, step) = init_pose;
+prev_enc_left = 0;
+prev_enc_right = 0;
+
+% initial position and velocity for IMU
+pose_imu(:, step) = init_pose;
+prev_imu_acc_z = 0;
+prev_imu_vel_z = 0;
+
+% kf
+% prior
+orient_mu = 0;
+orient_S  = 0.1;
+% motion model and measurement model
+[A,B,R,n] = motion_model(TIME_STEP);
+[C,D,Q,m] = measurement_model();
+% store in state space model (ssm)
+ssm.A = A;
+ssm.B = B;
+ssm.C = C;
+ssm.D = D;
+ssm.R = R;
+ssm.Q = Q;
+ssm.n = n;
+ssm.m = m;
+% kf initializations
+orient_x(1) = orient_mu+sqrt(orient_S)*randn(1);
+U = wb_gyro_get_values(imu_gyro);
+gyro_u(1) = U(2);
+pose_imu_kalman(:, step) = [position(1) position(3) orient_x(1)]';
+
+
+while wb_robot_step(TIME_STEP) ~= -1
+    step = step + 1;
+    
+    input = wb_keyboard_get_key();
+    
+    switch input 
+        case 314
+            % LEFT
+            left_speed = -1.0;
+            right_speed = 1.0;
+        case 315
+            % UP
+            left_speed = 1.0;
+            right_speed = 1.0;
+        case 316
+            % RIGHT
+            left_speed = 1.0;
+            right_speed = -1.0;
+        case 317
+            % DOWN
+            left_speed = -1.0;
+            right_speed = -1.0;
+        otherwise
+            % NOTHING 
+            left_speed = 0;
+            right_speed = 0;      
+    end
+    
+    % set the velocity of the motors
+    wb_motor_set_velocity(motor(1), left_speed*MAX_SPEED);
+    wb_motor_set_velocity(motor(2), right_speed*MAX_SPEED);
+    
+    %% ENCODER TO POSE USING KINEMATIC MODEL
+    new_enc_left = wb_position_sensor_get_value(ps(1));
+    new_enc_right = wb_position_sensor_get_value(ps(2));
+    
+    diff_enc_left = new_enc_left - prev_enc_left;
+    diff_enc_right = new_enc_right - prev_enc_right;
+    
+    if abs(diff_enc_left) < 0.001
+        diff_enc_left = 0;
+    end
+    
+    if abs(diff_enc_right) < 0.001
+        diff_enc_right = 0;
+    end
+    
+    W = enc2wheelvel(diff_enc_left, diff_enc_right, TIME_STEP);
+    mu = kinematicModel(pose_enc(:, step-1), W, TIME_STEP, WHEEL_RADIUS, WHEEL_FROM_CENTER);
+    
+    pose_enc(:, step) = mu;
+    prev_enc_left = new_enc_left;
+    prev_enc_right = new_enc_right;
+    
+    %% Kalman Filter
+    % Select motion disturbance
+    orient_e = normrnd(0, R);
+    % Input for motion model
+    U = wb_gyro_get_values(imu_gyro);
+    gyro_u(step) = U(2);
+    % Update state
+    orient_x(step) = A*orient_x(step-1)+ B*gyro_u(step-1) + orient_e;
+    
+    % Take measurement
+    % Select a measurement disturbance
+    orient_d = normrnd(0, Q);
+    % Determine measurement
+    Y = wb_inertial_unit_get_roll_pitch_yaw(imu_mag);
+    orient_z = Y(3);
+    orient_y(step) = C*orient_z + orient_d;
+    
+    % Kalman Filter Estimation
+    [orient_mu,orient_S,orient_mup,orient_Sp,K] = kalman_filter(ssm,orient_mu,orient_S,gyro_u(step-1),orient_y(step));
+    
+    % Store estimates
+    orient_mup_S(step) = orient_mup;
+    orient_mu_S(step) = orient_mu;
+    orient_kalman(step) = K;   
+   
+    %% IMU    
+    % IMU, Gyro
+    roll_pitch_yaw_array = wb_gyro_get_values(imu_gyro);
+    new_imu_gyro_y = roll_pitch_yaw_array(2);
+    
+    % IMU, Acce
+    x_y_z_array = wb_accelerometer_get_values(imu_acc);
+    new_imu_acc_z = x_y_z_array(3);
+    
+    vel_z = imu_acc2vel(new_imu_acc_z, prev_imu_vel_z, TIME_STEP);
+    
+    if abs(new_imu_gyro_y) < 0.001
+        new_imu_gyro_y = 0;
+    end
+    
+    if abs(vel_z) < 0.001
+        vel_z = 0;
+    end
+    
+    U_Kalman = [vel_z orient_mu_S(step)]';
+    U = [vel_z new_imu_gyro_y]';
+    
+    mu = kinematicModel_vw(pose_imu(:, step-1), U, TIME_STEP);
+    mu_kalman = kinematicModel_kalman(pose_imu_kalman(:, step-1), U_Kalman, TIME_STEP);
+    
+    pose_imu(:, step) = mu;
+    pose_imu_kalman(:, step) = mu_kalman;
+    prev_imu_acc_z = new_imu_acc_z;
+    prev_imu_vel_z = vel_z;
+    
+    %% TOF
+    image = wb_range_finder_get_range_image(tof);
+    
+    %% PLOTTING
+    % Getting true position
+    position = wb_supervisor_field_get_sf_vec3f(trans_field);
+    orientation = wb_supervisor_field_get_sf_rotation(orien_field);
+    true_pose(1, step) = position(1);
+    true_pose(2, step) = position(3);
+    true_pose(3, step) = orientation(4);
+    
+    % Plotting position of robot on a map (true, odometry)
+%     figure(1)
+%     plot(true_pose(1, step), -true_pose(2, step), 'ro');
+%     hold on;
+%     plot(pose_enc(1, step), -pose_enc(2, step), 'bx');
+%     hold on;
+%     plot(pose_imu(1, step), -pose_imu(2, step), 'g.');
+%     hold on;
+%     plot(pose_imu_kalman(1, step), -pose_imu_kalman(2, step), 'c*');
+%     hold on
+%     axis([-0.8 0.8 -0.6 0.6]);
+%     rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT])
+%     text(0.35,0.55,'True Position','Color','red');
+%     text(0.35,0.50,'Odometry','Color','blue');
+%     text(0.35,0.45,'IMU Estimation','Color','green');
+%     hold off;
+    
+    % Plotting compass showing current orientation of the robot
+    figure(2)
+    rad_enc = pose_enc(3, step);
+    rad_enc = rad_enc+pi/2;
+    u_enc = cos(rad_enc);
+    v_enc = sin(rad_enc);
+    
+    rad_imu = pose_imu(3, step);
+    rad_imu = rad_imu+pi/2;
+    u_imu = cos(rad_imu);
+    v_imu = sin(rad_imu);
+    
+    rad_kf = pose_imu_kalman(3, step);
+    rad_kf = rad_kf+pi/2;
+    u_kf = cos(rad_kf);
+    v_kf = sin(rad_kf);
+    
+    compass(u_enc,v_enc,'r'); hold on;
+    compass(u_kf,v_kf,'b');
+    compass(u_imu,v_imu,'g'); hold off;
+
+    % if your code plots some graphics, it needs to flushed like this:
+    drawnow;
+end
+
+%% CLEAN UP
+filename = "manual.mat";
+save(filename)
+
+% cleanup code goes here: write data to files, etc.
\ No newline at end of file
diff --git a/simulation/controllers/first_controller/Parameters.m b/simulation/controllers/first_controller/Parameters.m
new file mode 100644
index 0000000..1b0482d
--- /dev/null
+++ b/simulation/controllers/first_controller/Parameters.m
@@ -0,0 +1,55 @@
+%% SIMULATION PARAMETERS
+TIME_STEP = 32;             %[ms]
+
+%% ROBOT PARAMETERS
+ROBOT_NAME = 'TableUV';
+ROBOT_BASE_RADIUS = 0.03;   %[m]
+ROBOT_BASE_HEIGHT = 0.035;  %[m]
+WHEEL_RADIUS = 0.017;       %[m]
+WHEEL_FROM_CENTER = 0.04;   %[m]
+WHEEL_CIRCUM = WHEEL_RADIUS * 2 * pi;
+
+TABLE_WIDTH = 1.22;         %[m]
+TABLE_HEIGHT = 0.76;        %[m]
+
+MAX_SPEED = 3;              %[m/s]
+
+%% SENSOR PARAMETERS
+% ENCODER 
+ENC_RESOLUTION = -1;        %[rad]
+ENC_NOISE = 0;              %[rad]
+ENC_UNIT = WHEEL_CIRCUM / 6.28;
+
+% IMU_GYRO
+IMU_GYRO_RESOLUTION = -1;
+
+% IMU_ACC
+IMU_ACC_RESOLUTION = -1;
+
+% IMU_MAG
+IMU_MAG_RESOLUTION = -1;
+
+% TOF
+TOF_FOV = 0.471;            %[rad]
+TOF_WIDTH = 64;             %[px]
+TOF_HEIGHT = 64;            %[px]
+TOF_IS_SPHERICAL = false;
+TOF_NEAR = 0.01;            %[m]
+TOF_MIN_RANGE = 0.01;       %[m]
+TOF_MAX_RANGE = 4;          %[m]
+TOF_MOTION_BLUR = 0;
+TOF_NOISE = 0;
+TOF_RESOLUTION = -1;
+
+% IR
+
+% COLLISION
+
+
+
+
+
+
+
+
+
diff --git a/simulation/controllers/first_controller/Supervisor.m b/simulation/controllers/first_controller/Supervisor.m
new file mode 100644
index 0000000..7c0d492
--- /dev/null
+++ b/simulation/controllers/first_controller/Supervisor.m
@@ -0,0 +1,92 @@
+%% ROBOT SETUP
+% Getting the nodes
+c = containers.Map;
+robot_settings = { ROBOT_NAME,  'ROBOT_WHEEL_LEFT', 'ROBOT_WHEEL_RIGHT', ...
+                  'ROBOT_BASE', 'ROBOT_ENC_LEFT',   'ROBOT_ENC_RIGHT',   ...
+                  'ROBOT_TOF' , 'ROBOT_IMU_GYRO',   'ROBOT_IMU_ACC',     ...
+                  'ROBOT_IMU_MAG'};
+              
+for index = 1:length(robot_settings)
+    setting = char(robot_settings(index));
+    node = wb_supervisor_node_get_from_def(setting);
+    if isNull(node)
+      wb_console_print(sprintf('No DEF %s node found in the current world file', char(robot_settings(index))), WB_STDOUT);
+      quit(1);
+    end
+    c(char(robot_settings(index))) = node;
+end
+
+% Robot Initial Position
+trans_field = wb_supervisor_node_get_field(c(ROBOT_NAME), 'translation');
+orien_field = wb_supervisor_node_get_field(c(ROBOT_NAME), 'rotation');
+wb_supervisor_field_set_sf_vec3f(trans_field, [0 0.787 0])
+wb_supervisor_field_set_sf_rotation(orien_field, [0 1 0 0])
+
+% Robot left wheel
+left_wheel_rad = wb_supervisor_node_get_field(c('ROBOT_WHEEL_LEFT'), 'radius');
+right_wheel_rad = wb_supervisor_node_get_field(c('ROBOT_WHEEL_RIGHT'), 'radius');
+wb_supervisor_field_set_sf_float(left_wheel_rad, WHEEL_RADIUS);
+wb_supervisor_field_set_sf_float(right_wheel_rad, WHEEL_RADIUS);
+
+% Robot base 
+robot_base_rad = wb_supervisor_node_get_field(c('ROBOT_BASE'), 'radius');
+robot_height = wb_supervisor_node_get_field(c('ROBOT_BASE'), 'height');
+wb_supervisor_field_set_sf_float(robot_base_rad, ROBOT_BASE_RADIUS);
+wb_supervisor_field_set_sf_float(robot_height, ROBOT_BASE_HEIGHT);
+
+% Encoders
+robot_left_enc_noise = wb_supervisor_node_get_field(c('ROBOT_ENC_LEFT'), 'noise');
+robot_left_enc_res = wb_supervisor_node_get_field(c('ROBOT_ENC_LEFT'), 'resolution');
+robot_right_enc_noise = wb_supervisor_node_get_field(c('ROBOT_ENC_RIGHT'), 'noise');
+robot_right_enc_res = wb_supervisor_node_get_field(c('ROBOT_ENC_RIGHT'), 'resolution');
+wb_supervisor_field_set_sf_float(robot_left_enc_noise, ENC_NOISE);
+wb_supervisor_field_set_sf_float(robot_left_enc_res, ENC_RESOLUTION);
+wb_supervisor_field_set_sf_float(robot_right_enc_noise, ENC_NOISE);
+wb_supervisor_field_set_sf_float(robot_right_enc_res, ENC_RESOLUTION);
+
+% Range Finder (ToF)
+robot_tof_fov = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'fieldOfView');
+robot_tof_width = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'width');
+robot_tof_height = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'height');
+robot_tof_spherical = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'spherical');
+robot_tof_near = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'near');
+robot_tof_minR = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'minRange');
+robot_tof_maxR = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'maxRange');
+robot_tof_blur = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'motionBlur');
+robot_tof_noise = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'noise');
+robot_tof_res = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'resolution');
+wb_supervisor_field_set_sf_float(robot_tof_fov, TOF_FOV);
+wb_supervisor_field_set_sf_int32(robot_tof_width, TOF_WIDTH);
+wb_supervisor_field_set_sf_int32(robot_tof_height, TOF_HEIGHT);
+wb_supervisor_field_set_sf_bool(robot_tof_spherical, TOF_IS_SPHERICAL);
+wb_supervisor_field_set_sf_float(robot_tof_near, TOF_NEAR);
+wb_supervisor_field_set_sf_float(robot_tof_minR, TOF_MIN_RANGE);
+wb_supervisor_field_set_sf_float(robot_tof_maxR, TOF_MAX_RANGE);
+wb_supervisor_field_set_sf_float(robot_tof_blur, TOF_MOTION_BLUR);
+wb_supervisor_field_set_sf_float(robot_tof_noise, TOF_NOISE);
+wb_supervisor_field_set_sf_float(robot_tof_res, TOF_RESOLUTION);
+
+% IMU, Gyro
+robot_imu_gyro_res = wb_supervisor_node_get_field(c('ROBOT_IMU_GYRO'), 'resolution');
+wb_supervisor_field_set_sf_float(robot_imu_gyro_res, IMU_GYRO_RESOLUTION);
+
+% IMU, Acc
+robot_imu_acc_res = wb_supervisor_node_get_field(c('ROBOT_IMU_ACC'), 'resolution');
+wb_supervisor_field_set_sf_float(robot_imu_acc_res, IMU_ACC_RESOLUTION);
+
+% IMU, Mag
+robot_imu_mag_res = wb_supervisor_node_get_field(c('ROBOT_IMU_MAG'), 'resolution');
+wb_supervisor_field_set_sf_float(robot_imu_mag_res, IMU_MAG_RESOLUTION);
+
+%% TABLE SETUP
+table = wb_supervisor_node_get_from_def('TABLE');
+if isNull(table)
+  wb_console_print('No DEF TABLE node found in the current world file', WB_STDOUT);
+  quit(1);
+end
+
+table_size = wb_supervisor_node_get_field(table, 'size');
+size = [TABLE_WIDTH 0.76 TABLE_HEIGHT];
+wb_supervisor_field_set_sf_vec3f(table_size, size);
+
+
diff --git a/simulation/controllers/first_controller/enc2pose.m b/simulation/controllers/first_controller/enc2pose.m
new file mode 100644
index 0000000..50aa6f0
--- /dev/null
+++ b/simulation/controllers/first_controller/enc2pose.m
@@ -0,0 +1,12 @@
+function [x,z,theta] = enc2pose(left_enc, right_enc, prev_x, prev_z, prev_theta, ENC_UNIT, WHEEL_FROM_CENTER)
+    %enc2pose: Convert encoder values to robot pose
+    
+    left_dist = left_enc * ENC_UNIT;
+    right_dist = right_enc * ENC_UNIT;
+    center_dist = (left_dist + right_dist) / 2;
+       
+    z = prev_z + center_dist*cos(prev_theta);
+    x = prev_x + center_dist*sin(prev_theta);
+    theta = prev_theta + (right_dist-left_dist)/(2*WHEEL_FROM_CENTER);
+end
+
diff --git a/simulation/controllers/first_controller/enc2wheelvel.m b/simulation/controllers/first_controller/enc2wheelvel.m
new file mode 100644
index 0000000..80f4b8b
--- /dev/null
+++ b/simulation/controllers/first_controller/enc2wheelvel.m
@@ -0,0 +1,9 @@
+function W = enc2wheelvel(left_enc,right_enc,TIME_STEP)
+    %enc2wheelvel: convert encoder values (radians in webots) to wheel vel
+    left_radPsec = left_enc / (TIME_STEP * 0.001);
+    right_radPsec = right_enc / (TIME_STEP * 0.001);
+     
+    W = [left_radPsec;
+         right_radPsec];
+end
+
diff --git a/simulation/controllers/first_controller/first_controller.m b/simulation/controllers/first_controller/first_controller.m
new file mode 100644
index 0000000..47a75ac
--- /dev/null
+++ b/simulation/controllers/first_controller/first_controller.m
@@ -0,0 +1,38 @@
+clear; clc;
+
+% uncomment the next two lines if you want to use
+% MATLAB's desktop to interact with the controller:
+desktop;
+keyboard;
+
+% PARAMETERS
+Parameters;
+% DEVICE INITIALIZATION
+Initialize;
+% SUPERVISOR
+Supervisor;
+
+% GET INPUT
+wb_console_print(sprintf(['Select controller type:\n', ...
+    '1 - Autonomous\n', ...
+    '2 - Manual Control\n', ...
+    '3 - Exit\n'
+    ]), WB_STDOUT);
+
+while wb_robot_step(TIME_STEP) ~= -1
+    input = wb_keyboard_get_key();
+    switch input 
+        case 49
+            Mode_Autonomous
+        case 50
+            Mode_Manual
+        case 51
+            wb_supervisor_simulation_set_mode(WB_SUPERVISOR_SIMULATION_MODE_PAUSE)
+            wb_console_print('Paused. Press Ctrl+Shift+T to reset.', WB_STDOUT);
+            return
+        case -1
+            % do nothing
+        otherwise
+            wb_console_print('Invalid Input.', WB_STDOUT);
+    end
+end
diff --git a/simulation/controllers/first_controller/imu_acc2vel.m b/simulation/controllers/first_controller/imu_acc2vel.m
new file mode 100644
index 0000000..ad381e2
--- /dev/null
+++ b/simulation/controllers/first_controller/imu_acc2vel.m
@@ -0,0 +1,6 @@
+function vel_z = imu_acc2vel(acc_z, prev_vel_z, TIME_STEP)
+    dt = TIME_STEP * 0.001;
+    
+    vel_z = prev_vel_z + acc_z*dt;
+end
+
diff --git a/simulation/controllers/first_controller/kalman_filter.m b/simulation/controllers/first_controller/kalman_filter.m
new file mode 100644
index 0000000..0f17c36
--- /dev/null
+++ b/simulation/controllers/first_controller/kalman_filter.m
@@ -0,0 +1,44 @@
+function [mu,S,mup,Sp,K] = kalman_filter(ssm,mu,S,u,y)
+
+% Performs one iteration of Kalman Filtering
+
+% Inputs:
+%       ssm                 : State space model structure
+%       mu                  : Mean of current state
+%       S                   : Covariance of current state
+%       u                   : Control Input of current time step
+%       y                   : Measurment of current time step
+%       example             : Example under consideration
+%       t                   : Current time step
+%       freq                : Multirate kalman filter frequency
+% Outputs:
+%       mu                  : Final estimated mean after iteration
+%       S                   : Final estimated covariance after iteration
+%       mup                 : Predicted mean after motion update
+%       Sp                  : Predicted covariance after motion update
+%       K                   : Kalman gain for current time step 
+
+% State space model structure
+% x[n] = A*x[n-1] + B*u[n] + R (gaussian disturbance)
+% y[n] = C*x[n] + D*u[n] + Q (gaussian disturbance)
+
+% Unwrap state space model
+A = ssm.A; 
+B = ssm.B;
+C = ssm.C; 
+D = ssm.D;
+R = ssm.R;
+Q = ssm.Q;
+n = ssm.n;
+m = ssm.m;
+
+% Prediction Step: ------------------------------------------------
+% Predict new mean based on motion
+mup = A*mu + B*u;
+% Predict new covariance based on motion
+Sp = A*S*A' + R;
+
+% Measurement Step: ------------------------------------------------
+K = Sp*C'*inv(C*Sp*C'+Q);
+mu = mup + K*(y-C*mup);
+S = (eye(n)-K*C)*Sp;
diff --git a/simulation/controllers/first_controller/kinematicModel.m b/simulation/controllers/first_controller/kinematicModel.m
new file mode 100644
index 0000000..0c4c8ff
--- /dev/null
+++ b/simulation/controllers/first_controller/kinematicModel.m
@@ -0,0 +1,14 @@
+function X_curr = kinematicModel(X_prev,U,TIME_STEP,WHEEL_RADIUS,WHEEL_FROM_CENTER)
+    % kinematicModel
+    % X_prev = [x z theta]'
+    % U = [w_left w_right]'
+    
+    r = WHEEL_RADIUS;
+    l = WHEEL_FROM_CENTER;
+    dt = TIME_STEP * 0.001;
+
+    X_curr = [ X_prev(1) + (r*(U(1)+U(2)))*0.5*sin(X_prev(3))*dt;
+               X_prev(2) + (r*(U(1)+U(2)))*0.5*cos(X_prev(3))*dt;
+               X_prev(3) + (r*(U(2)-U(1)))/(2*l)*dt ];
+end
+
diff --git a/simulation/controllers/first_controller/kinematicModel_kalman.m b/simulation/controllers/first_controller/kinematicModel_kalman.m
new file mode 100644
index 0000000..9f1cbcd
--- /dev/null
+++ b/simulation/controllers/first_controller/kinematicModel_kalman.m
@@ -0,0 +1,13 @@
+function X_curr = kinematicModel_kalman(X_prev ,U ,TIME_STEP)
+    % kinematicModel_vw
+    % X_prev = [x z theta]'
+    % U = [v_z theta]'
+    
+    dt = TIME_STEP * 0.001;
+    
+    X_curr = [ X_prev(1) + U(1)*sin(X_prev(3))*dt;
+               X_prev(2) + U(1)*cos(X_prev(3))*dt;
+               U(2) ];
+    
+end
+
diff --git a/simulation/controllers/first_controller/kinematicModel_vw.m b/simulation/controllers/first_controller/kinematicModel_vw.m
new file mode 100644
index 0000000..167e95d
--- /dev/null
+++ b/simulation/controllers/first_controller/kinematicModel_vw.m
@@ -0,0 +1,13 @@
+function X_curr = kinematicModel_vw(X_prev ,U ,TIME_STEP)
+    % kinematicModel_vw
+    % X_prev = [x z theta]'
+    % U = [v_z w_y]'
+    
+    dt = TIME_STEP * 0.001;
+    
+    X_curr = [ X_prev(1) + U(1)*sin(X_prev(3))*dt;
+               X_prev(2) + U(1)*cos(X_prev(3))*dt;
+               X_prev(3) + U(2)*dt ];
+    
+end
+
diff --git a/simulation/controllers/first_controller/measurement_model.m b/simulation/controllers/first_controller/measurement_model.m
new file mode 100644
index 0000000..422c32e
--- /dev/null
+++ b/simulation/controllers/first_controller/measurement_model.m
@@ -0,0 +1,7 @@
+function [C,D,Q,m] = measurement_model()
+    C = 1;
+    D = 0;
+    Q = 0.065;
+    m = 1;
+end
+
diff --git a/simulation/controllers/first_controller/motion_model.m b/simulation/controllers/first_controller/motion_model.m
new file mode 100644
index 0000000..dc1256e
--- /dev/null
+++ b/simulation/controllers/first_controller/motion_model.m
@@ -0,0 +1,9 @@
+function [A,B,R,n] = motion_model(dt)
+    dt = dt * 0.001;
+    
+    A = 1;
+    B = dt;
+    R = 0.01;
+    n = 1;
+end
+
diff --git a/simulation/worlds/.desk.wbproj b/simulation/worlds/.desk.wbproj
new file mode 100644
index 0000000..e419f7e
--- /dev/null
+++ b/simulation/worlds/.desk.wbproj
@@ -0,0 +1,14 @@
+Webots Project File version R2021a
+perspectives: 000000ff00000000fd000000030000000000000320000003f8fc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff0000000100000329000003bdfc0200000001fb0000001400540065007800740045006400690074006f00720000000015000003bd0000004100ffffff000000030000078000000138fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff00000780000002be00000004000000040000000100000008fc00000000
+simulationViewPerspectives: 000000ff000000010000000200000168000006160100000002010000000101
+sceneTreePerspectives: 000000ff0000000100000002000000ea000000fa0100000002010000000201
+minimizedPerspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000123fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000029d00000004000000040000000100000008fc00000000
+maximizedDockId: -1
+centralWidgetVisible: 1
+projectionMode: PERSPECTIVE
+renderingMode: PLAIN
+orthographicViewHeight: 0.808765
+textFiles: -1
+globalOptionalRendering: CoordinateSystem::RangeFinderFrustums
+consoles: Console:All:All
+renderingDevicePerspectives: robot:tof;1;3.82813;0.00706033;0.0139969
diff --git a/simulation/worlds/desk.wbt b/simulation/worlds/desk.wbt
new file mode 100644
index 0000000..77e03a4
--- /dev/null
+++ b/simulation/worlds/desk.wbt
@@ -0,0 +1,160 @@
+#VRML_SIM R2021a utf8
+WorldInfo {
+  coordinateSystem "NUE"
+}
+Viewpoint {
+  orientation 1 0 0 4.71238898038469
+  position -0.00015104981722547305 3.1386728803420745 1.22777122401446e-08
+  follow "table"
+}
+TexturedBackground {
+}
+TexturedBackgroundLight {
+}
+DEF OBJECTS Group {
+  children [
+    Floor {
+      name "floor(1)"
+    }
+    DEF TABLE Table {
+      size 1.22 0.76 0.76
+      frameThickness 0.1
+    }
+  ]
+}
+DEF TableUV Robot {
+  translation -1.1922127978489346e-07 0.7869679750092429 -1.767872493506352e-11
+  rotation -0.00014828549525515916 1.630439687735275e-11 0.9999999890057059 4.417476471929301e-06
+  children [
+    DEF ROBOT_IMU_ACC Accelerometer {
+      name "imu_acc"
+    }
+    DEF ROBOT_IMU_GYRO Gyro {
+      name "imu_gyro"
+    }
+    DEF ROBOT_IMU_MAG InertialUnit {
+      name "imu_mag"
+    }
+    DEF ROBOT_TOF RangeFinder {
+      translation 0 0 0.02
+      rotation 0 1 0 3.14159
+      name "tof"
+      fieldOfView 0.471
+      maxRange 4
+    }
+    Solid {
+      translation 0 -0.017 0.02
+      children [
+        DEF CASTER_WHEEL Shape {
+          appearance PBRAppearance {
+            metalness 0
+          }
+          geometry Sphere {
+            radius 0.01
+          }
+        }
+      ]
+      name "caster_wheel"
+      boundingObject USE CASTER_WHEEL
+      physics Physics {
+      }
+    }
+    Solid {
+      translation 0 -0.017 -0.02
+      children [
+        DEF CASTER_WHEEL Shape {
+          appearance PBRAppearance {
+            metalness 0
+          }
+          geometry Sphere {
+            radius 0.01
+          }
+        }
+      ]
+      name "caster_wheel(1)"
+      boundingObject USE CASTER_WHEEL
+      physics Physics {
+      }
+    }
+    HingeJoint {
+      jointParameters HingeJointParameters {
+        position 7.41449794324498e-08
+        anchor -0.04 -0.01 0
+      }
+      device [
+        DEF ROBOT_ENC_RIGHT PositionSensor {
+          name "right_ps"
+        }
+        RotationalMotor {
+          name "right_motor"
+        }
+      ]
+      endPoint Solid {
+        translation -0.04000000040198491 -0.009998878983036004 5.8474013926954475e-09
+        rotation 1.546136810770788e-08 -1.518240091980168e-08 0.9999999999999998 1.5699999530302196
+        children [
+          DEF WHEEL Shape {
+            appearance PBRAppearance {
+              metalness 0
+            }
+            geometry DEF ROBOT_WHEEL_RIGHT Cylinder {
+              height 0.006
+              radius 0.017
+            }
+          }
+        ]
+        name "solid(1)"
+        boundingObject USE WHEEL
+        physics DEF WHEEL_PHYS Physics {
+        }
+      }
+    }
+    HingeJoint {
+      jointParameters HingeJointParameters {
+        position 3.187092853206427e-08
+        anchor 0.04 -0.01 0
+      }
+      device [
+        DEF ROBOT_ENC_LEFT PositionSensor {
+          name "left_ps"
+        }
+        RotationalMotor {
+          name "left_motor"
+        }
+      ]
+      endPoint Solid {
+        translation 0.04 -0.00999889 4.164319999411154e-09
+        rotation 9.142114502326543e-10 -1.1821627446781977e-09 1 1.5700000469740278
+        children [
+          DEF WHEEL Shape {
+            appearance PBRAppearance {
+              metalness 0
+            }
+            geometry DEF ROBOT_WHEEL_LEFT Cylinder {
+              height 0.006
+              radius 0.017
+            }
+          }
+        ]
+        boundingObject USE WHEEL
+        physics DEF WHEEL_PHYS Physics {
+        }
+      }
+    }
+    DEF BODY Shape {
+      appearance PBRAppearance {
+        baseColor 1 0 0
+        metalness 0
+      }
+      geometry DEF ROBOT_BASE Cylinder {
+        height 0.035
+        radius 0.03
+      }
+    }
+  ]
+  boundingObject USE BODY
+  physics Physics {
+  }
+  controller "first_controller"
+  supervisor TRUE
+}