├── LICENSE ├── README.md ├── challenge1_compare_resampling_algorithms.py ├── challenge1_compare_resampling_schemes.py ├── challenge1_degeneracy.py ├── challenge2_impoverishment.py ├── challenge3_divergence.py ├── challenge5_adaptive_particle_filter.py ├── core ├── particle_filters │ ├── __init__.py │ ├── adaptive_particle_filter_kld.py │ ├── adaptive_particle_filter_sl.py │ ├── auxiliary_particle_filter.py │ ├── extended_kalman_particle_filter.py │ ├── particle_filter_base.py │ ├── particle_filter_max_weight_resampling.py │ ├── particle_filter_nepr.py │ ├── particle_filter_range_only.py │ └── particle_filter_sir.py └── resampling │ ├── __init__.py │ ├── resampler.py │ └── resampling_helpers.py ├── demo_range_only.py ├── demo_running_example.py ├── demo_running_example_extended_Kalman_particle_filter.py ├── images └── running_example_screenshot.png ├── shared_simulation_settings.py └── simulator ├── __init__.py ├── robot.py ├── robot_range_measurement.py ├── visualizer.py └── world.py /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 jelfring 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Installation 2 | All code is written in Python. In order to run the code, the following packages must be installed: 3 | 4 | * numpy 5 | * matplotlib 6 | * scipy 7 | 8 | # Documentation 9 | 10 | Details on the algorithms, background information in general and an documentation for all the main scripts can be found [in this paper](https://www.mdpi.com/1424-8220/21/2/438). 11 | 12 | # Running the code 13 | 14 | The main scripts are 15 | * ``demo_running_example``: runs the basic particle filter 16 | * ``demo_range_only``: runs the basic particle filter with a lower number of landmarks (illustrates the particle filter's ability to represent non-Gaussian distributions). 17 | 18 | Whenver running the code, a robot localization problem will be simulated. For most scripts, the visualization below should appear. 19 | 20 | ![alt text](https://github.com/jelfring/particle-filter-tutorial/blob/master/images/running_example_screenshot.png?raw=true) 21 | 22 | The picture shows a top view of a 2D simulated world. Four landmarks can be observed by the robot (blue rectangles). The landmark positions are given in the map and therefore are used to estimate the tru robot position and orientation (red circle). The particles that together represent the posterior distribution are represented by the green dots. 23 | 24 | Besides the standard particle filter, more advanced particle filters are implemented, different resampling schemes and different resampling algorithms are available. This allows for trying many different particle filter is similar settings. 25 | 26 | The supported resampling algorithms are: 27 | * Multinomial resampling 28 | * Residual resampling 29 | * Stratified resampling 30 | * Systematic resampling 31 | 32 | Supported resampling schemes are: 33 | * Every time step 34 | * Based on approximated effective number of particles 35 | * Based on reciprocal of maximum particl weight 36 | 37 | More advanced particle filters that are supported: 38 | * Adaptive particle filter 39 | * Auxiliary particle filter 40 | * Extended Kalman particle filter 41 | 42 | # Reference 43 | In case you use code in your work, please cite: 44 | 45 | Elfring J, Torta E, van de Molengraft R. Particle Filters: A Hands-On Tutorial. Sensors. 2021; 21(2):438. -------------------------------------------------------------------------------- /challenge1_compare_resampling_algorithms.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Supported resampling methods 4 | from core.resampling import Resampler, ResamplingAlgorithms 5 | 6 | import numpy as np 7 | 8 | 9 | def get_states(weighted_samples): 10 | """ 11 | Get the states from set of weighted particles. 12 | 13 | :param weighted_samples: list of weighted particles 14 | :return: list of particle states without weights 15 | """ 16 | return [ws[1] for ws in weighted_samples] 17 | 18 | 19 | if __name__ == '__main__': 20 | """ 21 | In this program a particle set will be generated and different resampling algorithms will be used to resample (with 22 | replacement) from this particle set. The probability of selecting a particle is proportional to its weight. 23 | 24 | In the end the number of times each particle has been selected (this should be the same for all resampling 25 | algorithms) is printed. Furthermore, the standard deviations are shown (the variance in the number of times each 26 | particle has been selected. This is a measure of the predictability of the resampling algorithm. In other words, the 27 | diversity in sample set when performing resampling multiple times on exactly the same particle set. 28 | """ 29 | 30 | print("Compare four resampling algorithms.") 31 | 32 | # Number of particles 33 | number_of_particles = 5 34 | 35 | # Determine weights (random weights), normalize such that they sum up to one. 36 | unnormalized_weights = np.random.random_sample((number_of_particles,)) 37 | normalized_weights = [w / np.sum(unnormalized_weights) for w in unnormalized_weights] 38 | 39 | # States range from 1 to the number of particles 40 | particle_states = range(1, number_of_particles+1) 41 | 42 | # Weighted samples 43 | weighted_particles = list(zip(normalized_weights, particle_states)) 44 | 45 | # Resampling algorithms that must be compared 46 | resampler = Resampler() 47 | methods = [ResamplingAlgorithms.MULTINOMIAL, 48 | ResamplingAlgorithms.RESIDUAL, 49 | ResamplingAlgorithms.STRATIFIED, 50 | ResamplingAlgorithms.SYSTEMATIC] 51 | 52 | # Number of resample steps 53 | num_steps = 100000 54 | 55 | print('Input samples: {}'.format(weighted_particles)) 56 | 57 | # Compare resampling algorithms 58 | for method in methods: 59 | print('Testing {} resampling'.format(method.name)) 60 | all_results = [] 61 | for i in range(0, num_steps+1): 62 | # Resample 63 | weighted_samples = resampler.resample(weighted_particles, 64 | number_of_particles, 65 | method) 66 | 67 | # Store number of occurrences each particle in array 68 | sampled_states = [state[1] for state in weighted_samples] 69 | all_results.append([sampled_states.count(state_i) for state_i in particle_states]) 70 | 71 | # Print results for current resampling algorithm 72 | print('mean #occurrences: {}'.format(np.mean(all_results, axis=0))) 73 | print('std #occurrences: {}'.format(np.std(all_results, axis=0))) 74 | -------------------------------------------------------------------------------- /challenge1_compare_resampling_schemes.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Simulation + plotting requires a robot, visualizer and world 4 | from simulator import Robot, Visualizer, World 5 | 6 | # Supported resampling methods 7 | from core.resampling import ResamplingAlgorithms 8 | 9 | # Particle filters 10 | from core.particle_filters import ParticleFilterSIR, ParticleFilterNEPR, ParticleFilterMWR 11 | 12 | # Load variables 13 | from shared_simulation_settings import * 14 | 15 | # For computing errors 16 | import numpy as np 17 | 18 | if __name__ == '__main__': 19 | 20 | """ 21 | In this program three particle filter will be used for exactly the same problem. The filters are identical except 22 | for the resampling strategy. 23 | 1) The first particle filter resamples at every time step (SIR) 24 | 2) The second particle filter resamples if the approximate number of effective particles drops below a pre-defined 25 | threshold (NEPR) 26 | 3) The third particle filter resamples in case the reciprocal of the maximum weight drops below a pre-defined 27 | threshold (MWR) 28 | """ 29 | 30 | print("Compare three resampling schemes.") 31 | 32 | # Define simulated world 33 | world = World(world_size_x, world_size_y, landmark_positions) 34 | 35 | # Initialize simulated robot 36 | robot = Robot(x=world.x_max * 0.75, 37 | y=world.y_max / 5.0, 38 | theta=3.14 / 2.0, 39 | std_forward=true_robot_motion_forward_std, 40 | std_turn=true_robot_motion_turn_std, 41 | std_meas_distance=true_robot_meas_noise_distance_std, 42 | std_meas_angle=true_robot_meas_noise_angle_std) 43 | 44 | # Set resampling algorithm used (same for all filters) 45 | resampling_algorithm = ResamplingAlgorithms.MULTINOMIAL 46 | 47 | # Number of particles in the particle filters 48 | number_of_particles = 1000 49 | 50 | # Resampling thresholds 51 | number_of_effective_particles_threshold = number_of_particles / 4.0 52 | reciprocal_max_weight_resampling_threshold = 1.0 / 0.005 53 | 54 | ## 55 | # Simulation settings 56 | ## 57 | n_time_steps = 50 # Number of simulated time steps 58 | n_trials = 100 # Number of times each simulation will be repeated 59 | 60 | # Bookkeeping variables 61 | errors_sir = [] 62 | errors_nepr = [] 63 | errors_mwr = [] 64 | cnt_sir = 0 65 | cnt_nepr = 0 66 | cnt_mwr = 0 67 | 68 | # Start main simulation loop 69 | for trial in range(n_trials): 70 | print("Trial: ", trial) 71 | 72 | # (Re)initialize SIR particle filter: resample every time step 73 | particle_filter_sir = ParticleFilterSIR( 74 | number_of_particles, 75 | pf_state_limits, 76 | process_noise, 77 | measurement_noise, 78 | resampling_algorithm) 79 | particle_filter_sir.initialize_particles_uniform() 80 | 81 | # Resample if approximate number effective particle drops below threshold 82 | particle_filter_nepr = ParticleFilterNEPR( 83 | number_of_particles, 84 | pf_state_limits, 85 | process_noise, 86 | measurement_noise, 87 | resampling_algorithm, 88 | number_of_effective_particles_threshold) 89 | particle_filter_nepr.set_particles(particle_filter_sir.particles) 90 | 91 | # Resample based on reciprocal of maximum particle weight drops below threshold 92 | particle_filter_mwr = ParticleFilterMWR( 93 | number_of_particles, 94 | pf_state_limits, 95 | process_noise, 96 | measurement_noise, 97 | resampling_algorithm, 98 | reciprocal_max_weight_resampling_threshold) 99 | particle_filter_mwr.set_particles(particle_filter_sir.particles) 100 | 101 | for i in range(n_time_steps): 102 | 103 | # Move the simulated robot 104 | robot.move(robot_setpoint_motion_forward, 105 | robot_setpoint_motion_turn, 106 | world) 107 | 108 | # Simulate measurement 109 | measurements = robot.measure(world) 110 | 111 | # Update SIR particle filter (in this case: propagate + weight update + resample) 112 | res = particle_filter_sir.update(robot_setpoint_motion_forward, 113 | robot_setpoint_motion_turn, 114 | measurements, 115 | world.landmarks) 116 | if res: 117 | cnt_sir += 1 118 | 119 | # Update NEPR particle filter (in this case: propagate + weight update, resample if needed) 120 | res = particle_filter_nepr.update(robot_setpoint_motion_forward, 121 | robot_setpoint_motion_turn, 122 | measurements, 123 | world.landmarks) 124 | if res: 125 | cnt_nepr += 1 126 | 127 | # Update MWR particle filter (in this case: propagate + weight update, resample if needed) 128 | res = particle_filter_mwr.update(robot_setpoint_motion_forward, 129 | robot_setpoint_motion_turn, 130 | measurements, 131 | world.landmarks) 132 | if res: 133 | cnt_mwr += 1 134 | 135 | # Compute errors 136 | robot_pose = np.array([robot.x, robot.y, robot.theta]) 137 | e_sir = robot_pose - np.asarray(particle_filter_sir.get_average_state()) 138 | e_nepr = robot_pose - np.asarray(particle_filter_nepr.get_average_state()) 139 | e_mwr = robot_pose - np.asarray(particle_filter_mwr.get_average_state()) 140 | 141 | errors_sir.append(np.linalg.norm(e_sir)) 142 | errors_nepr.append(np.linalg.norm(e_nepr)) 143 | errors_mwr.append(np.linalg.norm(e_mwr)) 144 | 145 | print("SIR mean error: {}, std error: {}".format(np.mean(np.asarray(errors_sir)), np.std(np.asarray(errors_sir)))) 146 | print("NEPR mean error: {}, std error: {}".format(np.mean(np.asarray(errors_nepr)), np.std(np.asarray(errors_nepr)))) 147 | print("MWR mean error: {}, std error: {}".format(np.mean(np.asarray(errors_mwr)), np.std(np.asarray(errors_mwr)))) 148 | 149 | print("#updates in {} trials: {}, {}, {}".format(n_trials, cnt_sir, cnt_nepr, cnt_mwr)) 150 | -------------------------------------------------------------------------------- /challenge1_degeneracy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Simulation + plotting requires a robot, visualizer and world 4 | from simulator import Robot, Visualizer, World 5 | 6 | # Supported resampling methods (resampling algorithm enum for SIR and SIR-derived particle filters) 7 | from core.resampling import ResamplingAlgorithms 8 | 9 | # Load variables 10 | from shared_simulation_settings import * 11 | 12 | # Particle filters 13 | from core.particle_filters import ParticleFilterSIR 14 | 15 | # For showing plots (plt.show()) 16 | import matplotlib.pyplot as plt 17 | 18 | 19 | def never_resample(): 20 | """ 21 | Function that always returns false and thereby enables switching off resampling. This function is needed to switch 22 | off resampling. 23 | 24 | :return: Boolean that always equals false 25 | """ 26 | return False 27 | 28 | 29 | if __name__ == '__main__': 30 | 31 | """ 32 | This file demonstrates the particle filter degeneracy problem that occurs in case a particle filter never resamples. 33 | """ 34 | 35 | print("Starting demonstration of particle filter degeneracy.") 36 | 37 | ## 38 | # Set simulated world and visualization properties 39 | ## 40 | 41 | # Simulated world 42 | world = World(world_size_x, world_size_y, landmark_positions) 43 | 44 | # Initialize visualization 45 | show_particle_pose = False # only set to true for low #particles (very slow) 46 | visualizer = Visualizer(show_particle_pose) 47 | 48 | # Number of simulated time steps 49 | n_time_steps = 15 50 | 51 | # Simulated robot 52 | robot = Robot(robot_initial_x_position, 53 | robot_initial_y_position, 54 | robot_initial_heading, 55 | true_robot_motion_forward_std, 56 | true_robot_motion_turn_std, 57 | true_robot_meas_noise_distance_std, 58 | true_robot_meas_noise_angle_std) 59 | 60 | # Number of particles 61 | number_of_particles = 500 62 | 63 | # Set resampling algorithm used 64 | resampling_algorithm = ResamplingAlgorithms.MULTINOMIAL 65 | 66 | # Initialize the particle filter 67 | 68 | # Initialize SIR particle filter 69 | particle_filter_sir = ParticleFilterSIR( 70 | number_of_particles, 71 | pf_state_limits, 72 | process_noise, 73 | measurement_noise, 74 | resampling_algorithm) 75 | particle_filter_sir.initialize_particles_uniform() 76 | 77 | # Turn OFF resampling 78 | particle_filter_sir.needs_resampling = never_resample 79 | 80 | # Start simulation 81 | max_weights = [] 82 | for i in range(n_time_steps): 83 | 84 | # Simulate robot move, simulate measurement and update particle filter 85 | robot.move(robot_setpoint_motion_forward, 86 | robot_setpoint_motion_turn, 87 | world) 88 | 89 | measurements = robot.measure(world) 90 | particle_filter_sir.update(robot_setpoint_motion_forward, 91 | robot_setpoint_motion_turn, 92 | measurements, 93 | world.landmarks) 94 | 95 | # Show maximum normalized particle weight (converges to 1.0) 96 | w_max = particle_filter_sir.get_max_weight() 97 | max_weights.append(w_max) 98 | print("Time step {}: max weight: {}".format(i, w_max)) 99 | 100 | # Plot weights as function of time step 101 | fontSize = 14 102 | plt.rcParams.update({'font.size': fontSize}) 103 | plt.plot(range(n_time_steps), max_weights, 'k') 104 | plt.xlabel("Time index") 105 | plt.ylabel("Maximum particle weight") 106 | plt.xlim(0, n_time_steps-1) 107 | plt.ylim(0, 1.1) 108 | plt.show() 109 | -------------------------------------------------------------------------------- /challenge2_impoverishment.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Simulation + plotting requires a robot, visualizer and world 4 | from simulator import Robot, Visualizer, World 5 | 6 | # Supported resampling methods 7 | from core.resampling import ResamplingAlgorithms 8 | 9 | # Load variables 10 | from shared_simulation_settings import * 11 | 12 | # Particle filter 13 | from core.particle_filters import ParticleFilterSIR 14 | 15 | # For showing plots (plt.show()) 16 | import matplotlib.pyplot as plt 17 | 18 | if __name__ == '__main__': 19 | """ 20 | This particle filter demonstrates particle filter sample impoverishment. In order to enforce imporerishment, the 21 | process model noise is artificially lowered. 22 | """ 23 | 24 | print("Starting demonstration of particle filter sample impoverishment.") 25 | 26 | ## 27 | # Set simulated world and visualization properties 28 | ## 29 | 30 | # Simulated world 31 | world = World(world_size_x, world_size_y, landmark_positions) 32 | 33 | # Visualizer 34 | visualizer = Visualizer() 35 | 36 | # Number of simulated time steps 37 | num_time_steps = 25 38 | 39 | # Initialize simulated robot 40 | robot = Robot(x=world.x_max * 0.75, 41 | y=world.y_max / 5.0, 42 | theta=3.14 / 2.0, 43 | std_forward=true_robot_motion_forward_std, 44 | std_turn=true_robot_motion_turn_std, 45 | std_meas_distance=true_robot_meas_noise_distance_std, 46 | std_meas_angle=true_robot_meas_noise_angle_std) 47 | 48 | # Number of particles 49 | number_of_particles = 500 50 | 51 | # IMPOVERISHMENT: artificially set to unreasonably low value for process model noise 52 | # Note we are redefining the common settings used in our shared simulation settings file. 53 | motion_model_forward_std = 0.003 54 | motion_model_turn_std = 0.003 55 | process_noise = [motion_model_forward_std, motion_model_turn_std] 56 | 57 | # Resampling algorithm used 58 | resampling_algorithm = ResamplingAlgorithms.MULTINOMIAL 59 | 60 | # Initialize SIR particle filter 61 | particle_filter_sir = ParticleFilterSIR( 62 | number_of_particles, 63 | pf_state_limits, 64 | process_noise, 65 | measurement_noise, 66 | resampling_algorithm) 67 | particle_filter_sir.initialize_particles_uniform() 68 | 69 | # Start simulation 70 | for i in range(num_time_steps): 71 | 72 | # Simulate robot move, simulate measurement and update particle filter 73 | robot.move(robot_setpoint_motion_forward, 74 | robot_setpoint_motion_turn, 75 | world) 76 | measurements = robot.measure(world) 77 | particle_filter_sir.update(robot_setpoint_motion_forward, 78 | robot_setpoint_motion_turn, 79 | measurements,world.landmarks) 80 | 81 | # Visualize weighted average all particles only 82 | visualizer.draw_world(world, robot, particle_filter_sir.particles, hold_on=True) 83 | 84 | # Draw all particle at last time step 85 | visualizer.draw_world(world, robot, particle_filter_sir.particles, hold_on=True) 86 | 87 | plt.show() 88 | 89 | -------------------------------------------------------------------------------- /challenge3_divergence.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Simulation + plotting requires a robot, visualizer and world 4 | from simulator import Robot, Visualizer, World 5 | 6 | # Supported resampling methods (resampling algorithm enum for SIR and SIR-derived particle filters) 7 | from core.resampling import ResamplingAlgorithms 8 | 9 | # Load variables 10 | from shared_simulation_settings import * 11 | 12 | # Particle filters 13 | from core.particle_filters import ParticleFilterSIR 14 | 15 | # For showing plots (plt.show()) 16 | import matplotlib.pyplot as plt 17 | 18 | 19 | if __name__ == '__main__': 20 | """ 21 | This file demonstrates particle filter that is caused by setting a too low number of particles. Note that (due to 22 | the stochastic nature of the filter it could happen that the filter works fine during this short simulation. 23 | However, that will be the exception rather than the rule. 24 | """ 25 | 26 | print("Starting demonstration of particle filter divergence.") 27 | 28 | ## 29 | # Set simulated world and visualization properties 30 | ## 31 | world = World(world_size_x, world_size_y, landmark_positions) 32 | visualizer = Visualizer() 33 | num_time_steps = 20 34 | 35 | # Initialize simulated robot 36 | robot = Robot(x=world.x_max * 0.75, 37 | y=world.y_max / 5.0, 38 | theta=3.14 / 2.0, 39 | std_forward=true_robot_motion_forward_std, 40 | std_turn=true_robot_motion_turn_std, 41 | std_meas_distance=true_robot_meas_noise_distance_std, 42 | std_meas_angle=true_robot_meas_noise_angle_std) 43 | 44 | ## 45 | # Particle filter settings 46 | # The process and measurement model noise is not equal to true noise. 47 | ## 48 | 49 | # Demonstrate divergence -> set number of particles too low 50 | number_of_particles = 100 # instead of 500 or even 1000 51 | 52 | # Initialize particle filter 53 | 54 | # Set resampling algorithm used (where applicable) 55 | resampling_algorithm = ResamplingAlgorithms.MULTINOMIAL 56 | 57 | # Initialize SIR particle filter 58 | particle_filter_sir = ParticleFilterSIR( 59 | number_of_particles, 60 | pf_state_limits, 61 | process_noise, 62 | measurement_noise, 63 | resampling_algorithm) 64 | particle_filter_sir.initialize_particles_uniform() 65 | 66 | # Start simulation 67 | for i in range(num_time_steps): 68 | # Simulate robot move, perform measurements and update particle filter 69 | robot.move(robot_setpoint_motion_forward, 70 | robot_setpoint_motion_turn, 71 | world) 72 | measurements = robot.measure(world) 73 | particle_filter_sir.update(robot_setpoint_motion_forward, 74 | robot_setpoint_motion_turn, 75 | measurements, 76 | world.landmarks) 77 | 78 | # Visualize particles after initialization (to avoid cluttered visualization) 79 | visualizer.draw_world(world, robot, particle_filter_sir.particles, hold_on=True) 80 | 81 | # Draw all particle at last time step 82 | visualizer.draw_world(world, robot, particle_filter_sir.particles, hold_on=True) 83 | 84 | plt.show() -------------------------------------------------------------------------------- /challenge5_adaptive_particle_filter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Simulation + plotting requires a robot, visualizer and world 4 | from simulator import Robot, Visualizer, World 5 | 6 | # Supported resampling methods (resampling algorithm enum for SIR) 7 | from core.resampling import ResamplingAlgorithms 8 | 9 | # Load variables 10 | from shared_simulation_settings import * 11 | 12 | # Particle filters that will be compared 13 | from core.particle_filters import AdaptiveParticleFilterKld, ParticleFilterSIR 14 | 15 | # For showing plots (plt.show()) 16 | import matplotlib.pyplot as plt 17 | 18 | import numpy as np 19 | import copy 20 | 21 | if __name__ == '__main__': 22 | """ 23 | This file demonstrates the difference between the SIR particle filter that has a constant number of particles and 24 | the adaptive particle filter that varies the number of particles on the fly. Afterwards, the number of particles 25 | for both particle filters are plotted over time together with the estimation error of the robot's x-position. The 26 | results show that the adaptive particle filter achieves a similar estimation accuracy with less particles once the 27 | particles have converged. 28 | """ 29 | 30 | print("Starting adaptive particle filter demo.") 31 | 32 | ## 33 | # Set simulated world and visualization properties 34 | ## 35 | world = World(world_size_x, world_size_y, landmark_positions) 36 | visualizer = Visualizer() 37 | num_time_steps = 50 38 | 39 | # Initialize simulated robot 40 | robot = Robot(x=world.x_max * 0.75, 41 | y=world.y_max / 5.0, 42 | theta=3.14 / 2.0, 43 | std_forward=true_robot_motion_forward_std, 44 | std_turn=true_robot_motion_turn_std, 45 | std_meas_distance=true_robot_meas_noise_distance_std, 46 | std_meas_angle=true_robot_meas_noise_angle_std) 47 | 48 | ## 49 | # Particle filter settings 50 | # The process and measurement model noise is not equal to true noise. 51 | ## 52 | 53 | # Number of particles 54 | number_of_particles = 750 55 | 56 | # Initialize particle filter 57 | 58 | # Set resampling algorithm used (where applicable) 59 | resampling_algorithm = ResamplingAlgorithms.MULTINOMIAL 60 | 61 | # Process model noise (values used in the paper) 62 | motion_model_forward_std = 0.20 63 | motion_model_turn_std = 0.05 64 | process_noise = [motion_model_forward_std, motion_model_turn_std] 65 | 66 | # Initialize SIR particle filter 67 | particle_filter_sir = ParticleFilterSIR( 68 | number_of_particles, 69 | pf_state_limits, 70 | process_noise, 71 | measurement_noise, 72 | resampling_algorithm) 73 | particle_filter_sir.initialize_particles_uniform() 74 | 75 | # Adaptive particle filter specific settings (see AdaptiveParticleFilterKld constructor for more extensive 76 | # documentation 77 | resolutions_grid = [0.2, 0.2, 0.3] 78 | epsilon = 0.15 79 | upper_quantile = 3 80 | min_number_of_particles = 50 81 | max_number_of_particles = 2e4 82 | 83 | # Initialize adaptive particle filter with the same set of particles 84 | adaptive_particle_filter_kld = AdaptiveParticleFilterKld( 85 | number_of_particles, 86 | pf_state_limits, 87 | process_noise, 88 | measurement_noise, 89 | resolutions_grid, 90 | epsilon, 91 | upper_quantile, 92 | min_number_of_particles, 93 | max_number_of_particles) 94 | adaptive_particle_filter_kld.set_particles(copy.deepcopy(particle_filter_sir.particles)) 95 | 96 | ## 97 | # Start simulation 98 | ## 99 | errors_kld = [] 100 | npar_kld = [] 101 | errors_sir = [] 102 | npar_sir = [] 103 | for i in range(num_time_steps): 104 | 105 | 106 | # Simulate robot motion (required motion will not exactly be achieved) 107 | robot.move(desired_distance=robot_setpoint_motion_forward, 108 | desired_rotation=robot_setpoint_motion_turn, 109 | world=world) 110 | 111 | # Simulate measurement 112 | measurements = robot.measure(world) 113 | 114 | # Real robot pose 115 | robot_pose = np.array([robot.x, robot.y, robot.theta]) 116 | 117 | # Update adaptive particle filter KLD 118 | adaptive_particle_filter_kld.update(robot_forward_motion=robot_setpoint_motion_forward, 119 | robot_angular_motion=robot_setpoint_motion_turn, 120 | measurements=measurements, 121 | landmarks=world.landmarks) 122 | errors_kld.append(robot_pose - np.asarray(adaptive_particle_filter_kld.get_average_state())) 123 | npar_kld.append(len(adaptive_particle_filter_kld.particles)) 124 | 125 | # Update SIR particle filter 126 | particle_filter_sir.update(robot_forward_motion=robot_setpoint_motion_forward, 127 | robot_angular_motion=robot_setpoint_motion_turn, 128 | measurements=measurements, 129 | landmarks=world.landmarks) 130 | errors_sir.append(robot_pose - np.asarray(particle_filter_sir.get_average_state())) 131 | npar_sir.append(len(particle_filter_sir.particles)) 132 | 133 | print(npar_kld) 134 | 135 | # Show results 136 | fontSize = 18 137 | plt.rcParams.update({'font.size': fontSize}) 138 | plt.subplot(211) 139 | l1, = plt.plot([error[0] for error in errors_sir], 'k-') 140 | l2, = plt.plot([error[0] for error in errors_kld], 'r--') 141 | l1.set_label('Standard') 142 | l2.set_label('Adaptive') 143 | plt.legend() 144 | plt.ylabel('Error x-position [m]') 145 | plt.subplot(212) 146 | plt.plot([npar for npar in npar_sir], 'k-') 147 | plt.plot([npar for npar in npar_kld], 'r--') 148 | plt.yscale('symlog') 149 | plt.legend() 150 | plt.xlabel('Time index') 151 | plt.ylabel('Number of particles') 152 | plt.show() -------------------------------------------------------------------------------- /core/particle_filters/__init__.py: -------------------------------------------------------------------------------- 1 | from .particle_filter_sir import ParticleFilterSIR 2 | from .particle_filter_range_only import ParticleFilterRangeOnly 3 | from .auxiliary_particle_filter import AuxiliaryParticleFilter 4 | from .adaptive_particle_filter_kld import AdaptiveParticleFilterKld 5 | from .adaptive_particle_filter_sl import AdaptiveParticleFilterSl 6 | from .particle_filter_nepr import ParticleFilterNEPR 7 | from .particle_filter_max_weight_resampling import ParticleFilterMWR 8 | from .extended_kalman_particle_filter import KalmanParticleFilter 9 | -------------------------------------------------------------------------------- /core/particle_filters/adaptive_particle_filter_kld.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from .particle_filter_base import ParticleFilter 4 | 5 | from core.resampling import generate_sample_index, compute_required_number_of_particles_kld 6 | 7 | 8 | class AdaptiveParticleFilterKld(ParticleFilter): 9 | """ 10 | Notes: 11 | * State is (x, y, heading), where x and y are in meters and heading in radians 12 | * State space assumed limited size in each dimension, world is cyclic (hence leaving at x_max means entering at 13 | x_min) 14 | """ 15 | 16 | def __init__(self, 17 | number_of_particles, 18 | limits, 19 | process_noise, 20 | measurement_noise, 21 | resolutions, 22 | epsilon, 23 | upper_quantile, 24 | min_number_particles, 25 | max_number_particles): 26 | """ 27 | Initialize the adaptive particle filter using Kullback-Leibler divergence (KLD) sampling proposed in [1]. 28 | 29 | [1] Fox, Dieter. "Adapting the sample size in particle filters through KLD-sampling." The international Journal 30 | of robotics research 22.12 (2003): 985-1003. 31 | 32 | :param number_of_particles: Number of particles. 33 | :param limits: List with maximum and minimum values for x and y dimension: [xmin, xmax, ymin, ymax]. 34 | :param process_noise: Process noise parameters (standard deviations): [std_forward, std_angular]. 35 | :param measurement_noise: Measurement noise parameters (standard deviations): [std_range, std_angle]. 36 | :param resolutions: Resolution of 3D-histogram used to approximate the true posterior distribution 37 | (x, y, theta). 38 | :param epsilon: Maximum allowed distance (error) between true and estimated distribution when computing number 39 | of required particles. 40 | :param upper_quantile: Upper standard normal distribution quantile for (1-delta) where delta is the probability. 41 | that the error on the estimated distribution will be less than epsilon. 42 | :param min_number_particles: Minimum number of particles that must be used. 43 | :param max_number_particles: Maximum number of particles that can be used. 44 | """ 45 | # Initialize particle filter base class 46 | ParticleFilter.__init__(self, number_of_particles, limits, process_noise, measurement_noise) 47 | 48 | # Set adaptive particle filter specific properties 49 | self.resolutions = resolutions 50 | self.epsilon = epsilon 51 | self.upper_quantile = upper_quantile 52 | self.minimum_number_of_particles = int(min_number_particles) 53 | self.maximum_number_of_particles = int(max_number_particles) 54 | 55 | def update(self, robot_forward_motion, robot_angular_motion, measurements, landmarks): 56 | """ 57 | Process a measurement given the measured robot displacement and adopting the Kullback-Leibler divergence 58 | sampling method (KLD-sampling) proposed by Dieter Fox. 59 | 60 | Assumption in this implementation: world starts at x=0, y=0, theta=0 (for mapping particle state to histogram 61 | bin). 62 | 63 | :param robot_forward_motion: Measured forward robot motion in meters. 64 | :param robot_angular_motion: Measured angular robot motion in radians. 65 | :param measurements: Measurements. 66 | :param landmarks: Landmark positions. 67 | """ 68 | 69 | # Store new samples 70 | new_particles = [] 71 | 72 | bins_with_support = [] 73 | number_of_new_particles = 0 74 | number_of_bins_with_support = 0 75 | number_of_required_particles = self.minimum_number_of_particles 76 | while number_of_new_particles < number_of_required_particles: 77 | 78 | # Get sample from discrete distribution given by particle weights 79 | index_j = generate_sample_index(self.particles) 80 | 81 | # Propagate state of selected particle 82 | propaged_state = self.propagate_sample(self.particles[index_j][1], 83 | robot_forward_motion, 84 | robot_angular_motion) 85 | 86 | # Compute the weight that this propagated state would get with the current measurement 87 | importance_weight = self.compute_likelihood(propaged_state, measurements, landmarks) 88 | 89 | # Add weighted particle to new particle set 90 | new_particles.append([importance_weight, propaged_state]) 91 | number_of_new_particles += 1 92 | 93 | # Next, we convert the discrete distribution of all new samples into a histogram. We must check if the new 94 | # state (propagated_state) falls in a histogram bin with support or in an empty bin. We keep track of the 95 | # number of bins with support. Instead of adopting a (more efficient) tree, a simple list is used to 96 | # store all bin indices with support since there is are no performance requirements for our use case. 97 | 98 | # Map state to bin indices 99 | indices = [np.floor(propaged_state[0] / self.resolutions[0]), 100 | np.floor(propaged_state[1] / self.resolutions[1]), 101 | np.floor(propaged_state[2] / self.resolutions[2])] 102 | 103 | # Add indices if this bin is empty (i.e. is not in list yet) 104 | if indices not in bins_with_support: 105 | bins_with_support.append(indices) 106 | number_of_bins_with_support += 1 107 | 108 | # Update number of required particles (only defined if number of bins with support above 1) 109 | if number_of_bins_with_support > 1: 110 | number_of_required_particles = compute_required_number_of_particles_kld(number_of_bins_with_support, 111 | self.epsilon, 112 | self.upper_quantile) 113 | 114 | # Make sure number of particles constraints are not violated 115 | number_of_required_particles = max(number_of_required_particles, self.minimum_number_of_particles) 116 | number_of_required_particles = min(number_of_required_particles, self.maximum_number_of_particles) 117 | 118 | # Store new particle set and normalize weights 119 | self.particles = self.normalize_weights(new_particles) 120 | -------------------------------------------------------------------------------- /core/particle_filters/adaptive_particle_filter_sl.py: -------------------------------------------------------------------------------- 1 | from .particle_filter_base import ParticleFilter 2 | from core.resampling import generate_sample_index 3 | 4 | 5 | class AdaptiveParticleFilterSl(ParticleFilter): 6 | """ 7 | Notes: 8 | * State is (x, y, heading), where x and y are in meters and heading in radians 9 | * State space assumed limited size in each dimension, world is cyclic (hence leaving at x_max means entering at 10 | x_min) 11 | """ 12 | 13 | def __init__(self, 14 | number_of_particles, 15 | limits, 16 | process_noise, 17 | measurement_noise, 18 | sum_likelihoods_threshold, 19 | max_number_particles): 20 | """ 21 | Initialize the adaptive particle filter using sum of likelihoods sampling proposed explained in [1,2]. 22 | 23 | [1] Straka, Ondrej, and Miroslav Simandl. "A survey of sample size adaptation techniques for particle filters." 24 | IFAC Proceedings Volumes 42.10 (2009): 1358-1363. 25 | [2] Koller, Daphne, and Raya Fratkina. "Using Learning for Approximation in Stochastic Processes." ICML. 1998. 26 | 27 | :param number_of_particles: Number of particles. 28 | :param limits: List with maximum and minimum values for x and y dimension: [xmin, xmax, ymin, ymax]. 29 | :param process_noise: Process noise parameters (standard deviations): [std_forward, std_angular]. 30 | :param measurement_noise: Measurement noise parameters (standard deviations): [std_range, std_angle]. 31 | :param sum_likelihoods_threshold: Minimum sum of all measurement likelihoods after update step. 32 | :param max_number_particles: Maximum number of particles that can be used. 33 | """ 34 | # Initialize particle filter base class 35 | ParticleFilter.__init__(self, number_of_particles, limits, process_noise, measurement_noise) 36 | 37 | # Set adaptive particle filter specific properties 38 | self.maximum_number_of_particles = max_number_particles 39 | self.sum_likelihoods_threshold = sum_likelihoods_threshold 40 | 41 | def update(self, robot_forward_motion, robot_angular_motion, measurements, landmarks): 42 | """ 43 | Process a measurement given the measured robot displacement. Continue core as long as the sum of all 44 | sampled particles is below a predefined threshold and the number of particles does not exceed the predefined 45 | maximum number of particles. The minimum number of particles is equal to the threhold since a measurement 46 | likelihoods can not exceed one by definition. 47 | 48 | :param robot_forward_motion: Measured forward robot motion in meters. 49 | :param robot_angular_motion: Measured angular robot motion in radians. 50 | :param measurements: Measurements. 51 | :param landmarks: Landmark positions. 52 | """ 53 | 54 | # Store new samples 55 | new_particles = [] 56 | 57 | sum_likelihoods = 0 58 | number_of_new_particles = 0 59 | while sum_likelihoods < self.sum_likelihoods_threshold and \ 60 | number_of_new_particles < self.maximum_number_of_particles: 61 | 62 | # Get sample from discrete distribution given by particle weights 63 | index_j = generate_sample_index(self.particles) 64 | 65 | # Propagate state of selected particle 66 | propaged_state = self.propagate_sample(self.particles[index_j][1], 67 | robot_forward_motion, 68 | robot_angular_motion) 69 | 70 | # Compute the weight that this propagated state would get with the current measurement 71 | importance_weight = self.compute_likelihood(propaged_state, measurements, landmarks) 72 | 73 | # Add weighted particle to new particle set 74 | new_particles.append([importance_weight, propaged_state]) 75 | 76 | # Administration 77 | sum_likelihoods += importance_weight 78 | number_of_new_particles += 1 79 | 80 | # Store new particle set and normalize weights 81 | self.particles = self.normalize_weights(new_particles) 82 | -------------------------------------------------------------------------------- /core/particle_filters/auxiliary_particle_filter.py: -------------------------------------------------------------------------------- 1 | from .particle_filter_base import ParticleFilter 2 | from core.resampling import naive_search, cumulative_sum 3 | 4 | import numpy as np 5 | 6 | 7 | class AuxiliaryParticleFilter(ParticleFilter): 8 | """ 9 | Notes: 10 | * State is (x, y, heading), where x and y are in meters and heading in radians 11 | * State space assumed limited size in each dimension, world is cyclic (hence leaving at x_max means entering at 12 | x_min) 13 | """ 14 | 15 | @staticmethod 16 | def sample_multinomial_indices(samples): 17 | """ 18 | Particles indices are sampled with replacement proportional to their weight an in arbitrary order. This leads 19 | to a maximum variance on the number of times a particle will be resampled, since any particle will be resampled 20 | between 0 and N times. 21 | Computational complexity: O(N log(M) 22 | 23 | :param samples: Samples that must be resampled. 24 | :return: Resampled indices. 25 | """ 26 | 27 | # Number of samples 28 | N = len(samples) 29 | 30 | # Get list with only weights 31 | weights = [weighted_sample[0] for weighted_sample in samples] 32 | 33 | # Compute cumulative sum 34 | Q = cumulative_sum(weights) 35 | 36 | # As long as the number of new samples is insufficient 37 | n = 0 38 | new_indices = [] 39 | while n < N: 40 | # Draw a random sample u 41 | u = np.random.uniform(1e-6, 1, 1)[0] 42 | 43 | # Get first sample for which cumulative sum is above u using naive search 44 | m = naive_search(Q, u) 45 | 46 | # Store index 47 | new_indices.append(m) 48 | 49 | # Added another sample 50 | n += 1 51 | 52 | return new_indices 53 | 54 | def update(self, robot_forward_motion, robot_angular_motion, measurements, landmarks): 55 | """ 56 | Process a measurement given the measured robot displacement and adopting the auxiliary sampling importance 57 | core (ASIR) scheme explained in [1]. 58 | 59 | [1] M. S. Arulampalam, S. Maskell, N. Gordon and T. Clapp, "A tutorial on particle filters for online 60 | nonlinear/non-Gaussian Bayesian tracking," in IEEE Transactions on Signal Processing, vol. 50, no. 2, pp. 61 | 174-188, Feb. 2002. 62 | 63 | :param robot_forward_motion: Measured forward robot motion in meters. 64 | :param robot_angular_motion: Measured angular robot motion in radians. 65 | :param measurements: Measurements. 66 | :param landmarks: Landmark positions. 67 | """ 68 | 69 | # First loop: propagate characterizations and compute weights 70 | tmp_particles = [] 71 | tmp_likelihoods = [] 72 | for par in self.particles: 73 | 74 | # Compute characterization 75 | mu = self.propagate_sample(par[1], robot_forward_motion, robot_angular_motion) 76 | 77 | # Compute and store current particle's weight 78 | likelihood = self.compute_likelihood(mu, measurements, landmarks) 79 | weight = likelihood * par[0] 80 | tmp_likelihoods.append(likelihood) 81 | 82 | # Store (notice mu will not be used later) 83 | tmp_particles.append([weight, mu]) 84 | 85 | # Normalize particle weights 86 | tmp_particles = self.normalize_weights(tmp_particles) 87 | 88 | # Resample indices from propagated particles 89 | new_indices = self.sample_multinomial_indices(tmp_particles) 90 | 91 | # Second loop: now propagate the state of all particles indices that survived 92 | new_samples = [] 93 | for idx in new_indices: 94 | 95 | # Get particle state associated with current index from original set of particles 96 | par = self.particles[idx] 97 | 98 | # Propagate the particle state 99 | propagated_state = self.propagate_sample(par[1], robot_forward_motion, robot_angular_motion) 100 | 101 | # Compute current particle's weight using the measurement likelihood of the characterization 102 | wi_tmp = tmp_likelihoods[idx] 103 | if wi_tmp < 1e-10: 104 | wi_tmp = 1e-10 # avoid division by zero 105 | weight = self.compute_likelihood(propagated_state, measurements, landmarks) / wi_tmp 106 | 107 | # Store 108 | new_samples.append([weight, propagated_state]) 109 | 110 | # Update particles 111 | self.particles = self.normalize_weights(new_samples) 112 | -------------------------------------------------------------------------------- /core/particle_filters/extended_kalman_particle_filter.py: -------------------------------------------------------------------------------- 1 | from .particle_filter_base import ParticleFilter 2 | from core.resampling.resampler import Resampler 3 | 4 | import copy 5 | import numpy as np 6 | from scipy.stats import multivariate_normal 7 | from scipy import linalg 8 | 9 | 10 | class KalmanParticleFilter(ParticleFilter): 11 | """ 12 | Notes: 13 | * State is (x, y, heading), where x and y are in meters and heading in radians 14 | * State space assumed limited size in each dimension, world is cyclic (hence leaving at x_max means entering at 15 | x_min) 16 | * propagation and measurement models are largely hardcoded (except for standard deviations). 17 | 18 | For convenience reasons this class inherits from the generic particle filter class. Note however, that some of 19 | the methods are hardcoded and some of the members are unused. 20 | """ 21 | 22 | def __init__(self, 23 | number_of_particles, 24 | limits, 25 | process_noise, 26 | measurement_noise): 27 | """ 28 | Initialize the extended Kalman particle filter. Resampling method is hardcoded hence no argument. 29 | 30 | :param number_of_particles: Number of particles. 31 | :param limits: List with maximum and minimum values for x and y dimension: [xmin, xmax, ymin, ymax]. 32 | :param process_noise: Process noise parameters (standard deviations): [std_forward, std_angular]. 33 | :param measurement_noise: Measurement noise parameters (standard deviations): [std_range, std_angle]. 34 | """ 35 | 36 | # Initialize particle filter base class 37 | ParticleFilter.__init__(self, number_of_particles, limits, process_noise, measurement_noise) 38 | 39 | # Initialize covariance matrices for the EKF 40 | self.Q = np.diag([process_noise[0], process_noise[0], process_noise[1]]) 41 | self.R = np.diag([measurement_noise[0], measurement_noise[1]]) 42 | 43 | def initialize_particles_uniform(self): 44 | """ 45 | Initialize the particles uniformly over the world assuming a 3D state (x, y, heading). No arguments are required 46 | and function always succeeds hence no return value. 47 | """ 48 | 49 | # Initialize particles with uniform weight distribution 50 | super(KalmanParticleFilter, self).initialize_particles_uniform() 51 | 52 | # Add covariance matrices to particles (now particle is following list: [weight, particle_state, 53 | # covariance_matrix]) 54 | for particle in self.particles: 55 | particle.append(np.eye(3)) 56 | 57 | def multinomial_resampling(self): 58 | """ 59 | Particles are sampled with replacement proportional to their weight and in arbitrary order. This leads 60 | to a maximum variance on the number of times a particle will be resampled, since any particle will be 61 | resampled between 0 and N times. 62 | 63 | This function is reimplemented in this class since each particle now contains three elements instead of two 64 | (covariance of extended Kalman filter is added). 65 | """ 66 | 67 | # Get list with only weights 68 | weights = [weighted_sample[0] for weighted_sample in self.particles] 69 | 70 | # Compute cumulative sum 71 | Q = np.cumsum(weights).tolist() 72 | 73 | # As long as the number of new particles is insufficient 74 | n = 0 75 | new_particles = [] 76 | while n < self.n_particles: 77 | 78 | # Draw a random sample u 79 | u = np.random.uniform(1e-6, 1, 1)[0] 80 | 81 | # Naive search 82 | m = 0 83 | while Q[m] < u: 84 | m += 1 85 | 86 | # Add copy of the particle but set uniform weight 87 | new_sample = copy.deepcopy(self.particles[m]) 88 | new_sample[0] = 1.0/self.n_particles 89 | new_particles.append(new_sample) 90 | 91 | # Added another sample 92 | n += 1 93 | 94 | self.particles = new_particles 95 | 96 | def update(self, robot_forward_motion, robot_angular_motion, measurements, landmarks): 97 | """ 98 | Process a measurement given the measured robot displacement and resample. 99 | 100 | :param robot_forward_motion: Measured forward robot motion in meters. 101 | :param robot_angular_motion: Measured angular robot motion in radians. 102 | :param measurements: Measurements. 103 | :param landmarks: Landmark positions. 104 | """ 105 | 106 | # Loop over all particles 107 | new_particles = [] 108 | sum_weights = 0.0 109 | for par in self.particles: 110 | 111 | # This time an extended Kalman filter prediction is included in the propagation step 112 | # note: par = [weight, state, EKF_covariance] 113 | propagated_state = copy.deepcopy(par[1]) 114 | cov_ekf = copy.deepcopy(par[2]) 115 | 116 | # Propagate the particle state using the nonlinear process model 117 | propagated_state[2] += robot_angular_motion 118 | propagated_state[0] += robot_forward_motion * np.cos(propagated_state[2]) 119 | propagated_state[1] += robot_forward_motion * np.sin(propagated_state[2]) 120 | self.validate_state(propagated_state) 121 | 122 | # Compute Jacobian (df/dx) around current state 123 | F = np.eye(3) 124 | F[0,2] = -robot_forward_motion * np.sin(propagated_state[2]) 125 | F[1,2] = robot_forward_motion * np.cos(propagated_state[2]) 126 | 127 | # Update covariance 128 | cov_ekf = F * cov_ekf * np.transpose(F) + self.Q 129 | 130 | # Process measurements one by one (EKF update step) 131 | updated_state_ekf = propagated_state 132 | for idx, landmark in enumerate(landmarks): 133 | 134 | # Compute expected measurement 135 | dx = updated_state_ekf[0] - landmark[0] 136 | dy = updated_state_ekf[1] - landmark[1] 137 | z1_exp = np.sqrt((dx*dx + dy*dy)) 138 | z2_exp = np.arctan2(dy, dx) 139 | 140 | # Compute Jacobian (dh/dx) around propagated state 141 | H11 = dx / (np.sqrt(dx*dx + dy*dy)) 142 | H12 = dy / (np.sqrt(dx*dx + dy*dy)) 143 | H21 = 1 / (1 + (dy / dx)**2) * -dy / dx**2 144 | H22 = 1 / (1 + (dy / dx)**2) * 1 / dx 145 | H = np.array([[H11, H12, 0], [H21, H22, 0]]) 146 | 147 | # Innovation 148 | y_tilde = np.array([[measurements[idx][0] - z1_exp], [measurements[idx][1] - z2_exp]]) 149 | S = np.dot(np.dot(H, cov_ekf), np.transpose(H)) + self.R 150 | 151 | # Kalman gain 152 | K = np.dot(np.dot(cov_ekf, np.transpose(H)), linalg.pinv(S)) 153 | 154 | # Update state vector and covariance 155 | delta_state = np.dot(K, y_tilde) 156 | updated_state_ekf[0] += delta_state[0][0] 157 | updated_state_ekf[1] += delta_state[1][0] 158 | updated_state_ekf[2] += delta_state[2][0] 159 | self.validate_state(updated_state_ekf) 160 | cov_ekf = np.dot((np.eye(3) - np.dot(K, H)), cov_ekf) 161 | 162 | # New particle state: sample from normal distribution EKF 163 | updated_state_pf = np.random.multivariate_normal(updated_state_ekf, cov_ekf) 164 | self.validate_state(updated_state_pf) 165 | 166 | # Compute likelihood using sampled particle state 167 | likelihood = self.compute_likelihood(updated_state_pf, measurements, landmarks) 168 | # Compute prior (mean is zero vector by default) 169 | prior = multivariate_normal.pdf(updated_state_pf-propagated_state, cov=self.Q) 170 | # Importance density 171 | importance_density = multivariate_normal.pdf(updated_state_pf-updated_state_ekf, cov=cov_ekf) 172 | # Compute current particle's weight 173 | weight = likelihood * prior / importance_density 174 | sum_weights += weight 175 | 176 | # Store updated particle 177 | new_particles.append([weight, updated_state_pf, cov_ekf]) 178 | 179 | # Normalize particle weight 180 | if sum_weights < 1e-10: 181 | print("Warning: sum particles weights very low") 182 | for par in new_particles: 183 | par[0] /= sum_weights 184 | 185 | # Update particles 186 | self.particles = new_particles 187 | 188 | # Resample at each time step 189 | self.multinomial_resampling() 190 | -------------------------------------------------------------------------------- /core/particle_filters/particle_filter_base.py: -------------------------------------------------------------------------------- 1 | from abc import abstractmethod 2 | import copy 3 | import numpy as np 4 | 5 | 6 | class ParticleFilter: 7 | """ 8 | Notes: 9 | * State is (x, y, heading), where x and y are in meters and heading in radians 10 | * State space assumed limited size in each dimension, world is cyclic (hence leaving at x_max means entering at 11 | x_min) 12 | * Abstract class 13 | """ 14 | 15 | def __init__(self, number_of_particles, limits, process_noise, measurement_noise): 16 | """ 17 | Initialize the abstract particle filter. 18 | 19 | :param number_of_particles: Number of particles 20 | :param limits: List with maximum and minimum values for x and y dimension: [xmin, xmax, ymin, ymax] 21 | :param process_noise: Process noise parameters (standard deviations): [std_forward, std_angular] 22 | :param measurement_noise: Measurement noise parameters (standard deviations): [std_range, std_angle] 23 | """ 24 | 25 | if number_of_particles < 1: 26 | print("Warning: initializing particle filter with number of particles < 1: {}".format(number_of_particles)) 27 | 28 | # Initialize filter settings 29 | self.n_particles = number_of_particles 30 | self.particles = [] 31 | 32 | # State related settings 33 | self.state_dimension = 3 # x, y, theta 34 | self.x_min = limits[0] 35 | self.x_max = limits[1] 36 | self.y_min = limits[0] 37 | self.y_max = limits[1] 38 | 39 | # Set noise 40 | self.process_noise = process_noise 41 | self.measurement_noise = measurement_noise 42 | 43 | def initialize_particles_uniform(self): 44 | """ 45 | Initialize the particles uniformly over the world assuming a 3D state (x, y, heading). No arguments are required 46 | and function always succeeds hence no return value. 47 | """ 48 | 49 | # Initialize particles with uniform weight distribution 50 | self.particles = [] 51 | weight = 1.0 / self.n_particles 52 | for i in range(self.n_particles): 53 | # Add particle i 54 | self.particles.append( 55 | [weight, [ 56 | np.random.uniform(self.x_min, self.x_max, 1)[0], 57 | np.random.uniform(self.y_min, self.y_max, 1)[0], 58 | np.random.uniform(0, 2 * np.pi, 1)[0]] 59 | ] 60 | ) 61 | 62 | def initialize_particles_gaussian(self, mean_vector, standard_deviation_vector): 63 | """ 64 | Initialize particle filter using a Gaussian distribution with dimension three: x, y, heading. Only standard 65 | deviations can be provided hence the covariances are all assumed zero. 66 | 67 | :param mean_vector: Mean of the Gaussian distribution used for initializing the particle states 68 | :param standard_deviation_vector: Standard deviations (one for each dimension) 69 | :return: Boolean indicating success 70 | """ 71 | 72 | # Check input dimensions 73 | if len(mean_vector) != self.state_dimension or len(standard_deviation_vector) != self.state_dimension: 74 | print("Means and state deviation vectors have incorrect length in initialize_particles_gaussian()") 75 | return False 76 | 77 | # Initialize particles with uniform weight distribution 78 | self.particles = [] 79 | weight = 1.0 / self.n_particles 80 | for i in range(self.n_particles): 81 | 82 | # Get state sample 83 | state_i = np.random.normal(mean_vector, standard_deviation_vector, self.state_dimension).tolist() 84 | 85 | # Add particle i 86 | self.particles.append([weight, self.validate_state(state_i)]) 87 | 88 | def validate_state(self, state): 89 | """ 90 | Validate the state. State values outide allowed ranges will be corrected for assuming a 'cyclic world'. 91 | 92 | :param state: Input particle state. 93 | :return: Validated particle state. 94 | """ 95 | 96 | # Make sure state does not exceed allowed limits (cyclic world) 97 | while state[0] < self.x_min: 98 | state[0] += (self.x_max - self.x_min) 99 | while state[0] > self.x_max: 100 | state[0] -= (self.x_max - self.x_min) 101 | while state[1] < self.y_min: 102 | state[1] += (self.y_max - self.y_min) 103 | while state[1] > self.y_max: 104 | state[1] -= (self.y_max - self.y_min) 105 | 106 | # Angle must be [-pi, pi] 107 | while state[2] > np.pi: 108 | state[2] -= 2 * np.pi 109 | while state[2] < -np.pi: 110 | state[2] += 2 * np.pi 111 | 112 | return state 113 | 114 | def set_particles(self, particles): 115 | """ 116 | Initialize the particle filter using the given set of particles. 117 | 118 | :param particles: Initial particle set: [[weight_1, [x1, y1, theta1]], ..., [weight_n, [xn, yn, thetan]]] 119 | """ 120 | 121 | # Assumption: particle have correct format, set particles 122 | self.particles = copy.deepcopy(particles) 123 | self.n_particles = len(self.particles) 124 | 125 | def get_average_state(self): 126 | """ 127 | Compute average state according to all weighted particles 128 | 129 | :return: Average x-position, y-position and orientation 130 | """ 131 | 132 | # Compute sum of all weights 133 | sum_weights = 0.0 134 | for weighted_sample in self.particles: 135 | sum_weights += weighted_sample[0] 136 | 137 | # Compute weighted average 138 | avg_x = 0.0 139 | avg_y = 0.0 140 | avg_theta = 0.0 141 | for weighted_sample in self.particles: 142 | avg_x += weighted_sample[0] / sum_weights * weighted_sample[1][0] 143 | avg_y += weighted_sample[0] / sum_weights * weighted_sample[1][1] 144 | avg_theta += weighted_sample[0] / sum_weights * weighted_sample[1][2] 145 | 146 | return [avg_x, avg_y, avg_theta] 147 | 148 | def get_max_weight(self): 149 | """ 150 | Find maximum weight in particle filter. 151 | 152 | :return: Maximum particle weight 153 | """ 154 | return max([weighted_sample[0] for weighted_sample in self.particles]) 155 | 156 | 157 | def print_particles(self): 158 | """ 159 | Print all particles: index, state and weight. 160 | """ 161 | 162 | print("Particles:") 163 | for i in range(self.n_particles): 164 | print(" ({}): {} with w: {}".format(i+1, self.particles[i][1], self.particles[i][0])) 165 | 166 | @staticmethod 167 | def normalize_weights(weighted_samples): 168 | """ 169 | Normalize all particle weights. 170 | """ 171 | 172 | # Compute sum weighted samples 173 | sum_weights = 0.0 174 | for weighted_sample in weighted_samples: 175 | sum_weights += weighted_sample[0] 176 | 177 | # Check if weights are non-zero 178 | if sum_weights < 1e-15: 179 | print("Weight normalization failed: sum of all weights is {} (weights will be reinitialized)".format(sum_weights)) 180 | 181 | # Set uniform weights 182 | return [[1.0 / len(weighted_samples), weighted_sample[1]] for weighted_sample in weighted_samples] 183 | 184 | # Return normalized weights 185 | return [[weighted_sample[0] / sum_weights, weighted_sample[1]] for weighted_sample in weighted_samples] 186 | 187 | def propagate_sample(self, sample, forward_motion, angular_motion): 188 | """ 189 | Propagate an individual sample with a simple motion model that assumes the robot rotates angular_motion rad and 190 | then moves forward_motion meters in the direction of its heading. Return the propagated sample (leave input 191 | unchanged). 192 | 193 | :param sample: Sample (unweighted particle) that must be propagated 194 | :param forward_motion: Forward motion in meters 195 | :param angular_motion: Angular motion in radians 196 | :return: propagated sample 197 | """ 198 | # 1. rotate by given amount plus additive noise sample (index 1 is angular noise standard deviation) 199 | propagated_sample = copy.deepcopy(sample) 200 | propagated_sample[2] += np.random.normal(angular_motion, self.process_noise[1], 1)[0] 201 | 202 | # Compute forward motion by combining deterministic forward motion with additive zero mean Gaussian noise 203 | forward_displacement = np.random.normal(forward_motion, self.process_noise[0], 1)[0] 204 | 205 | # 2. move forward 206 | propagated_sample[0] += forward_displacement * np.cos(propagated_sample[2]) 207 | propagated_sample[1] += forward_displacement * np.sin(propagated_sample[2]) 208 | 209 | # Make sure we stay within cyclic world 210 | return self.validate_state(propagated_sample) 211 | 212 | def compute_likelihood(self, sample, measurement, landmarks): 213 | """ 214 | Compute likelihood p(z|sample) for a specific measurement given sample state and landmarks. 215 | 216 | :param sample: Sample (unweighted particle) that must be propagated 217 | :param measurement: List with measurements, for each landmark [distance_to_landmark, angle_wrt_landmark], units 218 | are meters and radians 219 | :param landmarks: Positions (absolute) landmarks (in meters) 220 | :return Likelihood 221 | """ 222 | 223 | # Initialize measurement likelihood 224 | likelihood_sample = 1.0 225 | 226 | # Loop over all landmarks for current particle 227 | for i, lm in enumerate(landmarks): 228 | 229 | # Compute expected measurement assuming the current particle state 230 | dx = sample[0] - lm[0] 231 | dy = sample[1] - lm[1] 232 | expected_distance = np.sqrt(dx*dx + dy*dy) 233 | expected_angle = np.arctan2(dy, dx) 234 | 235 | # Map difference true and expected distance measurement to probability 236 | p_z_given_x_distance = \ 237 | np.exp(-(expected_distance-measurement[i][0]) * (expected_distance-measurement[i][0]) / 238 | (2 * self.measurement_noise[0] * self.measurement_noise[0])) 239 | 240 | # Map difference true and expected angle measurement to probability 241 | p_z_given_x_angle = \ 242 | np.exp(-(expected_angle-measurement[i][1]) * (expected_angle-measurement[i][1]) / 243 | (2 * self.measurement_noise[1] * self.measurement_noise[1])) 244 | 245 | # Incorporate likelihoods current landmark 246 | likelihood_sample *= p_z_given_x_distance * p_z_given_x_angle 247 | 248 | # Return importance weight based on all landmarks 249 | return likelihood_sample 250 | 251 | @abstractmethod 252 | def update(self, robot_forward_motion, robot_angular_motion, measurements, landmarks): 253 | """ 254 | Process a measurement given the measured robot displacement. Abstract method that must be implemented in derived 255 | class. 256 | 257 | :param robot_forward_motion: Measured forward robot motion in meters. 258 | :param robot_angular_motion: Measured angular robot motion in radians. 259 | :param measurements: Measurements. 260 | :param landmarks: Landmark positions. 261 | """ 262 | 263 | pass 264 | -------------------------------------------------------------------------------- /core/particle_filters/particle_filter_max_weight_resampling.py: -------------------------------------------------------------------------------- 1 | from .particle_filter_sir import ParticleFilterSIR 2 | 3 | 4 | class ParticleFilterMWR(ParticleFilterSIR): 5 | """ 6 | Notes: 7 | * State is (x, y, heading), where x and y are in meters and heading in radians 8 | * State space assumed limited size in each dimension, world is cyclic (hence leaving at x_max means entering at 9 | x_min) 10 | * Apply core if the reciprocal of the maximum weight drops below a specific value: max weight core 11 | (MWR) 12 | """ 13 | 14 | def __init__(self, 15 | number_of_particles, 16 | limits, 17 | process_noise, 18 | measurement_noise, 19 | resampling_algorithm, 20 | resampling_threshold): 21 | """ 22 | Initialize a particle filter that performs resampling whenever the reciprocal of maximum particle weight 23 | falls below a user specified threshold value. 24 | 25 | :param number_of_particles: Number of particles. 26 | :param limits: List with maximum and minimum values for x and y dimension: [xmin, xmax, ymin, ymax]. 27 | :param process_noise: Process noise parameters (standard deviations): [std_forward, std_angular]. 28 | :param measurement_noise: Measurement noise parameters (standard deviations): [std_range, std_angle]. 29 | :param resampling_algorithm: Algorithm that must be used for core. 30 | :param resampling_threshold: Resample whenever the reciprocal of the maximum particle weight falls below this 31 | value. 32 | """ 33 | # Initialize sir particle filter class 34 | ParticleFilterSIR.__init__(self, number_of_particles, limits, process_noise, measurement_noise, resampling_algorithm) 35 | 36 | # Set MWR specific properties 37 | self.resampling_threshold = resampling_threshold 38 | 39 | def needs_resampling(self): 40 | """ 41 | Override method that determines whether or not a core step is needed for the current particle filter state 42 | estimate. Resampling only occurs if the reciprocal of the maximum particle weight falls below the user-specified 43 | threshold. The reciprocal of the maximum weight is defined by P_N^2 in [1]. 44 | 45 | [1] Martino, Luca, Victor Elvira, and Francisco Louzada. "Effective sample size for importance sampling based on 46 | discrepancy measures." Signal Processing 131 (2017): 386-401. 47 | 48 | :return: Boolean indicating whether or not core is needed. 49 | """ 50 | max_weight = 0 51 | for par in self.particles: 52 | max_weight = max(max_weight, par[0]) 53 | 54 | return 1.0 / max_weight < self.resampling_threshold 55 | -------------------------------------------------------------------------------- /core/particle_filters/particle_filter_nepr.py: -------------------------------------------------------------------------------- 1 | from .particle_filter_sir import ParticleFilterSIR 2 | 3 | 4 | class ParticleFilterNEPR(ParticleFilterSIR): 5 | """ 6 | Notes: 7 | * State is (x, y, heading), where x and y are in meters and heading in radians 8 | * State space assumed limited size in each dimension, world is cyclic (hence leaving at x_max means entering at 9 | x_min) 10 | * Apply approximate number of effective particles core (NEPR) 11 | """ 12 | 13 | def __init__(self, 14 | number_of_particles, 15 | limits, 16 | process_noise, 17 | measurement_noise, 18 | resampling_algorithm, 19 | number_of_effective_particles_threshold): 20 | """ 21 | Initialize a particle filter that performs resampling whenever the approximated number of effective particles 22 | falls below a user specified threshold value. 23 | 24 | :param number_of_particles: Number of particles. 25 | :param limits: List with maximum and minimum values for x and y dimension: [xmin, xmax, ymin, ymax]. 26 | :param process_noise: Process noise parameters (standard deviations): [std_forward, std_angular]. 27 | :param measurement_noise: Measurement noise parameters (standard deviations): [std_range, std_angle]. 28 | :param resampling_algorithm: Algorithm that must be used for core. 29 | :param number_of_effective_particles_threshold: Resample whenever approximate number of effective particles 30 | falls below this value. 31 | """ 32 | # Initialize sir particle filter class 33 | ParticleFilterSIR.__init__(self, number_of_particles, limits, process_noise, measurement_noise, resampling_algorithm) 34 | 35 | # Set NEPR specific properties 36 | self.resampling_threshold = number_of_effective_particles_threshold 37 | 38 | def needs_resampling(self): 39 | """ 40 | Override method that determines whether or not a step is needed for the current particle filter state 41 | estimate. Resampling only occurs if the approximated number of effective particles falls below the 42 | user-specified threshold. Approximate number of effective particles: 1 / (sum_i^N wi^2), P_N^2 in [1]. 43 | 44 | [1] Martino, Luca, Victor Elvira, and Francisco Louzada. "Effective sample size for importance sampling based on 45 | discrepancy measures." Signal Processing 131 (2017): 386-401. 46 | 47 | :return: Boolean indicating whether or not core is needed. 48 | """ 49 | # 50 | sum_weights_squared = 0 51 | for par in self.particles: 52 | sum_weights_squared += par[0] * par[0] 53 | 54 | return 1.0 / sum_weights_squared < self.resampling_threshold 55 | -------------------------------------------------------------------------------- /core/particle_filters/particle_filter_range_only.py: -------------------------------------------------------------------------------- 1 | from .particle_filter_base import ParticleFilter 2 | from core.resampling.resampler import Resampler 3 | 4 | import numpy as np 5 | 6 | 7 | class ParticleFilterRangeOnly(ParticleFilter): 8 | """ 9 | Notes: 10 | * State is (x, y, heading), where x and y are in meters and heading in radians 11 | * State space assumed limited size in each dimension, world is cyclic (hence leaving at x_max means entering at 12 | x_min) 13 | * propagation and measurement models are largely hardcoded (except for standard deviations). 14 | """ 15 | 16 | def __init__(self, 17 | number_of_particles, 18 | limits, 19 | process_noise, 20 | measurement_noise, 21 | resampling_algorithm): 22 | """ 23 | Initialize the SIR range measurement only particle filter. Largely copied from the SIR particle filter. 24 | 25 | :param number_of_particles: Number of particles. 26 | :param limits: List with maximum and minimum values for x and y dimension: [xmin, xmax, ymin, ymax]. 27 | :param process_noise: Process noise parameters (standard deviations): [std_forward, std_angular]. 28 | :param measurement_noise: Measurement noise parameter range (standard deviation): std_range. 29 | :param resampling_algorithm: Algorithm that must be used for core. 30 | """ 31 | # Initialize particle filter base class 32 | ParticleFilter.__init__(self, number_of_particles, limits, process_noise, measurement_noise) 33 | 34 | # Set SIR specific properties 35 | self.resampling_algorithm = resampling_algorithm 36 | self.resampler = Resampler() 37 | 38 | def needs_resampling(self): 39 | """ 40 | Method that determines whether not a core step is needed for the current particle filter state estimate. 41 | The sampling importance core (SIR) scheme resamples every time step hence always return true. 42 | 43 | :return: Boolean indicating whether or not core is needed. 44 | """ 45 | return True 46 | 47 | def compute_likelihood(self, sample, measurement, landmarks): 48 | """ 49 | Compute the importance weight p(z|sample) for a specific measurement given sample state and landmarks. 50 | 51 | :param sample: Sample (unweighted particle) that must be propagated 52 | :param measurement: List with measurements, for each landmark distance_to_landmark, unit is meters 53 | :param landmarks: Positions (absolute) landmarks (in meters) 54 | :return Importance weight 55 | """ 56 | 57 | # Initialize measurement likelihood 58 | measurement_likelihood_sample = 1.0 59 | 60 | # Loop over all landmarks for current particle 61 | for i, lm in enumerate(landmarks): 62 | 63 | # Compute expected measurement assuming the current particle state 64 | dx = sample[0] - lm[0] 65 | dy = sample[1] - lm[1] 66 | expected_distance = np.sqrt(dx*dx + dy*dy) 67 | 68 | # Map difference true and expected distance measurement to probability 69 | p_z_given_x_distance = \ 70 | np.exp(-(expected_distance-measurement[i]) * (expected_distance-measurement[i]) / 71 | (2.0 * self.measurement_noise * self.measurement_noise)) 72 | 73 | # Incorporate likelihoods current landmark 74 | measurement_likelihood_sample *= p_z_given_x_distance 75 | 76 | # Return importance weight based on all landmarks 77 | return measurement_likelihood_sample 78 | 79 | def update(self, robot_forward_motion, robot_angular_motion, measurements, landmarks): 80 | """ 81 | Process a measurement given the measured robot displacement and resample if needed. 82 | 83 | :param robot_forward_motion: Measured forward robot motion in meters. 84 | :param robot_angular_motion: Measured angular robot motion in radians. 85 | :param measurements: Measurements. 86 | :param landmarks: Landmark positions. 87 | """ 88 | 89 | # Loop over all particles 90 | new_particles = [] 91 | for par in self.particles: 92 | 93 | # Propagate the particle state according to the current particle 94 | propagated_state = self.propagate_sample(par[1], robot_forward_motion, robot_angular_motion) 95 | 96 | # Compute current particle's weight 97 | weight = par[0] * self.compute_likelihood(propagated_state, measurements, landmarks) 98 | 99 | # Store 100 | new_particles.append([weight, propagated_state]) 101 | 102 | # Update particles 103 | self.particles = self.normalize_weights(new_particles) 104 | 105 | # Resample if needed 106 | if self.needs_resampling(): 107 | self.particles = self.resampler.resample(self.particles, self.n_particles, self.resampling_algorithm) 108 | -------------------------------------------------------------------------------- /core/particle_filters/particle_filter_sir.py: -------------------------------------------------------------------------------- 1 | from .particle_filter_base import ParticleFilter 2 | from core.resampling.resampler import Resampler 3 | 4 | 5 | class ParticleFilterSIR(ParticleFilter): 6 | """ 7 | Notes: 8 | * State is (x, y, heading), where x and y are in meters and heading in radians 9 | * State space assumed limited size in each dimension, world is cyclic (hence leaving at x_max means entering at 10 | x_min) 11 | * propagation and measurement models are largely hardcoded (except for standard deviations. 12 | """ 13 | 14 | def __init__(self, 15 | number_of_particles, 16 | limits, 17 | process_noise, 18 | measurement_noise, 19 | resampling_algorithm): 20 | """ 21 | Initialize the SIR particle filter. 22 | 23 | :param number_of_particles: Number of particles. 24 | :param limits: List with maximum and minimum values for x and y dimension: [xmin, xmax, ymin, ymax]. 25 | :param process_noise: Process noise parameters (standard deviations): [std_forward, std_angular]. 26 | :param measurement_noise: Measurement noise parameters (standard deviations): [std_range, std_angle]. 27 | :param resampling_algorithm: Algorithm that must be used for core. 28 | """ 29 | # Initialize particle filter base class 30 | ParticleFilter.__init__(self, number_of_particles, limits, process_noise, measurement_noise) 31 | 32 | # Set SIR specific properties 33 | self.resampling_algorithm = resampling_algorithm 34 | self.resampler = Resampler() 35 | 36 | def needs_resampling(self): 37 | """ 38 | Method that determines whether not a core step is needed for the current particle filter state estimate. 39 | The sampling importance core (SIR) scheme resamples every time step hence always return true. 40 | 41 | :return: Boolean indicating whether or not core is needed. 42 | """ 43 | return True 44 | 45 | def update(self, robot_forward_motion, robot_angular_motion, measurements, landmarks): 46 | """ 47 | Process a measurement given the measured robot displacement and resample if needed. 48 | 49 | :param robot_forward_motion: Measured forward robot motion in meters. 50 | :param robot_angular_motion: Measured angular robot motion in radians. 51 | :param measurements: Measurements. 52 | :param landmarks: Landmark positions. 53 | """ 54 | 55 | # Loop over all particles 56 | new_particles = [] 57 | for par in self.particles: 58 | 59 | # Propagate the particle state according to the current particle 60 | propagated_state = self.propagate_sample(par[1], robot_forward_motion, robot_angular_motion) 61 | 62 | # Compute current particle's weight 63 | weight = par[0] * self.compute_likelihood(propagated_state, measurements, landmarks) 64 | 65 | # Store 66 | new_particles.append([weight, propagated_state]) 67 | 68 | # Update particles 69 | self.particles = self.normalize_weights(new_particles) 70 | 71 | # Resample if needed 72 | if self.needs_resampling(): 73 | self.particles = self.resampler.resample(self.particles, self.n_particles, self.resampling_algorithm) 74 | -------------------------------------------------------------------------------- /core/resampling/__init__.py: -------------------------------------------------------------------------------- 1 | from .resampler import ResamplingAlgorithms, Resampler 2 | from .resampling_helpers import naive_search, cumulative_sum, generate_sample_index, compute_required_number_of_particles_kld 3 | -------------------------------------------------------------------------------- /core/resampling/resampler.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Numpy 4 | import numpy as np 5 | 6 | # Enum 7 | from enum import Enum 8 | 9 | # Deep copy samples 10 | import copy 11 | 12 | # Helper functions 13 | from .resampling_helpers import * 14 | 15 | 16 | class ResamplingAlgorithms(Enum): 17 | MULTINOMIAL = 1 18 | RESIDUAL = 2 19 | STRATIFIED = 3 20 | SYSTEMATIC = 4 21 | 22 | 23 | class Resampler: 24 | """ 25 | Resample class that implements different resampling methods. 26 | """ 27 | 28 | def __init__(self): 29 | self.initialized = True 30 | 31 | def resample(self, samples, N, algorithm): 32 | """ 33 | Resampling interface, perform resampling using specified method 34 | 35 | :param samples: List of (weight, sample)-lists that need to be resampled 36 | :param N: Number of samples that must be resampled. 37 | :param algorithm: Preferred method used for resampling. 38 | :return: List of weighted samples. 39 | """ 40 | 41 | if algorithm is ResamplingAlgorithms.MULTINOMIAL: 42 | return self.__multinomial(samples, N) 43 | elif algorithm is ResamplingAlgorithms.RESIDUAL: 44 | return self.__residual(samples, N) 45 | elif algorithm is ResamplingAlgorithms.STRATIFIED: 46 | return self.__stratified(samples, N) 47 | elif algorithm is ResamplingAlgorithms.SYSTEMATIC: 48 | return self.__systematic(samples, N) 49 | 50 | print("Resampling method {} is not specified!".format(algorithm)) 51 | 52 | @staticmethod 53 | def __multinomial(samples, N): 54 | """ 55 | Particles are sampled with replacement proportional to their weight and in arbitrary order. This leads 56 | to a maximum variance on the number of times a particle will be resampled, since any particle will be 57 | resampled between 0 and N times. 58 | 59 | Computational complexity: O(N log(M) 60 | 61 | :param samples: Samples that must be resampled. 62 | :param N: Number of samples that must be generated. 63 | :return: Resampled weighted particles. 64 | """ 65 | 66 | # Get list with only weights 67 | weights = [weighted_sample[0] for weighted_sample in samples] 68 | 69 | # Compute cumulative sum 70 | Q = cumulative_sum(weights) 71 | 72 | # As long as the number of new samples is insufficient 73 | n = 0 74 | new_samples = [] 75 | while n < N: 76 | 77 | # Draw a random sample u 78 | u = np.random.uniform(1e-6, 1, 1)[0] 79 | 80 | # Naive search (alternative: binary search) 81 | m = naive_search(Q, u) 82 | 83 | # Add copy of the state sample (uniform weights) 84 | new_samples.append([1.0/N, copy.deepcopy(samples[m][1])]) 85 | 86 | # Added another sample 87 | n += 1 88 | 89 | return new_samples 90 | 91 | def __residual(self, samples, N): 92 | """ 93 | Particles should at least be present floor(wi/N) times due to first deterministic loop. First Nt new samples are 94 | always the same (when running the function with the same input multiple times). 95 | 96 | Computational complexity: O(M) + O(N-Nt), where Nt is number of samples in first deterministic loop 97 | 98 | :param samples: Samples that must be resampled. 99 | :param N: Number of samples that must be generated. 100 | :return: Resampled weighted particles. 101 | """ 102 | 103 | # Number of incoming samples 104 | M = len(samples) 105 | 106 | # Initialize 107 | weight_adjusted_samples = [] 108 | replication_samples = [] 109 | for m in range(0, M): 110 | # Copy sample and ease of writing 111 | wm, xm = copy.deepcopy(samples[m]) 112 | 113 | # Compute replication 114 | Nm = np.floor(N * wm) 115 | 116 | # Store weight adjusted sample (and avoid division of integers) 117 | weight_adjusted_samples.append([wm - float(Nm) / N, xm]) 118 | 119 | # Store sample to be used for replication 120 | replication_samples.append([xm, int(Nm)]) 121 | 122 | # Replicate samples 123 | new_samples_deterministic = replication(replication_samples) 124 | Nt = len(new_samples_deterministic) 125 | 126 | # Normalize new weights if needed 127 | if N != Nt: 128 | for m in range(0, M): 129 | weight_adjusted_samples[m][0] *= float(N) / (N - Nt) 130 | 131 | # Resample remaining samples (__multinomial return weighted samples, discard weights) 132 | new_samples_stochastic = [s[1] for s in self.__multinomial(weight_adjusted_samples, N - Nt)] 133 | 134 | # Return new samples 135 | weighted_new_samples = add_weights_to_samples(N * [1.0/N], new_samples_deterministic + new_samples_stochastic) 136 | return weighted_new_samples 137 | 138 | @staticmethod 139 | def __stratified(samples, N): 140 | """ 141 | Loop over cumulative sum once hence particles should keep same order (however some disappear, others are 142 | replicated). 143 | 144 | Computational complexity: O(N) 145 | 146 | :param samples: Samples that must be resampled. 147 | :param N: Number of samples that must be generated. 148 | :return: Resampled weighted particles. 149 | """ 150 | 151 | # Compute cumulative sum on normalized weights 152 | weights = [weighted_sample[0] for weighted_sample in samples] 153 | normalized_weights = [w / sum(weights) for w in weights] 154 | Q = cumulative_sum(normalized_weights) 155 | 156 | # As long as the number of new samples is insufficient 157 | n = 0 158 | m = 0 # index first element 159 | new_samples = [] 160 | while n < N: 161 | 162 | # Draw a random sample u0 and compute u 163 | u0 = np.random.uniform(1e-10, 1.0 / N, 1)[0] 164 | u = u0 + float(n) / N 165 | 166 | # u increases every loop hence we only move from left to right (once) while iterating Q 167 | 168 | # Get first sample for which cumulative sum is above u 169 | while Q[m] < u: 170 | m += 1 # no need to reset m, u always increases 171 | 172 | # Add state sample (weight, state) 173 | new_samples.append([1.0/N, copy.deepcopy(samples[m][1])]) 174 | 175 | # Added another sample 176 | n += 1 177 | 178 | # Return new samples 179 | return new_samples 180 | 181 | @staticmethod 182 | def __systematic(samples, N): 183 | """ 184 | Loop over cumulative sum once hence particles should keep same order (however some disappear, other are 185 | replicated). Variance on number of times a particle will be selected lower than with stratified resampling. 186 | 187 | Computational complexity: O(N) 188 | 189 | :param samples: Samples that must be resampled. 190 | :param N: Number of samples that must be generated. 191 | :return: Resampled weighted particles. 192 | """ 193 | # Compute cumulative sum 194 | weights = [weighted_sample[0] for weighted_sample in samples] 195 | Q = cumulative_sum(weights) 196 | 197 | # Only draw one sample 198 | u0 = np.random.uniform(1e-10, 1.0 / N, 1)[0] 199 | 200 | # As long as the number of new samples is insufficient 201 | n = 0 202 | m = 0 # index first element 203 | new_samples = [] 204 | while n < N: 205 | 206 | # Compute u for current particle (deterministic given u0) 207 | u = u0 + float(n) / N 208 | 209 | # u increases every loop hence we only move from left to right while iterating Q 210 | 211 | # Get first sample for which cumulative sum is above u 212 | while Q[m] < u: 213 | m += 1 214 | 215 | # Add state sample (uniform weights) 216 | new_samples.append([1.0/N, copy.deepcopy(samples[m][1])]) 217 | 218 | # Added another sample 219 | n += 1 220 | 221 | # Return new samples 222 | return new_samples 223 | -------------------------------------------------------------------------------- /core/resampling/resampling_helpers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | 5 | 6 | def cumulative_sum(weights): 7 | """ 8 | Compute cumulative sum of a list of scalar weights 9 | 10 | :param weights: list with weights 11 | :return: list containing cumulative weights, length equal to length input 12 | """ 13 | return np.cumsum(weights).tolist() 14 | 15 | 16 | def replication(samples): 17 | """ 18 | Deterministically replicate samples. 19 | 20 | :param samples: A list in which each element again is a list containing the sample 21 | and the number of times the sample needs to be replicated, e.g.: 22 | samples = [(x1, 2), (x2, 0), ..., (xM, 1)] 23 | :return: list of replicated samples: [x1, x1, ..., xM] 24 | """ 25 | 26 | # A perhaps more understandable way to solve this could be: 27 | # replicated_samples = [] 28 | 29 | # for m in range(1, len(samples)+1): 30 | # xk, Nk = samples[m-1] 31 | # for unused in range(1, Nk+1): 32 | # replicated_samples.append(xk) 33 | # return replicated_samples 34 | 35 | # Same result: repeat each state (denoted by s[0]) Nk times (denoted by s[1]) 36 | return [l for s in samples for l in s[1] * [s[0]]] 37 | 38 | 39 | def naive_search(cumulative_list, x): 40 | """ 41 | Find the index i for which cumulativeList[i-1] < x <= cumulativeList[i] within cumulativeList[lower:upper]. 42 | 43 | :param cumulative_list: List of elements that increase with increasing index. 44 | :param x: value for which has to be checked 45 | :return: Index 46 | """ 47 | 48 | m = 0 49 | while cumulative_list[m] < x: 50 | m += 1 51 | return m 52 | 53 | 54 | def add_weights_to_samples(weights, unweighted_samples): 55 | """ 56 | Combine weights and unweighted samples into a list of lists: 57 | [[w1, [particle_state]], [w2, [particle_state2]], ...] 58 | :param weights: Sample weights 59 | :param unweighted_samples: Sample states 60 | :return: list of lists 61 | """ 62 | weighted_samples = [list(ws) for ws in zip(weights, unweighted_samples)] 63 | return weighted_samples 64 | 65 | 66 | def generate_sample_index(weighted_samples): 67 | """ 68 | Sample a particle from the discrete distribution consisting out of all particle weights. 69 | 70 | :param weighted_samples: List of weighted particles 71 | :return: Sampled particle index 72 | """ 73 | 74 | # Check input 75 | if len(weighted_samples) < 1: 76 | print("Cannot sample from empty set") 77 | return -1 78 | 79 | # Get list with only weights 80 | weights = [weighted_sample[0] for weighted_sample in weighted_samples] 81 | 82 | # Compute cumulative sum for all weights 83 | Q = cumulative_sum(weights) 84 | 85 | # Draw a random sample u in [0, sum_all_weights] 86 | u = np.random.uniform(1e-6, Q[-1], 1)[0] 87 | 88 | # Return index of first sample for which cumulative sum is above u 89 | return naive_search(Q, u) 90 | 91 | 92 | def compute_required_number_of_particles_kld(k, epsilon, upper_quantile): 93 | """ 94 | Compute the number of samples needed within a particle filter when k bins in the multidimensional histogram contain 95 | samples. Use Wilson-Hilferty transformation to approximate the quantiles of the chi-squared distribution as proposed 96 | by Fox (2003). 97 | 98 | :param epsilon: Maxmimum allowed distance (error) between true and estimated distribution. 99 | :param upper_quantile: Upper standard normal distribution quantile for (1-delta) where delta is the probability that 100 | the error on the estimated distribution will be less than epsilon. 101 | :param k: Number of bins containing samples. 102 | :return: Number of required particles. 103 | """ 104 | # Helper variable (part between curly brackets in (7) in Fox paper 105 | x = 1.0 - 2.0 / (9.0*(k-1)) + np.sqrt(2.0 / (9.0*(k-1))) * upper_quantile 106 | return np.ceil((k-1) / (2.0*epsilon) * x * x * x) 107 | -------------------------------------------------------------------------------- /demo_range_only.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Simulation + plotting requires a robot, visualizer and world 4 | from simulator import RobotRange, Visualizer, World 5 | 6 | # Supported resampling methods (resampling algorithm enum for SIR and SIR-derived particle filters) 7 | from core.resampling import ResamplingAlgorithms 8 | 9 | # Particle filters 10 | from core.particle_filters import ParticleFilterRangeOnly 11 | 12 | # For showing plots (plt.show()) 13 | import matplotlib.pyplot as plt 14 | 15 | if __name__ == '__main__': 16 | 17 | print("Running example particle filter demo with range only measurements.") 18 | 19 | ## 20 | # Set simulated world and visualization properties 21 | ## 22 | # world = World(10.0, 10.0, [[5.0, 5.0]]) 23 | world = World(10.0, 10.0, [[2.5, 2.5], [7.5, 7.5]]) 24 | 25 | # Number of simulated time steps 26 | n_time_steps = 10 27 | 28 | # Initialize visualization 29 | show_particle_pose = False # only set to true for low #particles (very slow) 30 | visualizer = Visualizer(show_particle_pose) 31 | visualizer.update_robot_radius(0.2) 32 | visualizer.update_landmark_size(7) 33 | 34 | ## 35 | # True robot properties (simulator settings) 36 | ## 37 | 38 | # Setpoint (desired) motion robot 39 | robot_setpoint_motion_forward = 0.25 40 | robot_setpoint_motion_turn = 0.02 41 | 42 | # True simulated robot motion is set point plus additive zero mean Gaussian noise with these standard deviation 43 | true_robot_motion_forward_std = 0.005 44 | true_robot_motion_turn_std = 0.002 45 | 46 | # Robot measurements are corrupted by measurement noise 47 | true_robot_meas_noise_distance_std = 0.2 48 | 49 | # Initialize simulated robot 50 | robot = RobotRange(x=world.x_max * 0.8, 51 | y=world.y_max / 6.0, 52 | theta=3.14 / 2.0, 53 | std_forward=true_robot_motion_forward_std, 54 | std_turn=true_robot_motion_turn_std, 55 | std_meas_distance=true_robot_meas_noise_distance_std) 56 | 57 | ## 58 | # Particle filter settings 59 | ## 60 | 61 | number_of_particles = 1000 62 | pf_state_limits = [0, world.x_max, 0, world.y_max] 63 | 64 | # Process model noise (zero mean additive Gaussian noise) 65 | motion_model_forward_std = 0.1 66 | motion_model_turn_std = 0.20 67 | process_noise = [motion_model_forward_std, motion_model_turn_std] 68 | 69 | # Measurement noise (zero mean additive Gaussian noise) 70 | meas_model_distance_std = 0.4 71 | meas_model_angle_std = 0.3 72 | measurement_noise = meas_model_distance_std 73 | 74 | # Set resampling algorithm used 75 | algorithm = ResamplingAlgorithms.MULTINOMIAL 76 | 77 | # Initialize SIR particle filter with range only measurements 78 | particle_filter = ParticleFilterRangeOnly( 79 | number_of_particles=number_of_particles, 80 | limits=pf_state_limits, 81 | process_noise=process_noise, 82 | measurement_noise=measurement_noise, 83 | resampling_algorithm=algorithm) 84 | particle_filter.initialize_particles_uniform() 85 | 86 | ## 87 | # Start simulation 88 | ## 89 | for i in range(n_time_steps): 90 | 91 | # Simulate robot motion (required motion will not exactly be achieved) 92 | robot.move(desired_distance=robot_setpoint_motion_forward, 93 | desired_rotation=robot_setpoint_motion_turn, 94 | world=world) 95 | 96 | # Simulate measurement 97 | measurements = robot.measure(world) 98 | 99 | # Update particle filter 100 | particle_filter.update(robot_forward_motion=robot_setpoint_motion_forward, 101 | robot_angular_motion=robot_setpoint_motion_turn, 102 | measurements=measurements, 103 | landmarks=world.landmarks) 104 | 105 | # Visualization 106 | visualizer.draw_world(world, robot, particle_filter.particles, hold_on=False, particle_color='g') 107 | plt.pause(0.5) 108 | -------------------------------------------------------------------------------- /demo_running_example.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Simulation + plotting requires a robot, visualizer and world 4 | from simulator import Robot, Visualizer, World 5 | 6 | # Supported resampling methods (resampling algorithm enum for SIR and SIR-derived particle filters) 7 | from core.resampling import ResamplingAlgorithms 8 | 9 | # Particle filters 10 | from core.particle_filters import ParticleFilterSIR 11 | 12 | # For showing plots (plt.show()) 13 | import matplotlib.pyplot as plt 14 | 15 | if __name__ == '__main__': 16 | 17 | print("Running example particle filter demo.") 18 | 19 | ## 20 | # Set simulated world and visualization properties 21 | ## 22 | world = World(10.0, 10.0, [[2.0, 2.0], [2.0, 8.0], [9.0, 2.0], [8, 9]]) 23 | 24 | # Number of simulated time steps 25 | n_time_steps = 30 26 | 27 | # Initialize visualization 28 | show_particle_pose = False # only set to true for low #particles (very slow) 29 | visualizer = Visualizer(show_particle_pose) 30 | visualizer.update_robot_radius(0.2) 31 | visualizer.update_landmark_size(7) 32 | 33 | ## 34 | # True robot properties (simulator settings) 35 | ## 36 | 37 | # Setpoint (desired) motion robot 38 | robot_setpoint_motion_forward = 0.25 39 | robot_setpoint_motion_turn = 0.02 40 | 41 | # True simulated robot motion is set point plus additive zero mean Gaussian noise with these standard deviation 42 | true_robot_motion_forward_std = 0.005 43 | true_robot_motion_turn_std = 0.002 44 | 45 | # Robot measurements are corrupted by measurement noise 46 | true_robot_meas_noise_distance_std = 0.2 47 | true_robot_meas_noise_angle_std = 0.05 48 | 49 | # Initialize simulated robot 50 | robot = Robot(x=world.x_max * 0.75, 51 | y=world.y_max / 5.0, 52 | theta=3.14 / 2.0, 53 | std_forward=true_robot_motion_forward_std, 54 | std_turn=true_robot_motion_turn_std, 55 | std_meas_distance=true_robot_meas_noise_distance_std, 56 | std_meas_angle=true_robot_meas_noise_angle_std) 57 | 58 | ## 59 | # Particle filter settings 60 | ## 61 | 62 | number_of_particles = 1000 63 | pf_state_limits = [0, world.x_max, 0, world.y_max] 64 | 65 | # Process model noise (zero mean additive Gaussian noise) 66 | motion_model_forward_std = 0.1 67 | motion_model_turn_std = 0.20 68 | process_noise = [motion_model_forward_std, motion_model_turn_std] 69 | 70 | # Measurement noise (zero mean additive Gaussian noise) 71 | meas_model_distance_std = 0.4 72 | meas_model_angle_std = 0.3 73 | measurement_noise = [meas_model_distance_std, meas_model_angle_std] 74 | 75 | # Set resampling algorithm used 76 | algorithm = ResamplingAlgorithms.MULTINOMIAL 77 | 78 | # Initialize SIR particle filter: resample every time step 79 | particle_filter_sir = ParticleFilterSIR( 80 | number_of_particles=number_of_particles, 81 | limits=pf_state_limits, 82 | process_noise=process_noise, 83 | measurement_noise=measurement_noise, 84 | resampling_algorithm=algorithm) 85 | particle_filter_sir.initialize_particles_uniform() 86 | 87 | ## 88 | # Start simulation 89 | ## 90 | for i in range(n_time_steps): 91 | 92 | # Simulate robot motion (required motion will not exactly be achieved) 93 | robot.move(desired_distance=robot_setpoint_motion_forward, 94 | desired_rotation=robot_setpoint_motion_turn, 95 | world=world) 96 | 97 | # Simulate measurement 98 | measurements = robot.measure(world) 99 | 100 | # Update SIR particle filter 101 | particle_filter_sir.update(robot_forward_motion=robot_setpoint_motion_forward, 102 | robot_angular_motion=robot_setpoint_motion_turn, 103 | measurements=measurements, 104 | landmarks=world.landmarks) 105 | 106 | # Visualization 107 | visualizer.draw_world(world, robot, particle_filter_sir.particles, hold_on=False, particle_color='g') 108 | plt.pause(0.05) -------------------------------------------------------------------------------- /demo_running_example_extended_Kalman_particle_filter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Simulation + plotting requires a robot, visualizer and world 4 | from simulator import Robot, Visualizer, World 5 | 6 | # Load variables 7 | from shared_simulation_settings import * 8 | 9 | # Particle filter 10 | from core.particle_filters import KalmanParticleFilter 11 | 12 | # For showing plots (plt.show()) 13 | import matplotlib.pyplot as plt 14 | 15 | if __name__ == '__main__': 16 | 17 | """ 18 | This file demonstrates how the extended Kalman particle filter works. No divergence occurs even though the number of 19 | particles is low. In fact, the number of particles equals the number of particles in the divergence example. 20 | 21 | Note that the only the mean value for each particle is visualized (not the covariance associated with the mean 22 | value). 23 | """ 24 | 25 | print("Starting demonstration of extended Kalman particle filter.") 26 | 27 | ## 28 | # Set simulated world and visualization properties 29 | ## 30 | world = World(world_size_x, world_size_y, landmark_positions) 31 | visualizer = Visualizer() 32 | num_time_steps = 30 33 | 34 | # Initialize simulated robot 35 | robot = Robot(x=world.x_max * 0.75, 36 | y=world.y_max / 5.0, 37 | theta=3.14 / 2.0, 38 | std_forward=true_robot_motion_forward_std, 39 | std_turn=true_robot_motion_turn_std, 40 | std_meas_distance=true_robot_meas_noise_distance_std, 41 | std_meas_angle=true_robot_meas_noise_angle_std) 42 | 43 | ## 44 | # Particle filter settings 45 | ## 46 | 47 | # Demonstrate divergence -> set number of particles too low 48 | number_of_particles = 100 # instead of 500 or even 1000 49 | 50 | # Initialize particle filter 51 | 52 | # Initialize extended Kalman particle filter 53 | kalman_particle_filter = KalmanParticleFilter( 54 | number_of_particles, 55 | pf_state_limits, 56 | process_noise, 57 | measurement_noise) 58 | kalman_particle_filter.initialize_particles_uniform() 59 | 60 | ## 61 | # Start simulation 62 | ## 63 | for i in range(num_time_steps): 64 | # Simulate robot move, perform measurements and update extended Kalman particle filter 65 | robot.move(robot_setpoint_motion_forward, 66 | robot_setpoint_motion_turn, 67 | world) 68 | measurements = robot.measure(world) 69 | kalman_particle_filter.update(robot_setpoint_motion_forward, 70 | robot_setpoint_motion_turn, 71 | measurements, 72 | world.landmarks) 73 | 74 | # Visualize particles after initialization (to avoid cluttered visualization) 75 | visualizer.draw_world(world, robot, kalman_particle_filter.particles, hold_on=True) 76 | 77 | # Draw all particle at last time step 78 | visualizer.draw_world(world, robot, kalman_particle_filter.particles, hold_on=True) 79 | 80 | plt.show() -------------------------------------------------------------------------------- /images/running_example_screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jelfring/particle-filter-tutorial/2907c721f95e54d1af8c4f3d995618bf1cb72dc1/images/running_example_screenshot.png -------------------------------------------------------------------------------- /shared_simulation_settings.py: -------------------------------------------------------------------------------- 1 | ## 2 | # Define simulated world 3 | ## 4 | 5 | # Each landmark has a 2D position (all in meters) 6 | landmark_positions = [[2.0, 2.0], [2.0, 8.0], [9.0, 2.0], [8, 9]] 7 | world_size_x = 10.0 8 | world_size_y = 10.0 9 | 10 | ## 11 | # True robot properties (simulator settings) 12 | ## 13 | 14 | # Set point: desired motion robot (displacements) 15 | robot_setpoint_motion_forward = 0.25 # m per time step 16 | robot_setpoint_motion_turn = 0.02 # rad per time step 17 | 18 | # True simulated robot motion is set point plus additive zero mean Gaussian noise with these standard deviation 19 | true_robot_motion_forward_std = 0.005 # m 20 | true_robot_motion_turn_std = 0.002 # rad 21 | 22 | # Robot measurements are corrupted by measurement noise 23 | true_robot_meas_noise_distance_std = 0.2 # m 24 | true_robot_meas_noise_angle_std = 0.05 # rad 25 | 26 | # Initial simulated robot position 27 | robot_initial_x_position = world_size_x * 0.75 # m 28 | robot_initial_y_position = world_size_y * 5.0 # m 29 | robot_initial_heading = 3.14 / 2.0 # rad 30 | 31 | ## 32 | # Particle filter settings 33 | ## 34 | pf_state_limits = [0, world_size_x, 0, world_size_y] 35 | 36 | # Process model noise (zero mean additive Gaussian noise) 37 | motion_model_forward_std = 0.10 # m 38 | motion_model_turn_std = 0.02 # rad 39 | process_noise = [motion_model_forward_std, motion_model_turn_std] 40 | 41 | # Measurement noise (zero mean additive Gaussian noise) 42 | meas_model_distance_std = 0.4 # m 43 | meas_model_angle_std = 0.3 # rad 44 | measurement_noise = [meas_model_distance_std, meas_model_angle_std] -------------------------------------------------------------------------------- /simulator/__init__.py: -------------------------------------------------------------------------------- 1 | from .robot import Robot 2 | from .robot_range_measurement import RobotRange 3 | from .visualizer import Visualizer 4 | from .world import World 5 | -------------------------------------------------------------------------------- /simulator/robot.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from .world import * 4 | 5 | 6 | class Robot: 7 | 8 | def __init__(self, x, y, theta, std_forward, std_turn, std_meas_distance, std_meas_angle): 9 | """ 10 | Initialize the robot with given 2D pose. In addition set motion uncertainty parameters. 11 | 12 | :param x: Initial robot x-position (m) 13 | :param y: Initial robot y-position (m) 14 | :param theta: Initial robot heading (rad) 15 | :param std_forward: Standard deviation zero mean additive noise on forward motions (m) 16 | :param std_turn: Standard deviation zero mean Gaussian additive noise on turn actions (rad) 17 | :param std_meas_distance: Standard deviation zero mean Gaussian additive measurement noise (m) 18 | :param std_meas_angle: Standard deviation zero mean Gaussian additive measurement noise (rad) 19 | """ 20 | 21 | # Initialize robot pose 22 | self.x = x 23 | self.y = y 24 | self.theta = theta 25 | 26 | # Set standard deviations noise robot motion 27 | self.std_forward = std_forward 28 | self.std_turn = std_turn 29 | 30 | # Set standard deviation measurements 31 | self.std_meas_distance = std_meas_distance 32 | self.std_meas_angle = std_meas_angle 33 | 34 | def move(self, desired_distance, desired_rotation, world): 35 | """ 36 | Move the robot according to given arguments and within world of given dimensions. The true motion is the sum of 37 | the desired motion and additive Gaussian noise that represents the fact that the desired motion cannot exactly 38 | be realized, e.g., due to imperfect control and sensing. 39 | 40 | --- Suggestion for alternative implementation ------------------------------------------------------------------ 41 | An alternative approach could be to replace the argument names of this function to true_distance_driven and 42 | true_rotation. These true motion could be applied to the robot state without adding noise: 43 | self.theta += true_rotation 44 | self.x += true_distance_driven * np.cos(self.theta) 45 | self.y += true_distance_driven * np.sin(self.theta) 46 | Then robot displacement measurements can be modelled as follows: 47 | measured_distance_driven = self._get_gaussian_noise_sample(desired_distance, self.std_forward) 48 | measured_angle_rotated = self._get_gaussian_noise_sample(desired_rotation, self.std_turn) 49 | And one can return noisy measurements: 50 | return [measured_distance_driven, measured_angle_rotated] 51 | that are then being used as input for the particle filter propagation step. This would obviously still require 52 | the cyclic world checks (on both measurement and true robot state). The particle filter estimation results will 53 | roughly be the same in terms of performance, however, this might be more intuitive for some of the readers. 54 | ---------------------------------------------------------------------------------------------------------------- 55 | 56 | :param desired_distance: forward motion setpoint of the robot (m) 57 | :param desired_rotation: angular rotation setpoint of the robot (rad) 58 | :param world: dimensions of the cyclic world in which the robot executes its motion 59 | """ 60 | 61 | # Compute relative motion (true motion is desired motion with some noise) 62 | distance_driven = self._get_gaussian_noise_sample(desired_distance, self.std_forward) 63 | angle_rotated = self._get_gaussian_noise_sample(desired_rotation, self.std_turn) 64 | 65 | # Update robot pose 66 | self.theta += angle_rotated 67 | self.x += distance_driven * np.cos(self.theta) 68 | self.y += distance_driven * np.sin(self.theta) 69 | 70 | # Cyclic world assumption (i.e. crossing right edge -> enter on left hand side) 71 | self.x = np.mod(self.x, world.x_max) 72 | self.y = np.mod(self.y, world.y_max) 73 | 74 | # Angles in [0, 2*pi] 75 | self.theta = np.mod(self.theta, 2*np.pi) 76 | 77 | def measure(self, world): 78 | """ 79 | Perform a measurement. The robot is assumed to measure the distance to and angles with respect to all landmarks 80 | in meters and radians respectively. While doing so, the robot experiences zero mean additive Gaussian noise. 81 | 82 | :param world: World containing the landmark positions. 83 | :return: List of lists: [[dist_to_landmark1, angle_wrt_landmark1], dist_to_landmark2, angle_wrt_landmark2], ...] 84 | """ 85 | 86 | # Loop over measurements 87 | measurements = [] 88 | for lm in world.landmarks: 89 | dx = self.x - lm[0] 90 | dy = self.y - lm[1] 91 | 92 | # Measured distance perturbed by zero mean additive Gaussian noise 93 | z_distance = self._get_gaussian_noise_sample(np.sqrt(dx * dx + dy * dy), self.std_meas_distance) 94 | 95 | # Measured angle perturbed by zero mean additive Gaussian noise 96 | z_angle = self._get_gaussian_noise_sample(np.arctan2(dy, dx), self.std_meas_angle) 97 | 98 | # Store measurement 99 | measurements.append([z_distance, z_angle]) 100 | 101 | return measurements 102 | 103 | @staticmethod 104 | def _get_gaussian_noise_sample(mu, sigma): 105 | """ 106 | Get a random sample from a 1D Gaussian distribution with mean mu and standard deviation sigma. 107 | 108 | :param mu: mean of distribution 109 | :param sigma: standard deviation 110 | :return: random sample from distribution with given parameters 111 | """ 112 | return np.random.normal(loc=mu, scale=sigma, size=1)[0] 113 | -------------------------------------------------------------------------------- /simulator/robot_range_measurement.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from .robot import * 4 | from .world import * 5 | 6 | 7 | class RobotRange(Robot): 8 | 9 | def __init__(self, x, y, theta, std_forward, std_turn, std_meas_distance): 10 | """ 11 | Initialize the robot with given 2D pose. In addition set motion uncertainty parameters. 12 | 13 | :param x: Initial robot x-position (m) 14 | :param y: Initial robot y-position (m) 15 | :param theta: Initial robot heading (rad) 16 | :param std_forward: Standard deviation zero mean additive noise on forward motions (m) 17 | :param std_turn: Standard deviation zero mean Gaussian additive noise on turn actions (rad) 18 | :param std_meas_distance: Standard deviation zero mean Gaussian additive measurement noise (m) 19 | """ 20 | 21 | # Initialize robot base class without standard deviation for angular measurements 22 | Robot.__init__(self, x, y, theta, std_forward, std_turn, std_meas_distance, None) 23 | 24 | def measure(self, world): 25 | """ 26 | Perform a measurement. The robot is assumed to measure the distance to and angles with respect to all landmarks 27 | in meters and radians respectively. While doing so, the robot experiences zero mean additive Gaussian noise. 28 | 29 | :param world: World containing the landmark positions. 30 | :return: List of lists: [[dist_to_landmark1, angle_wrt_landmark1], dist_to_landmark2, angle_wrt_landmark2], ...] 31 | """ 32 | 33 | # Loop over measurements 34 | measurements = [] 35 | for lm in world.landmarks: 36 | dx = self.x - lm[0] 37 | dy = self.y - lm[1] 38 | 39 | # Measured distance perturbed by zero mean additive Gaussian noise 40 | z_distance = self._get_gaussian_noise_sample(np.sqrt(dx * dx + dy * dy), self.std_meas_distance) 41 | 42 | # Store measurement 43 | measurements.append(z_distance) 44 | 45 | return measurements 46 | -------------------------------------------------------------------------------- /simulator/visualizer.py: -------------------------------------------------------------------------------- 1 | # Plotting will be done with matplotlib 2 | import matplotlib.pyplot as plt 3 | 4 | # For generating plot more efficiently 5 | import numpy as np 6 | 7 | # Custom objects that must be visualized 8 | from .world import World 9 | from .robot import Robot 10 | 11 | 12 | class Visualizer: 13 | """ 14 | Class for visualing the world, the true robot pose and the discrete distribution that estimates the robot pose by 15 | means of a set of (weighted) particles. 16 | """ 17 | 18 | def __init__(self, draw_particle_pose=False): 19 | """ 20 | Initialize visualizer. By setting the flag to false the full 2D pose will be visualized. This makes 21 | visualization much slower hence is only recommended for a relatively low number of particles. 22 | 23 | :param draw_particle_pose: Flag that determines whether 2D positions (default) or poses must be visualized. 24 | """ 25 | 26 | self.x_margin = 1 27 | self.y_margin = 1 28 | self.circle_radius_robot = 0.02 # 0.25 29 | self.draw_particle_pose = draw_particle_pose 30 | self.landmark_size = 6 31 | self.scale = 2 # meter / inch 32 | self.robot_arrow_length = 0.5 / self.scale 33 | 34 | def update_robot_radius(self, robot_radius): 35 | """ 36 | Set the radius that must be used for drawing the robot in the simulated world. 37 | :param robot_radius: new robot radius 38 | """ 39 | self.circle_radius_robot = robot_radius 40 | self.robot_arrow_length = robot_radius 41 | 42 | def update_landmark_size(self, landmark_size): 43 | """ 44 | Set the size that must be used for drawing the landmarks in the simulated world. 45 | :param landmark_size: new landmark size 46 | """ 47 | self.landmark_size = landmark_size 48 | 49 | def draw_world(self, world, robot, particles, hold_on=False, particle_color='g'): 50 | """ 51 | Draw the simulated world with its landmarks, the robot 2D pose and the particles that represent the discrete 52 | probability distribution that estimates the robot pose. 53 | 54 | :param world: World object (includes dimensions and landmarks) 55 | :param robot: True robot 2D pose (x, y, heading) 56 | :param particles: Set of weighted particles (list of [weight, [x, y, heading]]-lists) 57 | :param hold_on: Boolean that indicates whether figure must be cleared or nog 58 | :param particle_color: Color used for particles (as matplotlib string) 59 | """ 60 | 61 | # Dimensions world 62 | x_min = -self.x_margin 63 | x_max = self.x_margin + world.x_max 64 | y_min = -self.y_margin 65 | y_max = self.y_margin + world.y_max 66 | 67 | # Draw world 68 | plt.figure(1, figsize=((x_max-x_min) / self.scale, (y_max-y_min) / self.scale)) 69 | if not hold_on: 70 | plt.clf() 71 | plt.plot([0, world.x_max], [0, 0], 'k-') # lower line 72 | plt.plot([0, 0], [0, world.y_max], 'k-') # left line 73 | plt.plot([0, world.x_max], [world.y_max, world.y_max], 'k-') # top line 74 | plt.plot([world.x_max, world.x_max], [0, world.y_max], 'k-') # right line 75 | 76 | # Set limits axes 77 | plt.xlim([x_min, x_max]) 78 | plt.ylim([y_min, y_max]) 79 | 80 | # No ticks on axes 81 | plt.xticks([]) 82 | plt.yticks([]) 83 | 84 | # Title 85 | plt.title("{} particles".format(len(particles))) 86 | 87 | # Add landmarks 88 | landmarks = np.array(world.landmarks) 89 | plt.plot(landmarks[:, 0], landmarks[:, 1], 'bs', linewidth=2, markersize=self.landmark_size) 90 | 91 | # Add particles 92 | if self.draw_particle_pose: 93 | # Warning: this is very slow for large numbers of particles 94 | radius_scale_factor = len(particles) / 10. 95 | for p in particles: 96 | self.add_pose2d(p[1][0], p[1][1], p[1][2], 1, particle_color, radius_scale_factor * p[0]) 97 | else: 98 | # Convert to numpy array for efficiency reasons (drop weights) 99 | states = np.array([np.array(state_i[1]) for state_i in particles]) 100 | plt.plot(states[:, 0], states[:, 1], particle_color+'.', linewidth=1, markersize=2) 101 | 102 | # Add robot pose 103 | self.add_pose2d(robot.x, robot.y, robot.theta, 1, 'r', self.circle_radius_robot) 104 | 105 | plt.savefig('multimodal_one_landmark.png') 106 | 107 | # Show 108 | plt.pause(0.05) 109 | 110 | def add_pose2d(self, x, y, theta, fig_num, color, radius): 111 | """ 112 | Plot a 2D pose in given figure with given color and radius (circle with line indicating heading). 113 | 114 | :param x: X-position (center circle). 115 | :param y: Y-position (center circle). 116 | :param theta: Heading (line from center circle with this heading will be added). 117 | :param fig_num: Figure in which pose must be added. 118 | :param color: Color of the lines. 119 | :param radius: Radius of the circle. 120 | :return: 121 | """ 122 | 123 | # Select correct figure 124 | plt.figure(fig_num) 125 | 126 | # Draw circle at given position (higher 'zorder' value means draw later, hence on top of other lines) 127 | circle = plt.Circle((x, y), radius, facecolor=color, edgecolor=color, alpha=0.4, zorder=20) 128 | plt.gca().add_patch(circle) 129 | 130 | # Draw line indicating heading 131 | plt.plot([x, x + radius * np.cos(theta)], 132 | [y, y + radius * np.sin(theta)], color) 133 | -------------------------------------------------------------------------------- /simulator/world.py: -------------------------------------------------------------------------------- 1 | class World: 2 | 3 | def __init__(self, size_x, size_y, landmarks): 4 | """ 5 | Initialize world with given dimensions. 6 | :param size_x: Length world in x-direction (m) 7 | :param size_y: Length world in y-direction (m) 8 | :param landmarks: List with 2D-positions of landmarks 9 | """ 10 | 11 | self.x_max = size_x 12 | self.y_max = size_y 13 | 14 | print("Initialize world with landmarks {}".format(landmarks)) 15 | 16 | # Check if each element is a list 17 | if any(not isinstance(lm, list) for lm in landmarks): 18 | 19 | # Not a list of lists, then exactly two elements allowed (x,y)-position single landmark 20 | if len(landmarks) != 2: 21 | print("Invalid landmarks provided to World: {}".format(landmarks)) 22 | else: 23 | self.landmarks = [landmarks] 24 | else: 25 | # Check if each list within landmarks list contains exactly two elements (x- and y-position) 26 | if any(len(lm) != 2 for lm in landmarks): 27 | print("Invalid landmarks provided to World: {}".format(landmarks)) 28 | else: 29 | self.landmarks = landmarks 30 | --------------------------------------------------------------------------------