rbpf_1.c

一个外国人网站上的资料

/*
  ratbot-slam (c) 2006 Kris Beevers

  This file is part of ratbot-slam.

  ratbot-slam is free software; you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.

  ratbot-slam 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
  General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with ratbot-slam; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
  USA
*/

// your basic particle filter (except, with multiscans).  no improved
// proposal.

#include "ratbot-slam.h"
#include <string.h>
#include <stdlib.h>
#ifdef _TESTING
#include <stdio.h>
#else
#include "rprintf.h"
#endif

#ifndef _TESTING
void printfp(char *pre, fixed_t n)
{
  rprintfStr(pre);
  rprintf(" ");
  rprintfNum(10, 12, 1, ' ', n);
  rprintf("\r\n");
}
#endif

typedef struct
{
  // trajectory
  pose_t trajectory[TRAJ_STORAGE_SIZE];
  int traj_begin, traj_end; // end is 1+index of last valid pose

  // map
  extent_feature map[MAX_LANDMARKS];
  int n; // num landmarks

  fixed_t w; // particle weight
} particle_t;

#ifdef _TESTING
particle_t *particles = 0;
static pose_t multiscan_mu[M];
static cov3_t multiscan_cov[M];
static int16_t multiscan_ranges[5*M];
static int multiscan_count;

static pose_t trajectory[N][MAX_TRAJECTORY_LENGTH];
static uint32_t traj_count = 0;
#else
//particle_t *particles = (particle_t *)0x1100; // start of extmem (4352)
particle_t *particles = (particle_t *)0x4400; // after all the other fixed-loc storage

pose_t *multiscan_mu = (pose_t *)0x1100; // start of extmem
cov3_t *multiscan_cov = (cov3_t *)4517;
int16_t *multiscan_ranges = (int16_t *)4847;
int multiscan_count;
#endif

//
// trajectory and map management
//

int inc_traj_counter(int count)
{
  ++count;
  if(count == TRAJ_STORAGE_SIZE)
    count = 0;
  return count;
}

pose_t * traj_current_pose(int particle)
{
  particle_t *p = &particles[particle];
  return &(p->trajectory[p->traj_begin]);
}

// implements a circular buffer
pose_t * traj_add_new_pose(int particle)
{
  particle_t *p = &particles[particle];
  p->traj_begin = inc_traj_counter(p->traj_begin);
  if(p->traj_begin == p->traj_end)
    p->traj_end = inc_traj_counter(p->traj_end);
  return traj_current_pose(particle);
}

//
// particle filter
//

void filter_init(const pose_t *start)
{
  int i = 0;
  pose_t *p;
#ifdef _TESTING
  particles = (particle_t *)malloc(N*sizeof(particle_t));
#endif
  for(; i < N; ++i) {
    particles[i].traj_begin = -1;
    particles[i].traj_end = 0;
    particles[i].n = 0;
    particles[i].w = one_over_N;
    p = traj_add_new_pose(i);
    p->x = start->x;
    p->y = start->y;
    p->t = start->t;
  }
  multiscan_count = 0;
  multiscan_mu[0].x = start->x;
  multiscan_mu[0].y = start->y;
  multiscan_mu[0].t = start->t;
  multiscan_cov[0].xx = multiscan_cov[0].xy = multiscan_cov[0].xt =
    multiscan_cov[0].yy = multiscan_cov[0].yt = multiscan_cov[0].tt = 0;
}


// gets points corresponding to the range readings from each of the
// infrared sensors over the course of the multiscan.  the points are
// in the frame of the last multiscan pose.  readings that are outside
// the valid range of the sensor are discarded.

// it is expensive to compute the real point covariances wrt to the
// multiscan frame, including pose uncertainty, etc.  instead we will
// compute weights for the points equal to the inverse of the trace of
// the pose uncertainty.  these weights will be used in the feature
// extraction.
void get_multiscan_points
  (fixed_t *x, fixed_t *y, fixed_t *u, int *n)
{
  int16_t *scan = multiscan_ranges;
  pose_t *pose = multiscan_mu;
  cov3_t *cov = multiscan_cov;
  *n = 0;
  int i = 0, s = 0;
  fixed_t range, trace;
  pose_t xform;

  //  static fixed_t lx[5], ly[5];
  fixed_t lx[5], ly[5];

  for(; i < M; ++i, s = 0) {
    scan = &multiscan_ranges[5*i];

    // right back
    range = fp_mul(int2fp(scan[0]), CONVERT_IR_RB);
    if(range >= IR_MIN && range <= IR_MAX) {
      lx[s] = -IR_OFF_X_BACK;
      ly[s] = -IR_OFF_Y - range;
      ++s;
    }

    // right front
    range = fp_mul(int2fp(scan[1]), CONVERT_IR_RF);
    if(range >= IR_MIN && range <= IR_MAX) {
      lx[s] = IR_OFF_X_CORNER;
      ly[s] = -IR_OFF_Y - range;
      ++s;
    }

    // front
    range = fp_mul(int2fp(scan[2]), CONVERT_IR_F);
    if(range >= IR_MIN && range <= IR_MAX) {
      lx[s] = IR_OFF_X_FRONT + range;
      ly[s] = 0;
      ++s;
    }

    // left front
    range = fp_mul(int2fp(scan[3]), CONVERT_IR_LF);
    if(range >= IR_MIN && range <= IR_MAX) {
      lx[s] = IR_OFF_X_CORNER;
      ly[s] = IR_OFF_Y + range;
      ++s;
    }

    // left back
    range = fp_mul(int2fp(scan[4]), CONVERT_IR_LB);
    if(range >= IR_MIN && range <= IR_MAX) {
      lx[s] = -IR_OFF_X_BACK;
      ly[s] = IR_OFF_Y + range;
      ++s;
    }

    if(s > 0) {
      pose = &multiscan_mu[i];
      cov = &multiscan_cov[i];

#if 0
      // FIXME: i really don't want to have to do the work to handle
      // orientation uncertainty properly so i'm just doing this for
      // now, but i think it's not enough and i'll probably have to
      // deal with it, or otherwise inflate the uncertainty
      trace = fp_div(fp_1, cov->xx + cov->yy + cov->tt); // FIXME overflow?
#endif

      // compute the transformation between the current MS pose and
      // the final MS pose, which is the origin for the MS
      //      xform.x = pose->x - multiscan_mu[M-1].x;
      //      xform.y = pose->y - multiscan_mu[M-1].y;
      //      xform.t = angular_distance(pose->t, multiscan_mu[M-1].t);
      /*
      xform.x = multiscan_mu[M-1].x - pose->x;
      xform.y = multiscan_mu[M-1].y - pose->y;
      xform.t = angular_distance(multiscan_mu[M-1].t, pose->t);
      */
      xform = *pose;

      // transform scan points to this frame
      transform_points(lx, ly, s, &xform, x + *n, y + *n);
      *n += s;

#if 0
      // compute "scalar uncertainties" for feature extraction equal
      // to the trace of the current pose uncertainty (and increment
      // *n)
      for(; s > 0; --s, ++(*n))
	u[*n] = trace;
#endif
    }
  }
#if 0
  for(i=0;i<M;++i) {
    printf("%g %g %g\r\n", fp2float(multiscan_mu[i].x), fp2float(multiscan_mu[i].y), fp2float(multiscan_mu[i].t));
  }
  printf("\n\n");
#endif
}

void resample_random(void)
{
  //  printf("resampling\n");
  // first, select particle indices with replacement according to the
  // weights of the corresponding particles
  fixed_t r;
  int i = 0, j;
  //  static int resample_count[N];
  int resample_count[N];
  memset(resample_count, 0, N*sizeof(int));
  for(; i < N; ++i) {
    r = fp_rand_0_1();
    for(j = 0; j < N; ++j) {
      r -= particles[j].w;
      if(r < 0) break;
    }
    ++resample_count[j];
  }

  // now, here's a tricky little method for doing the actual copying
  // of particles: some particles may have been resampled more than
  // once and others not at all.  copy the multiply-resampled ones
  // into the not-resampled particles' locations.  particles resampled
  // exactly once thus require no copying at all.
  int next_zero = 0;
  for(i = 0; i < N; ++i) {
    while(resample_count[i] > 1) {
      while(resample_count[next_zero] != 0)
	++next_zero;
      memcpy(&(particles[next_zero]), &(particles[i]), sizeof(particle_t));
#ifdef _TESTING
      memcpy(trajectory[next_zero], trajectory[i], traj_count*sizeof(pose_t));
#endif
      --resample_count[i];
      ++next_zero;
    }
  }
}

uint32_t start_predict, tot_predict = 0, num_predict = 0,
  start_update, tot_update = 0, num_update = 0;

void filter_update(int16_t left, int16_t right, int16_t ir[5])
{
  //
  // first, project the particles and multiscan mean trajectory
  // forward according to the motion model
  //

  pose_t mu;
  cov3_t cov;

#ifndef _TESTING
  TCNT1 = 0;
  start_predict = TCNT1;
#endif

  //  printf("%d %d\n",left,right);

  // project particles forward