diff --git a/packages/localization/CMakeLists.txt b/packages/localization/CMakeLists.txt
new file mode 100644
index 00000000..68e51cef
--- /dev/null
+++ b/packages/localization/CMakeLists.txt
@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.8)
+project(localization)
+
+add_compile_options(-Wall -Wextra -Wpedantic)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geom REQUIRED)
+find_package(common REQUIRED)
+find_package(libpointmatcher REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(rosbag2_cpp REQUIRED)
+find_package(
+  Boost
+  COMPONENTS thread
+             filesystem
+             system
+             program_options
+             date_time
+             chrono
+             REQUIRED
+)
+
+add_executable(
+  ${PROJECT_NAME}_node src/main.cpp src/localization_node.cpp
+                       src/conversion.cpp
+)
+
+ament_target_dependencies(
+  ${PROJECT_NAME}_node
+  rclcpp
+  geom
+  common
+  libpointmatcher
+  tf2_ros
+  tf2_msgs
+  tf2_geometry_msgs
+  sensor_msgs
+  visualization_msgs
+  pcl_conversions
+  rosbag2_cpp
+  Boost
+)
+
+install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME})
+
+target_include_directories(
+  ${PROJECT_NAME}_node
+  PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+         "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
+         "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+
+install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
+
+ament_package()
diff --git a/packages/localization/config/icp.yaml b/packages/localization/config/icp.yaml
new file mode 100644
index 00000000..751963c4
--- /dev/null
+++ b/packages/localization/config/icp.yaml
@@ -0,0 +1,32 @@
+readingDataPointsFilters:
+  - RandomSamplingDataPointsFilter:
+      prob: 0.5
+
+referenceDataPointsFilters:
+  - SamplingSurfaceNormalDataPointsFilter:
+      knn: 30
+
+matcher:
+  KDTreeMatcher:
+    knn: 1
+
+outlierFilters:
+  - TrimmedDistOutlierFilter:
+      ratio: 0.7
+
+errorMinimizer:
+  PointToPlaneErrorMinimizer
+
+transformationCheckers:
+  - CounterTransformationChecker:
+      maxIterationCount: 30
+  - DifferentialTransformationChecker:
+      minDiffRotErr: 0.02
+      minDiffTransErr: 0.02
+      smoothLength: 4
+
+inspector:
+  NullInspector
+
+logger:
+  NullLogger
diff --git a/packages/localization/include/localization/conversion.h b/packages/localization/include/localization/conversion.h
new file mode 100644
index 00000000..f8bc4cda
--- /dev/null
+++ b/packages/localization/include/localization/conversion.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include <pointmatcher/PointMatcher.h>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+using Matcher = PointMatcher<float>;
+using DataPoints = Matcher::DataPoints;
+
+namespace truck::localization {
+
+DataPoints toDataPoints(const sensor_msgs::msg::LaserScan& msg);
+
+DataPoints toDataPoints(const sensor_msgs::msg::PointCloud2& msg);
+
+sensor_msgs::msg::PointCloud2 toPointCloud2(
+    const std_msgs::msg::Header& header, const DataPoints& data_points);
+
+}  // namespace truck::localization
diff --git a/packages/localization/include/localization/localization_node.h b/packages/localization/include/localization/localization_node.h
new file mode 100644
index 00000000..fa72ba50
--- /dev/null
+++ b/packages/localization/include/localization/localization_node.h
@@ -0,0 +1,114 @@
+#pragma once
+
+#include "geom/vector.h"
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <pointmatcher/PointMatcher.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+namespace truck::localization {
+
+using Matcher = PointMatcher<float>;
+using ICP = Matcher::ICP;
+using DataPoints = Matcher::DataPoints;
+
+struct LocalizationParams {
+    std::chrono::duration<double> period;
+    bool verbose;
+    std::string icp_config;
+
+    struct BBoxFilter {
+        bool enable;
+        double radius;
+        double z_min;
+        double z_max;
+    } bbox_filter;
+
+    struct LocalScan {
+        struct Rendering {
+            struct BBoxFiltered {
+                bool enable;
+            } bbox_filtered;
+        } rendering;
+    } local_scan;
+
+    struct GlobalScan {
+        std::string config;
+
+        struct Rendering {
+            struct Main {
+                bool enable;
+                std::chrono::duration<double> period;
+            } main;
+
+            struct BBoxFiltered {
+                bool enable;
+            } bbox_filtered;
+        } rendering;
+    } global_scan;
+};
+
+class LocalizationNode : public rclcpp::Node {
+  public:
+    LocalizationNode();
+
+  private:
+    void initializeParams();
+    void initializeTopicHandlers();
+
+    void loadScanGlobal();
+
+    void onReset(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
+    void onLocalScan(const sensor_msgs::msg::LaserScan::SharedPtr msg);
+
+    void makeLocalizationTick();
+
+    void publishTf();
+    void publishScanGlobal();
+
+    std::optional<tf2::Transform> getLatestTranform(
+        const std::string& source, const std::string& target);
+
+    struct Signals {
+        rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tf = nullptr;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr global_scan = nullptr;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr global_scan_bbox_filtered =
+            nullptr;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr local_scan_bbox_filtered =
+            nullptr;
+    } signals_;
+
+    struct Slots {
+        rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr local_scan = nullptr;
+        rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose = nullptr;
+    } slots_;
+
+    struct Timers {
+        rclcpp::TimerBase::SharedPtr main = nullptr;
+        rclcpp::TimerBase::SharedPtr global_scan = nullptr;
+    } timers_;
+
+    struct Scans {
+        struct Global {
+            DataPoints data_points;
+            sensor_msgs::msg::PointCloud2 point_cloud;
+        } global;
+        sensor_msgs::msg::LaserScan local;
+    } scans_;
+
+    std::unique_ptr<tf2_ros::Buffer> tf_buffer_ = nullptr;
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener_ = nullptr;
+
+    ICP icp_;
+    tf2::Transform tf_world_ekf_;
+
+    LocalizationParams params_;
+};
+
+}  // namespace truck::localization
diff --git a/packages/localization/launch/localization.yaml b/packages/localization/launch/localization.yaml
new file mode 100644
index 00000000..7c1224f8
--- /dev/null
+++ b/packages/localization/launch/localization.yaml
@@ -0,0 +1,43 @@
+launch:
+  - arg: { name: "simulation", default: "false" }
+  - arg: { name: "lidar_map_config", default: "" }
+  - node:
+      pkg: "localization"
+      exec: "localization_node"
+      name: "localization_node"
+      output: "log"
+      param:
+        - { name: "use_sim_time", value: "$(var simulation)" }
+        - { name: "period", value: 0.1 }
+        - { name: "verbose", value: true }
+        - { name: "icp_config", value: "$(find-pkg-share localization)/config/icp.yaml" }
+
+        - name: "bbox_filter"
+          param:
+            - { name: "enable", value: true }
+            - { name: "radius", value: 8.0 }
+            - { name: "z_min", value: 0.0 }
+            - { name: "z_max", value: 2.0 }
+
+        - name: "local_scan"
+          param:
+            - name: "rendering"
+              param:
+                - name: "bbox_filtered"
+                  param:
+                    - { name: "enable", value: true }
+
+        - name: "global_scan"
+          param:
+            - { name: "config", value: "$(var lidar_map_config)" }
+
+            - name: "rendering"
+              param:
+                - name: "main"
+                  param:
+                    - { name: "enable", value: true }
+                    - { name: "period", value: 1.0 }
+
+                - name: "bbox_filtered"
+                  param:
+                    - { name: "enable", value: true }
diff --git a/packages/localization/package.xml b/packages/localization/package.xml
new file mode 100644
index 00000000..938ea15d
--- /dev/null
+++ b/packages/localization/package.xml
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>localization</name>
+  <version>0.0.0</version>
+  <description>localization</description>
+  <maintainer email="apmilko@outlook.com">apmilko</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>geom</depend>
+  <depend>common</depend>
+  <depend>libpointmatcher</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>rosbag2_cpp</depend>
+  <depend>Boost</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/packages/localization/src/conversion.cpp b/packages/localization/src/conversion.cpp
new file mode 100644
index 00000000..f6b9884c
--- /dev/null
+++ b/packages/localization/src/conversion.cpp
@@ -0,0 +1,107 @@
+#include "localization/conversion.h"
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <common/math.h>
+
+namespace truck::localization {
+
+DataPoints toDataPoints(const sensor_msgs::msg::LaserScan& msg) {
+    Limits range_limit{msg.range_min, msg.range_max};
+
+    DataPoints::Labels feature_labels;
+    feature_labels.push_back(DataPoints::Label("x", 1));
+    feature_labels.push_back(DataPoints::Label("y", 1));
+    feature_labels.push_back(DataPoints::Label("w", 1));
+
+    const DataPoints::Labels descriptor_labels;
+
+    auto is_valid = [&](double range) { return std::isfinite(range) && range_limit.isMet(range); };
+
+    const size_t point_count = std::count_if(
+        msg.ranges.begin(), msg.ranges.end(), [&](float range) { return is_valid(range); });
+
+    DataPoints result(feature_labels, descriptor_labels, point_count);
+
+    for (size_t i = 0, j = 0; i < msg.ranges.size(); ++i) {
+        const double range = msg.ranges[i];
+
+        if (!is_valid(range)) {
+            continue;
+        }
+
+        const double angle = msg.angle_min + i * msg.angle_increment;
+
+        result.features(0, j) = range * std::cos(angle);
+        result.features(1, j) = range * std::sin(angle);
+        result.features(2, j) = 1.0f;
+        j++;
+    }
+
+    return result;
+}
+
+DataPoints toDataPoints(const sensor_msgs::msg::PointCloud2& msg) {
+    pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
+    pcl::fromROSMsg(msg, pcl_cloud);
+
+    DataPoints::Labels feature_labels;
+    feature_labels.push_back(DataPoints::Label("x", 1));
+    feature_labels.push_back(DataPoints::Label("y", 1));
+    feature_labels.push_back(DataPoints::Label("w", 1));
+
+    const DataPoints::Labels descriptor_labels;
+
+    DataPoints result(feature_labels, descriptor_labels, pcl_cloud.size());
+
+    for (size_t i = 0; i < pcl_cloud.size(); i++) {
+        result.features(0, i) = pcl_cloud[i].x;
+        result.features(1, i) = pcl_cloud[i].y;
+        result.features(2, i) = pcl_cloud[i].z;
+    }
+
+    return result;
+}
+
+namespace {
+
+constexpr bool isBigendian() { return __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; }
+
+}  // namespace
+
+sensor_msgs::msg::PointCloud2 toPointCloud2(
+    const std_msgs::msg::Header& header, const DataPoints& data_points) {
+    sensor_msgs::msg::PointCloud2 result;
+
+    result.header = header;
+    result.height = 1;
+    result.width = data_points.features.cols();
+    result.fields.resize(3);
+
+    result.fields[0].name = "x";
+    result.fields[0].offset = 0;
+    result.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    result.fields[0].count = 1;
+
+    result.fields[1].name = "y";
+    result.fields[1].offset = 4;
+    result.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    result.fields[1].count = 1;
+
+    result.fields[2].name = "z";
+    result.fields[2].offset = 8;
+    result.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    result.fields[2].count = 1;
+
+    result.point_step = 12;
+    result.row_step = result.point_step * result.width;
+    result.is_bigendian = isBigendian();
+    result.is_dense = true;
+
+    result.data.resize(result.row_step * result.height);
+    std::memcpy(result.data.data(), data_points.features.data(), result.data.size());
+
+    return result;
+}
+
+}  // namespace truck::localization
diff --git a/packages/localization/src/localization_node.cpp b/packages/localization/src/localization_node.cpp
new file mode 100644
index 00000000..08661288
--- /dev/null
+++ b/packages/localization/src/localization_node.cpp
@@ -0,0 +1,294 @@
+#include "localization/localization_node.h"
+
+#include "geom/msg.h"
+#include "localization/conversion.h"
+
+#include <fstream>
+#include <rosbag2_cpp/reader.hpp>
+
+namespace truck::localization {
+
+using namespace std::placeholders;
+
+LocalizationNode::LocalizationNode() : Node("localization") {
+    initializeParams();
+    initializeTopicHandlers();
+    loadScanGlobal();
+
+    std::ifstream icp_config(params_.icp_config);
+    icp_.loadFromYaml(icp_config);
+
+    tf2::fromMsg(geom::msg::toPose({}), tf_world_ekf_);
+}
+
+void LocalizationNode::initializeParams() {
+    params_ = LocalizationParams{
+        .period = std::chrono::duration<double>(this->declare_parameter<double>("period")),
+        .verbose = this->declare_parameter<bool>("verbose"),
+        .icp_config = this->declare_parameter<std::string>("icp_config"),
+
+        .bbox_filter =
+            {.enable = this->declare_parameter<bool>("bbox_filter.enable"),
+             .radius = this->declare_parameter<double>("bbox_filter.radius"),
+             .z_min = this->declare_parameter<double>("bbox_filter.z_min"),
+             .z_max = this->declare_parameter<double>("bbox_filter.z_max")},
+
+        .local_scan =
+            {.rendering =
+                 {.bbox_filtered =
+                      {.enable = this->declare_parameter<bool>(
+                           "local_scan.rendering.bbox_filtered.enable")}}},
+
+        .global_scan = {
+            .config = this->declare_parameter<std::string>("global_scan.config"),
+
+            .rendering = {
+                .main =
+                    {.enable = this->declare_parameter<bool>("global_scan.rendering.main.enable"),
+                     .period = std::chrono::duration<double>(
+                         this->declare_parameter<double>("global_scan.rendering.main.period"))},
+
+                .bbox_filtered = {
+                    .enable = this->declare_parameter<bool>(
+                        "global_scan.rendering.bbox_filtered.enable")}}}};
+}
+
+void LocalizationNode::initializeTopicHandlers() {
+    tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+    const auto qos = static_cast<rmw_qos_reliability_policy_t>(
+        this->declare_parameter<int>("qos", RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT));
+
+    signals_ = Signals{
+        .tf = this->create_publisher<tf2_msgs::msg::TFMessage>(
+            "/tf", rclcpp::QoS(1).reliability(qos)),
+
+        .global_scan = this->create_publisher<sensor_msgs::msg::PointCloud2>(
+            "/scan/global", rclcpp::QoS(1).reliability(qos)),
+
+        .global_scan_bbox_filtered = this->create_publisher<sensor_msgs::msg::PointCloud2>(
+            "/scan/global/bbox_filtered", rclcpp::QoS(1).reliability(qos)),
+
+        .local_scan_bbox_filtered = this->create_publisher<sensor_msgs::msg::PointCloud2>(
+            "/scan/local/bbox_filtered", rclcpp::QoS(1).reliability(qos))};
+
+    slots_ = Slots{
+        .local_scan = this->create_subscription<sensor_msgs::msg::LaserScan>(
+            "/lidar/scan",
+            rclcpp::QoS(1).reliability(qos),
+            std::bind(&LocalizationNode::onLocalScan, this, _1)),
+
+        .pose = this->create_subscription<geometry_msgs::msg::PoseStamped>(
+            "/move_base_simple/goal",
+            rclcpp::QoS(1).reliability(qos),
+            std::bind(&LocalizationNode::onReset, this, _1))};
+
+    timers_ = Timers{
+        .main = this->create_wall_timer(
+            params_.period, std::bind(&LocalizationNode::makeLocalizationTick, this)),
+        .global_scan = this->create_wall_timer(
+            params_.global_scan.rendering.main.period,
+            std::bind(&LocalizationNode::publishScanGlobal, this))};
+}
+
+std::optional<tf2::Transform> LocalizationNode::getLatestTranform(
+    const std::string& source, const std::string& target) {
+    try {
+        const auto tf_msg = tf_buffer_->lookupTransform(target, source, tf2::TimePointZero);
+        tf2::Transform tf;
+        tf2::fromMsg(tf_msg.transform, tf);
+        return tf;
+    } catch (const tf2::TransformException& ex) {
+        RCLCPP_ERROR(
+            this->get_logger(), "No transform from '%s' to '%s'!", source.c_str(), target.c_str());
+        return std::nullopt;
+    }
+}
+
+void LocalizationNode::loadScanGlobal() {
+    rosbag2_cpp::Reader reader = rosbag2_cpp::Reader();
+    reader.open(params_.global_scan.config);
+
+    if (!reader.has_next()) {
+        RCLCPP_ERROR(
+            this->get_logger(),
+            "Corrupted bag '%s', stop parsing!",
+            params_.global_scan.config.c_str());
+        return;
+    }
+
+    const auto point_cloud = reader.read_next<sensor_msgs::msg::PointCloud2>();
+
+    RCLCPP_INFO(
+        this->get_logger(), "Bag '%s' was succesfully parsed", params_.global_scan.config.c_str());
+
+    DataPoints data_points = toDataPoints(point_cloud);
+    data_points.features.row(2).setZero();
+
+    scans_.global = Scans::Global{
+        .data_points = toDataPoints(point_cloud),
+        .point_cloud = toPointCloud2(point_cloud.header, data_points)};
+}
+
+void LocalizationNode::onReset(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
+    const auto tf_ekf_base = getLatestTranform("base", "odom_ekf");
+
+    if (!tf_ekf_base.has_value()) {
+        return;
+    }
+
+    tf2::Transform tf_world_base;
+    tf2::fromMsg(msg->pose, tf_world_base);
+
+    tf_world_ekf_ = tf_world_base * tf_ekf_base->inverse();
+    RCLCPP_INFO(this->get_logger(), "Update pose estimation");
+}
+
+void LocalizationNode::onLocalScan(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
+    scans_.local = *msg;
+}
+
+namespace {
+
+Eigen::Matrix3f transformationMatrix(const geom::Pose& pose) {
+    const double dtheta = pose.dir.angle().radians();
+    const double cos_dtheta = std::cos(dtheta);
+    const double sin_dtheta = std::sin(dtheta);
+
+    Eigen::Matrix3f tf_matrix = Eigen::Matrix3f::Identity();
+
+    // Rotation
+    tf_matrix(0, 0) = cos_dtheta;
+    tf_matrix(0, 1) = -1.0 * sin_dtheta;
+    tf_matrix(1, 0) = sin_dtheta;
+    tf_matrix(1, 1) = cos_dtheta;
+
+    // Translation
+    tf_matrix(0, 2) = pose.pos.x;
+    tf_matrix(1, 2) = pose.pos.y;
+
+    return tf_matrix;
+}
+
+geom::Pose toPose(const Eigen::Matrix3f& tf_matrix) {
+    const double tx = tf_matrix(0, 2);
+    const double ty = tf_matrix(1, 2);
+    const double dtheta = std::atan2(tf_matrix(1, 0), tf_matrix(0, 0));
+    return {geom::Vec2(tx, ty), geom::AngleVec2::fromRadians(dtheta)};
+}
+
+DataPoints transformDataPoints(const DataPoints& data_points, const tf2::Transform& tf) {
+    DataPoints data_points_tf = data_points;
+
+    geometry_msgs::msg::Pose tf_pose;
+    tf2::toMsg(tf, tf_pose);
+
+    const Eigen::Matrix3f tf_matrix = transformationMatrix(geom::toPose(tf_pose));
+    data_points_tf.features = tf_matrix * data_points_tf.features;
+
+    return data_points_tf;
+}
+
+}  // namespace
+
+void LocalizationNode::makeLocalizationTick() {
+    const auto tf_ekf_base = getLatestTranform("base", "odom_ekf");
+    const auto tf_base_lidar_link = getLatestTranform("lidar_link", "base");
+
+    if (!tf_ekf_base.has_value() || !tf_base_lidar_link.has_value()) {
+        return;
+    }
+
+    const auto tf_world_lidar_link =
+        tf_world_ekf_ * tf_ekf_base.value() * tf_base_lidar_link.value();
+    DataPoints local_scan_tf = transformDataPoints(toDataPoints(scans_.local), tf_world_lidar_link);
+
+    DataPoints global_scan_tf = scans_.global.data_points;
+
+    if (params_.bbox_filter.enable) {
+        const geom::Vec2 ego = geom::Transform(tf_world_ekf_ * tf_ekf_base.value()).t();
+
+        PointMatcherSupport::Parametrizable::Parameters bbox_filter_params = {
+            {"xMin", std::to_string(ego.x - params_.bbox_filter.radius)},
+            {"xMax", std::to_string(ego.x + params_.bbox_filter.radius)},
+            {"yMin", std::to_string(ego.y - params_.bbox_filter.radius)},
+            {"yMax", std::to_string(ego.y + params_.bbox_filter.radius)},
+
+            {"zMin", std::to_string(params_.bbox_filter.z_min)},
+            {"zMax", std::to_string(params_.bbox_filter.z_max)},
+
+            {"removeInside", std::to_string(false)}};
+
+        std::shared_ptr<Matcher::DataPointsFilter> bbox_filter =
+            Matcher::get().DataPointsFilterRegistrar.create(
+                "BoundingBoxDataPointsFilter", bbox_filter_params);
+
+        local_scan_tf = bbox_filter->filter(local_scan_tf);
+        global_scan_tf = bbox_filter->filter(global_scan_tf);
+    }
+
+    try {
+        const auto tf_icp_matrix = icp_(local_scan_tf, global_scan_tf);
+        const geom::Pose tf_icp_pose = toPose(tf_icp_matrix);
+
+        if (params_.verbose) {
+            RCLCPP_INFO(
+                this->get_logger(),
+                "Localization correction: (%.3f, %.3f, %.3f)",
+                tf_icp_pose.pos.x,
+                tf_icp_pose.pos.y,
+                tf_icp_pose.dir.angle().radians());
+        }
+
+        geometry_msgs::msg::Pose tf_world_ekf_geom_msg;
+        tf2::toMsg(tf_world_ekf_, tf_world_ekf_geom_msg);
+
+        geom::Pose tf_world_ekf_geom = geom::toPose(tf_world_ekf_geom_msg);
+        tf_world_ekf_geom.pos += tf_icp_pose.pos;
+        tf_world_ekf_geom.dir += tf_icp_pose.dir;
+
+        tf2::fromMsg(geom::msg::toPose(tf_world_ekf_geom), tf_world_ekf_);
+    } catch (const std::exception& e) {
+        RCLCPP_ERROR(this->get_logger(), "Localization error, update pose estimate!");
+    }
+
+    publishTf();
+
+    {
+        std_msgs::msg::Header header;
+        header.frame_id = "world";
+        header.stamp = this->now();
+
+        if (params_.global_scan.rendering.bbox_filtered.enable) {
+            global_scan_tf.features.row(2).setZero();
+            signals_.global_scan_bbox_filtered->publish(toPointCloud2(header, global_scan_tf));
+        }
+
+        if (params_.local_scan.rendering.bbox_filtered.enable) {
+            local_scan_tf.features.row(2).setZero();
+            signals_.local_scan_bbox_filtered->publish(toPointCloud2(header, local_scan_tf));
+        }
+    }
+}
+
+void LocalizationNode::publishTf() {
+    geometry_msgs::msg::TransformStamped tf_stamped;
+    tf_stamped.header.frame_id = "world";
+    tf_stamped.child_frame_id = "odom_ekf";
+    tf_stamped.header.stamp = now();
+    tf2::toMsg(tf_world_ekf_, tf_stamped.transform);
+
+    tf2_msgs::msg::TFMessage tf_msg;
+    tf_msg.transforms.push_back(tf_stamped);
+    signals_.tf->publish(tf_msg);
+}
+
+void LocalizationNode::publishScanGlobal() {
+    if (params_.global_scan.rendering.main.enable) {
+        scans_.global.point_cloud.header.stamp = this->now();
+        signals_.global_scan->publish(scans_.global.point_cloud);
+    }
+}
+
+}  // namespace truck::localization
diff --git a/packages/localization/src/main.cpp b/packages/localization/src/main.cpp
new file mode 100644
index 00000000..72e10657
--- /dev/null
+++ b/packages/localization/src/main.cpp
@@ -0,0 +1,10 @@
+#include "localization/localization_node.h"
+
+#include <rclcpp/rclcpp.hpp>
+
+int main(int argc, char* argv[]) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<truck::localization::LocalizationNode>());
+    rclcpp::shutdown();
+    return 0;
+}
diff --git a/packages/simulator_2d/src/simulator_node.cpp b/packages/simulator_2d/src/simulator_node.cpp
index bfaccd6d..a412f04c 100644
--- a/packages/simulator_2d/src/simulator_node.cpp
+++ b/packages/simulator_2d/src/simulator_node.cpp
@@ -182,30 +182,6 @@ void SimulatorNode::publishHardwareOdometryMessage(const TruckState& truck_state
     signals_.hardware_odometry->publish(odom_msg);
 }
 
-void SimulatorNode::publishTransformMessage(const TruckState& truck_state) {
-    if (!transforms_.ekf_base.has_value()) {
-        return;
-    }
-
-    const tf2::Transform& tf_ekf_base = *transforms_.ekf_base;
-    const auto pose = truck_state.odomBasePose();
-
-    tf2::Transform tf_world_base;
-    tf2::fromMsg(geom::msg::toPose(pose), tf_world_base);
-
-    const tf2::Transform tf_world_ekf = tf_world_base * tf_ekf_base.inverse();
-
-    geometry_msgs::msg::TransformStamped tf_msg_world_ekf;
-    tf_msg_world_ekf.header.frame_id = "world";
-    tf_msg_world_ekf.child_frame_id = "odom_ekf";
-    tf_msg_world_ekf.header.stamp = truck_state.time();
-    tf2::toMsg(tf_world_ekf, tf_msg_world_ekf.transform);
-
-    tf2_msgs::msg::TFMessage tf_msg;
-    tf_msg.transforms.push_back(tf_msg_world_ekf);
-    signals_.tf_publisher->publish(tf_msg);
-}
-
 void SimulatorNode::publishTelemetryMessage(const TruckState& truck_state) {
     truck_msgs::msg::HardwareTelemetry telemetry_msg;
     telemetry_msg.header.frame_id = "base";
@@ -298,7 +274,6 @@ void SimulatorNode::publishSimulationState() {
     publishTime(truck_state);
     publishSimulatorLocalizationMessage(truck_state);
     publishHardwareOdometryMessage(truck_state);
-    publishTransformMessage(truck_state);
     publishTelemetryMessage(truck_state);
     publishSimulationStateMessage(truck_state);
     publishLaserScanMessage(truck_state);
diff --git a/packages/simulator_2d/src/simulator_node.h b/packages/simulator_2d/src/simulator_node.h
index e762359a..14097768 100644
--- a/packages/simulator_2d/src/simulator_node.h
+++ b/packages/simulator_2d/src/simulator_node.h
@@ -35,7 +35,6 @@ class SimulatorNode : public rclcpp::Node {
     void publishTime(const TruckState& truck_state);
     void publishSimulatorLocalizationMessage(const TruckState& truck_state);
     void publishHardwareOdometryMessage(const TruckState& truck_state);
-    void publishTransformMessage(const TruckState& truck_state);
     void publishTelemetryMessage(const TruckState& truck_state);
     void publishSimulationStateMessage(const TruckState& truck_state);
     void publishLaserScanMessage(const TruckState& truck_state);
diff --git a/packages/truck/launch/common.yaml b/packages/truck/launch/common.yaml
index 07456470..a8740f2c 100644
--- a/packages/truck/launch/common.yaml
+++ b/packages/truck/launch/common.yaml
@@ -2,6 +2,7 @@ launch:
 - arg: { name: "simulation", default: "false" }
 - arg: { name: "qos", default: "0" }
 - arg: { name: "map_config", default: "$(find-pkg-share map)/data/map_6.geojson" }
+- arg: { name: "lidar_map_config", default: "/truck/packages/lidar_map/data/map_6.mcap" }
 
 - include:
     file: $(find-pkg-share model)/launch/model_tf.yaml
@@ -33,6 +34,12 @@ launch:
     - { name: "simulation", value: "$(var simulation)" }
     - { name: "qos", value: "$(var qos)" }
 
+- include:
+   file: $(find-pkg-share localization)/launch/localization.yaml
+   arg:
+    - { name: "simulation", value: "$(var simulation)" }
+    - { name: "lidar_map_config", value: "$(var lidar_map_config)" }
+
 - include:
     file: $(find-pkg-share occupancy_grid)/launch/occupancy_grid.yaml
     arg: