diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/ball_kick_area.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/ball_kick_area.py
index 3aa9147b8..54d02574a 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/ball_kick_area.py
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/ball_kick_area.py
@@ -25,6 +25,9 @@ def perform(self, reevaluate=False):
         ball_position = self.blackboard.world_model.get_ball_position_uv()
 
         self.publish_debug_data("ball_position", {"u": ball_position[0], "v": ball_position[1]})
+        self.publish_debug_data(
+            "smoothing_close", f"{self.no_near_decisions} ({self.no_near_decisions / self.smoothing * 10}%)"
+        )
 
         # Check if the ball is in the enter area
         if 0 <= ball_position[0] <= self.kick_x_enter and 0 <= abs(ball_position[1]) <= self.kick_y_enter:
diff --git a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml
index 4b2e179a5..cd65c1175 100644
--- a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml
+++ b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml
@@ -146,7 +146,7 @@ body_behavior:
     dribble_ball_distance_threshold: 0.5
     dribble_kick_angle: 0.6
 
-    kick_decision_smoothing: 5.0
+    kick_decision_smoothing: 1.0
 
     ##################
     # costmap params #
diff --git a/bitbots_navigation/bitbots_localization/config/config.yaml b/bitbots_navigation/bitbots_localization/config/config.yaml
index 22d0e5b52..13e135e94 100644
--- a/bitbots_navigation/bitbots_localization/config/config.yaml
+++ b/bitbots_navigation/bitbots_localization/config/config.yaml
@@ -3,15 +3,11 @@ bitbots_localization:
     misc:
       init_mode: 0
       percentage_best_particles: 100
-      min_motion_linear: 0.0
-      min_motion_angular: 0.0
       max_motion_linear: 0.5
       max_motion_angular: 3.14
-      filter_only_with_motion: false
     ros:
       line_pointcloud_topic: 'line_mask_relative_pc'
       goal_topic: 'goals_simulated'
-      fieldboundary_topic: 'field_boundary_relative'
       particle_publishing_topic: 'pose_particles'
       debug_visualization: true
     particle_filter:
@@ -46,10 +42,6 @@ bitbots_localization:
         goal:
           factor: 0.0
           out_of_field_score: 0.0
-        field_boundary:
-          factor: 0.0
-          out_of_field_score: 0.0
       confidences:
         line_element: 0.01
         goal_element: 0.0
-        field_boundary_element: 0.0
diff --git a/bitbots_navigation/bitbots_localization/include/bitbots_localization/ObservationModel.hpp b/bitbots_navigation/bitbots_localization/include/bitbots_localization/ObservationModel.hpp
index 2f50bc044..79065d1ff 100644
--- a/bitbots_navigation/bitbots_localization/include/bitbots_localization/ObservationModel.hpp
+++ b/bitbots_navigation/bitbots_localization/include/bitbots_localization/ObservationModel.hpp
@@ -6,17 +6,15 @@
 #define BITBOTS_LOCALIZATION_OBSERVATIONMODEL_H
 
 #include <particle_filter/ParticleFilter.h>
+#include <tf2_ros/buffer.h>
 
 #include <bitbots_localization/RobotState.hpp>
 #include <bitbots_localization/map.hpp>
 #include <bitbots_localization/tools.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
-#include <soccer_vision_3d_msgs/msg/field_boundary.hpp>
 #include <soccer_vision_3d_msgs/msg/goalpost.hpp>
 #include <soccer_vision_3d_msgs/msg/goalpost_array.hpp>
-#include <soccer_vision_3d_msgs/msg/marking_array.hpp>
-#include <soccer_vision_3d_msgs/msg/marking_intersection.hpp>
 
 #include "localization_parameters.hpp"
 
@@ -31,8 +29,7 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel<Robot
    * empty
    */
   RobotPoseObservationModel(std::shared_ptr<Map> map_lines, std::shared_ptr<Map> map_goals,
-                            std::shared_ptr<Map> map_field_boundary, const bitbots_localization::Params &config,
-                            const FieldDimensions &field_dimensions);
+                            const bitbots_localization::Params &config, const FieldDimensions &field_dimensions);
 
   /**
    *
@@ -45,15 +42,9 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel<Robot
 
   void set_measurement_goalposts(sv3dm::msg::GoalpostArray measurement);
 
-  void set_measurement_field_boundary(sv3dm::msg::FieldBoundary measurement);
+  const std::vector<std::pair<double, double>> get_measurement_lines() const;
 
-  void set_measurement_markings(sv3dm::msg::MarkingArray measurement);
-
-  std::vector<std::pair<double, double>> get_measurement_lines() const;
-
-  std::vector<std::pair<double, double>> get_measurement_goals() const;
-
-  std::vector<std::pair<double, double>> get_measurement_field_boundary() const;
+  const std::vector<std::pair<double, double>> get_measurement_goals() const;
 
   double get_min_weight() const override;
 
@@ -61,20 +52,26 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel<Robot
 
   bool measurements_available() override;
 
+  void set_movement_since_line_measurement(const tf2::Transform movement);
+  void set_movement_since_goal_measurement(const tf2::Transform movement);
+
  private:
   double calculate_weight_for_class(const RobotState &state,
                                     const std::vector<std::pair<double, double>> &last_measurement,
-                                    std::shared_ptr<Map> map, double element_weight) const;
+                                    std::shared_ptr<Map> map, double element_weight,
+                                    const tf2::Transform &movement_since_measurement) const;
 
   // Measurements
   std::vector<std::pair<double, double>> last_measurement_lines_;
   std::vector<std::pair<double, double>> last_measurement_goal_;
-  std::vector<std::pair<double, double>> last_measurement_field_boundary_;
+
+  // Movement since last measurement
+  tf2::Transform movement_since_line_measurement_ = tf2::Transform::getIdentity();
+  tf2::Transform movement_since_goal_measurement_ = tf2::Transform::getIdentity();
 
   // Reference to the maps for the different classes
   std::shared_ptr<Map> map_lines_;
   std::shared_ptr<Map> map_goals_;
-  std::shared_ptr<Map> map_field_boundary_;
 
   // Parameters
   bitbots_localization::Params config_;
diff --git a/bitbots_navigation/bitbots_localization/include/bitbots_localization/RobotState.hpp b/bitbots_navigation/bitbots_localization/include/bitbots_localization/RobotState.hpp
index aaab084fb..3104ccc76 100644
--- a/bitbots_navigation/bitbots_localization/include/bitbots_localization/RobotState.hpp
+++ b/bitbots_navigation/bitbots_localization/include/bitbots_localization/RobotState.hpp
@@ -7,6 +7,8 @@
 
 #include <particle_filter/ParticleFilter.h>
 #include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/utils.h>
 
 #include <Eigen/Core>
 #include <bitbots_localization/tools.hpp>
@@ -25,10 +27,17 @@ class RobotState {
   /**
    * @param x Position of the robot.
    * @param y Position of the robot.
-   * @param T Orientaion of the robot in radians.
+   * @param T Orientation of the robot in radians.
    */
   RobotState(double x, double y, double T);
 
+  /**
+   * @brief Constructor for a robot state based on a tf2::Transform.
+   *
+   * @param transform Transform of the robots base_footprint in the map frame.
+   */
+  explicit RobotState(tf2::Transform transform);
+
   RobotState operator*(float factor) const;
 
   RobotState &operator+=(const RobotState &other);
@@ -63,6 +72,8 @@ class RobotState {
   visualization_msgs::msg::Marker renderMarker(std::string n_space, std::string frame, rclcpp::Duration lifetime,
                                                std_msgs::msg::ColorRGBA color, rclcpp::Time stamp) const;
 
+  tf2::Transform getTransform() const;
+
  private:
   double m_XPos;
   double m_YPos;
diff --git a/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp b/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp
index ee996992b..2519bf53c 100644
--- a/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp
+++ b/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp
@@ -47,7 +47,6 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <soccer_vision_3d_msgs/msg/field_boundary.hpp>
 #include <soccer_vision_3d_msgs/msg/goalpost_array.hpp>
 #include <std_msgs/msg/color_rgba.hpp>
 #include <std_srvs/srv/trigger.hpp>
@@ -110,12 +109,6 @@ class Localization {
    */
   void GoalPostsCallback(const sv3dm::msg::GoalpostArray &msg);  // TODO
 
-  /**
-   * Callback for the relative field boundary measurements
-   * @param msg Message containing the field boundary points.
-   */
-  void FieldboundaryCallback(const sv3dm::msg::FieldBoundary &msg);
-
   /**
    * Resets the state distribution of the state space
    * @param distribution The type of the distribution
@@ -158,17 +151,13 @@ class Localization {
   // Declare subscribers
   rclcpp::Subscription<sm::msg::PointCloud2>::SharedPtr line_point_cloud_subscriber_;
   rclcpp::Subscription<sv3dm::msg::GoalpostArray>::SharedPtr goal_subscriber_;
-  rclcpp::Subscription<sv3dm::msg::FieldBoundary>::SharedPtr fieldboundary_subscriber_;
-
   rclcpp::Subscription<gm::msg::PoseWithCovarianceStamped>::SharedPtr rviz_initial_pose_subscriber_;
 
   // Declare publishers
   rclcpp::Publisher<gm::msg::PoseWithCovarianceStamped>::SharedPtr pose_with_covariance_publisher_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pose_particles_publisher_;
-  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr lines_publisher_;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr line_ratings_publisher_;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr goal_ratings_publisher_;
-  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr fieldboundary_ratings_publisher_;
   rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr field_publisher_;
 
   // Declare services
@@ -179,8 +168,8 @@ class Localization {
   rclcpp::TimerBase::SharedPtr publishing_timer_;
 
   // Declare tf2 objects
-  std::unique_ptr<tf2_ros::Buffer> tfBuffer;
-  std::shared_ptr<tf2_ros::TransformListener> tfListener;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
   std::shared_ptr<tf2_ros::TransformBroadcaster> br;
 
   // Declare particle filter components
@@ -201,7 +190,6 @@ class Localization {
   // Declare input message buffers
   sm::msg::PointCloud2 line_pointcloud_relative_;
   sv3dm::msg::GoalpostArray goal_posts_relative_;
-  sv3dm::msg::FieldBoundary fieldboundary_relative_;
 
   // Declare time stamps
   rclcpp::Time last_stamp_lines = rclcpp::Time(0);
@@ -219,16 +207,12 @@ class Localization {
   // Keep track of the odometry transform in the last step
   geometry_msgs::msg::TransformStamped previousOdomTransform_;
 
-  // Flag that checks if the robot is moving
-  bool robot_moved = false;
-
   // Keep track of the number of filter steps
   int timer_callback_count_ = 0;
 
   // Maps for the different measurement classes
   std::shared_ptr<Map> lines_;
   std::shared_ptr<Map> goals_;
-  std::shared_ptr<Map> field_boundary_;
 
   // RNG that is used for the different sampling steps
   particle_filter::CRandomNumberGenerator random_number_generator_;
@@ -271,8 +255,8 @@ class Localization {
    * @param map map for this class
    * @param publisher ros publisher for the type visualization_msgs::msg::Marker
    */
-  void publish_debug_rating(std::vector<std::pair<double, double>> measurements, double scale, const char name[24],
-                            std::shared_ptr<Map> map,
+  void publish_debug_rating(const std::vector<std::pair<double, double>> &measurements, double scale,
+                            const char name[24], std::shared_ptr<Map> map,
                             rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr &publisher);
 
   /**
diff --git a/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp b/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp
index 5acb8e9a5..b451634a5 100644
--- a/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp
+++ b/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp
@@ -45,8 +45,8 @@ class Map {
 
   double get_occupancy(double x, double y);
 
-  std::pair<double, double> observationRelative(std::pair<double, double> observation, double stateX, double stateY,
-                                                double stateT);
+  std::pair<double, double> getObservationCoordinatesInMapFrame(std::pair<double, double> observation, double stateX,
+                                                                double stateY, double stateT);
 
   nav_msgs::msg::OccupancyGrid get_map_msg(std::string frame_id, int threshold = -1);
 
diff --git a/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp b/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp
index 4f68664d0..d3f4c4fa5 100644
--- a/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp
+++ b/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp
@@ -7,13 +7,11 @@
 namespace bitbots_localization {
 
 RobotPoseObservationModel::RobotPoseObservationModel(std::shared_ptr<Map> map_lines, std::shared_ptr<Map> map_goals,
-                                                     std::shared_ptr<Map> map_field_boundary,
                                                      const bitbots_localization::Params &config,
                                                      const FieldDimensions &field_dimensions)
     : particle_filter::ObservationModel<RobotState>(),
       map_lines_(map_lines),
       map_goals_(map_goals),
-      map_field_boundary_(map_field_boundary),
       config_(config),
       field_dimensions_(field_dimensions) {
   particle_filter::ObservationModel<RobotState>::accumulate_weights_ = true;
@@ -21,10 +19,13 @@ RobotPoseObservationModel::RobotPoseObservationModel(std::shared_ptr<Map> map_li
 
 double RobotPoseObservationModel::calculate_weight_for_class(
     const RobotState &state, const std::vector<std::pair<double, double>> &last_measurement, std::shared_ptr<Map> map,
-    double element_weight) const {
+    double element_weight, const tf2::Transform &movement_since_measurement) const {
   double particle_weight_for_class;
   if (!last_measurement.empty()) {
-    std::vector<double> ratings = map->Map::provideRating(state, last_measurement);
+    // Subtract (reverse) the movement from our state to get the hypothetical state at the time of the measurement
+    const RobotState state_at_measurement(state.getTransform() * movement_since_measurement.inverse());
+    // Get the ratings for for all points in the measurement based on the map
+    const std::vector<double> ratings = map->Map::provideRating(state_at_measurement, last_measurement);
     // Take the average of the ratings (functional)
     particle_weight_for_class = std::pow(std::accumulate(ratings.begin(), ratings.end(), 0.0) / ratings.size(), 2);
   } else {
@@ -34,22 +35,19 @@ double RobotPoseObservationModel::calculate_weight_for_class(
 }
 
 double RobotPoseObservationModel::measure(const RobotState &state) const {
-  double particle_weight_lines = calculate_weight_for_class(state, last_measurement_lines_, map_lines_,
-                                                            config_.particle_filter.confidences.line_element);
-  double particle_weight_goal = calculate_weight_for_class(state, last_measurement_goal_, map_goals_,
-                                                           config_.particle_filter.confidences.goal_element);
-  double particle_weight_field_boundary =
-      calculate_weight_for_class(state, last_measurement_field_boundary_, map_field_boundary_,
-                                 config_.particle_filter.confidences.field_boundary_element);
+  double particle_weight_lines =
+      calculate_weight_for_class(state, last_measurement_lines_, map_lines_,
+                                 config_.particle_filter.confidences.line_element, movement_since_line_measurement_);
+  double particle_weight_goal =
+      calculate_weight_for_class(state, last_measurement_goal_, map_goals_,
+                                 config_.particle_filter.confidences.goal_element, movement_since_goal_measurement_);
 
   // Get relevant config values
   auto scoring_config = config_.particle_filter.scoring;
 
   // Calculate weight for the particle
   double weight = (((1 - scoring_config.lines.factor) + scoring_config.lines.factor * particle_weight_lines) *
-                   ((1 - scoring_config.goal.factor) + scoring_config.goal.factor * particle_weight_goal) *
-                   ((1 - scoring_config.field_boundary.factor) +
-                    scoring_config.field_boundary.factor * particle_weight_field_boundary));
+                   ((1 - scoring_config.goal.factor) + scoring_config.goal.factor * particle_weight_goal));
 
   if (weight < config_.particle_filter.weighting.min_weight) {
     weight = config_.particle_filter.weighting.min_weight;
@@ -68,6 +66,7 @@ double RobotPoseObservationModel::measure(const RobotState &state) const {
 }
 
 void RobotPoseObservationModel::set_measurement_lines_pc(sm::msg::PointCloud2 measurement) {
+  // convert to polar
   for (sm::PointCloud2ConstIterator<float> iter_xyz(measurement, "x"); iter_xyz != iter_xyz.end(); ++iter_xyz) {
     std::pair<double, double> linePolar = cartesianToPolar(iter_xyz[0], iter_xyz[1]);
     last_measurement_lines_.push_back(linePolar);
@@ -82,39 +81,33 @@ void RobotPoseObservationModel::set_measurement_goalposts(sv3dm::msg::GoalpostAr
   }
 }
 
-void RobotPoseObservationModel::set_measurement_field_boundary(sv3dm::msg::FieldBoundary measurement) {
-  // convert to polar
-  for (gm::msg::Point &point : measurement.points) {
-    std::pair<double, double> fieldBoundaryPointPolar = cartesianToPolar(point.x, point.y);
-    last_measurement_field_boundary_.push_back(fieldBoundaryPointPolar);
-  }
-}
-
-std::vector<std::pair<double, double>> RobotPoseObservationModel::get_measurement_lines() const {
+const std::vector<std::pair<double, double>> RobotPoseObservationModel::get_measurement_lines() const {
   return last_measurement_lines_;
 }
 
-std::vector<std::pair<double, double>> RobotPoseObservationModel::get_measurement_goals() const {
+const std::vector<std::pair<double, double>> RobotPoseObservationModel::get_measurement_goals() const {
   return last_measurement_goal_;
 }
 
-std::vector<std::pair<double, double>> RobotPoseObservationModel::get_measurement_field_boundary() const {
-  return last_measurement_field_boundary_;
-}
-
 double RobotPoseObservationModel::get_min_weight() const { return config_.particle_filter.weighting.min_weight; }
 
 void RobotPoseObservationModel::clear_measurement() {
   last_measurement_lines_.clear();
   last_measurement_goal_.clear();
-  last_measurement_field_boundary_.clear();
 }
 
 bool RobotPoseObservationModel::measurements_available() {
   bool available = false;
   available |= !last_measurement_lines_.empty();
   available |= !last_measurement_goal_.empty();
-  available |= !last_measurement_field_boundary_.empty();
   return available;
 }
+
+void RobotPoseObservationModel::set_movement_since_line_measurement(const tf2::Transform movement) {
+  movement_since_line_measurement_ = movement;
+}
+
+void RobotPoseObservationModel::set_movement_since_goal_measurement(const tf2::Transform movement) {
+  movement_since_goal_measurement_ = movement;
+}
 }  // namespace bitbots_localization
diff --git a/bitbots_navigation/bitbots_localization/src/RobotState.cpp b/bitbots_navigation/bitbots_localization/src/RobotState.cpp
index a8db34214..5cc5a9186 100644
--- a/bitbots_navigation/bitbots_localization/src/RobotState.cpp
+++ b/bitbots_navigation/bitbots_localization/src/RobotState.cpp
@@ -11,6 +11,11 @@ RobotState::RobotState() : is_explorer_(false), m_XPos(0.0), m_YPos(0.0), m_SinT
 RobotState::RobotState(double x, double y, double T)
     : is_explorer_(false), m_XPos(x), m_YPos(y), m_SinTheta(sin(T)), m_CosTheta(cos(T)) {}
 
+RobotState::RobotState(tf2::Transform transform)
+    : is_explorer_(false), m_XPos(transform.getOrigin().x()), m_YPos(transform.getOrigin().y()) {
+  setTheta(tf2::getYaw(transform.getRotation()));
+}
+
 RobotState RobotState::operator*(float factor) const {
   RobotState newState;
   newState.m_XPos = m_XPos * factor;
@@ -119,4 +124,14 @@ visualization_msgs::msg::Marker RobotState::renderMarker(std::string n_space, st
 
   return msg;
 }
+
+tf2::Transform RobotState::getTransform() const {
+  tf2::Transform transform;
+  transform.setOrigin(tf2::Vector3(getXPos(), getYPos(), 0));
+  tf2::Quaternion q;
+  q.setRPY(0, 0, getTheta());
+  transform.setRotation(q);
+  return transform;
+}
+
 }  // namespace bitbots_localization
diff --git a/bitbots_navigation/bitbots_localization/src/localization.cpp b/bitbots_navigation/bitbots_localization/src/localization.cpp
index 9a9ac7884..708fdf0d4 100644
--- a/bitbots_navigation/bitbots_localization/src/localization.cpp
+++ b/bitbots_navigation/bitbots_localization/src/localization.cpp
@@ -9,16 +9,16 @@ Localization::Localization(std::shared_ptr<rclcpp::Node> node)
     : node_(node),
       param_listener_(node->get_node_parameters_interface()),
       config_(param_listener_.get_params()),
-      tfBuffer(std::make_unique<tf2_ros::Buffer>(node->get_clock())),
-      tfListener(std::make_shared<tf2_ros::TransformListener>(*tfBuffer, node)),
+      tf_buffer_(std::make_shared<tf2_ros::Buffer>(node->get_clock())),
+      tf_listener_(std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, node)),
       br(std::make_shared<tf2_ros::TransformBroadcaster>(node)) {
   // Wait for transforms to become available and init them
-  bitbots_utils::wait_for_tf(node->get_logger(), node->get_clock(), tfBuffer.get(), {config_.ros.base_footprint_frame},
-                             config_.ros.odom_frame);
+  bitbots_utils::wait_for_tf(node->get_logger(), node->get_clock(), tf_buffer_.get(),
+                             {config_.ros.base_footprint_frame}, config_.ros.odom_frame);
 
   // Get the initial transform from odom to base_footprint
-  previousOdomTransform_ = tfBuffer->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame,
-                                                     rclcpp::Time(0), rclcpp::Duration::from_nanoseconds(1e9 * 20.0));
+  previousOdomTransform_ = tf_buffer_->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame,
+                                                       rclcpp::Time(0), rclcpp::Duration::from_nanoseconds(1e9 * 20.0));
 
   // Init subscribers
   line_point_cloud_subscriber_ = node->create_subscription<sm::msg::PointCloud2>(
@@ -27,9 +27,6 @@ Localization::Localization(std::shared_ptr<rclcpp::Node> node)
   goal_subscriber_ = node->create_subscription<sv3dm::msg::GoalpostArray>(
       config_.ros.goal_topic, 1, std::bind(&Localization::GoalPostsCallback, this, _1));
 
-  fieldboundary_subscriber_ = node->create_subscription<sv3dm::msg::FieldBoundary>(
-      config_.ros.fieldboundary_topic, 1, std::bind(&Localization::FieldboundaryCallback, this, _1));
-
   rviz_initial_pose_subscriber_ = node->create_subscription<gm::msg::PoseWithCovarianceStamped>(
       "initialpose", 1, std::bind(&Localization::SetInitialPositionCallback, this, _1));
 
@@ -38,18 +35,14 @@ Localization::Localization(std::shared_ptr<rclcpp::Node> node)
       node->create_publisher<visualization_msgs::msg::MarkerArray>(config_.ros.particle_publishing_topic, 1);
   pose_with_covariance_publisher_ =
       node->create_publisher<gm::msg::PoseWithCovarianceStamped>("pose_with_covariance", 1);
-  lines_publisher_ = node->create_publisher<visualization_msgs::msg::Marker>("lines", 1);
   line_ratings_publisher_ = node->create_publisher<visualization_msgs::msg::Marker>("line_ratings", 1);
   goal_ratings_publisher_ = node->create_publisher<visualization_msgs::msg::Marker>("goal_ratings", 1);
-  fieldboundary_ratings_publisher_ =
-      node->create_publisher<visualization_msgs::msg::Marker>("field_boundary_ratings", 1);
   field_publisher_ = node->create_publisher<nav_msgs::msg::OccupancyGrid>(
       "field/map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local());
 
   // Set the measurement timestamps to 0
   line_pointcloud_relative_.header.stamp = rclcpp::Time(0);
   goal_posts_relative_.header.stamp = rclcpp::Time(0);
-  fieldboundary_relative_.header.stamp = rclcpp::Time(0);
 
   // Get the static global configuration from the blackboard
   auto global_params = bitbots_utils::get_parameters_from_other_node(
@@ -100,14 +93,9 @@ void Localization::updateParams(bool force_reload) {
   if (config_.particle_filter.scoring.goal.factor) {
     goals_.reset(new Map(field_name_, "goals.png", config_.particle_filter.scoring.goal.out_of_field_score));
   }
-  if (config_.particle_filter.scoring.field_boundary.factor) {
-    field_boundary_.reset(
-        new Map(field_name_, "field_boundary.png", config_.particle_filter.scoring.field_boundary.out_of_field_score));
-  }
 
   // Init observation model
-  robot_pose_observation_model_.reset(
-      new RobotPoseObservationModel(lines_, goals_, field_boundary_, config_, field_dimensions_));
+  robot_pose_observation_model_.reset(new RobotPoseObservationModel(lines_, goals_, config_, field_dimensions_));
 
   // Init motion model
   auto drift_config = config_.particle_filter.drift;
@@ -175,10 +163,9 @@ void Localization::run_filter_one_step() {
     robot_motion_model_->diffuse_multiplier_ = config_.particle_filter.diffusion.multiplier;
   }
 
-  if ((config_.misc.filter_only_with_motion and robot_moved) or (!config_.misc.filter_only_with_motion)) {
-    robot_pf_->drift(linear_movement_, rotational_movement_);
-    robot_pf_->diffuse();
-  }
+  // Apply the motion model to the particles
+  robot_pf_->drift(linear_movement_, rotational_movement_);
+  robot_pf_->diffuse();
 
   // Apply ratings corresponding to the observations compared with each particle position
   robot_pf_->measure();
@@ -203,11 +190,9 @@ void Localization::LinePointcloudCallback(const sm::msg::PointCloud2 &msg) { lin
 
 void Localization::GoalPostsCallback(const sv3dm::msg::GoalpostArray &msg) { goal_posts_relative_ = msg; }
 
-void Localization::FieldboundaryCallback(const sv3dm::msg::FieldBoundary &msg) { fieldboundary_relative_ = msg; }
-
 void Localization::SetInitialPositionCallback(const gm::msg::PoseWithCovarianceStamped &msg) {
   // Transform the given pose to map frame
-  auto pose_in_map = tfBuffer->transform(msg, config_.ros.map_frame, tf2::durationFromSec(1.0));
+  auto pose_in_map = tf_buffer_->transform(msg, config_.ros.map_frame, tf2::durationFromSec(1.0));
 
   // Get yaw from quaternion
   double yaw = tf2::getYaw(pose_in_map.pose.pose.orientation);
@@ -290,22 +275,42 @@ void Localization::reset_filter(int distribution, double x, double y, double ang
 }
 
 void Localization::updateMeasurements() {
+  // Define an inner lambda function to calculate the odometry movement since a given timestamp
+  auto movement_since_stamp = [this](rclcpp::Time stamp) {
+    try {
+      tf2::Transform odom_at_measurement;
+      // Get the transform from the time the measurement was taken and copy the transform to a tf2 transform
+      tf2::fromMsg(
+          tf_buffer_->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, stamp).transform,
+          odom_at_measurement);
+      tf2::Transform odom_now;
+      // Get the latest transform and copy the transform to a tf2 transform
+      tf2::fromMsg(
+          tf_buffer_->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, rclcpp::Time(0))
+              .transform,
+          odom_now);
+      // Calculate the movement since the measurement was taken (in the local frame)
+      return odom_at_measurement.inverseTimes(odom_now);
+    } catch (const tf2::TransformException &ex) {
+      RCLCPP_WARN(node_->get_logger(), "Could not acquire movement since measurement at time: %s Assumed no movement.",
+                  ex.what());
+      return tf2::Transform::getIdentity();
+    }
+  };
+
   // Sets the measurements in the observation model
   if (line_pointcloud_relative_.header.stamp != last_stamp_lines && config_.particle_filter.scoring.lines.factor) {
     robot_pose_observation_model_->set_measurement_lines_pc(line_pointcloud_relative_);
+    robot_pose_observation_model_->set_movement_since_line_measurement(
+        movement_since_stamp(line_pointcloud_relative_.header.stamp));
+    last_stamp_lines = line_pointcloud_relative_.header.stamp;
   }
   if (config_.particle_filter.scoring.goal.factor && goal_posts_relative_.header.stamp != last_stamp_goals) {
     robot_pose_observation_model_->set_measurement_goalposts(goal_posts_relative_);
+    robot_pose_observation_model_->set_movement_since_goal_measurement(
+        movement_since_stamp(goal_posts_relative_.header.stamp));
+    last_stamp_goals = goal_posts_relative_.header.stamp;
   }
-  if (config_.particle_filter.scoring.field_boundary.factor &&
-      fieldboundary_relative_.header.stamp != last_stamp_fb_points) {
-    robot_pose_observation_model_->set_measurement_field_boundary(fieldboundary_relative_);
-  }
-
-  // Set timestamps to mark past messages
-  last_stamp_lines = line_pointcloud_relative_.header.stamp;
-  last_stamp_goals = goal_posts_relative_.header.stamp;
-  last_stamp_fb_points = fieldboundary_relative_.header.stamp;
 }
 
 void Localization::getMotion() {
@@ -314,26 +319,19 @@ void Localization::getMotion() {
   try {
     // Get current odometry transform
     transformStampedNow =
-        tfBuffer->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, rclcpp::Time(0));
-
-    // Get linear movement from odometry transform and the transform of the previous filter step
-    double global_diff_x, global_diff_y;
-    global_diff_x = (transformStampedNow.transform.translation.x - previousOdomTransform_.transform.translation.x);
-    global_diff_y = (transformStampedNow.transform.translation.y - previousOdomTransform_.transform.translation.y);
-
-    // Convert to local frame
-    auto [polar_rot, polar_dist] = cartesianToPolar(global_diff_x, global_diff_y);
-    auto [local_movement_x, local_movement_y] =
-        polarToCartesian(polar_rot - tf2::getYaw(previousOdomTransform_.transform.rotation), polar_dist);
-    linear_movement_.x = local_movement_x;
-    linear_movement_.y = local_movement_y;
-    linear_movement_.z = 0;
-
-    // Get angular movement from odometry transform and the transform of the previous filter step
-    rotational_movement_.x = 0;
-    rotational_movement_.y = 0;
-    rotational_movement_.z =
-        tf2::getYaw(transformStampedNow.transform.rotation) - tf2::getYaw(previousOdomTransform_.transform.rotation);
+        tf_buffer_->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, rclcpp::Time(0));
+
+    // Convert Msgs to tf2 transforms that support mathematical operations
+    tf2::Transform transformNow, transformPreviousOdom;
+    tf2::fromMsg(transformStampedNow.transform, transformNow);
+    tf2::fromMsg(previousOdomTransform_.transform, transformPreviousOdom);
+
+    // Calculate the movement in the local frame
+    auto movement = transformPreviousOdom.inverseTimes(transformNow);
+
+    // Copy the movement to the message to comply with the api, which expects messages and not tf2 transforms
+    tf2::convert(movement.getOrigin(), linear_movement_);
+    rotational_movement_.z = tf2::getYaw(movement.getRotation());
 
     // Get the time delta between the two transforms
     double time_delta = rclcpp::Time(transformStampedNow.header.stamp).seconds() -
@@ -355,15 +353,9 @@ void Localization::getMotion() {
         linear_movement_.y = 0;
         RCLCPP_WARN(node_->get_logger(), "Robot moved an unreasonable amount, dropping motion.");
       }
-
-      // Check if robot moved
-      robot_moved = linear_movement_normalized_x >= config_.misc.min_motion_linear or
-                    linear_movement_normalized_y >= config_.misc.min_motion_linear or
-                    rotational_movement_normalized_z >= config_.misc.min_motion_angular;
     } else {
       RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000,
                            "Time step delta of zero encountered! Odometry is unavailable.");
-      robot_moved = false;
     }
 
     // Set the variable for the transform of the previous step to the transform of the current step, because we finished
@@ -388,7 +380,7 @@ void Localization::publish_transforms() {
   // Get the transform from the last measurement timestamp until now
   geometry_msgs::msg::TransformStamped odomDuringMeasurement, odomNow;
   try {
-    odomNow = tfBuffer->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, rclcpp::Time(0));
+    odomNow = tf_buffer_->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, rclcpp::Time(0));
   } catch (const tf2::TransformException &ex) {
     RCLCPP_WARN(node_->get_logger(), "Could not acquire odom transforms: %s", ex.what());
   }
@@ -509,14 +501,9 @@ void Localization::publish_ratings() {
     publish_debug_rating(robot_pose_observation_model_->get_measurement_goals(), 0.2, "goal_ratings", goals_,
                          goal_ratings_publisher_);
   }
-  if (config_.particle_filter.scoring.field_boundary.factor) {
-    // Publish field boundary ratings
-    publish_debug_rating(robot_pose_observation_model_->get_measurement_field_boundary(), 0.2, "field_boundary_ratings",
-                         field_boundary_, fieldboundary_ratings_publisher_);
-  }
 }
 
-void Localization::publish_debug_rating(std::vector<std::pair<double, double>> measurements, double scale,
+void Localization::publish_debug_rating(const std::vector<std::pair<double, double>> &measurements, double scale,
                                         const char name[], std::shared_ptr<Map> map,
                                         rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr &publisher) {
   RobotState best_estimate = robot_pf_->getBestXPercentEstimate(config_.misc.percentage_best_particles);
@@ -531,18 +518,17 @@ void Localization::publish_debug_rating(std::vector<std::pair<double, double>> m
   marker.scale.x = scale;
   marker.scale.y = scale;
 
-  for (std::pair<double, double> &measurement : measurements) {
+  for (const std::pair<double, double> &measurement : measurements) {
     // lines are in polar form!
     std::pair<double, double> observationRelative;
 
-    observationRelative = map->observationRelative(measurement, best_estimate.getXPos(), best_estimate.getYPos(),
-                                                   best_estimate.getTheta());
+    observationRelative = map->getObservationCoordinatesInMapFrame(measurement, best_estimate.getXPos(),
+                                                                   best_estimate.getYPos(), best_estimate.getTheta());
     double occupancy = map->get_occupancy(observationRelative.first, observationRelative.second);
 
     geometry_msgs::msg::Point point;
     point.x = observationRelative.first;
     point.y = observationRelative.second;
-    point.z = 0;
 
     std_msgs::msg::ColorRGBA color;
     color.b = 1;
diff --git a/bitbots_navigation/bitbots_localization/src/map.cpp b/bitbots_navigation/bitbots_localization/src/map.cpp
index 2f3c83491..b20a9045c 100644
--- a/bitbots_navigation/bitbots_localization/src/map.cpp
+++ b/bitbots_navigation/bitbots_localization/src/map.cpp
@@ -57,7 +57,7 @@ std::vector<double> Map::provideRating(const RobotState &state,
     std::pair<double, double> lineRelative;
 
     // get rating per line
-    lineRelative = observationRelative(observation, state.getXPos(), state.getYPos(), state.getTheta());
+    lineRelative = getObservationCoordinatesInMapFrame(observation, state.getXPos(), state.getYPos(), state.getTheta());
     double occupancy = get_occupancy(lineRelative.first, lineRelative.second);
 
     rating.push_back(occupancy);
@@ -65,12 +65,12 @@ std::vector<double> Map::provideRating(const RobotState &state,
   return rating;
 }
 
-std::pair<double, double> Map::observationRelative(
-    std::pair<double, double> observation, double stateX, double stateY,
-    double stateT) {  // todo rename to a more correct name like observationonmap?
-  // transformes observation from particle to map (assumes particle is correct)
-  // input: obsservation relative in polar coordinates and particle
-  // output: hypothetical observation on map
+std::pair<double, double> Map::getObservationCoordinatesInMapFrame(std::pair<double, double> observation, double stateX,
+                                                                   double stateY, double stateT) {
+  // queries the Cartesian metric map coordinates for a given observation (in polar coordinates)
+  // taken relative to a given state (in Cartesian coordinates)
+  // Input: Observation coordinates in polar coordinates, state coordinates in Cartesian coordinates
+  // Output: Observation coordinates in Cartesian coordinates in the map frame
 
   // add theta and convert back to cartesian
   std::pair<double, double> observationWithTheta = polarToCartesian(observation.first + stateT, observation.second);
@@ -79,14 +79,6 @@ std::pair<double, double> Map::observationRelative(
   std::pair<double, double> observationRelative =
       std::make_pair(stateX + observationWithTheta.first, stateY + observationWithTheta.second);
 
-  // alternativ:
-  // Thrun 6.32, seite 169
-  //  but both equivalent
-  // double xGlobal = stateX + observation.second * (cos(stateT + observation.first));
-  // double yGlobal = stateY + observation.second * (sin(stateT + observation.first));
-
-  // std::pair<double, double> observationRelative = std::make_pair(xGlobal, yGlobal);
-
   return observationRelative;  // in cartesian
 }
 
diff --git a/bitbots_navigation/bitbots_localization/src/parameters.yml b/bitbots_navigation/bitbots_localization/src/parameters.yml
index ad4f6fcac..610a55754 100644
--- a/bitbots_navigation/bitbots_localization/src/parameters.yml
+++ b/bitbots_navigation/bitbots_localization/src/parameters.yml
@@ -10,29 +10,16 @@ bitbots_localization:
       description: "Percentage of particles which are considered to be the best ones. These particles are used to calculate the pose estimate"
       validation:
         bounds<>: [0, 100]
-    min_motion_linear:
-      type: double
-      description: "Minimum linear motion (m/s) which is considered to be a movement. This is relevant if we want to deactivate the filter if no movement is detected"
-      validation:
-        gt_eq<>: [0.0]
     max_motion_linear:
       type: double
       description: "Maximum linear motion (m/s) which is considered to be a movement. Updates larger than this are deemed unrealistic and are ignored"
       validation:
         gt_eq<>: [0.0]
-    min_motion_angular:
-      type: double
-      description: "Minimum angular motion (rad/s) which is considered to be a movement. This is relevant if we want to deactivate the filter if no movement is detected"
-      validation:
-        gt_eq<>: [0.0]
     max_motion_angular:
       type: double
       description: "Maximum angular motion (rad/s) which is considered to be a movement. Updates larger than this are deemed unrealistic and are ignored"
       validation:
         gt_eq<>: [0.0]
-    filter_only_with_motion:
-      type: bool
-      description: "If true, the filter is only active if a movement is detected"
   ros:
     line_pointcloud_topic:
       type: string
@@ -42,10 +29,6 @@ bitbots_localization:
       type: string
       description: "Topic for the goal input messages"
       read_only: true
-    fieldboundary_topic:
-      type: string
-      description: "Topic for the field boundary input messages"
-      read_only: true
     particle_publishing_topic:
       type: string
       description: "Topic for the particle publishing messages"
@@ -203,17 +186,6 @@ bitbots_localization:
           description: "Score which is given to a measurement (e.g. projected goal post) if it is out of the field"
           validation:
             bounds<>: [0.0, 100.0]
-      field_boundary:
-        factor:
-          type: double
-          description: "Weighing how much the field boundary information is considered for the scoring of a particle"
-          validation:
-            bounds<>: [0.0, 1.0]
-        out_of_field_score:
-          type: double
-          description: "Score which is given to a measurement (e.g. projected field boundary segment) if it is out of the field"
-          validation:
-            bounds<>: [0.0, 100.0]
     confidences:
       line_element:
         type: double
@@ -225,8 +197,3 @@ bitbots_localization:
         description: "Confidence we have in each data point of the goal information. Meaning each projected goal post"
         validation:
           bounds<>: [0.0, 1.0]
-      field_boundary_element:
-        type: double
-        description: "Confidence we have in each data point of the field boundary information. Meaning each projected field boundary segment"
-        validation:
-          bounds<>: [0.0, 1.0]