ゲイン計算

static auto calc_gain = [&]() {
      double v = hypot(estimate_vel_.x, estimate_vel_.y);//並進速度
      double angular = fabs(estimate_vel_.theta);//角速度
      v = std::clamp(v, 0.0, MAX_VEL);
      angular = std::clamp(angular, 0.0, MAX_ANGULAR);
      // 遅いときlrf　速いときオドメトリ
      double gain_xy = ((MAX_GAIN_XY - MIN_GAIN_XY) / MAX_VEL) * v + MIN_GAIN_XY;
      double gain_theta = ((MAX_GAIN_THETA - MIN_GAIN_THETA) / MAX_ANGULAR) * angular + MIN_GAIN_THETA;
      // 遅いときオドメトリ　速いときlrf
      // double gain_xy = MAX_GAIN_XY - ((MAX_GAIN_XY - MIN_GAIN_XY) / MAX_VEL) * v;
      // double gain_theta = MAX_GAIN_THETA - ((MAX_GAIN_THETA - MIN_GAIN_THETA) / MAX_ANGULAR) * angular;
      return std::pair<double, double>(gain_xy, gain_theta);
    };

基準格子点更新

static auto update_reference_grid = [&]() {
      auto old_grid_point = reference_grid_point_;
      reference_grid_point_.x = std::round(odometer_pos_.x / GRID_WIDTH) * GRID_WIDTH;
      reference_grid_point_.y = std::round(odometer_pos_.y / GRID_WIDTH) * GRID_WIDTH;
      auto initial_pos_xy = initial_pos_.make_vector2() - old_grid_point.get_rotated(initial_pos_.theta) +
                            reference_grid_point_.get_rotated(initial_pos_.theta);
      initial_pos_.x = initial_pos_xy.x;
      initial_pos_.y = initial_pos_xy.y;
      auto q = odometer_pos_.make_vector2() - reference_grid_point_;
      q.rotate(initial_pos_.theta);
      return q;
    };

LRFからの自己位置取得

auto q = update_reference_grid();
const auto &[gain_xy, gain_theta] = calc_gain();
initial_pos_.x -= gain_xy * (q.x + initial_pos_.x - laser_pos_.x);
initial_pos_.y -= gain_xy * (q.y + initial_pos_.y - laser_pos_.y);
initial_pos_.theta = Angle(initial_pos_.theta -
	gain_theta * (double)Angle(odometer_pos_.theta + initial_pos_.theta - laser_pos_.theta));

メインループ

auto q = update_reference_grid();
estimate_pos_.x = initial_pos_.x + q.x;
estimate_pos_.y = initial_pos_.y + q.y;
estimate_pos_.theta = Angle(initial_pos_.theta + odometer_pos_.theta);