 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Example of using a particle filter for localization in ROS by bfl library
Description: The tutorial demonstrates how to use the bfl library to create a particle filter for ROS. This particle filter will be used to track the pose of a robot against a known map. In this tutorial we will use the Gazebo model of AR.Drone equipmented three ultrasonic range sensors. However we can use this tutorial also with real AR.Drone provided that it is equipped with three ultrasonic range sensors or with any other model of robot.Keywords: particle filter, bfl
Tutorial Level: ADVANCED
Prerequisite
As aim of the tutorial is to show how to use the bfl library in ROS, so I strongly recommend primarily to read the getting started guide of bfl library.
To communicate with AR.Drone you need this fork of “ardrone_autonomy” package. This fork contains the code which allow to receive data from ultrasonic ranges in ROS.
Also you need to install “gazebo_simulation” package from “ardroneum” repository.
Creating the particle filter in ROS
This section will guide you step-by-step through the implementation of a particle filter using a nonlinear system model and a non linear measurement model in ROS.
Creating the package
The first step is to create a new package which depends on ros_cpp, std_msgs, nav_msgs, bfl, geometry_msgs and ardrone_autonomy.
The second step is to add the following lines to CMakeList.txt.
# bfl (Bayesian Filtering Library) is a third party package that uses pkg-config
find_package(PkgConfig)
pkg_check_modules(BFL REQUIRED orocos-bfl)
include_directories(${BFL_INCLUDE_DIRS}/bfl)
message("BFL include dirs:" ${BFL_INCLUDE_DIRS})
message("BFL library dirs:" ${BFL_LIBRARY_DIRS})
link_directories(${BFL_LIBRARY_DIRS})
Creating of non linear system model
In this tutorial state of robot includes two coordinates (x, y) and orientation (theta). So the nonlinear system model is same as model described in this tutorial.
First of all, we have to create a new class inherited from ConditionalPdf which will implement general nonlinear conditional probability density function (pdf) which represents the probability of the predicted position given the current position of the mobile robot.
1 #ifndef __NON_LINEAR_SYSTEM_MOBILE__
2 #define __NON_LINEAR_SYSTEM_MOBILE__
3
4 #include <pdf/conditionalpdf.h>
5 #include <pdf/gaussian.h>
6
7 namespace BFL
8 {
9 /// NonLinear Conditional Gaussian
10 class NonlinearSystemPdf : public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector>
11 {
12 public:
13 /// Constructor
14 /** @param additiveNoise Pdf representing the additive Gaussian uncertainty
15 */
16 NonlinearSystemPdf( const Gaussian& additiveNoise);
17
18 /// Destructor
19 virtual ~NonlinearSystemPdf();
20
21 // implement this virtual function for system model of a particle filter
22 virtual bool SampleFrom (Sample<MatrixWrapper::ColumnVector>& one_sample, int method=DEFAULT, void * args=NULL) const;
23
24 private:
25 Gaussian _additiveNoise;
26
27 };
28
29 } // End namespace BFL
30
31 #endif //
32
In this class we have to implement the ‘SampleFrom’ virtual function.
1 #include "nonlinearSystemPdf.h"
2 #include <wrappers/rng/rng.h> // Wrapper around several rng libraries
3
4 #define SYSMODEL_NUMCONDARGUMENTS_MOBILE 2
5 #define SYSMODEL_DIMENSION_MOBILE 3
6
7 namespace BFL
8 {
9 using namespace MatrixWrapper;
10
11 NonlinearSystemPdf::NonlinearSystemPdf(const Gaussian& additiveNoise)
12 : ConditionalPdf<ColumnVector,ColumnVector>(SYSMODEL_DIMENSION_MOBILE,SYSMODEL_NUMCONDARGUMENTS_MOBILE)
13 {
14 _additiveNoise = additiveNoise;
15 }
16
17
18 NonlinearSystemPdf::~NonlinearSystemPdf(){}
19
20
21 bool NonlinearSystemPdf::SampleFrom (Sample<ColumnVector>& one_sample, int method, void * args) const
22 {
23 ColumnVector state = ConditionalArgumentGet(0);
24 ColumnVector vel = ConditionalArgumentGet(1);
25
26 // system update
27 state(1) += cos(state(3)) * vel(1);
28 state(2) += sin(state(3)) * vel(1);
29 state(3) += vel(2);
30
31 // sample from additive noise
32 Sample<ColumnVector> noise;
33 _additiveNoise.SampleFrom(noise, method, args);
34
35 // store results in one_sample
36 one_sample.ValueSet(state + noise.ValueGet());
37
38 return true;
39 }
40
41 }//namespace BFL
42
Creating of nonlinear measurement model
As we have created the system model now we have to create a new class inhereted from ConditionalPdf. This class will implement a general nonlinear conditional probability density function (pdf) which represents the probability of the measured distances. Since we have three distance sensors we have to create an instrument to get a expected values of sensors. We will use a user-generated static map to get occupancy grid data (see the map_server package for documentation on building a map). We will use the map.h, map.c and map_range.c files from amcl node for internal representation of grid-based map and calculation of expected distances.
To generate our own class we start with the implementation of the header file: nonlinearMeasurementPdf.h.
1 #ifndef __NON_LINEAR_MEAS_MOBILE__
2 #define __NON_LINEAR_MEAS_MOBILE__
3
4 #include <pdf/conditionalpdf.h>
5 #include <pdf/gaussian.h>
6 #include "map/map.h"
7 #include <math.h>
8
9 namespace BFL
10 {
11 /// Non Linear Conditional Gaussian
12 class NonlinearMeasurementPdf : public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector>
13 {
14 public:
15 /// Constructor
16 /**
17 @param additiveNoise Pdf representing the additive Gaussian uncertainty
18 */
19 NonlinearMeasurementPdf( const Gaussian& measNoise, map_t* map);
20
21 /// Destructor
22 virtual ~NonlinearMeasurementPdf();
23
24 // implement this virtual function for measurement model of a particle filter
25 virtual Probability ProbabilityGet(const MatrixWrapper::ColumnVector& measurement) const;
26
27 private:
28 Gaussian _measNoise;
29 map_t* _map;
30
31 };
32
33 } // End namespace BFL
34
35 #endif //
36
As you can see we add a pointer to map_t structure into own class and ‘map’ parameters into constructor for initialization. We have to implement ‘ProbabilityGet’ virtual function for our measurement model. This function have to return a probability the received measurements. We will use the gaussian of measurement noise with a difference between measurement and expected measurement vectors to calculate this probability. Expected measurements will be calculated by map_calc_range function
1 double map_calc_range(map_t *map, double x, double y, double theta, double max_range)
1 #include "nonlinearMeasurementPdf.h"
2 #include <wrappers/rng/rng.h> // Wrapper around several rng libraries
3
4 #define MEASMODEL_NUMCONDARGUMENTS_MOBILE 1
5 #define MEASMODEL_DIMENSION_MOBILE 3
6
7 namespace BFL
8 {
9 using namespace MatrixWrapper;
10
11 NonlinearMeasurementPdf::NonlinearMeasurementPdf(const Gaussian& measNoise, map_t *map)
12 : ConditionalPdf<ColumnVector,ColumnVector>(MEASMODEL_DIMENSION_MOBILE,MEASMODEL_NUMCONDARGUMENTS_MOBILE)
13 {
14 _measNoise = measNoise;
15 _map = map;
16 }
17
18
19 NonlinearMeasurementPdf::~NonlinearMeasurementPdf(){}
20
21 Probability
22 NonlinearMeasurementPdf::ProbabilityGet(const ColumnVector& measurement) const
23 {
24 ColumnVector state = ConditionalArgumentGet(0);
25
26 ColumnVector expected_measurement(3);
27
28 // Compute the range according to the map
29 expected_measurement(1) = map_calc_range(_map, state(1), state(2), state(3) + 0, 10.0); //front
30 expected_measurement(2) = map_calc_range(_map, state(1), state(2), state(3) + M_PI_2, 10.0); //left
31 expected_measurement(2) = map_calc_range(_map, state(1), state(2), state(3) - M_PI_2, 10.0); //right
32
33 Probability prb = _measNoise.ProbabilityGet(measurement-expected_measurement);
34
35 return prb;
36 }
37
38 }//namespace BFL
39
Creating of custom particle filter class
In fact, there is a class for particles filter in BFL library. However, we will publish a particles cloud, so we need an access at weight samples of the filter. For that, we have to create an own class inherited from BootstrapFilter class. Create file customparticlefilter.h
1 #ifndef CUSTOMPARTICLEFILTER_H
2 #define CUSTOMPARTICLEFILTER_H
3
4 #include <filter/bootstrapfilter.h>
5
6 using namespace BFL;
7
8 class CustomParticleFilter : public BootstrapFilter<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>
9 {
10 public:
11 CustomParticleFilter(MCPdf<MatrixWrapper::ColumnVector> * prior,
12 int resampleperiod = 0,
13 double resamplethreshold = 0,
14 int resamplescheme = DEFAULT_RS);
15
16 vector<WeightedSample<MatrixWrapper::ColumnVector> > getNewSamples();
17
18 };
19
20 #endif // CUSTOMPARTICLEFILTER_H
21
Then create customparticlefilter.cpp file
1 #include "customparticlefilter.h"
2
3 using namespace MatrixWrapper;
4 using namespace BFL;
5
6 CustomParticleFilter::CustomParticleFilter(MCPdf<ColumnVector> *prior, int resampleperiod, double resamplethreshold, int resamplescheme):
7 BootstrapFilter<ColumnVector,ColumnVector>(prior, resampleperiod, resamplethreshold, resamplescheme)
8 { }
9
10 vector<WeightedSample<ColumnVector> > CustomParticleFilter::getNewSamples()
11 {
12 return _new_samples;
13 }
Creating a node of particle filter
Now we have to create a class that implements a ROS node. This class will be responsible for:
- creating an instance of the system model;
- creating an instance of the measurement model;
- creating an instance of the particle filter;
- handling of map request;
- handling of measurement messages;
- handling of system update messages;
- publish messages with current pose and particles cloud.
Create particlefilternode.cpp file.
1 #include <filter/bootstrapfilter.h>
2 #include <model/systemmodel.h>
3 #include <model/measurementmodel.h>
4 #include "nonlinearSystemPdf.h"
5 #include "nonlinearMeasurementPdf.h"
6 #include <iostream>
7 #include <fstream>
8 // Include file with properties
9 #include "constants.h"
10 #include "customparticlefilter.h"
11 #include <ros/ros.h>
12 #include <string>
13 #include <std_msgs/Empty.h>
14
15 #include <ardrone_autonomy/Navdata.h>
16 #include <ardrone_autonomy/Ranges.h>
17 #include <geometry_msgs/PoseArray.h>
18 #include <geometry_msgs/PoseStamped.h>
19 #include "nav_msgs/GetMap.h"
20 #include "map/map.h"
21
22 using namespace MatrixWrapper;
23 using namespace BFL;
24 using namespace std;
25
26 /*
27 The necessary SYSTEM MODEL is:
28 x_k = x_{k-1} + v_{k-1} * cos(theta) * delta_t
29 y_k = y_{k-1} + v_{k-1} * sin(theta) * delta_t
30 */
31
32 class ParticleFilterNode
33 {
34 ros::NodeHandle nh_;
35 ros::Subscriber navi_sub;
36 ros::Subscriber ranges_sub;
37 ros::Publisher pose_pub;
38 ros::Publisher particle_pub;
39 map_t* map_;
40 NonlinearSystemPdf *sys_pdf;
41 SystemModel<ColumnVector> *sys_model;
42 NonlinearMeasurementPdf *meas_pdf;
43 MeasurementModel<ColumnVector,ColumnVector> *meas_model;
44 MCPdf<ColumnVector> *prior_discr;
45 CustomParticleFilter *filter;
46 ros::Time prevNavDataTime;
47 double dt;
48 ardrone_autonomy::Navdata LastNavDataMsg;
49
50 public:
51 ParticleFilterNode()
52 {
53 navi_sub = nh_.subscribe("/ardrone/navdata", 1, &ParticleFilterNode::InputCb, this);
54 ranges_sub = nh_.subscribe("ardrone/ranges", 1, &ParticleFilterNode::MeasurementCb, this);
55 pose_pub = nh_.advertise<geometry_msgs::PoseStamped>("/pose_pf",1);
56 particle_pub = nh_.advertise<geometry_msgs::PoseArray>("/particle_cloud",1);
57 dt = 0.0;
58
59 sys_model = NULL;
60 meas_model = NULL;
61 filter = NULL;
62 map_ = NULL;
63 requestMap();
64
65 }
66
67 ~ParticleFilterNode()
68 {
69 delete sys_model;
70 delete meas_model;
71 delete filter;
72 }
73
74 void CreateParticleFilter()
75 {
76 /****************************
77 * NonLinear system model *
78 ***************************/
79
80 // create gaussian
81 ColumnVector sys_noise_Mu(STATE_SIZE);
82 sys_noise_Mu(1) = MU_SYSTEM_NOISE_X;
83 sys_noise_Mu(2) = MU_SYSTEM_NOISE_Y;
84 sys_noise_Mu(3) = MU_SYSTEM_NOISE_THETA;
85
86 SymmetricMatrix sys_noise_Cov(STATE_SIZE);
87 sys_noise_Cov = 0.0;
88 sys_noise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
89 sys_noise_Cov(1,2) = 0.0;
90 sys_noise_Cov(1,3) = 0.0;
91 sys_noise_Cov(2,1) = 0.0;
92 sys_noise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
93 sys_noise_Cov(2,3) = 0.0;
94 sys_noise_Cov(3,1) = 0.0;
95 sys_noise_Cov(3,2) = 0.0;
96 sys_noise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA;
97
98 Gaussian system_Uncertainty(sys_noise_Mu, sys_noise_Cov);
99
100 // create the nonlinear system model
101 sys_pdf = new NonlinearSystemPdf(system_Uncertainty);
102 sys_model = new SystemModel<ColumnVector> (sys_pdf);
103
104
105 /*********************************
106 * NonLinear Measurement model *
107 ********************************/
108
109
110 // Construct the measurement noise (a scalar in this case)
111 ColumnVector meas_noise_Mu(MEAS_SIZE);
112 meas_noise_Mu(1) = MU_MEAS_NOISE;
113 meas_noise_Mu(2) = MU_MEAS_NOISE;
114 meas_noise_Mu(3) = MU_MEAS_NOISE;
115 SymmetricMatrix meas_noise_Cov(MEAS_SIZE);
116 meas_noise_Cov(1,1) = SIGMA_MEAS_NOISE;
117 meas_noise_Cov(1,2) = 0.0;
118 meas_noise_Cov(1,3) = 0.0;
119 meas_noise_Cov(2,1) = 0.0;
120 meas_noise_Cov(2,2) = SIGMA_MEAS_NOISE;
121 meas_noise_Cov(2,3) = 0.0;
122 meas_noise_Cov(3,1) = 0.0;
123 meas_noise_Cov(3,2) = 0.0;
124 meas_noise_Cov(3,3) = SIGMA_MEAS_NOISE;
125
126 Gaussian measurement_Uncertainty(meas_noise_Mu, meas_noise_Cov);
127
128
129 meas_pdf = new NonlinearMeasurementPdf(measurement_Uncertainty, map_);
130 meas_model = new MeasurementModel<ColumnVector,ColumnVector>(meas_pdf);
131
132 /****************************
133 * Linear prior DENSITY *
134 ***************************/
135 // Continuous Gaussian prior (for Kalman filters)
136 ColumnVector prior_Mu(STATE_SIZE);
137 prior_Mu(1) = PRIOR_MU_X;
138 prior_Mu(2) = PRIOR_MU_Y;
139 prior_Mu(3) = PRIOR_MU_THETA;
140 SymmetricMatrix prior_Cov(STATE_SIZE);
141 prior_Cov(1,1) = PRIOR_COV_X;
142 prior_Cov(1,2) = 0.0;
143 prior_Cov(1,3) = 0.0;
144 prior_Cov(2,1) = 0.0;
145 prior_Cov(2,2) = PRIOR_COV_Y;
146 prior_Cov(2,3) = 0.0;
147 prior_Cov(3,1) = 0.0;
148 prior_Cov(3,2) = 0.0;
149 prior_Cov(3,3) = PRIOR_COV_THETA;
150 Gaussian prior_cont(prior_Mu,prior_Cov);
151
152 // Discrete prior for Particle filter (using the continuous Gaussian prior)
153 vector<Sample<ColumnVector> > prior_samples(NUM_SAMPLES);
154 prior_discr = new MCPdf<ColumnVector>(NUM_SAMPLES,STATE_SIZE);
155 prior_cont.SampleFrom(prior_samples,NUM_SAMPLES,CHOLESKY,NULL);
156 prior_discr->ListOfSamplesSet(prior_samples);
157
158 /******************************
159 * Construction of the Filter *
160 ******************************/
161 filter = new CustomParticleFilter (prior_discr, 0.5, NUM_SAMPLES/4.0);
162 }
163
164 void MeasurementCb(ardrone_autonomy::Ranges msg)
165 {
166 ColumnVector measurement(3);
167 measurement(1) = msg.range_front/100;
168 measurement(2) = msg.range_left/100;
169 measurement(3) = msg.range_right/100;
170 //ROS_INFO("Measurement: %f",measurement(1));
171 if (LastNavDataMsg.state==3 || LastNavDataMsg.state==7 || LastNavDataMsg.state==4)
172 {
173 filter->Update(meas_model, measurement);
174 PublishParticles();
175 PublishPose();
176 }
177 }
178
179 void InputCb(ardrone_autonomy::Navdata msg)
180 {
181 if(!prevNavDataTime.isZero()) dt = (msg.header.stamp - prevNavDataTime).toSec();
182 prevNavDataTime = msg.header.stamp;
183 LastNavDataMsg = msg;
184
185 ColumnVector input(2);
186 input(1) = msg.vx*dt*0.001;
187 input(2) = msg.vy*dt*0.001;
188 if (LastNavDataMsg.state==3 || LastNavDataMsg.state==7 || LastNavDataMsg.state==4)
189 filter->Update(sys_model,input);
190 }
191
192 void PublishParticles()
193 {
194 geometry_msgs::PoseArray particles_msg;
195 particles_msg.header.stamp = ros::Time::now();
196 particles_msg.header.frame_id = "/map";
197
198 vector<WeightedSample<ColumnVector> >::iterator sample_it;
199 vector<WeightedSample<ColumnVector> > samples;
200
201 samples = filter->getNewSamples();
202
203 for(sample_it = samples.begin(); sample_it<samples.end(); sample_it++)
204 {
205 geometry_msgs::Pose pose;
206 ColumnVector sample = (*sample_it).ValueGet();
207
208 pose.position.x = sample(1);
209 pose.position.y = sample(2);
210 pose.orientation.z = sample(3);
211
212 particles_msg.poses.insert(particles_msg.poses.begin(), pose);
213 }
214 particle_pub.publish(particles_msg);
215
216 }
217
218 void PublishPose()
219 {
220 Pdf<ColumnVector> * posterior = filter->PostGet();
221 ColumnVector pose = posterior->ExpectedValueGet();
222 SymmetricMatrix pose_cov = posterior->CovarianceGet();
223
224 geometry_msgs::PoseStamped pose_msg;
225 pose_msg.header.stamp = ros::Time::now();
226 pose_msg.header.frame_id = "/map";
227
228 pose_msg.pose.position.x = pose(1);
229 pose_msg.pose.position.y = pose(2);
230 pose_msg.pose.orientation.z = pose(3);
231
232 pose_pub.publish(pose_msg);
233 }
234
235 void requestMap()
236 {
237 // get map via RPC
238 nav_msgs::GetMap::Request req;
239 nav_msgs::GetMap::Response resp;
240 ROS_INFO("Requesting the map...");
241 while(!ros::service::call("static_map", req, resp))
242 {
243 ROS_WARN("Request for map failed; trying again...");
244 ros::Duration d(0.5);
245 d.sleep();
246 }
247 handleMapMessage( resp.map );
248 }
249
250 void handleMapMessage(const nav_msgs::OccupancyGrid& msg)
251 {
252 ROS_INFO("Received a %d X %d map @ %.3f m/pix Origin X %.3f Y %.3f\n",
253 msg.info.width,
254 msg.info.height,
255 msg.info.resolution,
256 msg.info.origin.position.x,
257 msg.info.origin.position.y);
258
259 freeMapDependentMemory();
260 map_ = convertMap(msg);
261 CreateParticleFilter();
262 }
263
264 void freeMapDependentMemory()
265 {
266 if( map_ != NULL ) {
267 map_free( map_ );
268 map_ = NULL;
269 }
270
271 if (sys_model)
272 delete sys_model;
273 if (meas_model)
274 delete meas_model;
275 if (filter)
276 delete filter;
277 }
278
279 /**
280 * Convert an OccupancyGrid map message into the internal
281 * representation. This allocates a map_t and returns it.
282 */
283 map_t* convertMap( const nav_msgs::OccupancyGrid& map_msg )
284 {
285 map_t* map = map_alloc();
286 ROS_ASSERT(map);
287
288 map->size_x = map_msg.info.width;
289 map->size_y = map_msg.info.height;
290 map->scale = map_msg.info.resolution;
291 map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
292 map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
293
294 ROS_INFO(" Size X=%d Y=%d Scale=%f Origin X=%f Y=%f", map->size_x, map->size_y, map->scale, map->origin_x, map->origin_y);
295 // Convert to player format
296 map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
297 ROS_ASSERT(map->cells);
298
299 for(int i=0;i<map->size_x * map->size_y;i++)
300 {
301 if(map_msg.data[i] == 0)
302 map->cells[i].occ_state = -1;
303 else if(map_msg.data[i] == 100)
304 map->cells[i].occ_state = +1;
305 else
306 map->cells[i].occ_state = 0;
307 }
308 return map;
309 }
310
311 };
312
313
314 int main(int argc, char** argv)
315 {
316 ros::init(argc, argv, "ParticleFilterNode");
317 ParticleFilterNode pfNode;
318 ros::spin();
319 return 0;
320 }
Testing the particle filter
Preparing of world file for Gazebo simulation
In this tutorial we will use a simple world (a rectangle room) for test.
You can download this world file here.
Preparing map filesNow we have to prepare a corresponding yaml and png files for map server. Yaml file have to contains name of png files, scale, coordinates of origin and some threshold.
image: newmap150x40.png
resolution: 0.1
origin: [-7.0, -2.0, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
As we have the 15x4 room and resolution of map is 0.1 m/px we have to create a image file with 150x40 dimensions. Obviously this image will contains only a black frame along borders. You can download this image file here.
Preparing launch file
First of all, we have to add a gazebo node with the simple world and spawn the model of AR.Drone.
<include file="$(find gazebo_simulation)/launch/start.launch">
<arg name="world" value="$(find gazebo_simulation)/worlds/box.world"/>
</include>
<include file="$(find gazebo_simulation)/launch/spawn_quadrotor.launch" >
<arg name="model" value="$(find gazebo_simulation)/urdf/ardrone_with_sonars.urdf.xacro"/>
</include>Now we have to add a node to control the AR.Drone. Here you can use any node like hector_quadrotor_teleop. We will use own control node (navigation) that is more complex than just teleop control. You can find this node in our repository.
Then we have to add a map_server node and tf node for static transformation
<node name="map_node" pkg="map_server" type="map_server"
args="$(find particles_filter_bfl)/map.yaml" respawn="true" output="screen" />
<node pkg="tf" type="static_transform_publisher" name="map_base_link" args="0 0 0 0 0 0 map nav 100" />Now we have to add own particle filter
<node pkg="particles_filter_bfl" type="particles_filter_node" name="particles_filter_node" output="screen" respawn="true"> </node>
Also we need to add rviz node for visualization of data
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find particles_filter)/pf.rviz" > </node>
The ready launch file you can download here.
Launch and result video To start you just need type the following command:
roslaunch particles_filter_bfl ardrone_gazebo_with_pf_bfl.launch
Then you need to publish start command for navigation node. Just type in new terminal the following command:
rostopic pub /start std_msgs/Empty
Example of working particle filter you can see on video below.
