gmcl_odom.cpp
Go to the documentation of this file.
1 //this package is based on amcl and has been modified to fit gmcl
2 /*
3  * Author: Mhd Ali Alshikh Khalil
4  * Date: 20 June 2021
5  *
6 */
7 
8 //amcl author clarification
9 /*
10  * Player - One Hell of a Robot Server
11  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
12  * gerkey@usc.edu kaspers@robotics.usc.edu
13  *
14  * This library is free software; you can redistribute it and/or
15  * modify it under the terms of the GNU Lesser General Public
16  * License as published by the Free Software Foundation; either
17  * version 2.1 of the License, or (at your option) any later version.
18  *
19  * This library is distributed in the hope that it will be useful,
20  * but WITHOUT ANY WARRANTY; without even the implied warranty of
21  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
22  * Lesser General Public License for more details.
23  *
24  * You should have received a copy of the GNU Lesser General Public
25  * License along with this library; if not, write to the Free Software
26  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
27  *
28  */
30 //
31 // Desc: AMCL odometry routines
32 // Author: Andrew Howard
33 // Date: 6 Feb 2003
34 // CVS: $Id: amcl_odom.cc 7057 2008-10-02 00:44:06Z gbiggs $
35 //
37 
38 #include <algorithm>
39 #include "ros/ros.h"
40 #include <sys/types.h> // required by Darwin
41 #include <math.h>
42 
43 #include "gmcl/sensors/gmcl_odom.h"
44 
45 using namespace gmcl;
46 
47 static double
48 normalize(double z)
49 {
50  return atan2(sin(z),cos(z));
51 }
52 static double
53 angle_diff(double a, double b)
54 {
55  double d1, d2;
56  a = normalize(a);
57  b = normalize(b);
58  d1 = a-b;
59  d2 = 2*M_PI - fabs(d1);
60  if(d1 > 0)
61  d2 *= -1.0;
62  if(fabs(d1) < fabs(d2))
63  return(d1);
64  else
65  return(d2);
66 }
67 
69 // Default constructor
70 GMCLOdom::GMCLOdom() : GMCLSensor()
71 {
72  this->time = 0.0;
73 }
74 
75 void
76 GMCLOdom::SetModelDiff(double alpha1,
77  double alpha2,
78  double alpha3,
79  double alpha4)
80 {
81  this->model_type = ODOM_MODEL_DIFF;
82  this->alpha1 = alpha1;
83  this->alpha2 = alpha2;
84  this->alpha3 = alpha3;
85  this->alpha4 = alpha4;
86 }
87 
88 void
89 GMCLOdom::SetModelOmni(double alpha1,
90  double alpha2,
91  double alpha3,
92  double alpha4,
93  double alpha5)
94 {
95  this->model_type = ODOM_MODEL_OMNI;
96  this->alpha1 = alpha1;
97  this->alpha2 = alpha2;
98  this->alpha3 = alpha3;
99  this->alpha4 = alpha4;
100  this->alpha5 = alpha5;
101 }
102 
103 void
104 GMCLOdom::SetModel( odom_model_t type,
105  double alpha1,
106  double alpha2,
107  double alpha3,
108  double alpha4,
109  double alpha5 )
110 {
111  this->model_type = type;
112  this->alpha1 = alpha1;
113  this->alpha2 = alpha2;
114  this->alpha3 = alpha3;
115  this->alpha4 = alpha4;
116  this->alpha5 = alpha5;
117 }
118 
120 // Apply the action model
121 bool GMCLOdom::UpdateAction(pf_t *pf, GMCLSensorData *data)
122 {
123  // Apply the laser sensor model
124  if(this->model_type == ODOM_MODEL_OMNI)
125  pf_update_action(pf, (pf_action_model_fn_t) OmniModel, data);
126  else if(this->model_type == ODOM_MODEL_OMNI_CORRECTED)
127  pf_update_action(pf, (pf_action_model_fn_t) OmniCorrectedModel, data);
128  else if(this->model_type == ODOM_MODEL_DIFF)
129  pf_update_action(pf, (pf_action_model_fn_t) DiffModel, data);
130  else if(this->model_type == ODOM_MODEL_DIFF_CORRECTED)
131  pf_update_action(pf, (pf_action_model_fn_t) DiffCorrectedModel, data);
132  else
133  pf_update_action(pf, (pf_action_model_fn_t) DiffModel, data);
134 
135  return true;
136 }
137 
138 
139 void GMCLOdom::OmniModel(GMCLOdomData *data , pf_sample_set_t* set)
140  { GMCLOdom *self;
141  self = (GMCLOdom*) data->sensor;
142  pf_t *pf;
143  pf_sample_t* aux_sample;
144  pf = set->pf;
145 
146  // Compute the new sample poses
147  pf_vector_t old_pose = pf_vector_sub(data->pose, data->delta);
148 
149  double delta_trans, delta_rot, delta_bearing;
150  double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
151 
152  delta_trans = sqrt(data->delta.v[0]*data->delta.v[0] +
153  data->delta.v[1]*data->delta.v[1]);
154  delta_rot = data->delta.v[2];
155 
156  // Precompute a couple of things
157  double trans_hat_stddev = (self->alpha3 * (delta_trans*delta_trans) +
158  self->alpha1 * (delta_rot*delta_rot));
159  double rot_hat_stddev = (self->alpha4 * (delta_rot*delta_rot) +
160  self->alpha2 * (delta_trans*delta_trans));
161  double strafe_hat_stddev = (self->alpha1 * (delta_rot*delta_rot) +
162  self->alpha5 * (delta_trans*delta_trans));
163 
164  if(pf->model.use_optimal_filter)
165  for (int i = 0; i < set->sample_count; i++)
166  {
167  pf_sample_t* sample = set->samples + i;
168  delta_bearing = angle_diff(atan2(data->delta.v[1], data->delta.v[0]),
169  old_pose.v[2]) + sample->pose.v[2];
170  double cs_bearing = cos(delta_bearing);
171  double sn_bearing = sin(delta_bearing);
172  for (int j = 0; j < pf->N_aux_particles; j++)
173  {
174  aux_sample = set->aux_samples + (i*pf->N_aux_particles+j) ;
175 
176  // Sample pose differences
177  delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
178  delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
179  delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
180 
181  // Apply sampled update to particle pose
182  aux_sample->pose.v[0] = (delta_trans_hat * cs_bearing +
183  delta_strafe_hat * sn_bearing) + sample->pose.v[0];
184  aux_sample->pose.v[1] = (delta_trans_hat * sn_bearing -
185  delta_strafe_hat * cs_bearing) + sample->pose.v[1];
186  aux_sample->pose.v[2] = delta_rot_hat + sample->pose.v[2];
187  }
188 
189  }
190 else
191  for (int i = 0; i < set->sample_count; i++)
192  {
193  pf_sample_t* sample = set->samples + i;
194 
195  delta_bearing = angle_diff(atan2(data->delta.v[1], data->delta.v[0]),
196  old_pose.v[2]) + sample->pose.v[2];
197  double cs_bearing = cos(delta_bearing);
198  double sn_bearing = sin(delta_bearing);
199 
200  // Sample pose differences
201  delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
202  delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
203  delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
204  // Apply sampled update to particle pose
205  sample->pose.v[0] += (delta_trans_hat * cs_bearing +
206  delta_strafe_hat * sn_bearing);
207  sample->pose.v[1] += (delta_trans_hat * sn_bearing -
208  delta_strafe_hat * cs_bearing);
209  sample->pose.v[2] += delta_rot_hat ;
210  }
211  }
212 
213 
214 
215 
216 void GMCLOdom::DiffModel(GMCLOdomData *data , pf_sample_set_t* set)
217  { GMCLOdom *self;
218  self = (GMCLOdom*) data->sensor ;
219  pf_t *pf;
220  pf_sample_t* aux_sample;
221  pf = set->pf;
222  // Compute the new sample poses
223 
224  pf_vector_t old_pose = pf_vector_sub(data->pose, data->delta);
225 
226  // Implement sample_motion_odometry (Prob Rob p 136)
227  double delta_rot1, delta_trans, delta_rot2;
228  double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
229  double delta_rot1_noise, delta_rot2_noise;
230 
231  // Avoid computing a bearing from two poses that are extremely near each
232  // other (happens on in-place rotation).
233  if(sqrt(data->delta.v[1]*data->delta.v[1] +
234  data->delta.v[0]*data->delta.v[0]) < 0.01)
235  delta_rot1 = 0.0;
236  else
237  delta_rot1 = angle_diff(atan2(data->delta.v[1], data->delta.v[0]),
238  old_pose.v[2]);
239  delta_trans = sqrt(data->delta.v[0]*data->delta.v[0] +
240  data->delta.v[1]*data->delta.v[1]);
241  delta_rot2 = angle_diff(data->delta.v[2], delta_rot1);
242 
243  // We want to treat backward and forward motion symmetrically for the
244  // noise model to be applied below. The standard model seems to assume
245  // forward motion.
246  delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
247  fabs(angle_diff(delta_rot1,M_PI)));
248  delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
249  fabs(angle_diff(delta_rot2,M_PI)));
250 
251  if(pf->model.use_optimal_filter)
252  for (int i = 0; i < set->sample_count; i++)
253  {
254  pf_sample_t* sample = set->samples + i;
255  for (int j = 0; j < pf->N_aux_particles; j++)
256  {
257  aux_sample = set->aux_samples + (i*pf->N_aux_particles+j) ;
258 
259  // Sample pose differences
260  delta_rot1_hat = angle_diff(delta_rot1,
261  pf_ran_gaussian(self->alpha1*delta_rot1_noise*delta_rot1_noise +
262  self->alpha2*delta_trans*delta_trans));
263  delta_trans_hat = delta_trans -
264  pf_ran_gaussian(self->alpha3*delta_trans*delta_trans +
265  self->alpha4*delta_rot1_noise*delta_rot1_noise +
266  self->alpha4*delta_rot2_noise*delta_rot2_noise);
267  delta_rot2_hat = angle_diff(delta_rot2,
268  pf_ran_gaussian(self->alpha1*delta_rot2_noise*delta_rot2_noise +
269  self->alpha2*delta_trans*delta_trans));
270 
271  // Apply sampled update to particle pose
272  aux_sample->pose.v[0] = delta_trans_hat *
273  cos(sample->pose.v[2] + delta_rot1_hat) + sample->pose.v[0];
274  aux_sample->pose.v[1] = delta_trans_hat *
275  sin(sample->pose.v[2] + delta_rot1_hat) + sample->pose.v[1];
276  aux_sample->pose.v[2] = delta_rot1_hat + delta_rot2_hat + sample->pose.v[2];
277  }
278 
279  }
280  else
281  for (int i = 0; i < set->sample_count; i++)
282  {
283  pf_sample_t* sample = set->samples + i;
284 
285  // Sample pose differences
286  delta_rot1_hat = angle_diff(delta_rot1,
287  pf_ran_gaussian(self->alpha1*delta_rot1_noise*delta_rot1_noise +
288  self->alpha2*delta_trans*delta_trans));
289  delta_trans_hat = delta_trans -
290  pf_ran_gaussian(self->alpha3*delta_trans*delta_trans +
291  self->alpha4*delta_rot1_noise*delta_rot1_noise +
292  self->alpha4*delta_rot2_noise*delta_rot2_noise);
293  delta_rot2_hat = angle_diff(delta_rot2,
294  pf_ran_gaussian(self->alpha1*delta_rot2_noise*delta_rot2_noise +
295  self->alpha2*delta_trans*delta_trans));
296 
297  // Apply sampled update to particle pose
298  sample->pose.v[0] += delta_trans_hat *
299  cos(sample->pose.v[2] + delta_rot1_hat);
300  sample->pose.v[1] += delta_trans_hat *
301  sin(sample->pose.v[2] + delta_rot1_hat);
302  sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
303  }
304 
305  }
306 
307  void GMCLOdom::OmniCorrectedModel(GMCLOdomData *data , pf_sample_set_t* set)
308  { GMCLOdom *self;
309  self = (GMCLOdom*) data->sensor;
310  pf_t *pf;
311  pf_sample_t* aux_sample;
312  pf = set->pf;
313  // Compute the new sample poses
314  pf_vector_t old_pose = pf_vector_sub(data->pose, data->delta);
315  double delta_trans, delta_rot, delta_bearing;
316  double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
317 
318  delta_trans = sqrt(data->delta.v[0]*data->delta.v[0] +
319  data->delta.v[1]*data->delta.v[1]);
320  delta_rot = data->delta.v[2];
321  // Precompute a couple of things
322  double trans_hat_stddev = sqrt( self->alpha3 * (delta_trans*delta_trans) +
323  self->alpha4 * (delta_rot*delta_rot) );
324  double rot_hat_stddev = sqrt( self->alpha1 * (delta_rot*delta_rot) +
325  self->alpha2 * (delta_trans*delta_trans) );
326  double strafe_hat_stddev = sqrt( self->alpha4 * (delta_rot*delta_rot) +
327  self->alpha5 * (delta_trans*delta_trans) );
328  if(pf->model.use_optimal_filter)
329  for (int i = 0; i < set->sample_count; i++)
330  {
331  pf_sample_t* sample = set->samples + i;
332  delta_bearing = angle_diff(atan2(data->delta.v[1], data->delta.v[0]),
333  old_pose.v[2]) + sample->pose.v[2];
334  double cs_bearing = cos(delta_bearing);
335  double sn_bearing = sin(delta_bearing);
336  for (int j = 0; j < pf->N_aux_particles; j++)
337  {
338  aux_sample = set->aux_samples + (i*pf->N_aux_particles+j) ;
339 
340  // Sample pose differences
341  delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
342  delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
343  delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
344 
345  // Apply sampled update to particle pose
346  aux_sample->pose.v[0] = (delta_trans_hat * cs_bearing +
347  delta_strafe_hat * sn_bearing) + sample->pose.v[0];
348  aux_sample->pose.v[1] = (delta_trans_hat * sn_bearing -
349  delta_strafe_hat * cs_bearing) + sample->pose.v[1];
350  aux_sample->pose.v[2] = delta_rot_hat + sample->pose.v[2];
351  }
352 
353  }
354 else
355  for (int i = 0; i < set->sample_count; i++)
356  {
357  pf_sample_t* sample = set->samples + i;
358 
359  delta_bearing = angle_diff(atan2(data->delta.v[1], data->delta.v[0]),
360  old_pose.v[2]) + sample->pose.v[2];
361  double cs_bearing = cos(delta_bearing);
362  double sn_bearing = sin(delta_bearing);
363 
364  // Sample pose differences
365  delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
366  delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
367  delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
368  // Apply sampled update to particle pose
369  sample->pose.v[0] += (delta_trans_hat * cs_bearing +
370  delta_strafe_hat * sn_bearing);
371  sample->pose.v[1] += (delta_trans_hat * sn_bearing -
372  delta_strafe_hat * cs_bearing);
373  sample->pose.v[2] += delta_rot_hat ;
374  }
375  }
376 void GMCLOdom::DiffCorrectedModel(GMCLOdomData *data , pf_sample_set_t* set)
377  { GMCLOdom *self;
378  self = (GMCLOdom*) data->sensor;
379  pf_t *pf;
380  pf_sample_t* aux_sample;
381  pf = set->pf;
382  // Compute the new sample poses
383 
384  pf_vector_t old_pose = pf_vector_sub(data->pose, data->delta);
385  // Implement sample_motion_odometry (Prob Rob p 136)
386  double delta_rot1, delta_trans, delta_rot2;
387  double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
388  double delta_rot1_noise, delta_rot2_noise;
389 
390  // Avoid computing a bearing from two poses that are extremely near each
391  // other (happens on in-place rotation).
392  if(sqrt(data->delta.v[1]*data->delta.v[1] +
393  data->delta.v[0]*data->delta.v[0]) < 0.01)
394  delta_rot1 = 0.0;
395  else
396  delta_rot1 = angle_diff(atan2(data->delta.v[1], data->delta.v[0]),
397  old_pose.v[2]);
398  delta_trans = sqrt(data->delta.v[0]*data->delta.v[0] +
399  data->delta.v[1]*data->delta.v[1]);
400  delta_rot2 = angle_diff(data->delta.v[2], delta_rot1);
401 
402  // We want to treat backward and forward motion symmetrically for the
403  // noise model to be applied below. The standard model seems to assume
404  // forward motion.
405  delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
406  fabs(angle_diff(delta_rot1,M_PI)));
407  delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
408  fabs(angle_diff(delta_rot2,M_PI)));
409 if(pf->model.use_optimal_filter)
410  for (int i = 0; i < set->sample_count; i++)
411  {
412  pf_sample_t* sample = set->samples + i;
413  for (int j = 0; j < pf->N_aux_particles; j++)
414  {
415  aux_sample = set->aux_samples + (i*pf->N_aux_particles+j) ;
416 
417  // Sample pose differences
418  delta_rot1_hat = angle_diff(delta_rot1,
419  pf_ran_gaussian(sqrt(self->alpha1*delta_rot1_noise*delta_rot1_noise +
420  self->alpha2*delta_trans*delta_trans)));
421  delta_trans_hat = delta_trans -
422  pf_ran_gaussian(sqrt(self->alpha3*delta_trans*delta_trans +
423  self->alpha4*delta_rot1_noise*delta_rot1_noise +
424  self->alpha4*delta_rot2_noise*delta_rot2_noise));
425  delta_rot2_hat = angle_diff(delta_rot2,
426  pf_ran_gaussian(sqrt(self->alpha1*delta_rot2_noise*delta_rot2_noise +
427  self->alpha2*delta_trans*delta_trans)));
428 
429  // Apply sampled update to particle pose
430  aux_sample->pose.v[0] = delta_trans_hat *
431  cos(sample->pose.v[2] + delta_rot1_hat) + sample->pose.v[0];
432  aux_sample->pose.v[1] = delta_trans_hat *
433  sin(sample->pose.v[2] + delta_rot1_hat) + sample->pose.v[1];
434  aux_sample->pose.v[2] = delta_rot1_hat + delta_rot2_hat + sample->pose.v[2];
435  }
436 
437  }
438  else
439  for (int i = 0; i < set->sample_count; i++)
440  {
441  pf_sample_t* sample = set->samples + i;
442 
443  // Sample pose differences
444  delta_rot1_hat = angle_diff(delta_rot1,
445  pf_ran_gaussian(sqrt(self->alpha1*delta_rot1_noise*delta_rot1_noise +
446  self->alpha2*delta_trans*delta_trans)));
447  delta_trans_hat = delta_trans -
448  pf_ran_gaussian(sqrt(self->alpha3*delta_trans*delta_trans +
449  self->alpha4*delta_rot1_noise*delta_rot1_noise +
450  self->alpha4*delta_rot2_noise*delta_rot2_noise));
451  delta_rot2_hat = angle_diff(delta_rot2,
452  pf_ran_gaussian(sqrt(self->alpha1*delta_rot2_noise*delta_rot2_noise +
453  self->alpha2*delta_trans*delta_trans)));
454 
455  // Apply sampled update to particle pose
456  sample->pose.v[0] += delta_trans_hat *
457  cos(sample->pose.v[2] + delta_rot1_hat);
458  sample->pose.v[1] += delta_trans_hat *
459  sin(sample->pose.v[2] + delta_rot1_hat);
460  sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
461  }
462  }
463 
464 
465 
466 
normalize
static double normalize(double z)
Definition: gmcl_odom.cpp:48
gmcl::GMCLOdom::alpha2
double alpha2
Definition: gmcl_odom.h:101
gmcl::GMCLOdom
Definition: gmcl_odom.h:67
gmcl::odom_model_t
odom_model_t
Definition: gmcl_odom.h:46
gmcl::GMCLOdom::alpha5
double alpha5
Definition: gmcl_odom.h:101
gmcl
Definition: gmcl_laser.h:43
gmcl::GMCLOdom::UpdateAction
virtual bool UpdateAction(pf_t *pf, GMCLSensorData *data)
Definition: gmcl_odom.cpp:121
pf_vector_t
Definition: pf_vector.h:46
gmcl::GMCLOdom::GMCLOdom
GMCLOdom()
Definition: gmcl_odom.cpp:70
gmcl::GMCLSensorData
Definition: gmcl_sensor.h:93
gmcl::GMCLOdom::SetModel
void SetModel(odom_model_t type, double alpha1, double alpha2, double alpha3, double alpha4, double alpha5=0)
Definition: gmcl_odom.cpp:104
ros.h
gmcl::GMCLOdom::OmniModel
static void OmniModel(GMCLOdomData *data, pf_sample_set_t *set)
Definition: gmcl_odom.cpp:139
_pf_sample_set_t
Definition: pf.h:107
gmcl::GMCLSensor
Definition: gmcl_sensor.h:49
data
data
pf_update_action
void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data)
Definition: pf.c:309
gmcl::GMCLOdom::DiffCorrectedModel
static void DiffCorrectedModel(GMCLOdomData *data, pf_sample_set_t *set)
Definition: gmcl_odom.cpp:376
_pf_t
Definition: pf.h:150
gmcl::GMCLOdom::time
double time
Definition: gmcl_odom.h:95
gmcl::GMCLOdom::SetModelOmni
void SetModelOmni(double alpha1, double alpha2, double alpha3, double alpha4, double alpha5)
Definition: gmcl_odom.cpp:89
gmcl::GMCLOdom::alpha4
double alpha4
Definition: gmcl_odom.h:101
gmcl::GMCLOdom::OmniCorrectedModel
static void OmniCorrectedModel(GMCLOdomData *data, pf_sample_set_t *set)
Definition: gmcl_odom.cpp:307
gmcl::GMCLOdom::DiffModel
static void DiffModel(GMCLOdomData *data, pf_sample_set_t *set)
Definition: gmcl_odom.cpp:216
gmcl::ODOM_MODEL_DIFF
@ ODOM_MODEL_DIFF
Definition: gmcl_odom.h:48
gmcl::GMCLOdom::SetModelDiff
void SetModelDiff(double alpha1, double alpha2, double alpha3, double alpha4)
Definition: gmcl_odom.cpp:76
gmcl::ODOM_MODEL_OMNI_CORRECTED
@ ODOM_MODEL_OMNI_CORRECTED
Definition: gmcl_odom.h:51
pf_sample_t
Definition: pf.h:72
set
ROSCPP_DECL void set(const std::string &key, bool b)
gmcl::GMCLOdom::model_type
odom_model_t model_type
Definition: gmcl_odom.h:98
gmcl::GMCLOdomData
Definition: gmcl_odom.h:57
pf_sample_t::pose
pf_vector_t pose
Definition: pf.h:75
gmcl_odom.h
pf_ran_gaussian
double pf_ran_gaussian(double sigma)
Definition: pf_pdf.c:141
gmcl::GMCLOdom::alpha1
double alpha1
Definition: gmcl_odom.h:101
pf_vector_sub
pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b)
Definition: pf_vector.c:101
angle_diff
static double angle_diff(double a, double b)
Definition: gmcl_odom.cpp:53
gmcl::ODOM_MODEL_DIFF_CORRECTED
@ ODOM_MODEL_DIFF_CORRECTED
Definition: gmcl_odom.h:50
pf_action_model_fn_t
void(* pf_action_model_fn_t)(void *action_data, struct _pf_sample_set_t *set)
Definition: pf.h:59
pf_vector_t::v
double v[3]
Definition: pf_vector.h:53
gmcl::GMCLOdom::alpha3
double alpha3
Definition: gmcl_odom.h:101
gmcl::ODOM_MODEL_OMNI
@ ODOM_MODEL_OMNI
Definition: gmcl_odom.h:49

gmcl
Author(s): Mhd Ali Alshikh Khalil, adler1994@gmail.com
autogenerated on Wed Mar 2 2022 00:20:14