modified structure of the simulator; student workspace created

algorithms/particle_filter/compute_lidar_measurement.m

@@ -0,0 +1,7 @@
+function [measurement] = compute_lidar_measurement(map, pose, lidar_config)
+%COMPUTE_MEASUREMENTS Summary of this function goes here
+
+measurement = zeros(1, length(lidar_config));
+
+end
+

algorithms/particle_filter/update_particle_filter.m

@@ -9,11 +9,11 @@
 end
 
 % II. Correction
-measurements = zeros(size(particles,1), length(read_only_vars.sensors));
+measurements = zeros(size(particles,1), length(read_only_vars.lidar_config));
 for i=1:size(particles, 1)
-    measurements(i,:) = compute_measurement(read_only_vars.map, particles(i,:), read_only_vars.sensors);
+    measurements(i,:) = compute_lidar_measurement(read_only_vars.map, particles(i,:), read_only_vars.lidar_config);
 end
-weights = weight_particles(measurements, read_only_vars.measurement_distances);
+weights = weight_particles(measurements, read_only_vars.lidar_distances);
 
 % III. Resampling
 particles = resample_particles(particles, weights);

algorithms/particle_filter/weight_particles.m

@@ -1,4 +1,4 @@
-function [weights] = weight_particles(particle_measurements, measurement_distances)
+function [weights] = weight_particles(particle_measurements, lidar_distances)
 %WEIGHT_PARTICLES Summary of this function goes here
 
 N = size(particle_measurements, 1);

algorithms/setup.m

@@ -1,3 +1,3 @@
 start_position = [1, 1, pi/2]; % (x, y, theta)
 
-map_name = 'maps/outdoor_1.txt';
+map_name = 'maps/indoor_1.txt';

algorithms/student_workspace.m

@@ -0,0 +1,30 @@
+function [public_vars] = student_workspace(read_only_vars,public_vars)
+%STUDENT_WORKSPACE Summary of this function goes here
+
+% 7. Perform initialization procedure
+if (read_only_vars.counter == 1)
+
+    public_vars = init_particle_filter(read_only_vars, public_vars);
+    public_vars = init_kalman_filter(read_only_vars, public_vars);
+
+end
+
+% 8. Update particle filter
+public_vars.particles = update_particle_filter(read_only_vars, public_vars);
+
+% 9. Update Kalman filter
+[public_vars.mu, public_vars.sigma] = update_kalman_filter(read_only_vars, public_vars);
+
+% 10. Estimate current robot position
+public_vars.estimated_pose = estimate_pose(public_vars); % (x,y,theta)
+
+% 11. Path planning
+public_vars.path = plan_path(read_only_vars, public_vars);
+
+% 12. Plan next motion command
+public_vars = plan_motion(read_only_vars, public_vars);
+
+
+
+end
+

book_src/resources/simulator/text.md

@@ -26,25 +26,23 @@ The simulator workspace comprise *main* script stored in `main.m`, which contain
 4. **Check particles**: check the particle limit.
 5. **Lidar measurement**: read the lidar data and save them into the `read_only_vars` structure.
 6. **GNSS measurement**: read the GNSS data and save them into the `read_only_vars` structure.
-7. **Evaluate environment**: student function, can be used to handle switching between environments and related tasks.
-8. **Initialization procedure**: student function, is called in the first iteration only.
-9. **Initialize filters**: student functions, called when the iteration counter reaches modifiable `init_iterations` value. *Note*: depends on `pf_enabled` / `kf_enabled` flags.
-10. **Update particle filter**: student function, modifies the set of particles used in your **particle filter** algorithm. *Note*: depends on `pf_enabled` flag.
-11. **Update Kalman filter**: student function, modifies the mean and variance used in your **Kalman filter** algorithm. *Note*: depends on `kf_enabled` flag.
-12. **Estimate pose**: student function, use the filters outputs to acquire the estimate.
-13. **Path planning**: student function, returns the result of your **path planning** algorithm.
-14. **Plan motion**: student function, returns the result of your **motion control** algorithm. Save the result into the `motion_vector` variable (`[v_right, v_left]`). 
-15. **Move robot**: physically moves the robot according the `motion_vector` control variable.
-16. **GUI rendering**: render the simulator state in a Figure window.
-17. **Increment counter**: modifies read-only variable `counter` to record the number of finished iterations. 
-
-Steps 10 to 14 are performed after the initialization only.
+7. **Initialization procedure**: by default, it is called in the first iteration only; used to init filters and other tasks performed only once.
+8. **Update particle filter**: modifies the set of particles used in your **particle filter** algorithm.
+9. **Update Kalman filter**: modifies the mean and variance used in your **Kalman filter** algorithm.
+10. **Estimate pose**: use the filters outputs to acquire the estimate.
+11. **Path planning**: returns the result of your **path planning** algorithm.
+12. **Plan motion**: returns the result of your **motion control** algorithm. Save the result into the `motion_vector` variable (`[v_right, v_left]`).
+13. **Move robot**: physically moves the robot according the `motion_vector` control variable.
+14. **GUI rendering**: render the simulator state in a Figure window.
+15. **Increment counter**: modifies read-only variable `counter` to record the number of finished iterations. 
+
+Steps 7 to 12 are located in a separate `student_workspace.m` function.
 
 You should be able to complete all the assignments witnout modifying the `main.m` file.
 
 ## Custom Functions
 
-You are welcome to add as many custom functions in the *algorithms* folder as you like; however, try to follow the proposed folder structure (e.g., put the Kalman filter-related functions in the *kalman_filter* folder). You may also arbitrarily modify contents (**not headers**) of the student functions called from the *main* (steps 7 to 14 of the simulation loop).  
+You are welcome to add as many custom functions in the *algorithms* folder as you like; however, try to follow the proposed folder structure (e.g., put the Kalman filter-related functions in the *kalman_filter* folder). You may also arbitrarily modify the content (**not header**) of the *student workspace* function (steps 7 to 12 of the simulation loop) and other functions called by it.  
 
 ## Maps and testing
 

main.m

@@ -30,11 +30,11 @@
 read_only_vars.agent_drive.interwheel_dist = 0.2; % in case of diff drive
 read_only_vars.agent_drive.max_vel = 1;
 read_only_vars.measurement_distances = [];
-read_only_vars.gnss_pose = [];
+read_only_vars.gnss_position = [];
 
 
 % III. Set other given parameters
-read_only_vars.sensors = [0, 45, 90, 135, 180, 225, 270, 315] / 180 * pi; % (rad)
+read_only_vars.lidar_config = [0, 45, 90, 135, 180, 225, 270, 315] / 180 * pi; % (rad)
 read_only_vars.sampling_period = 0.1; % (s)
 read_only_vars.max_particles = 1000; % (-)
 read_only_vars.counter = 1;
@@ -44,8 +44,8 @@
 public_vars.path = []; % row = (x,y)
 public_vars.particles = []; % Nx3..m matrix, row = (x,y,theta, ...)
 
-public_vars.est_position_history = nan(1,3); % row = (x,y,theta)
-public_vars.gnss_history = [];
+read_only_vars.est_position_history = nan(1,3); % row = (x,y,theta)
+read_only_vars.gnss_history = [];
 
 % V. Init vizualization
 figure(1);
@@ -106,64 +106,25 @@
     end
 
     % 5. Do Lidar measurements
-    [read_only_vars.measurement_distances, private_vars.raycasts] = perform_measurements(read_only_vars.map, private_vars.agent_pose, read_only_vars.sensors);
+    [read_only_vars.lidar_distances, private_vars.raycasts] = lidar_measure(read_only_vars.map, private_vars.agent_pose, read_only_vars.lidar_config);
 
     % 6. Measure GNSS position
-    read_only_vars.gnss_pose = gnss_measure(private_vars.agent_pose, read_only_vars.map.gnss_denied);
-    public_vars.gnss_history = [public_vars.gnss_history; read_only_vars.gnss_pose];
+    read_only_vars.gnss_position = gnss_measure(private_vars.agent_pose, read_only_vars.map.gnss_denied);
+    read_only_vars.gnss_history = [read_only_vars.gnss_history; read_only_vars.gnss_position];
 
+    % 7.-12. Student workspace
+    public_vars = student_workspace(read_only_vars, public_vars);
 
-    % 7. Evaluate environment (indoor / outdoor)
-    public_vars = eval_environment(read_only_vars, public_vars);
+    read_only_vars.est_position_history = [read_only_vars.est_position_history; public_vars.estimated_pose];
 
-    % 8. Perform initialization procedure (first iteration)
-    if (read_only_vars.counter == 1)
-        public_vars = init_procedure(read_only_vars, public_vars);
-    end
-
-    % 9. Initialize filters
-    if (read_only_vars.counter == public_vars.init_iterations)
-
-        if public_vars.pf_enabled            
-            public_vars = init_particle_filter(read_only_vars, public_vars);
-        end
-
-        if public_vars.kf_enabled
-            public_vars = init_kalman_filter(read_only_vars, public_vars);
-        end
-
-    elseif (read_only_vars.counter > public_vars.init_iterations)
-
-        % 10. Update particle filter
-        if public_vars.pf_enabled
-            public_vars.particles = update_particle_filter(read_only_vars, public_vars);
-        end
-
-        % 11. Update Kalman filter
-        if public_vars.kf_enabled
-            [public_vars.mu, public_vars.sigma] = update_kalman_filter(read_only_vars, public_vars);
-        end
-
-        % 12. Estimate current robot position
-        public_vars.estimated_pose = estimate_pose(public_vars); % (x,y,theta)
-        public_vars.est_position_history = [public_vars.est_position_history; public_vars.estimated_pose];
-
-        % 13. Path planning
-        public_vars.path = plan_path(read_only_vars, public_vars);
-
-        % 14. Plan next motion command
-        public_vars = plan_motion(read_only_vars, public_vars);
-
-    end
-
-    % 15. Move robot
+    % 13. Move robot
     private_vars.agent_pose = move_agent(private_vars.agent_pose, public_vars.motion_vector, read_only_vars.agent_drive, read_only_vars.sampling_period);  
     private_vars.agent_position_history = [private_vars.agent_position_history; private_vars.agent_pose];    
 
-    % 16. GUI rendering
+    % 14. GUI rendering
     h = render_game(private_vars, read_only_vars, public_vars, h);
 
-    % 17. Increment counter
+    % 15. Increment counter
     read_only_vars.counter = read_only_vars.counter + 1;
 
 end