Particle filter tutorial <META http-equiv="Content-Type" content="text/html; charset=iso-8859-1"> <META http-equiv="Content-Language" content="en"> <META name="description" content="Tinne De Laet"> <link rel="stylesheet" type="text/css" href="style.css" /> </HEAD> <BODY> <h1>Abstract</h1> <p>This tutorial shows the possible application of a Monte Carlo Simulation method (particle filter) for mobile robot tracking and localisation. </p> <h1>Introduction</h1> <p>Many problems in science require estimation of the state of a system that changes over time using a sequence of noisy measurements made on the system. </p> <p> In order to analyze and make inference about a dynamic description, at least two models are required: First, a model describing the evolution of the state with time (<em>the system model</em>); second, a model relating the noisy measurements to the state (<em>the measurement model</em>). In this case these models are available in probablistic form. A probablistic state-space formulation and the requirement for the updating of information on receipt of new measurements are ideally suited for the <em>Bayesian approach</em>. This approach provides a rigorous general framework for dynamic state estimation problems. </p> <h2>The Bayesian approach</h2> <p> In the Bayesian approach to dynamic state estimation one attempts to construct the posterior probability density function (pdf) of the state based on all available information including the set of received measurements. Since this pdf embodies all available statistical information, it may be considered as the "complete" solution to the estimation problem. In principle, an optimal (with respect to any criterion) estimate of the state may be obtained from the pdf. </p> <h1>Particle filter</h1> <p>The main objective of a particle filter is to "track" a variable of interest as it evolves over time. In contrast with Kalman filters a particle filter can typically handle non-Gaussian and potentially multi-model pdf. The basis of the method is to construct a sample-based representation of the entire pdf. A series of actions are taken, each one modifying the state of the variable of interest according to some model. Moreover at certain times an observation is made that contains information about the state of the variable of interest at that time. </p> <p>Multiple copies (particles) of the variable of interest are used, each associated with a weigth that is a measure for the the quality (the likelihook) of that particle. An estimate of the variable of interest at each time step can be obtained by considering all particles at that moment, eg by taking the weighted mean. </p> <p>The particle filter is recursive in nature and operates in two phases: <em>prediction</em> and <em>update</em>. After each action, each particle is modified according to the existing model (<em>prediction</em> stage), including the addition of random noise in order to simulate the effect of noise on the variable of interest. Then, each particle's weight is re-evaluated based on the latest observations (eg sensor measurements), this is the <em>update</em> stage. At times the particles with small weights are eliminated and particles with higher weights are duplicated, a process called resampling. </p> <p>More formally, the variable of interest, in this case the pose (position and orientation) of the moving robot: x<sup>k</sup>=[x<sup>k</sup>,y<sup>k</sup>,theta<sup>k</sup>]<sup>T</sup> at time k is prepresented by a set of M samples (the particles): S<sub>i</sub><sup>k</sup>=[x<sub>j</sub><sup>k</sup>,w<sub>j</sub><sup>k</sup>] : j=1..M, the variable of interest and a weight (w<sub>j</sub><sup>k</sup>) that defines the contribution of this particle to the overall estimate of the variable.</p> <p>If at time t=k we know the pdf of the system at the previous instant (time t=k-1) then we model the effect of the action to obtain a prior of the pdf at time t=k (prediction). In other words, the <em>prediction</em> phase uses a model in order to simulate the effect an action has on the set of particles with the appropriate noise added. The <em>update</em> phase uses the information obtained from sensing to update the particle weights in order to accurately describe the moving robot's pdf. </p> <p>Given a particle distribution it is often needed to take actions based on the robot pose. Three different methods of evaluation have been used in order to obtain an estimate of the pose. First, the weighted mean can be used; second the best particle and third the weighted mean in a small window around the best particle (also called the robust mean) can be used. Each method has its advantages and disadvantages: the weighted mean fails when faced with multi-modal distributions, while the best particle often introduces a discritization error. The best method is the robust mean but it is also the most computationally expensive. </p> <h2>Prediction</h2> <h2>Resampling</h2> <h2>Update</h2> <h2>Numerical aspects/ algorithmic details</h2> <p>eg. the normalisation of the weights </p> <h1>M-files </h1> <p>A zip of all m-files: <A HREF="../M_files.zip">M-files</A></p> <ul> <li><A HREF="../data.m">data.m</A></li> <li><A HREF="../data_RF.m">data_RF.m</A></li> <li><A HREF="../data_US.m">data_US.m</A></li> <li><A HREF="../drawRF.m">drawRF.m</A></li> <li><A HREF="../drawwall.m">drawwall.m</A></li> <li><A HREF="../ess.m">ess.m</A></li> <li><A HREF="../estimate.m">estimate.m</A></li> <li><A HREF="../linear_time_resampling.m">linear_time_resampling.m</A></li> <li><A HREF="../liu_resampling.m">liu_resampling.m</A></li> <li><A HREF="../particle_filter.m">particle_filter.m</A></li> <li><A HREF="../plot_Estimate.m">plot_Estimate.m</A></li> <li><A HREF="../plot_particles.m">plot_particles.m</A></li> <li><A HREF="../rotation.m">rotation.m</A></li> <li><A HREF="../select_with_replacement.m">select_with_replacement.m</A></li> <li><A HREF="../Sense.m">Sense.m</A></li> <li><A HREF="../test_atan.m">test_atan.m</A></li> <li><A HREF="../weight.m">weight.m</A></li> </ul> <h1>Parameters</h1> <p> The parameters in the different m-files </p> <h2>data.m</h2> <ol> <li><h3>parameter:</h3> <ul> <li><b>type:</b> int</li> <li>1=US</li> <li>2=RF</li> <li>3=US and RF</li> </ul> </li> <li><h3>initial particles:</h3> <ul> <li><b>type: </b> matrix</li> <li>Everey column is a particle</li> <li>[xp;yp;thetap;weigthp]</li> </ul> </li> <li><h3>real states:</h3> <ul> <li><b>type:</b> double</li> <li>xm</li> <li>ym</li> <li>thetam</li> </ul> </li> <li><h3>measurement_noise:</h3> <ul> <li><b>type:</b> boolean <ul> <li>0: no measurement noise</li> <li>1: measurement noise</li> </ul> </li> </ul> </li> <li><h3>M_rot, sigma_rot:</h3> <ul> <li><b>type:</b> double, double</li> <li>N(M_rot,sigma_rot) is the normal distribution of the noise on the rotation angle</li> </ul> </li> <li><h3>M_trs, sigma_trs:</h3> <ul> <li><b>type:</b> double, double</li> <li>N(M_trs,sigma_trs) is the normal distribution of the noise of the translation</li> </ul> </li> <li><h3>M_drft, sigma_drft:</h3> <ul> <li><b>type:</b> double, double</li> <li>N(M_drft, sigma_drft) is the normal distribution of the noise on the angle during translation</li> </ul> </li> <li><h3>memory:</h3> <ul> <li><b>type:</b> boolean</li> <li>0: weight<sub>k+1</sub>=update<sub>weight</sub></li> <li>1: weight<sub>k+1</sub>=weight<sub>k</sub>*update<sub>weight</sub></li> </ul> </li> </ol> <h2>dataUS.m</h2> <ol> <li><h3>wall:</h3> <ul> <li>[a,b] (y=a*x+b)</li> </ul> </li> <li><h3>mu_US, sigma_US:<h3> <ul> <li> N(mu_US,sigma_US) is the normal distribution of US measurement</li> </ul> </ol> <h2>dataRF.m</h2> <ol> <li><h3>[xs,ys,thetas]:</h3> <ul> <li>position and orientation of RF beaken</li> </ul> </li> <li><h3>mu_rho, sigma_rho:</h3> <ul> <li>N(mu_rho,sigma_rho) is the normal distribution of US measurement</li> </ul> </li> <li><h3>mu_rho:</h3> <ul> <li>expected value of normal distribuiton of position measurement of RF </li> </li> <li><h3>mu_theta:</h3> <ul> <li>expected value of normal distribuiton of angle measurement with RF</li> </ul> </li> </ol> <h1>Simulations</h1> <h2>Tracking</h2> <p>In a tracking problem, the initial position of the mobile robot is approximately known and the position of the robot has to be estimated during the further movement.</p> <h3>Prediction</h3> <p>The situation in which only prediction is used is achieved by skipping the resampling. This situation is also known as deadreckoning.<br/> <b>Remark:</b> In the movies the weigth of each particle is shown based on an US and RF measurement although this weight isn't used to resample.</p> <ul> <li><h4>Example 1</h4> <p> The mobile robot follows a trajectory. The initial position and orientation is exactly known, but there is some uncertainty about the traveled distance and the angle during translation.</br> <a href="../USRF5000_startknown_traject_without_resample_anglposunkn.avi">Example 1 </a> </br> <a href="../USRF5000_startknown_traject_without_resample_anglposunkn_memory.avi">Example 1b: memory </a> <h5>Interpretation</h5> <ul> <li>The particles spread out due to the uncertainty in the traveled distance and the angle during translation.</li> <li>Since there is no resampling based on the available measurements all particles are maintained.</li> <li>The color of the particles indicates the chance the mobile robot is in this position and orientation taking into account the measurement with the US and RF sensor. It is obious the information of these sensor could provide sufficient information to locate the robot.</li> <li>If the weights take into account the weight on the previos moment, the 'memory' of the proces is used.</li> </ul> </p> </li> <li><h4>Example 2</h4> <p> The mobile robot follows a trajectory. The initial position and orientation is exactly known, but there is some uncertainty about the angle during translation.</br> <a href="../USRF5000_startknown_traject_without_resample_angunc.avi">Example 2 </a> <h5>Interpretation</h5> <ul> <li>The particles spread out due to the uncertainty in the angle during translation.</li> <li>Since there is no resampling based on the available measurements all particles are maintained.</li> <li>The color of the particles indicates the chance the mobile robot is in this position and orientation taking into account the measurement with the US and RF sensor. It is obious the information of these sensor could provide sufficient information to locate the robot.</li> </ul> </p> </li> <li><h4>Example 3</h4> <p> The mobile robot follows a trajectory. The initial position and orientation is exactly known, but there is some uncertainty about the translation distance.</br> <a href="../USRF5000_startknown_traject_without_resample_posunc.avi">Example 3 </a> <h5>Interpretation</h5> <ul> <li>The particles spread out due to the uncertainty in the traveled distance and the angle during translation.</li> <li>Since there is no resampling based on the available measurements all particles are maintained.</li> <li>The color of the particles indicates the chance the mobile robot is in this position and orientation taking into account the measurement with the US and RF sensor. It is obious the information of these sensor could provide sufficient information to locate the robot.</li> </ul> </p> </li> </ul> <h2>US measurement</h2> <p>Every time-step the robot measures it's distance to a fixed wall with an ultrasonic sensor.<br /> <b>Remark:</b> In the movies the weigth of each particle is shown based on an US measurement. <p> <ul> <li><h3>Example 1</h3> <p> The mobile robot follows a trajectory. The initial position and orientation is exactly known, but there is some uncertainty about the traveled distance and angle during translation.</br> <a href="../US5000_startknown_traject.avi">Example 1 </a> <h5>Interpretation</h5> <ul> <li>The particles spread out due to the uncertainty in the traveled distance and the angle during translation.</li> <li>Since there is resampling based on the available US measurement not all particles are maintained, those with the lowest weights disappear and those with the highest weights are duplicated both in a statistical manner.</li> <li>The color of the particles indicates the chance the mobile robot is in this position and orientation taking into account the measurement with the US sensor. The information available from this measurement is not sufficient to locate the robot in space, since there is no information provided about the direction parallel to the wall. Therefore it is obvious the particles will spread most in the direction parallel to the wall since the resampling has no effect in this direction.</li> </ul> </p> </ul> <h2>RF measurement</h2> <p>Every time-step the robot measures it's distance and orientation relative to a beaken with a range finder.<br /> <b>Remark:</b> In the movies the weigth of each particle is shown based on a RF measurement. <p> <ul> <li><h3>Example 1</h3> <p> The mobile robot follows a trajectory. The initial position and orientation is exactly known but there is some uncertainty about the translation distance, the angle during translation and the rotation angle.</br> <a href="../RF5000_startknown_traject.avi">Example 1 </a> <h5>Interpretation</h5> <ul> <li>The particles spread out due to the uncertainty in the traveled distance, the angle during translation and the rotation angle.</li> <li>Since there is resampling based on the available RF measurement not all particles are maintained, those with the lowest weights disappear and those with the highest weights are duplicated both in a statistical manner.</li> <li>The color of the particles indicates the chance the mobile robot is in this position and orientation taking into account the measurement with the RF sensor. The information available from this measurement is sufficient in this case to locate the robot in space although in theory one measurement doens't provide a unique solution for the pose of the robot (see localisation simulations).</li> </ul> </p> </li> </ul> <h2>US and RF measurement</h2> <p>Every time-step the robot measures it's distance to a fixed wall with an US sensor and it's distance and orientation relative to a beaken with a range finder.<br /> <b>Remark:</b> In the movies the weigth of each particle is showed based on the US and RF measurements. <p> <ul> <li><h3>Example 1</h3> <p> The mobile robot follows a trajectory. The initial position and orientation is exactly known but there is some uncertainty about the translation distance, the angle during translation and the rotation angle.</br> <a href="../USRF5000_startknown_traject.avi">Example 1 </a> <h5>Interpretation</h5> <ul> <li>The particles spread out due to the uncertainty in the traveled distance, the angle during translation and the rotation angle.</li> <li>Since there is resampling based on the available US and RF measurements not all particles are maintained, those with the lowest weights disappear and those with the highest weights are duplicated both in a statistical manner.</li> <li>The color of the particles indicates the chance the mobile robot is in this position and orientation taking into account the measurement with the US and RF sensor. The information available from this measurement is sufficient to locate the robot in space.</li> </ul> </br> </li> </ul> <h2>Localisation</h2> <p>In a localisation problem, the initial pose (position and orientation) of the robot is not known. The pose of the robot has to be estimated based on the available information from the measurements.</p> <h2>US measurement</h2> <p>Every time-step the robot measures it's distance to a fixed wall with an ultrasonic sensor.<br /> <b>Remark:</b> In the movies the weigth of each particle is shown based on an US measurement. <p> <ul> <li><h3>Example 1</h3> <p> The mobile robot follows a trajectory. The initial position and orientation is unknown and there is some uncertainty about the translation distance, the angle during translation and the rotation angle.</br> <a href="../US10000_angleunc.avi">Example 1 </a> <h5>Interpretation</h5> <ul> <li>Initially the particles are distributed statistically over the space of possible poses (position and orientation)</li> <li>Since there is resampling based on the available US measurement not all particles are maintained, those with the lowest weights disappear and those with the highest weights are duplicated both in a statistical manner.</li> <li>Since a lot of information is provided by the first measurement, a lot of particles are deleted during the first resampling.</li> <li>The color of the particles indicates the chance the mobile robot is in this position and orientation taking into account the measurement with the US sensor. The information available from this measurement is not sufficient to locate the robot in space, since there is no information provided about the direction parallel to the wall. Therefore it is obvious that there is no selection of 'good' particles in the direction parallel to the wall.</li> </ul> </ul> <h2>RF measurement</h2> <p>Every time-step the robot measures it's distance and orientation relative to a beaken with a range finder.<br /> <b>Remark:</b> In the movies the weigth of each particle is shown based on a RF measurement. <p> <ul> <li><h3>Example 1</h3> <p> The mobile robot follows a trajectory. The initial orientation is known, but the initial position is unknown. There is some uncertainty about the traveled distance, the angle during translation and the rotation angle.</br> <a href="../RF5000_angleknown.avi">Example 1 </a> <h5>Interpretation</h5> <ul> <li>Initially the particles are distributed statistically over the space of possible positions, but the orientation is known.</li> <li>The particles spread out due to the uncertainty in the traveled distance, the angle during translation and the rotation angle.</li> <li>Since there is resampling based on the available RF measurement not all particles are maintained, those with the lowest weights disappear and those with the highest weights are duplicated both in a statistical manner.</li> <li>The color of the particles indicates the chance the mobile robot is in this position and orientation taking into account the measurement with the RF sensor. The information available from this measurement is sufficient in this case to locate the robot in space although in theory one measurement doens't provide a unique solution for the pose of the robot. This is due to initially known orientation of the robot.</li> </ul> </p> </li> <li><h3>Example 2</h3> <p> The mobile robot follows a trajectory. The initial position and orientation is unknown and there is some uncertainty about the translation distance, the angle during translation and the rotation angle.</br> <a href="../RF5000_angleunc.avi">Example 2 </a> <h5>Interpretation</h5> <ul> <li>Initially the particles are distributed statistically over the space of possible poses (position and orientation).</li> <li>The particles spread out due to the uncertainty in the traveled distance, the angle during translation and the rotation angle.</li> <li>Since there is resampling based on the available RF measurement not all particles are maintained, those with the lowest weights disappear and those with the highest weights are duplicated both in a statistical manner.</li> <li>The color of the particles indicates the chance the mobile robot is in this position and orientation taking into account the measurement with the RF sensor. The information available from this measurement is unsufficient to locate the robot in space. This is very obvious after the first resampling. There are different groups of particles that are sustained after the resampling indicating that multiple solutions are possible. One RF measurement doesn't determine the position and orientation of the robot in an unique way. But as the mobile robot moves only one particle group will be sustained as the real solution. </li> <li>Due to the too low number of intial particles there is no particle in the neighbourhood of the initial pose. Therefore there will be only particles in the neigbourhood of the real pose of the robot after a wall due to the noise on the particle-movement. </li> </ul> </p> </ul> <h2>US and RF measurement</h2> <p>Every time-step the robot measures it's distance to a fixed wall with an US sensor and it's distance and orientation relative to a beaken with a range finder.<br /> <b>Remark:</b> In the movies the weigth of each particle is showed based on the US and RF measurements. <p> <ul> </li> <li><h3>Example 1</h3> <p> The mobile robot follows a trajectory. The initial orientation is known, but the initial position is unknown. There is some uncertainty about the traveled distance, the angle during translation and the rotation angle.</br> <a href="../RFUS5000_angleknown.avi">Example 1 </a> <h5>Interpretation</h5> <ul> <li>Initially the particles are distributed statistically over the space of possible postions, but the orienation is known.</li> <li>The particles spread out due to the uncertainty in the traveled distance, the angle during translation and the rotation angle.</li> <li>Since there is resampling based on the available US and RF measurement not all particles are maintained, those with the lowest weights disappear and those with the highest weights are duplicated both in a statistical manner.</li> <li>The color of the particles indicates the chance the mobile robot is in this position and orientation taking into account the measurement with the US and RF sensor.</li> </ul> </p> </li> <li><h3>Example 2</h3> <p> The mobile robot follows a trajectory. The initial position and orientation is unknown and there is some uncertainty about the translation distance, the angle during translation and the rotation angle.</br> <a href="../RFUS10000_angleunc.avi">Example 2 </a> <h5>Interpretation</h5> <ul> <li>Initially the particles are distributed statistically over the space of possible postions, but the orienation is known.</li> <li>The particles spread out due to the uncertainty in the traveled distance, the angle during translation and the rotation angle.</li> <li>Since there is resampling based on the available US and RF measurement not all particles are maintained, those with the lowest weights disappear and those with the highest weights are duplicated both in a statistical manner.</li> <li>The color of the particles indicates the chance the mobile robot is in this position and orientation taking into account the measurement with the US and RF sensor. The information available from this measurements doesn't provide immediately sufficient information to locate the robot in space. This is very obvious after the first resampling. There are different groups of particles that are sustained after the resampling indicating that multiple solutions are possible. But as the mobile robot moves only one particle group will be sustained as the real solution. </li> </ul> </p> </li> <li><h3>Example 3</h3> <p> The mobile robot follows a trajectory. The initial orientation is known, but the initial position is unknown. There is some uncertainty about the traveled distance, the angle during translation and the rotation angle. The initial distribution of the particles is assymmetrical around the real initial position.</br> <a href="../USRF10000_angleunc_onevenwichtig.avi">Example 4 </a> <h5>Interpretation</h5> <ul> <li>See the interpretation of Example2.</li> </ul> </br> </li> </ul> <h1>Work to do</h1> <ol> <li>The beaken is normally placed on the robot</li> <li>Use prediction in stead of deadreckoning</li> <li>Interpretation of the movies <ul> <li>The turning of the particle cloud if drift</li> <li>tracking versus localisation</li> <li>effect of the modelling: drift, rotation, translation</li> <li>resampling</li> <li>the different resampling algorithms</li> <li>the different measurements and the information they offer</li> </ul> <li> Link with the BFL</li> </ol> </BODY> </HTML>