[Nav2] AMCL(Adaptive Monte Carlo Localization) package 분석

이 글은 Particle Filter에 대한 기본 이해가 필요합니다.

 

Nav2에서 기본적으로 로봇의 localization 문제를 AMCL(Adaptive Monte Carlo Localization)을 통해 해결한다. AMCL package가 어떻게 동작되는지 분석해 봤다.

AMCL(Adaptive Monte Carlo Localization) 이란?

AMCL은 Particle Filter를 이용하여 로봇의 위치를 추정한다. Particle Filter에 대해 간단하게 소개하자면 특정 분포를 가지고 있지 않는 nonparametric 한 모델을 위한 베이즈 필터를 구현한 것이다. 이 포스트는 주요한 아래 4가지를 어떻게 구현하였는지에 초점을 둔다.

1. 로봇의 위치 sample(particle)을 어떻게 생성하고 업데이트하는가?
2. 각각의 sample의 importance factor(weight) 값을 어떻게 계산하는가?
3. Resampling을 어떻게 구현했는가?
4. 최적의 로봇의 위치를 어떻게 추정하는가?

1. Particle 생성 및 Pose 업데이트

1-1. Particle 생성

앞에서 언급했듯이 AMCL의 Particle Filter 기반의 로봇 위치 추정 알고리즘이다. non-paramatic에서 확률적으로 직접 sampling이  안되므로 Gaussian 분포를 따르는 함수에서 sampling을 진행한다. 이를 어떻게 구현했는지 확인해 보자. Particle은 AMCL node를 실행한 후 initial_pose topic 값을 subscribe 하거나 AMCL Parameter를 통하여 초기 위치가 정해졌을 때 pf_init 함수를 통해 생성한다.

void
AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg)
{
...
// Re-initialize the filter
  pf_vector_t pf_init_pose_mean = pf_vector_zero();
  pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
  pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
  pf_init_pose_mean.v[2] = tf2::getYaw(pose_new.getRotation());

  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
  // Copy in the covariance, converting from 6-D to 3-D
  for (int i = 0; i < 2; i++) {
    for (int j = 0; j < 2; j++) {
      pf_init_pose_cov.m[i][j] = msg.pose.covariance[6 * i + j];
    }
  }

  pf_init_pose_cov.m[2][2] = msg.pose.covariance[6 * 5 + 5];

  pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
...
}

위 AmclNode::handleInitialPose 함수는 초기 위치가 지정 됐을 때 호출되는 함수다. pf_init_pose_mean에 $x, y, yaw$를 저장하고 pf_init_pose_cov에는 각 변수에 대한 covariance 값을 저장한다. 그 후 pf_init를 호출하여 Particle Filter를 initialize 한다. 

void pf_init(pf_t * pf, pf_vector_t mean, pf_matrix_t cov)
{
...
  pf_pdf_gaussian_t * pdf;
  pdf = pf_pdf_gaussian_alloc(mean, cov);

  // 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);
  }
...
}

mean과 covariance 값으로 pf_pdf_gaussian_t를 만들고 pf_pdf_gaussian_sample 함수를 통해 pose를 생성하게 된다. 이때 sample들의 weight는 총합이 1이 되도록 normalize 하는 것을 볼 수 있다. 이 코드에서 우리가 주로 봐야 할 점은 pf_pdf_gaussian_t 구조체와 pf_pdf_gaussian_sample 함수이다.  먼저 pf_pdf_gaussian_t 구조체를 확인해 보자.

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;

pf_pdf_gaussian_t 구조체는 위처럼 구성되어 있다. x와 cx에는 각각 mean값과 covariance 값이며, eigen decomposition을 통하여 covariance의 rotation과 diagonal을 분리한다. 좀 더 직관적이게 말하자면 어떤 방향으로 분산이 되어 있는지가 cr이 되고 그 방향으로 얼마큼 분산되어 있는지 $ \sigma^2 $ 값이 cd가 된다. 그다음 pf_pdf_gaussian_sample 함수를 확인해 보자.

pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t * pdf)
{
  int i, j;
  pf_vector_t r;
  pf_vector_t x;

  // Generate a random vector
  for (i = 0; i < 3; i++) {
    // r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]);
    r.v[i] = pf_ran_gaussian(pdf->cd.v[i]);
  }

  for (i = 0; i < 3; i++) {
    x.v[i] = pdf->x.v[i];
    for (j = 0; j < 3; j++) {
      x.v[i] += pdf->cr.m[i][j] * r.v[j];
    }
  }

  return x;
}

먼저 pf_ran_gaussian 함수를 통해 $ \sigma^2 $ 에서의 랜덤값을 구한다. $\mathbf {s}$ 그다음에 새로운 pose 변수 x에 동일한 mean 값을 저장하고 covariance의 방향성분과 랜덤값을 곱한 값을 더해 임의의 pose을 생성한다.

\begin{equation} \mathbf{x}_n = \mathbf {x}_0 + \mathbf{R} \mathbf{s} \end{equation}

1-2. Particle pose 업데이트

로봇의 움직임에 따라 각각의 particle들도 pose update 해야 된다. 먼저 AMCL에서는 LiDAR 데이터가 들어올 때 TF를 통하여 Odometry pose를 구한 뒤 일정 임계값 이상 움직일 경우 업데이트를 한다. (이렇게 구현한 이유는 resampling 주기와 관련이 있다.) 각각 AmclNode::getOdomPose, AmclNode::shouldUpdateFilter 함수에 구현되어 있다.

bool
AmclNode::getOdomPose(
  geometry_msgs::msg::PoseStamped & odom_pose,
  double & x, double & y, double & yaw,
  const rclcpp::Time & sensor_timestamp, const std::string & frame_id)
{
  // Get the robot's pose
  geometry_msgs::msg::PoseStamped ident;
  ident.header.frame_id = nav2_util::strip_leading_slash(frame_id);
  ident.header.stamp = sensor_timestamp;
  tf2::toMsg(tf2::Transform::getIdentity(), ident.pose);

  try {
    tf_buffer_->transform(ident, odom_pose, odom_frame_id_);
  } catch (tf2::TransformException & e) {
    ++scan_error_count_;
    if (scan_error_count_ % 20 == 0) {
      RCLCPP_ERROR(
        get_logger(), "(%d) consecutive laser scan transforms failed: (%s)", scan_error_count_,
        e.what());
    }
    return false;
  }

  scan_error_count_ = 0;  // reset since we got a good transform
  x = odom_pose.pose.position.x;
  y = odom_pose.pose.position.y;
  yaw = tf2::getYaw(odom_pose.pose.orientation);

  return true;
}
...
bool AmclNode::shouldUpdateFilter(const pf_vector_t pose, pf_vector_t & delta)
{
  delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
  delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
  delta.v[2] = angleutils::angle_diff(pose.v[2], pf_odom_pose_.v[2]);

  // See if we should update the filter
  bool update = fabs(delta.v[0]) > d_thresh_ ||
    fabs(delta.v[1]) > d_thresh_ ||
    fabs(delta.v[2]) > a_thresh_;
  update = update || force_update_;
  return update;
}

 

다음 sample의 pose는 아래 식과 같이 추정하게 된다. ideal 한 motion model이 아닌 Gaussian 분포를 따르는 noise가 포함된 real motion model로 pose를 추정하게 된다. (이 글에서는 AMCL에 대한 내용이므로 아래 식에 대해서는 깊게 이야기하지 않는다.)

코드로는 아래와 같이 구현되어 있다.

void
DifferentialMotionModel::odometryUpdate(
  pf_t * pf, const pf_vector_t & pose,
  const pf_vector_t & delta)
{
  // Compute the new sample poses
  pf_sample_set_t * set;

  set = pf->sets + pf->current_set;
  pf_vector_t old_pose = pf_vector_sub(pose, delta);

  // Implement sample_motion_odometry (Prob Rob p 136)
  double delta_rot1, delta_trans, delta_rot2;
  double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
  double delta_rot1_noise, delta_rot2_noise;

  // Avoid computing a bearing from two poses that are extremely near each
  // other (happens on in-place rotation).
  if (sqrt(
      delta.v[1] * delta.v[1] +
      delta.v[0] * delta.v[0]) < 0.01)
  {
    delta_rot1 = 0.0;
  } else {
    delta_rot1 = angleutils::angle_diff(
      atan2(delta.v[1], delta.v[0]),
      old_pose.v[2]);
  }
  delta_trans = sqrt(
    delta.v[0] * delta.v[0] +
    delta.v[1] * delta.v[1]);
  delta_rot2 = angleutils::angle_diff(delta.v[2], delta_rot1);

  // We want to treat backward and forward motion symmetrically for the
  // noise model to be applied below.  The standard model seems to assume
  // forward motion.
  delta_rot1_noise = std::min(
    fabs(angleutils::angle_diff(delta_rot1, 0.0)),
    fabs(angleutils::angle_diff(delta_rot1, M_PI)));
  delta_rot2_noise = std::min(
    fabs(angleutils::angle_diff(delta_rot2, 0.0)),
    fabs(angleutils::angle_diff(delta_rot2, M_PI)));

  for (int i = 0; i < set->sample_count; i++) {
    pf_sample_t * sample = set->samples + i;

    // Sample pose differences
    delta_rot1_hat = angleutils::angle_diff(
      delta_rot1,
      pf_ran_gaussian(
        sqrt(
          alpha1_ * delta_rot1_noise * delta_rot1_noise +
          alpha2_ * delta_trans * delta_trans)));
    delta_trans_hat = delta_trans -
      pf_ran_gaussian(
      sqrt(
        alpha3_ * delta_trans * delta_trans +
        alpha4_ * delta_rot1_noise * delta_rot1_noise +
        alpha4_ * delta_rot2_noise * delta_rot2_noise));
    delta_rot2_hat = angleutils::angle_diff(
      delta_rot2,
      pf_ran_gaussian(
        sqrt(
          alpha1_ * delta_rot2_noise * delta_rot2_noise +
          alpha2_ * delta_trans * delta_trans)));

    // Apply sampled update to particle pose
    sample->pose.v[0] += delta_trans_hat *
      cos(sample->pose.v[2] + delta_rot1_hat);
    sample->pose.v[1] += delta_trans_hat *
      sin(sample->pose.v[2] + delta_rot1_hat);
    sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
  }
}

alpha 값은 odometry의 예상되는 noise 값이며 parameter로 설정 가능하다. $ \hat{\delta} $ 를 보게 되면 alpha값이 높을수록 odometry 값에 noise가 크게 된다는 점을 알 수 있다. 각각의 역할에 대한 자세한 내용은 Nav2 Document에서 확인 가능하다.

AMCL — Nav2 1.0.0 documentation

2. importance factor 구하기

AMCL에서는 importance factor(weight)를 구하기 위해 기본적으로 LikelihoodFieldModel을 사용한다. LikelihoodFieldModel은  sample의 위치에서의 LiDAR 데이터를 각각의 scan 데이터를 map 좌표계로 변경한 뒤에 map에서 가장 가까운 장애물까지의 거리를 이용하여 계산한다. $ z $

\begin{equation} p_{t+1} = p_t + z_{hit} * e^{-z^2 / z_{denom}}  \end{equation}

그래프로 그려보면 Gaussian 분포를 따르는 것을 확인할 수 있다.

$ z_{hit} $과 $ z_{denom} $은 parameter로 정해진다. $ z_{hit} $ 과 weight값은 비례하며 $ z_{denom} $ 과 분산값이 비례한다. 즉 $ z_{denom} $ 이 커질수록 LiDAR 센서 혹은 map에 불확실성이 커진다. 그다음 random 값을 더한 후 세제곱한다. 코드로 확인해 보자.

double
LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
{
...

  // Compute the sample weights
  for (j = 0; j < set->sample_count; j++) {
  	...

    // Pre-compute a couple of things
    double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
    double z_rand_mult = 1.0 / data->range_max;
    ...

    for (i = 0; i < data->range_count; i += step) {
      obs_range = data->ranges[i][0];
      obs_bearing = data->ranges[i][1];

      // This model ignores max range readings
      if (obs_range >= data->range_max) {
        continue;
      }

      // Check for NaN
      if (obs_range != obs_range) {
        continue;
      }

      pz = 0.0;

      // Compute the endpoint of the beam
      hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
      hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);

      // Convert to map grid coords.
      int mi, mj;
      mi = MAP_GXWX(self->map_, hit.v[0]);
      mj = MAP_GYWY(self->map_, hit.v[1]);

      // Part 1: Get distance from the hit to closest obstacle.
      // Off-map penalized as max distance
      if (!MAP_VALID(self->map_, mi, mj)) {
        z = self->map_->max_occ_dist;
      } else {
        z = self->map_->cells[MAP_INDEX(self->map_, mi, mj)].occ_dist;
      }
      // Gaussian model
      // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
      pz += self->z_hit_ * exp(-(z * z) / z_hit_denom);
      // Part 2: random measurements
      pz += self->z_rand_ * z_rand_mult;

      // TODO(?): outlier rejection for short readings

      assert(pz <= 1.0);
      assert(pz >= 0.0);
      //      p *= pz;
      // here we have an ad-hoc weighting scheme for combining beam probs
      // works well, though...
      p += pz * pz * pz;
    }

    sample->weight *= p;
    total_weight += sample->weight;
  }

  return total_weight;
}

LiDAR scan 데이터를 통하여 p를 구하고 sample의 weight에 p를 곱하여 업데이트된다. (pz를 세제곱 하는 이유는 정확하진 않지만 개인적인 생각으로는 pz는 확률값 이므로 의도적으로 값을 낮추어 하나의 data에 대한 weight를 조정한 거 같다.) 그다음 weight를 1로 normalize 하고 $ w_{slow} $와 $ w_{fast} $ 값을 업데이트한다. 두 값은 나중에 resample 과정에서 사용된다. 이는 pf_update_sensor에 구현되어 있다.

void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data)
{
  int i;
  pf_sample_set_t * set;
  pf_sample_t * sample;
  double total;

  set = pf->sets + pf->current_set;

  // Compute the sample weights
  total = (*sensor_fn)(sensor_data, set);

  if (total > 0.0) {
    // 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;
    }
    // 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);
    }
  } else {
    // Handle zero total
    for (i = 0; i < set->sample_count; i++) {
      sample = set->samples + i;
      sample->weight = 1.0 / set->sample_count;
    }
  }
}

3. Resampling 구현

먼저 어느 시점에서 Resampling 하는지 확인해 보자. resample_count_ 을 resample_interval_ 로 나눌 때 나머지가 0인 경우 실행한다. resample_count_는 sample들의 pose를 업데이트할 때 1씩 증가하게 된다. 즉 resampling을 너무 자주 할 경우 수렴이 제대로 되지 않으므로 로봇이 일정 임계값 이상 움직였을 때 업데이트 한다.

// Resample the particles
if (!(++resample_count_ % resample_interval_)) {
  pf_update_resample(pf_, reinterpret_cast<void *>(map_));
  resampled = true;
}

resampling 할 때 2가지 경우가 있다. 첫 번째로 $ w_{diff} $ 값과 랜덤 값을 비교하여  $ w_{diff} $가 크다면  pose를 생성하고 작다면 resampling 한다. pose를 생성할 때는 AmclNode::uniformPoseGenerator 함수로 map에 free space에 빈 공간에 랜덤으로 생성한다. $ w_{diff} $ 은 아래와 같다.

 

\begin{equation} w_{diff} = 1 - \frac{w_{fast}}{w_{slow}} \end{equation}

$ w_{diff} $ 가 커질수록 ($w_{fast}$가 작아지고, $w_{slow} $가 커질수록) random pose가 생길 확률이 커진다.

 

resampling 할 때는 Naive discrete event sampler 방식이다.

void pf_update_resample(pf_t * pf, void * random_pose_data)
{
...
    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;
    }
    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)(random_pose_data);
        } else {
          // 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;
        }
    }
...
}

c라는 배열에 1 ~ (m - 1)까지의 가중치 합에 t번째 가중치를 더하는 식으로 누적되는 가중치 배열을 생성한다. 그 뒤에 랜덤 한 변수 r을 통하여 sample을 선택한다.

\begin{equation} c_{t-1} \leq r < c_{t} \end{equation}

직관적으로 생각해 보면 $ c_{t-1} $ 과 $ c_{t} $ 의 차이가 클수록 resampling 될 가능성이 높아진다고 생각할 수 있다.

4. 최적의 로봇의 위치 추정

AMCL은 sample들을 kd-tree 구조로 저장해 놓는다. 이때 sample의 pose를 일정 값으로 나눈다. 그 이유는 비슷한 위치의 sample을 clustering 하기 쉽게 하기 위함이다. 이는 pf_kdtree_insert 함수에서 확인 가능하다.

void pf_kdtree_insert(pf_kdtree_t * self, pf_vector_t pose, double value)
{
  int key[3];

  key[0] = floor(pose.v[0] / self->size[0]);
  key[1] = floor(pose.v[1] / self->size[1]);
  key[2] = floor(pose.v[2] / self->size[2]);

  self->root = pf_kdtree_insert_node(self, NULL, self->root, key, value);
}

그 뒤에 AmclNode::getMaxWeightHyp에서 생성된 cluster에 포함된 sample의 weight의 평균을 값을 이용하여 가장 높은 weight를 가진 cluster를 선택한 뒤 amcl_pose와 map -> odom TF를 업데이트하게 된다.

bool
AmclNode::getMaxWeightHyp(
  std::vector<amcl_hyp_t> & hyps, amcl_hyp_t & max_weight_hyps,
  int & max_weight_hyp)
{
  // Read out the current hypotheses
  double max_weight = 0.0;
  hyps.resize(pf_->sets[pf_->current_set].cluster_count);
  for (int hyp_count = 0;
    hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
  {
    double weight;
    pf_vector_t pose_mean;
    pf_matrix_t pose_cov;
    if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov)) {
      RCLCPP_ERROR(get_logger(), "Couldn't get stats on cluster %d", hyp_count);
      return false;
    }

    hyps[hyp_count].weight = weight;
    hyps[hyp_count].pf_pose_mean = pose_mean;
    hyps[hyp_count].pf_pose_cov = pose_cov;

    if (hyps[hyp_count].weight > max_weight) {
      max_weight = hyps[hyp_count].weight;
      max_weight_hyp = hyp_count;
    }
  }

  if (max_weight > 0.0) {
    RCLCPP_DEBUG(
      get_logger(), "Max weight pose: %.3f %.3f %.3f",
      hyps[max_weight_hyp].pf_pose_mean.v[0],
      hyps[max_weight_hyp].pf_pose_mean.v[1],
      hyps[max_weight_hyp].pf_pose_mean.v[2]);

    max_weight_hyps = hyps[max_weight_hyp];
    return true;
  }
  return false;
}

 

cluster의 pose는 sample의 pose와 weight를 결합하여 weight가 클수록 비중이 커지는 식으로 pose를 생성하게 된다. 이는 pf_cluster_stats에 구현되어 있다.

void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
{
    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]);

        // 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->weight += sample->weight;

        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]));
      }
  }

마무리

AMCL 패키지를 분석해 봤다. 막연하게 Particle Filter로 구현되어 있다고만 알고 있었는데 전체적인 Process를 알게 된 거 같다. 특히 weight를 생성하는 부분에서 특별한 landmark나 다른 센서를 사용해서 update 할 수 있다면 localization 성능을 높일 수 있을 거 같아 상황에 맞게 수정하면 좋을 거 같다. 뿐 아니라 로봇을 사람이 물리적으로 이동시킬 경우 자동으로 re-localization 기능이 있으면 어떨까?라는  생각이 들기도 한다.

참고

AMCL — Nav2 1.0.0 documentation

 

GitHub - yvonshong/Probabilistic-Robotics: 《概率机器人》书和课后习题