git commit -m "first commit"

navigations/amcl/include/amcl/map/map.h

@@ -0,0 +1,150 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
+ *                      gerkey@usc.edu    kaspers@robotics.usc.edu
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+/**************************************************************************
+ * Desc: Global map (grid-based)
+ * Author: Andrew Howard
+ * Date: 6 Feb 2003
+ * CVS: $Id: map.h 1713 2003-08-23 04:03:43Z inspectorg $
+ **************************************************************************/
+
+#ifndef MAP_H
+#define MAP_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Forward declarations
+struct _rtk_fig_t;
+
+  
+// Limits
+#define MAP_WIFI_MAX_LEVELS 8
+
+  
+// Description for a single map cell.
+typedef struct
+{
+  // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
+  int occ_state;
+
+  // Distance to the nearest occupied cell
+  double occ_dist;
+
+  // Wifi levels
+  //int wifi_levels[MAP_WIFI_MAX_LEVELS];
+
+} map_cell_t;
+
+
+// Description for a map
+typedef struct
+{
+  // Map origin; the map is a viewport onto a conceptual larger map.
+  double origin_x, origin_y;
+  
+  // Map scale (m/cell)
+  double scale;
+
+  // Map dimensions (number of cells)
+  int size_x, size_y;
+  
+  // The map data, stored as a grid
+  map_cell_t *cells;
+
+  // Max distance at which we care about obstacles, for constructing
+  // likelihood field
+  double max_occ_dist;
+  
+} map_t;
+
+
+
+/**************************************************************************
+ * Basic map functions
+ **************************************************************************/
+
+// Create a new (empty) map
+map_t *map_alloc(void);
+
+// Destroy a map
+void map_free(map_t *map);
+
+// Get the cell at the given point
+map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa);
+
+// Load an occupancy map
+int map_load_occ(map_t *map, const char *filename, double scale, int negate);
+
+// Load a wifi signal strength map
+//int map_load_wifi(map_t *map, const char *filename, int index);
+
+// Update the cspace distances
+void map_update_cspace(map_t *map, double max_occ_dist);
+
+
+/**************************************************************************
+ * Range functions
+ **************************************************************************/
+
+// Extract a single range reading from the map
+double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range);
+
+
+/**************************************************************************
+ * GUI/diagnostic functions
+ **************************************************************************/
+
+// Draw the occupancy grid
+void map_draw_occ(map_t *map, struct _rtk_fig_t *fig);
+
+// Draw the cspace map
+void map_draw_cspace(map_t *map, struct _rtk_fig_t *fig);
+
+// Draw a wifi map
+void map_draw_wifi(map_t *map, struct _rtk_fig_t *fig, int index);
+
+
+/**************************************************************************
+ * Map manipulation macros
+ **************************************************************************/
+
+// Convert from map index to world coords
+#define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale)
+#define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale)
+
+// Convert from world coords to map coords
+#define MAP_GXWX(map, x) (floor((x - map->origin_x) / map->scale + 0.5) + map->size_x / 2)
+#define MAP_GYWY(map, y) (floor((y - map->origin_y) / map->scale + 0.5) + map->size_y / 2)
+
+// Test to see if the given map coords lie within the absolute map bounds.
+#define MAP_VALID(map, i, j) ((i >= 0) && (i < map->size_x) && (j >= 0) && (j < map->size_y))
+
+// Compute the cell index for the given map coords.
+#define MAP_INDEX(map, i, j) ((i) + (j) * map->size_x)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

navigations/amcl/include/amcl/pf/eig3.h

@@ -0,0 +1,11 @@
+
+/* Eigen-decomposition for symmetric 3x3 real matrices.
+   Public domain, copied from the public domain Java library JAMA. */
+
+#ifndef _eig_h
+
+/* Symmetric matrix A => eigenvectors in columns of V, corresponding
+   eigenvalues in d. */
+void eigen_decomposition(double A[3][3], double V[3][3], double d[3]);
+
+#endif

navigations/amcl/include/amcl/pf/pf.h

@@ -0,0 +1,210 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
+ *                      gerkey@usc.edu    kaspers@robotics.usc.edu
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+/**************************************************************************
+ * Desc: Simple particle filter for localization.
+ * Author: Andrew Howard
+ * Date: 10 Dec 2002
+ * CVS: $Id: pf.h 3293 2005-11-19 08:37:45Z gerkey $
+ *************************************************************************/
+
+#ifndef PF_H
+#define PF_H
+
+#include "pf_vector.h"
+#include "pf_kdtree.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Forward declarations
+struct _pf_t;
+struct _rtk_fig_t;
+struct _pf_sample_set_t;
+
+// Function prototype for the initialization model; generates a sample pose from
+// an appropriate distribution.
+typedef pf_vector_t (*pf_init_model_fn_t) (void *init_data);
+
+// Function prototype for the action model; generates a sample pose from
+// an appropriate distribution
+typedef void (*pf_action_model_fn_t) (void *action_data, 
+                                      struct _pf_sample_set_t* set);
+
+// Function prototype for the sensor model; determines the probability
+// for the given set of sample poses.
+typedef double (*pf_sensor_model_fn_t) (void *sensor_data, 
+                                        struct _pf_sample_set_t* set);
+
+
+// Information for a single sample
+typedef struct
+{
+  // Pose represented by this sample
+  pf_vector_t pose;
+
+  // Weight for this pose
+  double weight;
+  
+} pf_sample_t;
+
+
+// Information for a cluster of samples
+typedef struct
+{
+  // Number of samples
+  int count;
+
+  // Total weight of samples in this cluster
+  double weight;
+
+  // Cluster statistics
+  pf_vector_t mean;
+  pf_matrix_t cov;
+
+  // Workspace
+  double m[4], c[2][2];
+  
+} pf_cluster_t;
+
+
+// Information for a set of samples
+typedef struct _pf_sample_set_t
+{
+  // The samples
+  int sample_count;
+  pf_sample_t *samples;
+
+  // A kdtree encoding the histogram
+  pf_kdtree_t *kdtree;
+
+  // Clusters
+  int cluster_count, cluster_max_count;
+  pf_cluster_t *clusters;
+
+  // Filter statistics
+  pf_vector_t mean;
+  pf_matrix_t cov;
+  int converged; 
+  double n_effective;
+} pf_sample_set_t;
+
+
+// Information for an entire filter
+typedef struct _pf_t
+{
+  // This min and max number of samples
+  int min_samples, max_samples;
+
+  // Population size parameters
+  double pop_err, pop_z;
+
+  // Resample limit cache
+  int *limit_cache;
+  
+  // The sample sets.  We keep two sets and use [current_set]
+  // to identify the active set.
+  int current_set;
+  pf_sample_set_t sets[2];
+
+  // Running averages, slow and fast, of likelihood
+  double w_slow, w_fast;
+
+  // Decay rates for running averages
+  double alpha_slow, alpha_fast;
+
+  // Function used to draw random pose samples
+  pf_init_model_fn_t random_pose_fn;
+  void *random_pose_data;
+
+  double dist_threshold; //distance threshold in each axis over which the pf is considered to not be converged
+  int converged; 
+
+  // boolean parameter to enamble/diable selective resampling
+  int selective_resampling;
+} pf_t;
+
+
+// Create a new filter
+pf_t *pf_alloc(int min_samples, int max_samples,
+               double alpha_slow, double alpha_fast,
+               pf_init_model_fn_t random_pose_fn, void *random_pose_data);
+
+// Free an existing filter
+void pf_free(pf_t *pf);
+
+// Initialize the filter using a guassian
+void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov);
+
+// Initialize the filter using some model
+void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data);
+
+// Update the filter with some new action
+void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data);
+
+// Update the filter with some new sensor observation
+void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);
+
+// Resample the distribution
+void pf_update_resample(pf_t *pf);
+
+// set selective resampling parameter
+void pf_set_selective_resampling(pf_t *pf, int selective_resampling);
+
+// Compute the CEP statistics (mean and variance).
+void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var);
+
+// Compute the statistics for a particular cluster.  Returns 0 if
+// there is no such cluster.
+int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight,
+                         pf_vector_t *mean, pf_matrix_t *cov);
+
+// Re-compute the cluster statistics for a sample set
+void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set);
+
+
+// Display the sample set
+void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples);
+
+// Draw the histogram (kdtree)
+void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig);
+
+// Draw the CEP statistics
+void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig);
+
+// Draw the cluster statistics
+void pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig);
+
+//calculate if the particle filter has converged - 
+//and sets the converged flag in the current set and the pf 
+int pf_update_converged(pf_t *pf);
+
+//sets the current set and pf converged values to zero
+void pf_init_converged(pf_t *pf);
+
+void pf_copy_set(pf_sample_set_t* set_a, pf_sample_set_t* set_b);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif

navigations/amcl/include/amcl/pf/pf_kdtree.h

@@ -0,0 +1,109 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
+ *                      gerkey@usc.edu    kaspers@robotics.usc.edu
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+/**************************************************************************
+ * Desc: KD tree functions
+ * Author: Andrew Howard
+ * Date: 18 Dec 2002
+ * CVS: $Id: pf_kdtree.h 6532 2008-06-11 02:45:56Z gbiggs $
+ *************************************************************************/
+
+#ifndef PF_KDTREE_H
+#define PF_KDTREE_H
+
+#ifdef INCLUDE_RTKGUI
+#include "rtk.h"
+#endif
+
+
+// Info for a node in the tree
+typedef struct pf_kdtree_node
+{
+  // Depth in the tree
+  int leaf, depth;
+
+  // Pivot dimension and value
+  int pivot_dim;
+  double pivot_value;
+
+  // The key for this node
+  int key[3];
+
+  // The value for this node
+  double value;
+
+  // The cluster label (leaf nodes)
+  int cluster;
+
+  // Child nodes
+  struct pf_kdtree_node *children[2];
+
+} pf_kdtree_node_t;
+
+
+// A kd tree
+typedef struct
+{
+  // Cell size
+  double size[3];
+
+  // The root node of the tree
+  pf_kdtree_node_t *root;
+
+  // The number of nodes in the tree
+  int node_count, node_max_count;
+  pf_kdtree_node_t *nodes;
+
+  // The number of leaf nodes in the tree
+  int leaf_count;
+
+} pf_kdtree_t;
+
+
+// Create a tree
+extern pf_kdtree_t *pf_kdtree_alloc(int max_size);
+
+// Destroy a tree
+extern void pf_kdtree_free(pf_kdtree_t *self);
+
+// Clear all entries from the tree
+extern void pf_kdtree_clear(pf_kdtree_t *self);
+
+// Insert a pose into the tree
+extern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value);
+
+// Cluster the leaves in the tree
+extern void pf_kdtree_cluster(pf_kdtree_t *self);
+
+// Determine the probability estimate for the given pose
+extern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose);
+
+// Determine the cluster label for the given pose
+extern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose);
+
+
+#ifdef INCLUDE_RTKGUI
+
+// Draw the tree
+extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig);
+
+#endif
+
+#endif

navigations/amcl/include/amcl/pf/pf_pdf.h

@@ -0,0 +1,85 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
+ *                      gerkey@usc.edu    kaspers@robotics.usc.edu
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+/**************************************************************************
+ * Desc: Useful pdf functions
+ * Author: Andrew Howard
+ * Date: 10 Dec 2002
+ * CVS: $Id: pf_pdf.h 6345 2008-04-17 01:36:39Z gerkey $
+ *************************************************************************/
+
+#ifndef PF_PDF_H
+#define PF_PDF_H
+
+#include "pf_vector.h"
+
+//#include <gsl/gsl_rng.h>
+//#include <gsl/gsl_randist.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**************************************************************************
+ * Gaussian
+ *************************************************************************/
+
+// Gaussian PDF info
+typedef struct
+{
+  // Mean, covariance and inverse covariance
+  pf_vector_t x;
+  pf_matrix_t cx;
+  //pf_matrix_t cxi;
+  double cxdet;
+
+  // Decomposed covariance matrix (rotation * diagonal)
+  pf_matrix_t cr;
+  pf_vector_t cd;
+
+  // A random number generator
+  //gsl_rng *rng;
+
+} pf_pdf_gaussian_t;
+
+
+// Create a gaussian pdf
+pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx);
+
+// Destroy the pdf
+void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf);
+
+// Compute the value of the pdf at some point [z].
+//double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z);
+
+// Draw randomly from a zero-mean Gaussian distribution, with standard
+// deviation sigma.
+// We use the polar form of the Box-Muller transformation, explained here:
+//   http://www.taygeta.com/random/gaussian.html
+double pf_ran_gaussian(double sigma);
+
+// Generate a sample from the pdf.
+pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

navigations/amcl/include/amcl/pf/pf_vector.h

@@ -0,0 +1,94 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
+ *                      gerkey@usc.edu    kaspers@robotics.usc.edu
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+/**************************************************************************
+ * Desc: Vector functions
+ * Author: Andrew Howard
+ * Date: 10 Dec 2002
+ * CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $
+ *************************************************************************/
+
+#ifndef PF_VECTOR_H
+#define PF_VECTOR_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdio.h>
+  
+// The basic vector
+typedef struct
+{
+  double v[3];
+} pf_vector_t;
+
+
+// The basic matrix
+typedef struct
+{
+  double m[3][3];
+} pf_matrix_t;
+
+
+// Return a zero vector
+pf_vector_t pf_vector_zero();
+
+// Check for NAN or INF in any component
+int pf_vector_finite(pf_vector_t a);
+
+// Print a vector
+void pf_vector_fprintf(pf_vector_t s, FILE *file, const char *fmt);
+
+// Simple vector addition
+pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b);
+
+// Simple vector subtraction
+pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b);
+
+// Transform from local to global coords (a + b)
+pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);
+
+// Transform from global to local coords (a - b)
+pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b);
+
+
+// Return a zero matrix
+pf_matrix_t pf_matrix_zero();
+
+// Check for NAN or INF in any component
+int pf_matrix_finite(pf_matrix_t a);
+
+// Print a matrix
+void pf_matrix_fprintf(pf_matrix_t s, FILE *file, const char *fmt);
+
+// Compute the matrix inverse.  Will also return the determinant,
+// which should be checked for underflow (indicated singular matrix).
+//pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det);
+
+// Decompose a covariance matrix [a] into a rotation matrix [r] and a
+// diagonal matrix [d] such that a = r * d * r^T.
+void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

navigations/amcl/include/amcl/sensors/amcl_laser.h

@@ -0,0 +1,156 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey et al.
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+///////////////////////////////////////////////////////////////////////////
+//
+// Desc: LASER sensor model for AMCL
+// Author: Andrew Howard
+// Date: 17 Aug 2003
+// CVS: $Id: amcl_laser.h 6443 2008-05-15 19:46:11Z gerkey $
+//
+///////////////////////////////////////////////////////////////////////////
+
+#ifndef AMCL_LASER_H
+#define AMCL_LASER_H
+
+#include "amcl_sensor.h"
+#include "../map/map.h"
+
+namespace amcl
+{
+
+typedef enum
+{
+  LASER_MODEL_BEAM,
+  LASER_MODEL_LIKELIHOOD_FIELD,
+  LASER_MODEL_LIKELIHOOD_FIELD_PROB
+} laser_model_t;
+
+// Laser sensor data
+class AMCLLaserData : public AMCLSensorData
+{
+  public:
+    AMCLLaserData () {ranges=NULL;};
+    virtual ~AMCLLaserData() {delete [] ranges;};
+  // Laser range data (range, bearing tuples)
+  public: int range_count;
+  public: double range_max;
+  public: double (*ranges)[2];
+};
+
+
+// Laseretric sensor model
+class AMCLLaser : public AMCLSensor
+{
+  // Default constructor
+  public: AMCLLaser(size_t max_beams, map_t* map);
+
+  public: virtual ~AMCLLaser(); 
+
+  public: void SetModelBeam(double z_hit,
+                            double z_short,
+                            double z_max,
+                            double z_rand,
+                            double sigma_hit,
+                            double lambda_short,
+                            double chi_outlier);
+
+  public: void SetModelLikelihoodField(double z_hit,
+                                       double z_rand,
+                                       double sigma_hit,
+                                       double max_occ_dist);
+
+  //a more probabilistically correct model - also with the option to do beam skipping
+  public: void SetModelLikelihoodFieldProb(double z_hit,
+					   double z_rand,
+					   double sigma_hit,
+					   double max_occ_dist, 
+					   bool do_beamskip, 
+					   double beam_skip_distance, 
+					   double beam_skip_threshold, 
+					   double beam_skip_error_threshold);
+
+  // Update the filter based on the sensor model.  Returns true if the
+  // filter has been updated.
+  public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
+
+  // Set the laser's pose after construction
+  public: void SetLaserPose(pf_vector_t& laser_pose) 
+          {this->laser_pose = laser_pose;}
+
+  // Determine the probability for the given pose
+  private: static double BeamModel(AMCLLaserData *data, 
+                                   pf_sample_set_t* set);
+  // Determine the probability for the given pose
+  private: static double LikelihoodFieldModel(AMCLLaserData *data, 
+                                              pf_sample_set_t* set);
+
+  // Determine the probability for the given pose - more probablistic model 
+  private: static double LikelihoodFieldModelProb(AMCLLaserData *data, 
+					     pf_sample_set_t* set);
+
+  private: void reallocTempData(int max_samples, int max_obs);
+
+  private: laser_model_t model_type;
+
+  // Current data timestamp
+  private: double time;
+
+  // The laser map
+  private: map_t *map;
+
+  // Laser offset relative to robot
+  private: pf_vector_t laser_pose;
+  
+  // Max beams to consider
+  private: int max_beams;
+
+  // Beam skipping parameters (used by LikelihoodFieldModelProb model)
+  private: bool do_beamskip; 
+  private: double beam_skip_distance; 
+  private: double beam_skip_threshold; 
+  //threshold for the ratio of invalid beams - at which all beams are integrated to the likelihoods 
+  //this would be an error condition 
+  private: double beam_skip_error_threshold;
+
+  //temp data that is kept before observations are integrated to each particle (requried for beam skipping)
+  private: int max_samples;
+  private: int max_obs;
+  private: double **temp_obs;
+
+  // Laser model params
+  //
+  // Mixture params for the components of the model; must sum to 1
+  private: double z_hit;
+  private: double z_short;
+  private: double z_max;
+  private: double z_rand;
+  //
+  // Stddev of Gaussian model for laser hits.
+  private: double sigma_hit;
+  // Decay rate of exponential model for short readings.
+  private: double lambda_short;
+  // Threshold for outlier rejection (unused)
+  private: double chi_outlier;
+};
+
+
+}
+
+#endif

navigations/amcl/include/amcl/sensors/amcl_laser.puml

@@ -0,0 +1,54 @@
+@startuml
+
+class Player {
+  + Player()
+}
+
+class AMCLLaserData {
+  + AMCLLaserData()
+  + ~AMCLLaserData()
+  # range_count: int
+  # range_max: double
+  # ranges: double[][]
+
+}
+
+class AMCLLaser {
+  - model_type: laser_model_t
+  - time: double
+  - map: map_t
+  - laser_pose: pf_vector_t
+  - max_beams: int
+  - do_beamskip: bool
+  - beam_skip_distance: double
+  - beam_skip_threshold: double
+  - beam_skip_error_threshold: double
+  - max_samples: int
+  - max_obs: int
+  - temp_obs: double[][]
+  - z_hit: double
+  - z_short: double
+  - z_max: double
+  - z_rand: double
+  - sigma_hit: double
+  - lambda_short: double
+  - chi_outlier: double
+
+  + AMCLLaser(max_beams: size_t, map: map_t)
+  + ~AMCLLaser()
+  + SetModelBeam(z_hit: double, z_short: double, z_max: double, z_rand: double, sigma_hit: double, lambda_short: double, chi_outlier: double)
+  + SetModelLikelihoodField(z_hit: double, z_rand: double, sigma_hit: double, max_occ_dist: double)
+  + SetModelLikelihoodFieldProb(z_hit: double, z_rand: double, sigma_hit: double, max_occ_dist: double, do_beamskip: bool, beam_skip_distance: double, beam_skip_threshold: double, beam_skip_error_threshold: double)
+  + UpdateSensor(pf: pf_t, data: AMCLSensorData): bool
+  + SetLaserPose(laser_pose: pf_vector_t)
+  + BeamModel(data: AMCLLaserData, set: pf_sample_set_t): double
+  + LikelihoodFieldModel(data: AMCLLaserData, set: pf_sample_set_t): double
+  + LikelihoodFieldModelProb(data: AMCLLaserData, set: pf_sample_set_t): double
+  + reallocTempData(max_samples: int, max_obs: int)
+}
+
+Player --|> AMCLSensor
+
+AMCLLaserData --|> AMCLSensorData
+
+@enduml

navigations/amcl/include/amcl/sensors/amcl_odom.h

@@ -0,0 +1,98 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey et al.
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+///////////////////////////////////////////////////////////////////////////
+//
+// Desc: Odometry sensor model for AMCL
+// Author: Andrew Howard
+// Date: 17 Aug 2003
+// CVS: $Id: amcl_odom.h 4135 2007-08-23 19:58:48Z gerkey $
+//
+///////////////////////////////////////////////////////////////////////////
+
+#ifndef AMCL_ODOM_H
+#define AMCL_ODOM_H
+
+#include "amcl_sensor.h"
+#include "../pf/pf_pdf.h"
+
+namespace amcl
+{
+
+typedef enum
+{
+  ODOM_MODEL_DIFF,
+  ODOM_MODEL_OMNI,
+  ODOM_MODEL_DIFF_CORRECTED,
+  ODOM_MODEL_OMNI_CORRECTED
+} odom_model_t;
+
+// Odometric sensor data
+class AMCLOdomData : public AMCLSensorData
+{
+  // Odometric pose
+  public: pf_vector_t pose;
+
+  // Change in odometric pose
+  public: pf_vector_t delta;
+};
+
+
+// Odometric sensor model
+class AMCLOdom : public AMCLSensor
+{
+  // Default constructor
+  public: AMCLOdom();
+
+  public: void SetModelDiff(double alpha1, 
+                            double alpha2, 
+                            double alpha3, 
+                            double alpha4);
+
+  public: void SetModelOmni(double alpha1, 
+                            double alpha2, 
+                            double alpha3, 
+                            double alpha4,
+                            double alpha5);
+
+  public: void SetModel( odom_model_t type,
+                         double alpha1,
+                         double alpha2,
+                         double alpha3,
+                         double alpha4,
+                         double alpha5 = 0 );
+
+  // Update the filter based on the action model.  Returns true if the filter
+  // has been updated.
+  public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
+
+  // Current data timestamp
+  private: double time;
+  
+  // Model type
+  private: odom_model_t model_type;
+
+  // Drift parameters
+  private: double alpha1, alpha2, alpha3, alpha4, alpha5;
+};
+
+
+}
+
+#endif

navigations/amcl/include/amcl/sensors/amcl_odom.puml

@@ -0,0 +1,54 @@
+@startuml
+
+class Player {
+  + Player()
+}
+
+class AMCLSensor {
+  - is_action: bool
+  - pose: pf_vector_t
+
+  + AMCLSensor()
+  + ~AMCLSensor()
+  + UpdateAction(pf: pf_t, data: AMCLSensorData): bool
+  + InitSensor(pf: pf_t, data: AMCLSensorData): bool
+  + UpdateSensor(pf: pf_t, data: AMCLSensorData): bool
+}
+
+class AMCLSensorData {
+  - sensor: AMCLSensor
+
+  + AMCLSensorData()
+  + ~AMCLSensorData()
+}
+
+class AMCLOdomData {
+  - pose: pf_vector_t
+  - delta: pf_vector_t
+
+  + AMCLOdomData()
+}
+
+class AMCLOdom {
+  - time: double
+  - model_type: odom_model_t
+  - alpha1: double
+  - alpha2: double
+  - alpha3: double
+  - alpha4: double
+  - alpha5: double
+
+  + AMCLOdom()
+  + SetModelDiff(alpha1: double, alpha2: double, alpha3: double, alpha4: double)
+  + SetModelOmni(alpha1: double, alpha2: double, alpha3: double, alpha4: double, alpha5: double)
+  + SetModel(type: odom_model_t, alpha1: double, alpha2: double, alpha3: double, alpha4: double, alpha5: double = 0)
+  + UpdateAction(pf: pf_t, data: AMCLSensorData): bool
+}
+
+Player --|> AMCLSensor
+
+AMCLSensorData --|> AMCLSensor
+AMCLOdomData --|> AMCLSensorData
+AMCLOdom --|> AMCLSensor
+
+@enduml

navigations/amcl/include/amcl/sensors/amcl_sensor.h

@@ -0,0 +1,97 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey et al.
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+///////////////////////////////////////////////////////////////////////////
+//
+// Desc: Adaptive Monte-Carlo localization
+// Author: Andrew Howard
+// Date: 6 Feb 2003
+// CVS: $Id: amcl_sensor.h 6443 2008-05-15 19:46:11Z gerkey $
+//
+///////////////////////////////////////////////////////////////////////////
+
+#ifndef AMCL_SENSOR_H
+#define AMCL_SENSOR_H
+
+#include "../pf/pf.h"
+
+namespace amcl
+{
+
+// Forward declarations
+class AMCLSensorData;
+
+
+// Base class for all AMCL sensors
+class AMCLSensor
+{
+  // Default constructor
+  public: AMCLSensor();
+         
+  // Default destructor
+  public: virtual ~AMCLSensor();
+
+  // Update the filter based on the action model.  Returns true if the filter
+  // has been updated.
+  public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
+
+  // Initialize the filter based on the sensor model.  Returns true if the
+  // filter has been initialized.
+  public: virtual bool InitSensor(pf_t *pf, AMCLSensorData *data);
+
+  // Update the filter based on the sensor model.  Returns true if the
+  // filter has been updated.
+  public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
+
+  // Flag is true if this is the action sensor
+  public: bool is_action;
+
+  // Action pose (action sensors only)
+  public: pf_vector_t pose;
+
+  // AMCL Base
+  //protected: AdaptiveMCL & AMCL;
+
+#ifdef INCLUDE_RTKGUI
+  // Setup the GUI
+  public: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
+
+  // Finalize the GUI
+  public: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
+
+  // Draw sensor data
+  public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data);
+#endif
+};
+
+
+
+// Base class for all AMCL sensor measurements
+class AMCLSensorData
+{
+  // Pointer to sensor that generated the data
+  public: AMCLSensor *sensor;
+          virtual ~AMCLSensorData() {}
+
+  // Data timestamp
+  public: double time;
+};
+
+}
+
+#endif

navigations/amcl/include/amcl/sensors/amcl_sensor.puml

@@ -0,0 +1,29 @@
+@startuml
+
+class Player {
+  + Player()
+}
+
+class AMCLSensor {
+  - is_action: bool
+  - pose: pf_vector_t
+
+  + AMCLSensor()
+  + ~AMCLSensor()
+  + UpdateAction(pf: pf_t, data: AMCLSensorData): bool
+  + InitSensor(pf: pf_t, data: AMCLSensorData): bool
+  + UpdateSensor(pf: pf_t, data: AMCLSensorData): bool
+}
+
+class AMCLSensorData {
+  - sensor: AMCLSensor
+
+  + AMCLSensorData()
+  + ~AMCLSensorData()
+}
+
+Player --|> AMCLSensor
+
+AMCLSensorData --|> AMCLSensor
+
+@enduml