pf.c

Go to the documentation of this file.

//this package is based on amcl and has been modified to fit gmcl 
/* 
 * Author: Mhd Ali Alshikh Khalil
 * Date: 20 June 2021
 * 
*/
 
//amcl author clarification
/*
 *  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.c 6345 2008-04-17 01:36:39Z gerkey $
 *************************************************************************/
 
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
 
#include "gmcl/map/map.h"
#include "gmcl/pf/pf.h"
#include "gmcl/pf/pf_pdf.h"
#include "gmcl/pf/pf_kdtree.h"
#include "portable_utils.hpp"
 
// Compute the required number of samples, given that there are k bins
// with samples in them.
static int pf_resample_limit(pf_t *pf, int k);
 
 
 
// Create a new filter
pf_t *pf_alloc(int min_samples, int max_samples,int N_aux_particles,
               double alpha_slow, double alpha_fast,double crossover_alpha ,double mutation_prob ,
               pf_init_model_fn_t random_pose_fn, void *random_pose_data , void *e_random_pose_data)
{
  int i, j;
  pf_t *pf;
  pf_sample_set_t *set;
  pf_sample_t *sample; 
  
  srand48(time(NULL));
 
  pf = calloc(1, sizeof(pf_t));
 
  pf->random_pose_fn = random_pose_fn;
  pf->random_pose_data = random_pose_data ;
  pf->e_random_pose_data = e_random_pose_data ;
 
  pf->min_samples = min_samples;
  pf->max_samples = max_samples;
  pf->N_aux_particles = N_aux_particles;
  // Control parameters for the population size calculation.  [err] is
  // the max error between the true distribution and the estimated
  // distribution.  [z] is the upper standard normal quantile for (1 -
  // p), where p is the probability that the error on the estimated
  // distrubition will be less than [err].
  pf->pop_err = 0.01;
  pf->pop_z = 3;
  pf->dist_threshold = 0.5; 
 
 
  
  pf->current_set = 0;
  for (j = 0; j < 2; j++)
  {
    set = pf->sets + j;
      
    set->updated_index = NULL;
    set->updated_count = 0;
    set->sample_count = max_samples;
    set->samples = calloc(max_samples, sizeof(pf_sample_t));
 
    for (i = 0; i < set->sample_count; i++)
    {
      sample = set->samples + i;
      sample->pose.v[0] = 0.0;
      sample->pose.v[1] = 0.0;
      sample->pose.v[2] = 0.0;
      sample->weight = 1.0 / max_samples;
    }
 
    
 
    set->cluster_count = 0;
    set->cluster_max_count = max_samples;
    set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));
 
    set->mean = pf_vector_zero();
    set->cov = pf_matrix_zero();
    
    set->pf = pf ;
  }
 
  pf->w_slow = 0.0;
  pf->w_fast = 0.0;
 
  pf->alpha_slow = alpha_slow;
  pf->alpha_fast = alpha_fast;
  pf->crossover_alpha = crossover_alpha;
  pf->mutation_prob = mutation_prob;
 
  //set converged to 0
  pf_init_converged(pf);
 
  return pf;
}
 
// Free an existing filter
void pf_free(pf_t *pf)
{
  int i;
 if(pf->model.use_adaptive_sampling)
   free(pf->limit_cache);
 
  for (i = 0; i < 2; i++)
  {
    free(pf->sets[i].clusters);
    if(pf->model.use_adaptive_sampling)
      pf_kdtree_free(pf->sets[i].kdtree);
    free(pf->sets[i].samples);
    if(pf->model.use_optimal_filter)
     free(pf->sets[i].aux_samples);
  }
  free(pf);
  
  return;
}
 
// Initialize the filter using a guassian
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
{
  int i;
  pf_sample_set_t *set;
  pf_sample_t *sample, *aux_sample;
  pf_pdf_gaussian_t *pdf;
  
  set = pf->sets + pf->current_set;
  
  if(pf->model.use_adaptive_sampling) // Create the kd tree for adaptive sampling
    pf_kdtree_clear(set->kdtree);
 
  set->sample_count = pf->max_samples;
 
  pdf = pf_pdf_gaussian_alloc(mean, cov);
    
  if(pf->model.use_adaptive_sampling) // Compute the new sample poses
   for (i = 0; i < set->sample_count; i++)
   {
    sample = set->samples + i;
    sample->weight = 1.0 / pf->max_samples;
    sample->pose = pf_pdf_gaussian_sample(pdf);
 
    // Add sample to histogram
    pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
  }
 else
    for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;
    sample->weight = 1.0 / pf->max_samples;
    sample->pose = pf_pdf_gaussian_sample(pdf);
 
  }
  
  if(pf->model.use_optimal_filter)
  {  for (int j = 0; j < set->sample_count; j++)
     {  sample = set->samples + j ;
        for ( int loop=0 ; loop < pf->N_aux_particles ; loop++)
        {
           aux_sample = set->aux_samples + (j*pf->N_aux_particles+loop) ;         
           aux_sample->pose = sample->pose; 
           aux_sample->weight = 1.0;
        }
     }  
   }   
   
  pf->w_slow = pf->w_fast = 0.0;
 
  pf_pdf_gaussian_free(pdf);
    
  // Re-compute cluster statistics
  pf_cluster_stats(pf, set); 
 
  //set converged to 0
  pf_init_converged(pf);
 
  return;
}
 
 
// Initialize the filter using some model
void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data , void *e_init_data)
{
  int i;
  pf_sample_set_t *set;
  pf_sample_t *sample, *aux_sample;
 
  set = pf->sets + pf->current_set;
 
  if(pf->model.use_adaptive_sampling) // Create the kd tree for adaptive sampling
    pf_kdtree_clear(set->kdtree);
 
  set->sample_count = pf->max_samples;
 
  if(pf->model.use_adaptive_sampling) // Compute the new sample poses
   for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;
    sample->weight = 1.0 / pf->max_samples;
    sample->pose = (*init_fn) (pf, init_data , e_init_data);
 
    // Add sample to histogram
    pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
  }
 else
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;
    sample->weight = 1.0 / pf->max_samples;
    sample->pose = (*init_fn) (pf, init_data , e_init_data);
 
  }
  if(pf->model.use_optimal_filter)
  {  for (int j = 0; j < set->sample_count; j++)
     {  sample = set->samples + j ;
        for ( int loop=0 ; loop < pf->N_aux_particles ; loop++)
        {
           aux_sample = set->aux_samples + (j*pf->N_aux_particles+loop) ;         
           aux_sample->pose = sample->pose;
           aux_sample->weight = 1.0 ;
        }
     }  
   }    
           
  pf->w_slow = pf->w_fast = 0.0;
 
  // Re-compute cluster statistics
  pf_cluster_stats(pf, set);
  
  //set converged to 0
  pf_init_converged(pf);
 
  return;
}
 
void pf_init_converged(pf_t *pf){
  pf_sample_set_t *set;
  set = pf->sets + pf->current_set;
  set->converged = 0; 
  pf->converged = 0; 
}
 
int pf_update_converged(pf_t *pf)
{
  int i;
  pf_sample_set_t *set;
  pf_sample_t *sample;
  double total;
 
  set = pf->sets + pf->current_set;
  double mean_x = 0, mean_y = 0;
 
  for (i = 0; i < set->sample_count; i++){
    sample = set->samples + i;
 
    mean_x += sample->pose.v[0];
    mean_y += sample->pose.v[1];
  }
  mean_x /= set->sample_count;
  mean_y /= set->sample_count;
  
  for (i = 0; i < set->sample_count; i++){
    sample = set->samples + i;
    if(fabs(sample->pose.v[0] - mean_x) > pf->dist_threshold || 
       fabs(sample->pose.v[1] - mean_y) > pf->dist_threshold){
      set->converged = 0; 
      pf->converged = 0; 
      return 0;
    }
  }
  set->converged = 1; 
  pf->converged = 1; 
  return 1; 
}
 
// Update the filter with some new action
void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data)
{
  pf_sample_set_t *set;
 
  set = pf->sets + pf->current_set;
 
  (*action_fn) (action_data, set);
  
  return;
}
 
#include <float.h>
 
void pf_normlize_weight(pf_t *pf)  
{ 
 
  return;
 
}                                                  
// Update the filter with some new sensor observation
void pf_update_sensor(pf_t *pf, pf_laser_model_fn_t laser_fn, void *laser_data)                                                    
{ 
  int i;
  pf_sample_set_t *set;
  pf_sample_t *sample, *aux_sample;
  double total;
  double best;
  
  set = pf->sets + pf->current_set;
  total = 0.0 ;
  // Compute the sample weights
  
  (*laser_fn) (laser_data, set);
  
   if(pf->model.use_optimal_filter && set->updated_count == 0)
     for (int j = 0; j < set->sample_count; j++)
     {  sample = set->samples + j ;
        total += sample->weight;
        best = 0.0 ;
        for ( int loop=0 ; loop < pf->N_aux_particles ; loop++)
        {
          aux_sample = set->aux_samples + (j*pf->N_aux_particles+loop) ;
          if(aux_sample->weight > best)
          {
            sample->pose = aux_sample->pose;         //cause if intelligent true || resample interval > 1                
            best = aux_sample->weight;
          }          
        }                   
    }      
  else 
  for (i = 0; i < set->sample_count; i++)
  {
      sample = set->samples + i;
      total += sample->weight;    
  }
  
  set->n_effective = 0;
  
  if (total > 0.0)
  {  if(pf->model.use_crossover_mutation && set->updated_count == 0) 
    {  // for (i = 0; i < set->sample_count; i++)
     // {
       set->total_weight = total;
       // sample = set->samples + i;
        //sample->weight /= total;
 
  //     }
         
      return ;
      }
    // Normalize weights
    double w_avg=0.0;
    for (i = 0; i < set->sample_count; i++)
    {
      sample = set->samples + i;      
      w_avg += sample->weight;
      sample->weight /= total;
      set->n_effective += sample->weight*sample->weight;
              
    }
   
    // Update running averages of likelihood of samples (Prob Rob p258)
    w_avg /= set->sample_count;
    if(pf->w_slow == 0.0)
      pf->w_slow = w_avg;
    else
      pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
    if(pf->w_fast == 0.0)
      pf->w_fast = w_avg;
    else
      pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
   // printf("w_avg: %e slow: %e fast: %e\n", 
     //      w_avg, pf->w_slow, pf->w_fast);
  }
  else
  {
    // Handle zero total
    for (i = 0; i < set->sample_count; i++)
    {
      sample = set->samples + i;
      sample->weight = 1.0 / set->sample_count;
    }
  }
 
  set->n_effective = 1.0/set->n_effective; 
 
 
  
  return;   
}
 
// copy set a to set b
void copy_set(pf_sample_set_t* set_a, pf_sample_set_t* set_b)
{
  int i;
  double total;
  pf_sample_t *sample_a, *sample_b;
  pf_t* pf ;
  
  pf = set_a->pf;
  
  if(set_a->pf->model.use_adaptive_sampling)// Clean set b's kdtree
    pf_kdtree_clear(set_b->kdtree);
 
  // Copy samples from set a to create set b
  total = 0;
  set_b->sample_count = 0;
 if(pf->model.use_adaptive_sampling)
  for(i = 0; i < set_a->sample_count; i++)
  {
    sample_b = set_b->samples + set_b->sample_count++;
 
    sample_a = set_a->samples + i;
 
    assert(sample_a->weight > 0);
 
    // Copy sample a to sample b
    sample_b->pose = sample_a->pose;
    sample_b->weight = sample_a->weight;
 
    total += sample_b->weight;
 
    // Add sample to histogram
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
  }
 else 
   for(i = 0; i < set_a->sample_count; i++)
  {
    sample_b = set_b->samples + set_b->sample_count++;
 
    sample_a = set_a->samples + i;
 
    assert(sample_a->weight > 0);
 
    // Copy sample a to sample b
    sample_b->pose = sample_a->pose;
    sample_b->weight = sample_a->weight;
 
    total += sample_b->weight;
 
  }
 
  // Normalize weights
  for (i = 0; i < set_b->sample_count; i++)
  {
    sample_b = set_b->samples + i;
    sample_b->weight /= total;
  }
 
  set_b->converged = set_a->converged;
}
 
// Resample the distribution
void pf_update_resample(pf_t *pf)
{
  int i;
  double total;
  pf_sample_set_t *set_a, *set_b;
  pf_sample_t *sample_a, *sample_b;
 
  //double r,c,U;
  //int m;
  //double count_inv;
  double* c;
 
  double w_diff;
 
  set_a = pf->sets + pf->current_set;
  set_b = pf->sets + (pf->current_set + 1) % 2;
 
  if (pf->selective_resampling != 0)
  {
    if (set_a->n_effective > 0.5*(set_a->sample_count))
    { // printf("N_eff: %9.6f\n", set_a->n_effective );
      // copy set a to b
      copy_set(set_a,set_b);
 
      // Re-compute cluster statistics
      pf_cluster_stats(pf, set_b);
 
      // Use the newly created sample set
      pf->current_set = (pf->current_set + 1) % 2;
      return;
    }
  }
 
  // Build up cumulative probability table for resampling.
  // TODO: Replace this with a more efficient procedure
  // (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html)
  c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));
  c[0] = 0.0;
  for(i=0;i<set_a->sample_count;i++)
    c[i+1] = c[i]+set_a->samples[i].weight;
 
 
 
  
  // Draw samples from set a to create set b.
  total = 0;
  set_b->sample_count = 0;
 
  w_diff = 1.0 - pf->w_fast / pf->w_slow;
  if(w_diff < 0.0)
    w_diff = 0.0;
  //printf("w_diff: %9.6f\n", w_diff);
 
  // Can't (easily) combine low-variance sampler with KLD adaptive
  // sampling, so we'll take the more traditional route.
  /*
  // Low-variance resampler, taken from Probabilistic Robotics, p110
  count_inv = 1.0/set_a->sample_count;
  r = drand48() * count_inv;
  c = set_a->samples[0].weight;
  i = 0;
  m = 0;
  */
 if(pf->model.use_adaptive_sampling)
 { pf_kdtree_clear(set_b->kdtree);  // Create the kd tree for adaptive sampling
   while(set_b->sample_count < pf->max_samples)
   {
    sample_b = set_b->samples + set_b->sample_count++;
 
    if(drand48() < w_diff)
      sample_b->pose = (pf->random_pose_fn)(pf, pf->random_pose_data , pf->e_random_pose_data );
    else
    {
      // Can't (easily) combine low-variance sampler with KLD adaptive
      // sampling, so we'll take the more traditional route.
      /*
      // Low-variance resampler, taken from Probabilistic Robotics, p110
      U = r + m * count_inv;
      while(U>c)
      {
        i++;
        // Handle wrap-around by resetting counters and picking a new random
        // number
        if(i >= set_a->sample_count)
        {
          r = drand48() * count_inv;
          c = set_a->samples[0].weight;
          i = 0;
          m = 0;
          U = r + m * count_inv;
          continue;
        }
        c += set_a->samples[i].weight;
      }
      m++;
      */
 
      // Naive discrete event sampler
      double r;
      r = drand48();
      for(i=0;i<set_a->sample_count;i++)
      {
        if((c[i] <= r) && (r < c[i+1]))
          break;
      }
      assert(i<set_a->sample_count);
 
      sample_a = set_a->samples + i;
 
      assert(sample_a->weight > 0);
 
      // Add sample to list
      sample_b->pose = sample_a->pose;
    }
 
    sample_b->weight = 1.0;
    total += sample_b->weight;
 
    // Add sample to histogram
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
 
    // See if we have enough samples yet
    if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
      break;
     }
   } 
 else 
  for( int loop= 0 ; loop < set_a->sample_count ; loop++)
  {
    sample_b = set_b->samples + set_b->sample_count++;
 
    if(drand48() < w_diff)
      sample_b->pose = (pf->random_pose_fn)(pf, pf->random_pose_data , pf->e_random_pose_data );
    else
    {
      // Can't (easily) combine low-variance sampler with KLD adaptive
      // sampling, so we'll take the more traditional route.
      /*
      // Low-variance resampler, taken from Probabilistic Robotics, p110
      U = r + m * count_inv;
      while(U>c)
      {
        i++;
        // Handle wrap-around by resetting counters and picking a new random
        // number
        if(i >= set_a->sample_count)
        {
          r = drand48() * count_inv;
          c = set_a->samples[0].weight;
          i = 0;
          m = 0;
          U = r + m * count_inv;
          continue;
        }
        c += set_a->samples[i].weight;
      }
      m++;
      */
 
      // Naive discrete event sampler
      double r;
      r = drand48();
      for(i=0;i<set_a->sample_count;i++)
      {
        if((c[i] <= r) && (r < c[i+1]))
          break;
      }
      assert(i<set_a->sample_count);
 
      sample_a = set_a->samples + i;
 
      assert(sample_a->weight > 0);
 
      // Add sample to list
      sample_b->pose = sample_a->pose;
    }
 
    sample_b->weight = 1.0;
    total += sample_b->weight;
 
  }
  
  // Reset averages, to avoid spiraling off into complete randomness.
  if(w_diff > 0.0)
    pf->w_slow = pf->w_fast = 0.0;
 
  //fprintf(stderr, "\n\n");
 
  // Normalize weights
  for (i = 0; i < set_b->sample_count; i++)
  {
    sample_b = set_b->samples + i;
    sample_b->weight /= total;
  }
  
  // Re-compute cluster statistics
  pf_cluster_stats(pf, set_b);
 
  // Use the newly created sample set
  pf->current_set = (pf->current_set + 1) % 2; 
 
  pf_update_converged(pf);
 
  free(c);
  return;
}
 
void pf_update_crossover_mutation(pf_t *pf, pf_reupdate_sensor_fn_t laser_fn, void *laser_data)
{ int i ,j, num_ch , num_cl;
  double r ;
  int* C_h ;
  int* C_l ;
  double total;
  pf_sample_set_t *set ;
  pf_sample_t *sample ,  *sample_l , *sample_h  ;
  
  num_ch = num_cl = 0 ;
  set = pf->sets + pf->current_set;
  
  C_h = (int*)malloc(sizeof(int)*(set->sample_count));
  C_l = (int*)malloc(sizeof(int)*(set->sample_count));  
  
if(set->total_weight ==0)
  return;  
total = set->total_weight ;  
    for(i=0 ; i<set->sample_count ; i++)
    {
     sample = set->samples + i ;
     
     if(sample->weight/total >= 1.0/set->sample_count)
        C_h[num_ch++] = i ;
                   
     else 
        C_l[num_cl++] = i ;
     } 
    
 
  if(num_cl ==0)
  { 
         return;    
  }       
  
  num_cl /=3;        // take only third of particles to make sure of diversity of particles
 //printf("num_cl: %9.6d\n", num_cl);
  for(i=0 ; i<num_cl ; i++)
  { r = drand48()*num_ch;
    //printf("r: %9.6f\n", r);
    sample_l = set->samples +C_l[i] ;
    sample_h = set->samples +C_h[(int)r] ;
    
    sample_l->weight /= sample_l->last_obs ;
    r = drand48() ;
      
    if ( r <= pf->mutation_prob) 
    {
      sample_l->pose.v[0] =  pf->crossover_alpha*(2*sample_h->pose.v[0] -  sample_l->pose.v[0]) 
                                                      + (1 - pf->crossover_alpha)*sample_h->pose.v[0];
      sample_l->pose.v[1] =  pf->crossover_alpha*(2*sample_h->pose.v[1] -  sample_l->pose.v[1]) 
                                                      + (1 - pf->crossover_alpha)*sample_h->pose.v[1];
      sample_l->pose.v[2] =  pf->crossover_alpha*(2*sample_h->pose.v[2] -  sample_l->pose.v[2]) 
                                                      + (1 - pf->crossover_alpha)*sample_h->pose.v[2];
    }
    else
    {
      sample_l->pose.v[0] =  pf->crossover_alpha*sample_l->pose.v[0]
                                                      + (1 - pf->crossover_alpha)*sample_h->pose.v[0];
      sample_l->pose.v[1] =  pf->crossover_alpha*sample_l->pose.v[1] 
                                                      + (1 - pf->crossover_alpha)*sample_h->pose.v[1];
      sample_l->pose.v[2] =  pf->crossover_alpha*sample_l->pose.v[2]
                                                      + (1 - pf->crossover_alpha)*sample_h->pose.v[2];       
    }
                                                            
 
  }  
  set->updated_index = C_l;
  set->updated_count = num_cl;
  
   
  (*laser_fn) (pf, laser_data); //update weights again for changed particles over particels 
   
  set->updated_index = NULL;
  set->updated_count = 0;
  free(C_h) ;free(C_l);  
}
 
 
// Compute the required number of samples, given that there are k bins
// with samples in them.  This is taken directly from Fox et al.
int pf_resample_limit(pf_t *pf, int k)
{
  double a, b, c, x;
  int n;
 
  // Return max_samples in case k is outside expected range, this shouldn't
  // happen, but is added to prevent any runtime errors
  if (k < 1 || k > pf->max_samples)
      return pf->max_samples;
 
  // Return value if cache is valid, which means value is non-zero positive
  if (pf->limit_cache[k-1] > 0)
    return pf->limit_cache[k-1];
 
  if (k == 1)
  {
    pf->limit_cache[k-1] = pf->max_samples;
    return pf->max_samples;
  }
 
  a = 1;
  b = 2 / (9 * ((double) k - 1));
  c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z;
  x = a - b + c;
 
  n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x);
 
  if (n < pf->min_samples)
  {
    pf->limit_cache[k-1] = pf->min_samples;
    return pf->min_samples;
  }
  if (n > pf->max_samples)
  {
    pf->limit_cache[k-1] = pf->max_samples;
    return pf->max_samples;
  }
  
  pf->limit_cache[k-1] = n;
  return n;
}
 
 
// Re-compute the cluster statistics for a sample set
void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)
{
 if(pf->model.use_adaptive_sampling)// Cluster the samples
{  int i, j, k, cidx;
  pf_sample_t *sample;
  pf_cluster_t *cluster;
  
  // Workspace
  double m[4], c[2][2];
  size_t count;
  double weight;
 
 
  pf_kdtree_cluster(set->kdtree);  // Initialize cluster stats
  set->cluster_count = 0;
    
   
 
  for (i = 0; i < set->cluster_max_count; i++)
  {
    cluster = set->clusters + i;
    cluster->count = 0;
    cluster->weight = 0;
    cluster->mean = pf_vector_zero();
    cluster->cov = pf_matrix_zero();
 
    for (j = 0; j < 4; j++)
      cluster->m[j] = 0.0;
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
        cluster->c[j][k] = 0.0;
  }
 
  // Initialize overall filter stats
  count = 0;
  weight = 0.0;
  set->mean = pf_vector_zero();
  set->cov = pf_matrix_zero();
  for (j = 0; j < 4; j++)
    m[j] = 0.0;
  for (j = 0; j < 2; j++)
    for (k = 0; k < 2; k++)
      c[j][k] = 0.0;
  
 
 
  for (i = 0; i < set->sample_count; i++) // Compute cluster stats
  {
    sample = set->samples + i;
 
    //printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]);
 
    // Get the cluster label for this sample
 
    cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose);
    assert(cidx >= 0);
    if (cidx >= set->cluster_max_count)
      continue;
    if (cidx + 1 > set->cluster_count)
      set->cluster_count = cidx + 1;
    
    cluster = set->clusters + cidx;
   
    cluster->count += 1;
    cluster->weight += sample->weight;
 
    count += 1;
    weight += sample->weight;
 
    // Compute mean
    cluster->m[0] += sample->weight * sample->pose.v[0];
    cluster->m[1] += sample->weight * sample->pose.v[1];
    cluster->m[2] += sample->weight * cos(sample->pose.v[2]);
    cluster->m[3] += sample->weight * sin(sample->pose.v[2]);
 
    m[0] += sample->weight * sample->pose.v[0];
    m[1] += sample->weight * sample->pose.v[1];
    m[2] += sample->weight * cos(sample->pose.v[2]);
    m[3] += sample->weight * sin(sample->pose.v[2]);
 
    // Compute covariance in linear components
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
      {
        cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
        c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
      }
  }
       
  // Normalize
  for (i = 0; i < set->cluster_count; i++)
  {
    cluster = set->clusters + i;
        
    cluster->mean.v[0] = cluster->m[0] / cluster->weight;
    cluster->mean.v[1] = cluster->m[1] / cluster->weight;
    cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]);
 
    cluster->cov = pf_matrix_zero();
 
    // Covariance in linear components
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
          cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
          cluster->mean.v[j] * cluster->mean.v[k];
 
    // Covariance in angular components; I think this is the correct
    // formula for circular statistics.
    cluster->cov.m[2][2] = -2 * log(sqrt(cluster->m[2] * cluster->m[2] +
                                         cluster->m[3] * cluster->m[3]));
 
   // printf("cluster %d %d %f (%f %f %f)\n", i, cluster->count, cluster->weight,
      //     cluster->mean.v[0], cluster->mean.v[1], cluster->mean.v[2]);
  }
  // Compute overall filter stats
  set->mean.v[0] = m[0] / weight;
  set->mean.v[1] = m[1] / weight;
  set->mean.v[2] = atan2(m[3], m[2]);
 
  // Covariance in linear components
  for (j = 0; j < 2; j++)
    for (k = 0; k < 2; k++)
      set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];
 
    set->cov.m[2][0] =  set->cov.m[0][0] + set->cov.m[1][1];
  // Covariance in angular components; I think this is the correct
  // formula for circular statistics.
  set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));
 
  return;
 }
   else
 {
  int i, j, k;
  pf_sample_t *sample;
  pf_cluster_t *cluster;
  
  // Workspace
  double m[4], c[2][2];
  size_t count;
  double weight;
  set->cluster_count = 1;
    
  cluster = set->clusters + 0;
  cluster->count = set->sample_count;
  cluster->weight = 1.0;
  cluster->mean = pf_vector_zero();
  cluster->cov = pf_matrix_zero();
 
    for (j = 0; j < 4; j++)
      cluster->m[j] = 0.0;
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
        cluster->c[j][k] = 0.0;
  
 
  // Initialize overall filter stats
 
  set->mean = pf_vector_zero();
  set->cov = pf_matrix_zero();
  for (j = 0; j < 4; j++)
    m[j] = 0.0;
  for (j = 0; j < 2; j++)
    for (k = 0; k < 2; k++)
      c[j][k] = 0.0;
 
 //   c[2][0] = 0.0; 
// Compute cluster stats
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;
 
    //printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]);
 
 
    // Compute mean
 
    m[0] += sample->weight * sample->pose.v[0];
    m[1] += sample->weight * sample->pose.v[1];
    m[2] += sample->weight * cos(sample->pose.v[2]);
    m[3] += sample->weight * sin(sample->pose.v[2]);
 
    // Compute covariance in linear components
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
      {
        c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
         
      }
 //  c[2][0] += sample->weight * sqrt( sample->pose.v[0] * sample->pose.v[0] + sample->pose.v[1] * sample->pose.v[1]) * 
            //                           sqrt( sample->pose.v[0] * sample->pose.v[0] + sample->pose.v[1] * sample->pose.v[1]) ;
  } 
 
  // Compute overall filter stats
 
  cluster->mean.v[0] = m[0];
  cluster->mean.v[1] = m[1];
  cluster->mean.v[2] = atan2(m[3], m[2]);
  
 
  set->mean.v[0] = cluster->mean.v[0];
  set->mean.v[1] = cluster->mean.v[1];
  set->mean.v[2] = cluster->mean.v[2];
 
  
  // Covariance in linear components
  for (j = 0; j < 2; j++)
    for (k = 0; k < 2; k++)
      {  
       cluster->cov.m[j][k] = c[j][k]  - set->mean.v[j] * set->mean.v[k];
       set->cov.m[j][k] = cluster->cov.m[j][k];
      }
     cluster->cov.m[2][0] =  cluster->cov.m[0][0] +cluster->cov.m[1][1];
     
     //  cluster->cov.m[2][0] = c[2][0]  - sqrt( set->mean.v[0] * set->mean.v[0] +set->mean.v[1] * set->mean.v[1]) *sqrt( set->mean.v[0] * set->mean.v[0] +set->mean.v[1] * set->mean.v[1]) ;
       set->cov.m[2][0] = cluster->cov.m[2][0];
 
    //  printf("cluster  %f \n",set->cov.m[2][0] );
  // Covariance in angular components; I think this is the correct
  // formula for circular statistics.
  cluster->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));
  set->cov.m[2][2] = cluster->cov.m[2][2];
  
  return;
   }
   
   
}
 
void pf_set_model_type(pf_t *pf, pf_model_type *pf_model)
{
  pf->model.use_optimal_filter = pf_model->use_optimal_filter;
  pf->model.use_crossover_mutation = pf_model->use_crossover_mutation;
  pf->model.use_self_adaptive = pf_model->use_self_adaptive;
  pf->model.use_adaptive_sampling = pf_model->use_adaptive_sampling;
  
 
  for (int j = 0; j < 2; j++)
  {   pf_sample_set_t *set = pf->sets + j;    
      if(pf->model.use_adaptive_sampling)  
         set->kdtree = pf_kdtree_alloc(3 * pf->max_samples);
      if(pf->model.use_optimal_filter)
      {  int aux_count = pf->max_samples*pf->N_aux_particles;
         set->aux_samples = calloc(pf->max_samples*pf->N_aux_particles, sizeof(pf_sample_t));
         for (int i = 0; i < pf->max_samples*pf->N_aux_particles ; i++)
        {
           pf_sample_t* aux_sample = set->aux_samples + i;
           aux_sample->pose.v[0] = 0.0;
           aux_sample->pose.v[1] = 0.0;
           aux_sample->pose.v[2] = 0.0;
           aux_sample->weight = 1.0 ;
        }
     }
  }  
   if(pf->model.use_adaptive_sampling)  // Number of leaf nodes is never higher than the max number of samples
       pf->limit_cache = calloc(pf->max_samples, sizeof(int));    
}
 
void pf_set_selective_resampling(pf_t *pf, int selective_resampling)
{
  pf->selective_resampling = selective_resampling;
}
 
// Compute the CEP statistics (mean and variance).
void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var)
{
  int i;
  double mn, mx, my, mrr;
  pf_sample_set_t *set;
  pf_sample_t *sample;
  
  set = pf->sets + pf->current_set;
 
  mn = 0.0;
  mx = 0.0;
  my = 0.0;
  mrr = 0.0;
  
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;
 
    mn += sample->weight;
    mx += sample->weight * sample->pose.v[0];
    my += sample->weight * sample->pose.v[1];
    mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0];
    mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1];
  }
 
  mean->v[0] = mx / mn;
  mean->v[1] = my / mn;
  mean->v[2] = 0.0;
 
  *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn));
 
  return;
}
 
 
// Get the statistics for a particular cluster.
int pf_get_cluster_stats(pf_t *pf, int clabel, double *weight,
                         pf_vector_t *mean, pf_matrix_t *cov)
{
  pf_sample_set_t *set;
  pf_cluster_t *cluster;
 
  set = pf->sets + pf->current_set;
 
  if (clabel >= set->cluster_count)
    return 0;
  cluster = set->clusters + clabel;
 
  *weight = cluster->weight;
  *mean = cluster->mean;
  *cov = cluster->cov;
 
  return 1;
}