diff --git a/CMakeLists.txt b/CMakeLists.txt
index f5d00c4..e685cf8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -18,6 +18,7 @@ find_package(ament_index_cpp REQUIRED)
 find_package(aruku REQUIRED)
 find_package(aruku_interfaces REQUIRED)
 find_package(atama_interfaces REQUIRED)
+find_package(gyakuenki_interfaces REQUIRED)
 find_package(jitsuyo REQUIRED)
 find_package(kansei REQUIRED)
 find_package(kansei_interfaces REQUIRED)
@@ -47,6 +48,7 @@ ament_target_dependencies(${PROJECT_NAME}
   aruku
   aruku_interfaces
   atama_interfaces
+  gyakuenki_interfaces
   jitsuyo
   kansei
   kansei_interfaces
@@ -112,6 +114,7 @@ ament_export_dependencies(
   aruku
   aruku_interfaces
   atama_interfaces
+  gyakuenki_interfaces
   jitsuyo
   kansei
   kansei_interfaces
diff --git a/include/suiryoku/locomotion/model/field.hpp b/include/suiryoku/locomotion/model/field.hpp
new file mode 100644
index 0000000..9e75fb6
--- /dev/null
+++ b/include/suiryoku/locomotion/model/field.hpp
@@ -0,0 +1,81 @@
+// Copyright (c) 2025 Ichiro ITS
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#ifndef SUIRYOKU__LOCOMOTION__MODEL__FIELD_HPP_
+#define SUIRYOKU__LOCOMOTION__MODEL__FIELD_HPP_
+
+#include <vector>
+
+#include "keisan/keisan.hpp"
+
+namespace suiryoku
+{
+
+struct Field
+{
+public:
+  int width;
+  int length;
+  std::vector<keisan::Point2> landmarks_L;
+  std::vector<keisan::Point2> landmarks_T;
+  std::vector<keisan::Point2> landmarks_X;
+  std::vector<keisan::Point2> landmarks_goalpost;
+
+  Field()
+  : width(600),
+    length(900),
+    landmarks_L({
+      {0.0, 0.0},
+      {0.0, 600.0},
+      {100.0, 50.0},
+      {100.0, 550.0},
+      {900.0, 0.0},
+      {900.0, 600.0},
+      {800.0, 50.0},
+      {800.0, 550.0}
+    }),
+    landmarks_T({
+      {0.0, 50.0},
+      {0.0, 550.0},
+      {450.0, 0.0},
+      {450.0, 600.0},
+      {900.0, 50.0},
+      {900.0, 550.0}
+    }),
+    landmarks_X({
+      {210, 300},
+      {690, 300},
+      {450, 300},
+      {450, 375},
+      {450, 225}
+    }),
+    landmarks_goalpost({
+      {0.0, 170.0},
+      {0.0, 430.0},
+      {900.0, 170.0},
+      {900.0, 430.0}
+    })
+  {
+  }
+};
+
+}  // namespace suiryoku
+
+#endif  // SUIRYOKU__LOCOMOTION__MODEL__FIELD_HPP_
diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp
index 86c4517..e5fecfb 100644
--- a/include/suiryoku/locomotion/model/robot.hpp
+++ b/include/suiryoku/locomotion/model/robot.hpp
@@ -21,13 +21,34 @@
 #ifndef SUIRYOKU__LOCOMOTION__MODEL__ROBOT_HPP_
 #define SUIRYOKU__LOCOMOTION__MODEL__ROBOT_HPP_
 
+#include <random>
 #include <string>
 
 #include "keisan/keisan.hpp"
+#include "keisan/hungarian.hpp"
+#include "suiryoku/locomotion/model/field.hpp"
 
 namespace suiryoku
 {
 
+struct ProjectedObject
+{
+  std::string label;
+  keisan::Point3 position;
+};
+
+struct Particle
+{
+  keisan::Point2 position;
+  keisan::Angle<double> orientation;
+  double weight;
+};
+
+struct LandmarkGroup {
+  std::vector<ProjectedObject>* projected;
+  std::vector<keisan::Point2>* landmarks;
+};
+
 class Robot
 {
 public:
@@ -36,10 +57,60 @@ class Robot
   keisan::Angle<double> get_pan() const;
   keisan::Angle<double> get_tilt() const;
 
+  // localizations
+  void localize();
+  void reset_localization();
+  void clear_projected_objects();
+  void init_particles(const keisan::Point2 init_position);
+  void resample_particles();
+  void update_motion();
+  void calculate_weight();
+  void estimate_position();
+  void print_particles();
+  void print_estimate_position();
+  void set_initial_localization(bool initial) { initial_localization = initial; }
+
+  double calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2);
+  double calculate_total_likelihood(const Particle & particle);
+  double calculate_object_likelihood(const ProjectedObject & measurement,
+                                     const keisan::Point2 & landmark,
+                                     const Particle & particle);
+
+  double get_sum_weight();
+  keisan::Matrix<6, 6> calculate_cost_matrix(const Particle & particle,
+                                             const std::vector<ProjectedObject> & projected_objects,
+                                             const std::vector<keisan::Point2> & landmarks);
+
+  std::vector<Particle> particles;
+  keisan::Point2 estimated_position;
+
+  int num_particles;
+  double min_centered_particles_ratio;
+  double reset_particles_threshold;
+  double short_term_avg_ratio;
+  double long_term_avg_ratio;
+  double sigma_x;
+  double sigma_y;
+
+  bool use_localization;
+  bool apply_localization;
+  bool print_debug;
+
+  // IPM
+  std::vector<ProjectedObject> projected_X;
+  std::vector<ProjectedObject> projected_L;
+  std::vector<ProjectedObject> projected_T;
+  std::vector<ProjectedObject> projected_goalpost;
+  keisan::Point2 max_object_distance;
+  int num_projected_objects;
+
   // member for getting
   bool is_calibrated;
   keisan::Angle<double> orientation;
+  keisan::Angle<double> orientation_roll;
+  keisan::Angle<double> orientation_pitch;
   keisan::Point2 position;
+  keisan::Point2 delta_position;
 
   bool is_walking;
 
@@ -57,6 +128,29 @@ class Robot
   double y_speed;
   double a_speed;
   bool aim_on;
+
+private:
+  Field field;
+  Particle* best_particle;
+  std::vector<Particle*> center_particles;
+
+  double xvar;
+  double yvar;
+
+  std::mt19937 rand_gen;
+  keisan::Hungarian<6> hungarian;
+
+  double weight_avg;
+  double short_term_avg;
+  double long_term_avg;
+  double last_weight_avg;
+  double prob;
+
+  bool initial_localization;
+  bool reset_particles;
+
+  int pending_accept_count;
+  keisan::Point2 pending_estimated_position;
 };
 
 }  // namespace suiryoku
diff --git a/include/suiryoku/locomotion/node/locomotion_node.hpp b/include/suiryoku/locomotion/node/locomotion_node.hpp
index fb2e800..6a9ce67 100644
--- a/include/suiryoku/locomotion/node/locomotion_node.hpp
+++ b/include/suiryoku/locomotion/node/locomotion_node.hpp
@@ -28,10 +28,14 @@
 #include "aruku_interfaces/msg/set_walking.hpp"
 #include "aruku_interfaces/msg/status.hpp"
 #include "atama_interfaces/msg/head.hpp"
+#include "gyakuenki_interfaces/msg/projected_objects.hpp"
 #include "kansei_interfaces/msg/status.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "suiryoku/locomotion/model/robot.hpp"
 #include "suiryoku/locomotion/process/locomotion.hpp"
+#include "suiryoku_interfaces/msg/particle.hpp"
+#include "suiryoku_interfaces/msg/particles.hpp"
+
 
 namespace suiryoku
 {
@@ -41,7 +45,10 @@ class LocomotionNode
 public:
   using Head = atama_interfaces::msg::Head;
   using MeasurementStatus = kansei_interfaces::msg::Status;
+  using Particle = suiryoku_interfaces::msg::Particle;
+  using Particles = suiryoku_interfaces::msg::Particles;
   using Point2 = aruku_interfaces::msg::Point2;
+  using ProjectedObjects = gyakuenki_interfaces::msg::ProjectedObjects;
   using SetWalking = aruku_interfaces::msg::SetWalking;
   using WalkingStatus = aruku_interfaces::msg::Status;
 
@@ -57,18 +64,27 @@ class LocomotionNode
 private:
   void publish_walking();
   void publish_odometry();
+  void publish_particles();
 
   rclcpp::Node::SharedPtr node;
 
   rclcpp::Publisher<SetWalking>::SharedPtr set_walking_publisher;
 
   rclcpp::Publisher<Point2>::SharedPtr set_odometry_publisher;
+
+  rclcpp::Publisher<Particles>::SharedPtr particles_publisher;
+
   rclcpp::Subscription<MeasurementStatus>::SharedPtr
     measurement_status_subscriber;
+
   rclcpp::Subscription<WalkingStatus>::SharedPtr walking_status_subscriber;
 
   rclcpp::Subscription<Head>::SharedPtr head_subscriber;
 
+  rclcpp::Subscription<ProjectedObjects>::SharedPtr projected_objects_subscriber;
+
+  rclcpp::Subscription<Point2>::SharedPtr delta_position_subscriber;
+
   std::shared_ptr<Locomotion> locomotion;
   std::shared_ptr<Robot> robot;
 
diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp
index e12dd0a..072e70e 100755
--- a/include/suiryoku/locomotion/process/locomotion.hpp
+++ b/include/suiryoku/locomotion/process/locomotion.hpp
@@ -47,12 +47,15 @@ class Locomotion
 
   void move_forward(const keisan::Angle<double> & direction);
   bool move_forward_to(const keisan::Point2 & target);
+  bool move_forward_by_distance(keisan::Point2 & delta_target);
 
   bool rotate_to_target(const keisan::Angle<double> & direction);
   bool rotate_to(const keisan::Angle<double> & direction, bool a_move_only);
 
   bool move_follow_head();
   bool move_follow_head(const keisan::Angle<double> & min_tilt);
+  bool move_follow_head(const keisan::Angle<double> & pan, const keisan::Angle<double> & tilt);
+  bool move_follow_head(const keisan::Angle<double> & pan, const keisan::Angle<double> & tilt, const keisan::Angle<double> & min_tilt);
 
   bool move_skew(const keisan::Angle<double> & direction);
   bool move_skew(const keisan::Angle<double> & direction, bool skew_left);
@@ -67,8 +70,8 @@ class Locomotion
     const keisan::Angle<double> & direction);
   bool position_left_kick(const keisan::Angle<double> & direction);
   bool position_right_kick(const keisan::Angle<double> & direction);
-  bool position_kick_custom_pan_tilt(const keisan::Angle<double> & direction, const keisan::Angle<double> & min_pan, 
-                                     const keisan::Angle<double> & max_pan, const keisan::Angle<double> & min_tilt, 
+  bool position_kick_custom_pan_tilt(const keisan::Angle<double> & direction, const keisan::Angle<double> & min_pan,
+                                     const keisan::Angle<double> & max_pan, const keisan::Angle<double> & min_tilt,
                                      const keisan::Angle<double> & max_tilt);
   bool position_kick_general(const keisan::Angle<double> & direction);
   bool position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, bool is_positioning_center);
diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp
index 6e18c78..cd11e43 100644
--- a/src/suiryoku/locomotion/model/robot.cpp
+++ b/src/suiryoku/locomotion/model/robot.cpp
@@ -18,6 +18,7 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 // THE SOFTWARE.
 
+#include <cmath>
 #include <string>
 
 #include "suiryoku/locomotion/model/robot.hpp"
@@ -31,9 +32,16 @@ namespace suiryoku
 
 Robot::Robot()
 : pan(0_deg), tilt(0_deg), pan_center(0_deg), tilt_center(0_deg), x_speed(0.0),
-  y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false),
-  orientation(0_deg), position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0),
-  a_amplitude(0.0), is_calibrated(false)
+  y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), position(0.0, 0.0),
+  orientation(0_deg), orientation_roll(0_deg), orientation_pitch(0_deg),
+  x_amplitude(0.0), y_amplitude(0.0), a_amplitude(0.0), is_calibrated(false),
+  use_localization(false), apply_localization(false), print_debug(false),
+  num_particles(500), min_centered_particles_ratio(0.3), reset_particles_threshold(0.3),
+  short_term_avg_ratio(0.1), long_term_avg_ratio(0.001), sigma_x(5.0), sigma_y(5.0),
+  xvar(10.0), yvar(10.0), best_particle(nullptr), rand_gen(std::random_device{}()),
+  weight_avg(0.0), short_term_avg(0.0), long_term_avg(0.0), last_weight_avg(0.0),
+  estimated_position(-1.0, -1.0), initial_localization(true), reset_particles(false),
+  num_projected_objects(0), max_object_distance(300.0, 300.0), pending_accept_count(0)
 {
 }
 
@@ -47,4 +55,464 @@ keisan::Angle<double> Robot::get_tilt() const
   return tilt + tilt_center;
 }
 
+void Robot::localize()
+{
+  if (!use_localization) {
+    return;
+  }
+
+  if (initial_localization) {
+    init_particles(keisan::Point2(-1, -1));
+  } else {
+    update_motion();
+  }
+
+  bool evaluate_particles = num_projected_objects > 0;
+  evaluate_particles &= fabs(orientation_roll.degree()) <= 30;
+  evaluate_particles &= fabs(orientation_pitch.degree()) <= 30;
+  evaluate_particles &= fabs(get_pan().degree()) <= 90;
+  evaluate_particles &= get_tilt().degree() > -50;
+
+  if (evaluate_particles) {
+    weight_avg = 0.0;
+    calculate_weight();
+    if (weight_avg <= 0 || std::isnan(weight_avg)) {
+      weight_avg = last_weight_avg;
+    } else {
+      if (initial_localization) {
+        short_term_avg = weight_avg;
+        long_term_avg = weight_avg;
+      }
+      last_weight_avg = weight_avg;
+    }
+
+    if (!std::isnan(weight_avg)) {
+      short_term_avg += short_term_avg_ratio * (weight_avg - short_term_avg);
+      long_term_avg += long_term_avg_ratio * (weight_avg - long_term_avg);
+    }
+
+    resample_particles();
+  }
+  estimate_position();
+
+  clear_projected_objects();
+}
+
+void Robot::reset_localization() {
+  apply_localization = true;
+  initial_localization = true;
+  particles.clear();
+  center_particles.clear();
+  clear_projected_objects();
+  estimated_position = keisan::Point2(-1.0, -1.0);
+  best_particle = nullptr;
+  weight_avg = 0.0;
+  short_term_avg = 0.0;
+  long_term_avg = 0.0;
+  last_weight_avg = 0.0;
+  reset_particles = false;
+  prob = 0.0;
+  pending_accept_count = 0;
+  pending_estimated_position = keisan::Point2(-1.0, -1.0);
+
+  if (use_localization) {
+    localize();
+  }
+}
+
+void Robot::clear_projected_objects()
+{
+  projected_X.clear();
+  projected_L.clear();
+  projected_T.clear();
+  projected_goalpost.clear();
+  num_projected_objects = 0;
+}
+
+void Robot::init_particles(const keisan::Point2 init_position)
+{
+  double uniform_weight = 1.0 / num_particles;
+  particles.clear();
+  particles.resize(num_particles);
+  initial_localization = true;
+  estimated_position = init_position;
+
+  if (init_position.x == -1 && init_position.y == -1) {
+    std::uniform_int_distribution<int> xrg(0, 900);
+    std::uniform_int_distribution<int> yrg(0, 600);
+
+    for (int i = 0; i < num_particles; ++i) {
+      particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen));
+      particles[i].orientation = orientation;
+      particles[i].weight = uniform_weight;
+    }
+  } else {
+    std::normal_distribution<double> xrg(init_position.x, 50);
+    std::normal_distribution<double> yrg(init_position.y, 50);
+
+    for (int i = 0; i < num_particles; ++i) {
+      particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen));
+      particles[i].orientation = orientation;
+      particles[i].weight = uniform_weight;
+    }
+  }
+}
+
+void Robot::resample_particles()
+{
+  std::vector<Particle> new_particles(num_particles);
+  std::uniform_int_distribution<int> rand_index(0, num_particles - 1);
+  int index = rand_index(rand_gen);
+
+  double beta = 0.0;
+  double max_weight = 0.0;
+  double sum_weight = get_sum_weight();
+  prob = std::max(0.0, 1.0 - short_term_avg / long_term_avg);
+  // reset_particles = prob > reset_particles_threshold;
+
+  // find the best particle
+  if (sum_weight > 0) {
+    for (auto & p : particles) {
+      if (p.weight > max_weight) {
+        max_weight = p.weight;
+        best_particle = &p;
+      }
+    }
+  }
+
+  // determine resample interval area
+  int interval_x[2] = {10, 890};
+  int interval_y[2] = {10, 590};
+
+  bool resample_nearby = num_projected_objects == 1;
+  resample_nearby |= fabs(get_pan().degree()) > 80;
+  resample_nearby |= get_tilt().degree() < -40;
+  resample_nearby |= reset_particles;
+
+  if (resample_nearby) {
+    interval_x[0] = position.x - 50;
+    interval_x[1] = position.x + 50;
+    interval_y[0] = position.y - 50;
+    interval_y[1] = position.y + 50;
+    reset_particles = false;
+    prob = std::min(0.05, prob);
+  }
+
+  // resample particles
+  std::uniform_real_distribution<double> rand_prob(0.0, 1.0);
+  std::uniform_real_distribution<double> rand_beta(0.0, 2.0 * max_weight);
+  std::uniform_int_distribution<int> xrg(interval_x[0], interval_x[1]);
+  std::uniform_int_distribution<int> yrg(interval_y[0], interval_y[1]);
+
+  // adaptive noise
+  double max_sigma = 3.0;
+  double min_sigma = 0.1;
+
+  double sigma_factor = 1.0 - prob;
+  double resample_sigma_x = min_sigma + sigma_factor * (max_sigma - min_sigma);
+  double resample_sigma_y = min_sigma + sigma_factor * (max_sigma - min_sigma);
+
+  std::normal_distribution<double> noise_x(0.0, resample_sigma_x);
+  std::normal_distribution<double> noise_y(0.0, resample_sigma_y);
+
+  for (int i = 0; i < num_particles; ++i) {
+    if (rand_prob(rand_gen) < prob) {
+      new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen));
+      new_particles[i].orientation = orientation;
+      new_particles[i].weight = 0.0;
+    } else {
+      beta += rand_beta(rand_gen);
+
+      while (beta > particles[index].weight) {
+        beta -= particles[index].weight;
+        index = (index + 1) % num_particles;
+      }
+      new_particles[i] = particles[index];
+
+      new_particles[i].position.x += noise_x(rand_gen);
+      new_particles[i].position.y += noise_y(rand_gen);
+    }
+
+    // check is particle out of bound
+    if (new_particles[i].position.x < 0.0 || new_particles[i].position.x > 900.0 ||
+        new_particles[i].position.y < 0.0 || new_particles[i].position.y > 600.0) {
+      new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen));
+      new_particles[i].orientation = orientation;
+      new_particles[i].weight = 0.0;
+    }
+  }
+
+  particles = new_particles;
+}
+
+void Robot::update_motion()
+{
+  static std::normal_distribution<double> xgen(0.0, xvar), ygen(0.0, yvar);
+  bool update_with_noise = num_projected_objects > 0;
+  update_with_noise &= fabs(orientation_roll.degree()) <= 30;
+  update_with_noise &= fabs(orientation_pitch.degree()) <= 30;
+  update_with_noise &= fabs(get_pan().degree()) <= 90;
+  update_with_noise &= get_tilt().degree() > -50;
+
+  if (update_with_noise || true) {
+    for (auto & p : particles) {
+      double static_noise_x = xgen(rand_gen) / 10.0;
+      double static_noise_y = ygen(rand_gen) / 10.0;
+      double dynamic_noise_x = fabs(delta_position.x) * xgen(rand_gen) / 5.0;
+      double dynamic_noise_y = fabs(delta_position.y) * ygen(rand_gen) / 5.0;
+      double x_yterm = fabs(delta_position.y) * xgen(rand_gen) / 30.0;
+      double y_xterm = fabs(delta_position.x) * ygen(rand_gen) / 30.0;
+
+      p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm;
+      p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm;
+      p.orientation = orientation;
+
+      p.position.x = keisan::clamp(p.position.x, 0.0, 900.0);
+      p.position.y = keisan::clamp(p.position.y, 0.0, 600.0);
+    }
+  } else {
+    for (auto & p : particles) {
+      p.position.x += delta_position.x;
+      p.position.y += delta_position.y;
+      p.orientation = orientation;
+
+      p.position.x = keisan::clamp(p.position.x, 0.0, 900.0);
+      p.position.y = keisan::clamp(p.position.y, 0.0, 600.0);
+    }
+  }
+
+  estimated_position += delta_position;
+}
+
+double Robot::get_sum_weight()
+{
+  double sum_weight = 0.0;
+  for (const auto & p : particles) {
+    sum_weight += p.weight;
+  }
+  return sum_weight;
+}
+
+void Robot::calculate_weight()
+{
+  double sum_weight = 0.0;
+
+  for (auto & p : particles) {
+    p.weight = calculate_total_likelihood(p);
+    sum_weight += p.weight;
+  }
+
+  if (!std::isnan(sum_weight) && sum_weight > 0) {
+    weight_avg = sum_weight / num_particles;
+    for (auto & p : particles) {
+      p.weight /= sum_weight;
+    }
+  } else {
+    weight_avg = 0.0;
+  }
+}
+
+double Robot::calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2) {
+  double dx = point_1.x - point_2.x;
+  double dy = point_1.y - point_2.y;
+
+  return sqrt(dx * dx + dy * dy);
+}
+
+keisan::Matrix<6, 6> Robot::calculate_cost_matrix(
+  const Particle & particle, const std::vector<ProjectedObject> & projected_objects,
+  const std::vector<keisan::Point2> & landmarks) {
+  auto cost_matrix = keisan::Matrix<6, 6>::infinite();
+  int num_projected_objects = projected_objects.size();
+  int num_landmarks = landmarks.size();
+
+  for (int i = 0; i < num_projected_objects; ++i) {
+    for (int j = 0; j < num_landmarks; ++j) {
+      double dx = projected_objects[i].position.x * 100;
+      double dy = projected_objects[i].position.y * 100;
+
+      double x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin();
+      double y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos();
+
+      double relative_position_x = particle.position.x + x_rot;
+      double relative_position_y = particle.position.y + y_rot;
+
+      keisan::Point2 projected_object(relative_position_x, relative_position_y);
+
+      cost_matrix[i][j] = calculate_distance(landmarks[j], projected_object);
+    }
+  }
+
+  return cost_matrix;
+}
+
+double Robot::calculate_total_likelihood(const Particle & particle) {
+  double total_likelihood = 1.0;
+
+  std::vector<LandmarkGroup> landmark_groups = {
+    {&projected_X, &field.landmarks_X},
+    {&projected_L, &field.landmarks_L},
+    {&projected_T, &field.landmarks_T},
+    {&projected_goalpost, &field.landmarks_goalpost}
+  };
+
+  for (const auto & group : landmark_groups) {
+    if (group.projected->empty() || group.landmarks->empty()) {
+      continue;
+    }
+
+    auto cost_matrix = calculate_cost_matrix(particle, *group.projected, *group.landmarks);
+    auto result = hungarian.solve(cost_matrix, group.landmarks->size());
+
+    for (int i = 0; i < group.projected->size(); ++i) {
+      for (int j = 0; j < group.landmarks->size(); ++j) {
+        if (result[i][j] == 1) {
+          total_likelihood *= calculate_object_likelihood((*group.projected)[i], (*group.landmarks)[j], particle);
+        }
+      }
+    }
+  }
+
+  return total_likelihood;
+}
+
+double Robot::calculate_object_likelihood(
+  const ProjectedObject & measurement, const keisan::Point2 & landmark, const Particle & particle) {
+
+  double dx = measurement.position.x * 100;
+  double dy = measurement.position.y * 100;
+
+  double x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin();
+  double y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos();
+
+  double relative_position_x = particle.position.x + x_rot;
+  double relative_position_y = particle.position.y + y_rot;
+
+  double dist = sqrt(dx * dx + dy * dy);
+  double dist_factor = keisan::map(dist, 150.0, 500.0, 1.0, 0.9);
+
+  double exponent =
+    -0.5 *
+    (pow((landmark.x - relative_position_x), 2) / pow(sigma_x, 2) +
+    pow((landmark.y - relative_position_y), 2) / pow(sigma_y, 2));
+
+  return exp(exponent) / (2 * M_PI * sigma_x * sigma_y) * dist_factor;
+}
+
+void Robot::estimate_position() {
+  if (best_particle == nullptr || num_projected_objects == 0) {
+    return;
+  }
+
+  keisan::Point2 sum_position = {0.0, 0.0};
+  int centered_particles = 0;
+  center_particles.clear();
+
+  // Find numbers of particles closed to the best particle
+  for (int i = 0; i < num_particles; ++i) {
+    double distance = calculate_distance(particles[i].position, best_particle->position);
+
+    if (distance < 25.0) {
+      centered_particles++;
+      sum_position.x += particles[i].position.x;
+      sum_position.y += particles[i].position.y;
+
+      center_particles.push_back(&particles[i]);
+    }
+  }
+
+  // Use mean of centered particles position if more than threshold are centered
+  if (centered_particles > min_centered_particles_ratio * num_particles) {
+    auto last_estimated_position = estimated_position;
+    estimated_position.x = sum_position.x / centered_particles;
+    estimated_position.y = sum_position.y / centered_particles;
+
+    estimated_position.x = keisan::clamp(estimated_position.x, 0.0, 900.0);
+    estimated_position.y = keisan::clamp(estimated_position.y, 0.0, 600.0);
+
+    double jump_distance = calculate_distance(estimated_position, position);
+
+    if (jump_distance <= 100.0 || initial_localization) {
+      position = estimated_position;
+      apply_localization = true;
+      if (initial_localization) {
+        initial_localization = false;
+      }
+    } else {
+      double delta_estimate = calculate_distance(pending_estimated_position, estimated_position);
+      if (delta_estimate < 20.0) {
+        pending_accept_count++;
+      } else {
+        pending_accept_count = 0;
+      }
+      pending_estimated_position = estimated_position;
+
+      if (pending_accept_count >= 10) {
+        position = estimated_position;
+        apply_localization = true;
+        pending_accept_count = 0;
+      } else {
+        estimated_position = last_estimated_position;
+        apply_localization = false;
+        reset_particles = true;
+      }
+    }
+  }
+}
+
+void Robot::print_particles() {
+  if (!print_debug) {
+    return;
+  }
+
+  printf("Particles num: %d\n", particles.size());
+  printf("Prob: %.2f | Threshold: %.2f | Above threshold: %s\n",
+          prob,
+          reset_particles_threshold,
+          (prob > reset_particles_threshold) ? "true" : "false");
+  printf("Reset particles: %s\n", reset_particles ? "true" : "false");
+  printf("Short term avg: %.8f\n", short_term_avg);
+  printf("Long term avg: %.8f\n", long_term_avg);
+  printf("Last weight avg: %.8f\n", last_weight_avg);
+  printf("Weight avg: %.8f\n", weight_avg);
+  printf("Sum weights: %.8f\n", get_sum_weight());
+
+  printf("========================================\n");
+  printf("Minimal Centered Particles: %.0f\n", min_centered_particles_ratio * num_particles);
+
+  printf("Centered particles: %d\n", center_particles.size());
+  for (int i = 0; i < center_particles.size(); ++i) {
+    printf("Centered Particle %d  weight: %.5f  [%.2f, %.2f, %.2f]\n",
+            i,
+            center_particles[i]->weight,
+            center_particles[i]->position.x,
+            center_particles[i]->position.y,
+            center_particles[i]->orientation.degree());
+    if (i >= 10) {
+      printf("... and %d more\n", center_particles.size() - 10);
+      break;
+    }
+  }
+
+  if (best_particle == nullptr) {
+    printf("Best particle: NULL\n");
+  } else {
+    printf("Best particle: %.5f  [%.2f, %.2f, %.2f]\n",
+            best_particle->weight,
+            best_particle->position.x,
+            best_particle->position.y,
+            best_particle->orientation.degree());
+  }
+
+  printf("Num objects: %d\n", num_projected_objects);
+  printf("Num particles: %d\n", num_particles);
+  printf("Projected objects: %d\n", num_projected_objects);
+  printf("Initial Localization %d\n", initial_localization);
+  print_estimate_position();
+}
+
+void Robot::print_estimate_position() {
+  printf("Pose estimation: [%.2f, %.2f])\n", estimated_position.x, estimated_position.y);
+}
+
 }  // namespace suiryoku
diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp
index ad21df9..edeac0c 100644
--- a/src/suiryoku/locomotion/node/locomotion_node.cpp
+++ b/src/suiryoku/locomotion/node/locomotion_node.cpp
@@ -50,11 +50,16 @@ LocomotionNode::LocomotionNode(
     [this](const MeasurementStatus::SharedPtr message) {
       this->robot->is_calibrated = message->is_calibrated;
       this->robot->orientation = keisan::make_degree(message->orientation.yaw);
+      this->robot->orientation_roll = keisan::make_degree(message->orientation.roll);
+      this->robot->orientation_pitch = keisan::make_degree(message->orientation.pitch);
     });
 
   set_odometry_publisher = node->create_publisher<Point2>(
     aruku::WalkingNode::set_odometry_topic(), 10);
 
+  particles_publisher = node->create_publisher<Particles>(
+    "/localization/particles", 10);
+
   walking_status_subscriber = node->create_subscription<WalkingStatus>(
     aruku::WalkingNode::status_topic(), 10,
     [this](const WalkingStatus::SharedPtr message)
@@ -74,14 +79,58 @@ LocomotionNode::LocomotionNode(
       this->robot->tilt = keisan::make_degree(message->tilt_angle);
     });
 
+  delta_position_subscriber = node->create_subscription<Point2>(
+    aruku::WalkingNode::delta_position_topic(), 10,
+    [this](const Point2::SharedPtr message) {
+      this->robot->delta_position.x = message->x;
+      this->robot->delta_position.y = message->y;
+    });
+
+  projected_objects_subscriber = node->create_subscription<ProjectedObjects>(
+    "/gyakuenki_cpp/projected_objects", 10,
+    [this](const ProjectedObjects::SharedPtr message) {
+      this->robot->clear_projected_objects();
+      for (const auto & obj : message->projected_objects) {
+        bool ignore_object = obj.label == "ball" || obj.label == "robot" || obj.label == "self";
+        ignore_object |= obj.position.x * 100 > this->robot->max_object_distance.x;
+        ignore_object |= obj.position.y * 100 < -this->robot->max_object_distance.y;
+        ignore_object |= obj.position.y * 100 > this->robot->max_object_distance.y;
+        if (ignore_object) {
+          continue;
+        }
+
+        auto projected_object = ProjectedObject{
+          obj.label,
+          keisan::Point3{obj.position.x, obj.position.y, obj.position.z}
+        };
+
+        if (obj.label == "X-Intersection") {
+          this->robot->projected_X.push_back(projected_object);
+        } else if (obj.label == "L-Intersection") {
+          this->robot->projected_L.push_back(projected_object);
+        } else if (obj.label == "T-Intersection") {
+          this->robot->projected_T.push_back(projected_object);
+        } else if (obj.label == "goalpost") {
+          this->robot->projected_goalpost.push_back(projected_object);
+        }
+        this->robot->num_projected_objects++;
+      }
+    });
+
   locomotion->stop = [this]() {this->walking_state = false;};
   locomotion->start = [this]() {this->walking_state = true;};
 }
 
 void LocomotionNode::update()
 {
+  if (this->robot->use_localization) {
+    this->robot->localize();
+    publish_particles();
+  }
+
   publish_walking();
-  if (set_odometry) {
+  if (set_odometry || this->robot->apply_localization) {
+    this->robot->apply_localization = false;
     publish_odometry();
   }
 }
@@ -110,4 +159,31 @@ void LocomotionNode::publish_odometry()
   set_odometry = false;
 }
 
+void LocomotionNode::publish_particles()
+{
+  Particles particles_msg;
+  double max_weight = 0.0;
+
+  for (const auto & p : robot->particles) {
+    Particle particle_msg;
+    particle_msg.x = p.position.x;
+    particle_msg.y = p.position.y;
+    particle_msg.orientation = p.orientation.degree();
+    particle_msg.weight = p.weight;
+
+    particles_msg.particles.push_back(particle_msg);
+
+    if (p.weight > max_weight) {
+      max_weight = p.weight;
+    }
+  }
+
+  particles_msg.estimated_position.x = robot->estimated_position.x;
+  particles_msg.estimated_position.y = robot->estimated_position.y;
+  particles_msg.estimated_position.orientation = robot->orientation.degree();
+  particles_msg.estimated_position.weight = max_weight;
+
+  particles_publisher->publish(particles_msg);
+}
+
 }  // namespace suiryoku
diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp
index aaa40b2..a01b07d 100755
--- a/src/suiryoku/locomotion/process/locomotion.cpp
+++ b/src/suiryoku/locomotion/process/locomotion.cpp
@@ -73,7 +73,7 @@ void Locomotion::set_config(const nlohmann::json & json)
   } else {
     valid_config = false;
   }
-  
+
   nlohmann::json rotate_section;
   if (jitsuyo::assign_val(json, "rotate", rotate_section)) {
     bool valid_section = true;
@@ -86,7 +86,7 @@ void Locomotion::set_config(const nlohmann::json & json)
   } else {
     valid_config = false;
   }
-  
+
   nlohmann::json backward_section;
   if (jitsuyo::assign_val(json, "backward", backward_section)) {
     bool valid_section = true;
@@ -100,7 +100,7 @@ void Locomotion::set_config(const nlohmann::json & json)
   } else {
     valid_config = false;
   }
-  
+
   nlohmann::json dribble_section;
   if (jitsuyo::assign_val(json, "dribble", dribble_section)) {
     bool valid_section = true;
@@ -119,7 +119,7 @@ void Locomotion::set_config(const nlohmann::json & json)
   } else {
     valid_config = false;
   }
-  
+
   nlohmann::json follow_section;
   if (jitsuyo::assign_val(json, "follow", follow_section)) {
     bool valid_section = true;
@@ -148,7 +148,7 @@ void Locomotion::set_config(const nlohmann::json & json)
   } else {
     valid_config = false;
   }
-  
+
   nlohmann::json skew_section;
   if (jitsuyo::assign_val(json, "skew", skew_section)) {
     bool valid_section = true;
@@ -164,7 +164,7 @@ void Locomotion::set_config(const nlohmann::json & json)
   } else {
     valid_config = false;
   }
-  
+
   nlohmann::json pivot_section;
   if (jitsuyo::assign_val(json, "pivot", pivot_section)) {
     bool valid_section = true;
@@ -181,7 +181,7 @@ void Locomotion::set_config(const nlohmann::json & json)
     valid_section &= jitsuyo::assign_val(pivot_section, "pan_range_a_speed", pivot_pan_range_a_speed);
     valid_section &= jitsuyo::assign_val(pivot_section, "target_tilt", pivot_target_tilt_double);
     valid_section &= jitsuyo::assign_val(pivot_section, "pivot_stop_limit", pivot_stop_limit_double);
-    
+
     pivot_target_tilt = keisan::make_degree(pivot_target_tilt_double);
     pivot_stop_limit = keisan::make_degree(pivot_stop_limit_double);
 
@@ -192,7 +192,7 @@ void Locomotion::set_config(const nlohmann::json & json)
   } else {
     valid_config = false;
   }
-  
+
   nlohmann::json position_section;
   if (jitsuyo::assign_val(json, "position", position_section)) {
     bool valid_section = true;
@@ -265,7 +265,7 @@ void Locomotion::set_config(const nlohmann::json & json)
   } else {
     valid_config = false;
   }
-  
+
   nlohmann::json right_kick_section;
   if (jitsuyo::assign_val(json, "right_kick", right_kick_section)) {
     bool valid_section = true;
@@ -287,6 +287,30 @@ void Locomotion::set_config(const nlohmann::json & json)
     valid_config = false;
   }
 
+  nlohmann::json localization_section;
+  if (jitsuyo::assign_val(json, "localization", localization_section)) {
+    bool valid_section = true;
+
+    valid_section &= jitsuyo::assign_val(localization_section, "enable", robot->use_localization);
+    valid_section &= jitsuyo::assign_val(localization_section, "debug", robot->print_debug);
+    valid_section &= jitsuyo::assign_val(localization_section, "num_particles", robot->num_particles);
+    valid_section &= jitsuyo::assign_val(localization_section, "min_centered_particles_ratio", robot->min_centered_particles_ratio);
+    valid_section &= jitsuyo::assign_val(localization_section, "short_term_avg_ratio", robot->short_term_avg_ratio);
+    valid_section &= jitsuyo::assign_val(localization_section, "long_term_avg_ratio", robot->long_term_avg_ratio);
+    valid_section &= jitsuyo::assign_val(localization_section, "reset_particles_threshold", robot->reset_particles_threshold);
+    valid_section &= jitsuyo::assign_val(localization_section, "sigma_x", robot->sigma_x);
+    valid_section &= jitsuyo::assign_val(localization_section, "sigma_y", robot->sigma_y);
+    valid_section &= jitsuyo::assign_val(localization_section, "max_distance_x", robot->max_object_distance.x);
+    valid_section &= jitsuyo::assign_val(localization_section, "max_distance_y", robot->max_object_distance.y);
+
+    if (!valid_section) {
+      std::cout << "Error found at section `localization`" << std::endl;
+      valid_config = false;
+    }
+  } else {
+    valid_config = false;
+  }
+
   if (!valid_config) {
     throw std::runtime_error("Failed to load config file `locomotion.json`");
   }
@@ -415,7 +439,43 @@ bool Locomotion::move_forward_to(const keisan::Point2 & target)
   if (target_distance < 8.0) {
     return true;
   }
-  
+
+  auto direction = keisan::signed_arctan(delta_y, delta_x).normalize();
+  double delta_direction = (direction - robot->orientation).normalize().degree();
+
+  double x_speed = keisan::map(std::abs(delta_direction), 0.0, 15.0, move_max_x, move_min_x);
+  if (target_distance < 100.0) {
+    x_speed = keisan::map(target_distance, 0.0, 100.0, move_max_x * 0.25, move_max_x);
+  }
+
+  double a_speed = keisan::map(delta_direction, -25.0, 25.0, move_max_a, -move_max_a);
+  if (std::abs(delta_direction) > 15.0) {
+    a_speed = (delta_direction < 0.0) ? move_max_a : -move_max_a;
+    x_speed = keisan::map(std::abs(a_speed), 0.0, move_max_a, move_max_x, 0.0);
+  }
+
+  x_speed = keisan::smooth(robot->x_speed, x_speed, 0.4);
+
+  robot->x_speed = x_speed;
+  robot->y_speed = 0.0;
+  robot->a_speed = a_speed;
+  robot->aim_on = false;
+  start();
+
+  return false;
+}
+
+bool Locomotion::move_forward_by_distance(keisan::Point2 & delta_target)
+{
+  double delta_x = delta_target.x;
+  double delta_y = delta_target.y;
+
+  double target_distance = std::hypot(delta_x, delta_y);
+
+  if (target_distance < 8.0) {
+    return true;
+  }
+
   auto direction = keisan::signed_arctan(delta_y, delta_x).normalize();
   double delta_direction = (direction - robot->orientation).normalize().degree();
 
@@ -438,6 +498,9 @@ bool Locomotion::move_forward_to(const keisan::Point2 & target)
   robot->aim_on = false;
   start();
 
+  delta_target.x -= robot->delta_position.x;
+  delta_target.y -= robot->delta_position.y;
+
   return false;
 }
 
@@ -491,6 +554,11 @@ bool Locomotion::move_follow_head()
   return move_follow_head(follow_min_tilt);
 }
 
+bool Locomotion::move_follow_head(const keisan::Angle<double> & pan, const keisan::Angle<double> & tilt)
+{
+  return move_follow_head(pan, tilt, follow_min_tilt);
+}
+
 bool Locomotion::move_follow_head(const keisan::Angle<double> & min_tilt)
 {
   double a_speed = 0.0;
@@ -532,6 +600,47 @@ bool Locomotion::move_follow_head(const keisan::Angle<double> & min_tilt)
   return robot->get_tilt() + robot->tilt_center < min_tilt;
 }
 
+bool Locomotion::move_follow_head(const keisan::Angle<double> & pan, const keisan::Angle<double> & tilt, const keisan::Angle<double> & min_tilt)
+{
+  double a_speed = 0.0;
+  if (pan.degree() < 0.0) {
+    a_speed = keisan::map(pan.degree(), -30.0, follow_pan_ratio * right_kick_target_pan.degree(), -follow_max_a, 0.0);
+  } else {
+    a_speed = keisan::map(pan.degree(), follow_pan_ratio * left_kick_target_pan.degree(), 30.0, 0.0, follow_max_a);
+  }
+
+  double x_speed = 0.0;
+  if (follow_max_a != 0) {
+    x_speed = keisan::map(std::abs(a_speed), 0.0, follow_max_a, follow_max_x, 0.0);
+    x_speed = keisan::map((tilt + robot->tilt_center - min_tilt).degree(), 10.0, 0.0, x_speed, follow_min_x);
+  } else {
+    x_speed = keisan::map((tilt + robot->tilt_center - min_tilt).degree(), 10.0, 0.0, follow_max_x, follow_min_x);
+    const auto max_a_speed = (pan.degree() > 3.0) ? follow_l_a_offset : follow_r_a_offset;
+    a_speed = keisan::map(x_speed, follow_min_x, follow_max_x, 0.0, max_a_speed);
+  }
+
+  double y_speed = 0.0;
+  if (follow_y_move){
+    if (pan.degree() < -3.0) {
+      y_speed = keisan::map(pan.degree(), -15.0, 0.0, follow_max_ry, follow_min_ry);
+    } else if (pan.degree() > 3.0) {
+      y_speed = keisan::map(pan.degree(), 0.0, 15.0, follow_min_ly, follow_max_ly);
+    }
+  }
+
+  double smooth_ratio = 1.0;
+
+  x_speed = keisan::smooth(robot->x_speed, x_speed, smooth_ratio);
+
+  robot->x_speed = x_speed;
+  robot->y_speed = y_speed;
+  robot->a_speed = a_speed;
+  robot->aim_on = false;
+  start();
+
+  return tilt + robot->tilt_center < min_tilt;
+}
+
 bool Locomotion::move_skew(const keisan::Angle<double> & direction)
 {
   auto current_direction = (robot->orientation - robot->pan).normalize();
@@ -691,7 +800,7 @@ bool Locomotion::pivot(const keisan::Angle<double> & direction)
   if (fabs(pan) > pivot_pan_range_a_speed) {
     a_speed = keisan::map(pan, -pivot_pan_range_a_speed, pivot_pan_range_a_speed, pivot_max_a, -pivot_max_a);
   }
-  
+
   robot->x_speed = x_speed;
   robot->y_speed = y_speed;
   robot->a_speed = a_speed;
@@ -832,8 +941,8 @@ bool Locomotion::position_kick_general(const keisan::Angle<double> & direction)
     (left_kick_target_tilt > right_kick_target_tilt ? left_kick_target_tilt : right_kick_target_tilt) + position_min_delta_tilt);
 }
 
-bool Locomotion::position_kick_custom_pan_tilt(const keisan::Angle<double> & direction, const keisan::Angle<double> & min_pan, 
-  const keisan::Angle<double> & max_pan, const keisan::Angle<double> & min_tilt, const keisan::Angle<double> & max_tilt) 
+bool Locomotion::position_kick_custom_pan_tilt(const keisan::Angle<double> & direction, const keisan::Angle<double> & min_pan,
+  const keisan::Angle<double> & max_pan, const keisan::Angle<double> & min_tilt, const keisan::Angle<double> & max_tilt)
 {
   double pan = (robot->get_pan() + robot->pan_center).degree();
   double tilt = (robot->get_tilt() + robot->tilt_center).degree();
@@ -916,7 +1025,7 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle<double> & dire
 
   double delta_pan = (target_pan - pan).degree();
   double y_speed = 0.0;
-  
+
   if (!pan_in_range) {
     if (delta_pan < -position_min_delta_pan.degree()) {
       y_speed = keisan::map(delta_pan, -20.0, -position_min_delta_pan.degree(), position_max_ly, position_min_ly);