diff --git a/.gitignore b/.gitignore
index 9af41aca7..52ae0a3c0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -9,7 +9,6 @@ install/
 log/
 build/
 bin/
-lib/
 msg_gen/
 srv_gen/
 msg/*Action.msg
diff --git a/filtering/pose_action_server/src/pose_action_server_ros.cpp b/filtering/pose_action_server/src/pose_action_server_ros.cpp
index bf31ad621..115bcefe0 100644
--- a/filtering/pose_action_server/src/pose_action_server_ros.cpp
+++ b/filtering/pose_action_server/src/pose_action_server_ros.cpp
@@ -133,7 +133,7 @@ Eigen::Quaterniond PoseActionServerNode::average_quaternions(
     const std::vector<Eigen::Quaterniond>& quaternions) {
     Eigen::Matrix4d M = Eigen::Matrix4d::Zero();
     std::ranges::for_each(quaternions, [&](const auto& q) {
-        M += q.coeffs() * q.coeffs().transpose();
+        M += q.normalized().coeffs() * q.normalized().coeffs().transpose();
     });
 
     Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(M);
diff --git a/filtering/pose_filtering/CMakeLists.txt b/filtering/pose_filtering/CMakeLists.txt
new file mode 100644
index 000000000..b1bcf15d1
--- /dev/null
+++ b/filtering/pose_filtering/CMakeLists.txt
@@ -0,0 +1,112 @@
+cmake_minimum_required(VERSION 3.8)
+project(pose_filtering)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(vortex_utils_ros REQUIRED)
+find_package(vortex_utils_ros_tf REQUIRED)
+find_package(vortex_filtering REQUIRED)
+find_package(vortex_msgs REQUIRED)
+
+include_directories(include)
+
+set(CORE_LIB_NAME "${PROJECT_NAME}")
+
+add_library(${CORE_LIB_NAME} SHARED
+  src/lib/pose_track_manager.cpp
+  src/lib/orientation_filter.cpp
+)
+
+target_include_directories(${CORE_LIB_NAME}
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(${CORE_LIB_NAME}
+  vortex_filtering
+  vortex_utils
+  vortex_utils_ros
+  vortex_utils_ros_tf
+  vortex_msgs
+)
+
+set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component")
+
+add_library(${COMPONENT_LIB_NAME} SHARED
+  src/ros/${PROJECT_NAME}_ros.cpp
+  src/ros/${PROJECT_NAME}_ros_debug.cpp
+  src/ros/${PROJECT_NAME}_ros_conversions.cpp
+)
+
+ament_target_dependencies(${COMPONENT_LIB_NAME}
+  rclcpp
+  rclcpp_components
+  geometry_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  message_filters
+  vortex_filtering
+  vortex_utils
+  vortex_utils_ros
+  vortex_utils_ros_tf
+)
+
+target_link_libraries(${COMPONENT_LIB_NAME} ${PROJECT_NAME})
+
+rclcpp_components_register_node(
+  ${COMPONENT_LIB_NAME}
+  PLUGIN "PoseFilteringNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+install(
+  TARGETS
+    ${CORE_LIB_NAME}
+    ${COMPONENT_LIB_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  vortex_filtering
+  vortex_utils
+)
+ament_export_include_directories(include)
+ament_export_libraries(${CORE_LIB_NAME})
+
+if(BUILD_TESTING)
+  add_subdirectory(test)
+endif()
+
+ament_package()
diff --git a/filtering/pose_filtering/README.md b/filtering/pose_filtering/README.md
new file mode 100644
index 000000000..63abc463f
--- /dev/null
+++ b/filtering/pose_filtering/README.md
@@ -0,0 +1,119 @@
+# pose_filtering
+
+ROS 2 package providing pose-based multi-target tracking using an IPDA
+filter for position together with independent orientation logic.
+
+This package transforms incoming pose-like messages into a common
+target frame, associates measurements to tracks using spatial and angular
+gating, maintains an IPDA filter per track for position estimation and
+existence management, and performs a separate orientation update by
+averaging measurement quaternions and applying spherical linear
+interpolation (slerp) to smoothly update the track orientation.
+
+## Features
+
+- Multi-target tracking using an IPDA (Integrated Probabilistic Data
+  Association) filter for 3D position state estimation.
+- Separate orientation handling: angular gating, quaternion averaging and
+  slerp-based smoothing so orientation updates remain numerically stable
+  and physically meaningful.
+- TF2-aware subscriptions (message_filters + tf2 message filter) so
+  incoming messages are automatically dropped until transform to the
+  configured target frame is available.
+- Supports several ROS message inputs (see below) and publishes a
+  `geometry_msgs/PoseArray` containing the current tracked poses.
+
+## Supported input messages
+
+The node accepts the following message types as input measurements:
+
+- `geometry_msgs/msg/PoseStamped`
+- `geometry_msgs/msg/PoseArray`
+- `geometry_msgs/msg/PoseWithCovarianceStamped`
+- `vortex_msgs/msg/LandmarkArray`
+
+The code uses a templated message type (PoseMsgT) and checks at
+compile-time which types are supported. Incoming measurements are
+transformed to the configured `target_frame` before being processed.
+
+## Outputs
+
+- `geometry_msgs/msg/PoseArray` — the current set of tracked poses in the
+  configured target frame. Each Pose contains the track position and a
+  quaternion orientation representing the track's current orientation.
+- (DEBUG) `vortex_msgs/msg/PoseEulerStamped` topics with
+  measurement-level and track-state-level pose messages when debug mode
+  is enabled. Only publishes the first element of incoming measurements and stored tracks.
+
+
+## How the filtering works
+
+Position (IPDA):
+
+- Each track runs a 3D IPDA filter instance (a Gaussian state: mean +
+  covariance) where the motion model is a small constant-dynamics model
+  and the sensor model maps state to direct position measurements.
+- IPDA takes care of measurement association probabilities, gating and
+  existence management (confirmation / deletion thresholds). This
+  provides robust data association for cluttered measurements and
+  handles tracks appearing/disappearing over time.
+
+Orientation (SO(3) PDAF):
+
+Orientation is handled by a dedicated PDAF-style filter operating on
+the SO(3) manifold. Each track
+maintains:
+
+- a quaternion mean q (the track orientation), and
+- a small Gaussian error state in the 3D tangent space (so(3)) around
+  that mean.
+
+
+The log and exp maps are mathematically defined for all unit quaternions
+but the Kalman/PDAF linearization on the tangent space assumes small
+errors. The implementation gates measurements and uses configurable
+covariances to keep tangent residuals small; if large-angle residuals
+occur they will either be gated out or yield small β (limited effect).
+
+## Library export and usage
+
+The core tracking logic (`PoseTrackManager`) is built and exported as a
+shared library by this package so other packages can reuse the track
+management functionality without pulling in the ROS node.
+
+CMake / ament usage
+
+In a consuming CMake-based package (ament_cmake) you can link against
+the library like this:
+
+```cmake
+find_package(pose_filtering REQUIRED)
+
+add_executable(my_node src/my_node.cpp)
+
+# Link to the exported library target (the package exports the target
+# with the same name as the package: `pose_filtering`)
+target_link_libraries(my_node PRIVATE pose_filtering)
+
+```
+
+The package installs its public headers under `include/` so simply
+including the header below in your code will work after linking:
+
+```cpp
+#include <pose_filtering/lib/pose_track_manager.hpp>
+
+int main() {
+  vortex::filtering::TrackManagerConfig cfg;
+  // configure cfg.ipda, cfg.dyn_mod, cfg.sensor_mod, cfg.existence, etc.
+
+  vortex::filtering::PoseTrackManager manager(cfg);
+
+  std::vector<vortex::utils::types::Pose> measurements;
+  double dt = 0.1; // seconds
+  manager.step(measurements, dt);
+
+  const auto& tracks = manager.get_tracks();
+  // iterate tracks for id, pose, existence probability, etc.
+}
+```
diff --git a/filtering/pose_filtering/config/pose_filtering.yaml b/filtering/pose_filtering/config/pose_filtering.yaml
new file mode 100644
index 000000000..98f99a294
--- /dev/null
+++ b/filtering/pose_filtering/config/pose_filtering.yaml
@@ -0,0 +1,54 @@
+/**:
+  ros__parameters:
+    pose_sub_topic: "/aruco_detector/board"
+    pose_array_pub_topic: "/filtered_pose_array"
+    landmark_pub_topic: "/filtered_landmarks"
+    target_frame: "odom"
+    timer_rate_ms: 100
+    enu_ned_rotation: true
+
+    max_angle_gate_threshold: 0.4 # in radians
+
+    # IPDA config
+    position:
+      ipda:
+        prob_of_survival: 0.95
+        estimate_clutter: false
+        prob_of_detection: 0.5
+        mahalanobis_gate_threshold: 2.5 # in std deviations
+        min_gate_threshold: 1.0 # in meters
+        max_gate_threshold: 10.0 # in meters
+        clutter_intensity: 0.01
+      initial_position_std: 0.1
+
+      # Dyn model config
+      dyn_mod:
+        std_dev: 0.2
+
+      # Sensor model config
+      sensor_mod:
+        std_dev: 0.2
+
+      # Existence management config
+      existence:
+        confirmation_threshold: 0.6
+        deletion_threshold: 0.2
+        initial_existence_probability: 0.4
+
+    orientation:
+      pdaf:
+        prob_of_detection: 0.95
+        clutter_intensity: 0.01
+        mahalanobis_gate_threshold: 2.0 # in std deviations
+      initial_orientation_std: 0.1
+
+      dyn_mod:
+        std_dev: 0.05
+
+      sensor_mod:
+        std_dev: 0.05
+
+    debug:
+      enable: true
+      topic_name_meas: "/pose_debug_meas"
+      topic_name_state: "/pose_debug_state"
diff --git a/filtering/pose_filtering/include/pose_filtering/lib/orientation_filter.hpp b/filtering/pose_filtering/include/pose_filtering/lib/orientation_filter.hpp
new file mode 100644
index 000000000..d9b013360
--- /dev/null
+++ b/filtering/pose_filtering/include/pose_filtering/lib/orientation_filter.hpp
@@ -0,0 +1,64 @@
+#ifndef POSE_FILTERING__LIB__ORIENTATION_FILTER_HPP_
+#define POSE_FILTERING__LIB__ORIENTATION_FILTER_HPP_
+
+#include <Eigen/Dense>
+#include <vector>
+#include <vortex_filtering/vortex_filtering.hpp>
+#include "typedefs.hpp"
+
+namespace vortex::filtering {
+
+/**
+ * @brief Orientation filter implementing prediction and PDAF update on SO(3).
+ *
+ * The filter represents the orientation mean as a quaternion and keeps
+ * an error-state Gaussian in the 3D tangent space. Measurement updates
+ * are performed by mapping quaternion residuals into the tangent space
+ * (log map), performing a PDAF-style weighted update, and applying the
+ * update via the exponential map back to the quaternion manifold.
+ */
+class OrientationFilter {
+   public:
+    /**
+     * @brief Construct an OrientationFilter
+     * @param config Configuration struct
+     */
+    explicit OrientationFilter(const OrientationFilterConfig& config);
+
+    /**
+     * @brief Run a single filter step (prediction + update) for orientation.
+     * @param measurements Quaternion measurements associated with the
+     *        current track
+     * @param dt Time step in seconds used for prediction / covariance growth
+     * @param state OrientationState instance that will be updated with the
+     *        new orientation mean and error-state
+     */
+    void step(const std::vector<Eigen::Quaterniond>& measurements,
+              double dt,
+              OrientationState& state);
+
+   private:
+    /**
+     * @brief Map a quaternion to the so(3) tangent vector (log map).
+     * @param q_in Quaternion representing the relative rotation
+     * @return Vector in R^3 representing the axis * angle
+     */
+    Eigen::Vector3d so3_log_quat(const Eigen::Quaterniond& q_in);
+
+    /**
+     * @brief Map a tangent vector back to a quaternion (exp map).
+     * @param rvec Tangent vector (axis * angle)
+     * @return Quaternion corresponding to the rotation
+     */
+    Eigen::Quaterniond so3_exp_quat(const Eigen::Vector3d& rvec);
+
+    DynMod dyn_mod_;
+
+    SensorMod sensor_mod_;
+
+    vortex::filter::PDAF<DynMod, SensorMod>::Config pdaf_config_;
+};
+
+}  // namespace vortex::filtering
+
+#endif  // POSE_FILTERING__LIB__ORIENTATION_FILTER_HPP_
diff --git a/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp b/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp
new file mode 100644
index 000000000..ccc07d33d
--- /dev/null
+++ b/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp
@@ -0,0 +1,143 @@
+#ifndef POSE_FILTERING__LIB__POSE_TRACK_MANAGER_HPP_
+#define POSE_FILTERING__LIB__POSE_TRACK_MANAGER_HPP_
+
+#include <vector>
+#include <vortex_filtering/vortex_filtering.hpp>
+#include "orientation_filter.hpp"
+#include "typedefs.hpp"
+
+namespace vortex::filtering {
+
+/**
+ * @file pose_track_manager.hpp
+ * @brief Track management utilities for pose-based measurements.
+ *
+ * The PoseTrackManager implements track creation, confirmation,
+ * deletion and update logic using an IPDA filter for each track. It
+ * handles spatial and angular gating for associating pose measurements
+ * to existing tracks.
+ */
+
+/**
+ * @brief Class responsible for maintaining a set of tracks based on
+ * pose measurements.
+ *
+ * Public API:
+ *  - construct with a TrackManagerConfig
+ *  - call step() each frame with a list of measurements and the time step
+ *  - retrieve current tracks with get_tracks()
+ */
+class PoseTrackManager {
+   public:
+    /**
+     * @brief Construct a PoseTrackManager
+     * @param config Configuration parameters for IPDA, models and existence
+     * management
+     */
+    explicit PoseTrackManager(const TrackManagerConfig& config);
+
+    /**
+     * @brief Perform a single tracking step: associate measurements, update
+     * filters and manage track lifecycle.
+     * @param measurements Vector of pose measurements (will be modified by
+     * the manager to remove consumed measurements)
+     * @param dt Time step in seconds
+     */
+    void step(std::vector<Pose>& measurements, double dt);
+
+    /**
+     * @brief Get the list of currently maintained tracks.
+     * @return const reference to internal track vector
+     */
+    const std::vector<Track>& get_tracks() const { return tracks_; }
+
+   private:
+    /**
+     * @brief Compute angular gating for a given track against measurements.
+     * @param track Track to gate
+     * @param measurements Candidate measurements
+     * @return Indices of measurements passing the angular gate
+     */
+    std::vector<Eigen::Index> angular_gate_measurements(
+        const Track& track,
+        const std::vector<Pose>& measurements) const;
+
+    /**
+     * @brief Confirm tracks which have exceeded the confirmation threshold.
+     */
+    void confirm_tracks();
+
+    /**
+     * @brief Delete tracks which have fallen below the deletion threshold.
+     */
+    void delete_tracks();
+
+    /**
+     * @brief Create new tracks from unassociated measurements.
+     * @param measurements Measurements to be used for new track creation
+     */
+    void create_tracks(const std::vector<Pose>& measurements);
+
+    /**
+     * @brief Sort tracks by priority to determine processing / deletion order.
+     */
+    void sort_tracks_by_priority();
+
+    /**
+     * @brief Build a matrix of 3D positions for a subset of measurements.
+     * @param measurements All measurements
+     * @param indices Indices selecting which measurements to include
+     * @return 3xN Eigen matrix with positions in columns
+     */
+    Eigen::Matrix<double, 3, Eigen::Dynamic> build_position_matrix(
+        const std::vector<Pose>& measurements,
+        const std::vector<Eigen::Index>& indices) const;
+
+    /**
+     * @brief Collect quaternions from gated measurements and mark consumed
+     * indices.
+     * @param measurements All measurements
+     * @param angular_gate_indices Candidate indices that passed angular gate
+     * @param gated_measurements Boolean mask of gated measurements
+     * @param consumed_indices_out Output vector filled with indices consumed
+     * by this collection
+     * @return Vector of quaternions corresponding to consumed measurements
+     */
+    std::vector<Eigen::Quaterniond> collect_used_quaternions(
+        const std::vector<Pose>& measurements,
+        const std::vector<Eigen::Index>& angular_gate_indices,
+        const Eigen::Array<bool, 1, Eigen::Dynamic>& gated_measurements,
+        std::vector<Eigen::Index>& consumed_indices_out) const;
+
+    /**
+     * @brief Erase measurements which have been gated / consumed.
+     * @param measurements Measurements vector (modified in place)
+     * @param indices Indices of measurements to erase
+     */
+    void erase_gated_measurements(std::vector<Pose>& measurements,
+                                  std::vector<Eigen::Index>& indices);
+
+    // Internal bookkeeping
+    int track_id_counter_{0};
+
+    std::vector<Track> tracks_;
+
+    DynMod dyn_mod_;
+
+    SensorMod sensor_mod_;
+
+    vortex::filter::IPDA<DynMod, SensorMod>::Config ipda_config_;
+
+    ExistenceManagementConfig existence_config_;
+
+    double max_angle_gate_threshold_{};
+
+    double initial_position_std_{};
+    double initial_orientation_std_{};
+
+    OrientationFilter orientation_filter_;
+};
+
+}  // namespace vortex::filtering
+
+#endif  // POSE_FILTERING__LIB__POSE_TRACK_MANAGER_HPP_
diff --git a/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp b/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp
new file mode 100644
index 000000000..e4ca49fa0
--- /dev/null
+++ b/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp
@@ -0,0 +1,154 @@
+#ifndef POSE_FILTERING__LIB__TYPEDEFS_HPP_
+#define POSE_FILTERING__LIB__TYPEDEFS_HPP_
+
+#include <eigen3/Eigen/Dense>
+#include <vortex/utils/types.hpp>
+#include <vortex_filtering/filters/ipda.hpp>
+#include <vortex_filtering/vortex_filtering.hpp>
+
+namespace vortex::filtering {
+
+/**
+ * @file typedefs.hpp
+ * @brief Common type aliases and configuration structs used by the
+ * pose_filtering library.
+ *
+ * This header exposes convenient using-aliases for commonly used types
+ * (state, dynamic and sensor models, IPDA filter instance, and Pose type)
+ * and configuration structs that are passed to the track manager.
+ */
+
+/**
+ * @brief Gaussian 3d state representation (mean + covariance).
+ */
+using State3d = vortex::prob::Gauss3d;
+
+/**
+ * @brief Discrete-time constant dynamic model of order 3.
+ */
+using DynMod = vortex::models::ConstantDynamicModel<3>;
+
+/**
+ * @brief Identity sensor model mapping state to measurement space.
+ */
+using SensorMod = vortex::models::IdentitySensorModel<3, 3>;
+
+/**
+ * @brief IPDA filter type specialized for the chosen dynamic and sensor models.
+ */
+using IPDA = vortex::filter::IPDA<DynMod, SensorMod>;
+
+/**
+ * @brief PDAF filter type specialized for the chosen dynamic and sensor models.
+ */
+using PDAF = vortex::filter::PDAF<DynMod, SensorMod>;
+
+/**
+ * @brief Pose type (position + quaternion) from vortex utils.
+ */
+using Pose = vortex::utils::types::Pose;
+
+/**
+ * @brief Configuration for the dynamic model (process noise standard
+ * deviation).
+ */
+struct DynModConfig {
+    double std_dev{1.0};
+};
+
+/**
+ * @brief Configuration for the sensor model (measurement noise standard
+ * deviation).
+ */
+struct SensorModConfig {
+    double std_dev{1.0};
+};
+
+/**
+ * @brief Parameters for existence management (track confirmation / deletion).
+ */
+struct ExistenceManagementConfig {
+    double confirmation_threshold{0.6};
+    double deletion_threshold{0.2};
+    double initial_existence_probability{0.5};
+};
+
+/**
+ * @brief Configuration struct for the orientation filter.
+ */
+struct OrientationFilterConfig {
+    vortex::filter::PDAF<DynMod, SensorMod>::Config pdaf;
+    DynModConfig dyn_mod;
+    SensorModConfig sensor_mod;
+};
+
+/**
+ * @brief High-level track manager configuration struct.
+ *
+ * Contains IPDA-specific configuration and parameters for dynamics,
+ * sensor model, existence management and gating thresholds.
+ */
+struct TrackManagerConfig {
+    vortex::filter::IPDA<DynMod, SensorMod>::Config ipda;
+    double initial_position_std{1.0};
+    double initial_orientation_std{1.0};
+    DynModConfig dyn_mod;
+    SensorModConfig sensor_mod;
+    ExistenceManagementConfig existence;
+    double max_angle_gate_threshold{};
+    OrientationFilterConfig ori;
+};
+
+/**
+ * @brief Orientation filter state representation.
+ */
+struct OrientationState {
+    Eigen::Quaterniond q;
+    State3d error_state;
+};
+
+/**
+ * @brief Representation of a single track maintained by the track manager.
+ *
+ * The Track stores the filter state, orientation quaternions and bookkeeping
+ * fields used for existence management and confirmation.
+ */
+struct Track {
+    /// Unique track identifier
+    int id{};
+
+    /// Filter position state (mean + covariance)
+    State3d state_pos;
+
+    OrientationState orientation_filter;
+
+    /// Probability that the track exists (0..1)
+    double existence_probability{0.0};
+
+    /// Whether the track has been confirmed (passed confirmation threshold)
+    bool confirmed{false};
+
+    /**
+     * @brief Convert the track position + orientation to a Pose object.
+     * @return vortex::utils::types::Pose constructed from the state mean and
+     * current_orientation
+     */
+    Pose to_pose() const {
+        return vortex::utils::types::Pose::from_eigen(state_pos.mean(),
+                                                      orientation_filter.q);
+    }
+
+    /**
+     * @brief Compute angular distance between the track orientation and a
+     * measurement pose orientation.
+     * @param z Measurement pose to compare against
+     * @return Angular distance in radians (double)
+     */
+    double angular_distance(const Pose& z) const {
+        return orientation_filter.q.angularDistance(z.ori_quaternion());
+    }
+};
+
+}  // namespace vortex::filtering
+
+#endif  // POSE_FILTERING__LIB__TYPEDEFS_HPP_
diff --git a/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp
new file mode 100644
index 000000000..aa1709e5a
--- /dev/null
+++ b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp
@@ -0,0 +1,90 @@
+#ifndef POSE_FILTERING__ROS__POSE_FILTERING_ROS_HPP_
+#define POSE_FILTERING__ROS__POSE_FILTERING_ROS_HPP_
+
+#include <message_filters/subscriber.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/create_timer_ros.h>
+#include <tf2_ros/message_filter.h>
+#include <tf2_ros/transform_listener.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <vector>
+#include <vortex_msgs/msg/landmark_array.hpp>
+#include <vortex_msgs/msg/pose_euler_stamped.hpp>
+#include "pose_filtering/lib/pose_track_manager.hpp"
+
+#include <concepts>
+
+namespace vortex::filtering {
+
+using PoseMsgT = geometry_msgs::msg::PoseStamped;
+
+template <typename T>
+concept ValidPoseMsg =
+    std::same_as<T, geometry_msgs::msg::PoseStamped> ||
+    std::same_as<T, geometry_msgs::msg::PoseArray> ||
+    std::same_as<T, geometry_msgs::msg::PoseWithCovarianceStamped> ||
+    std::same_as<T, vortex_msgs::msg::LandmarkArray>;
+
+static_assert(ValidPoseMsg<PoseMsgT>,
+              "PoseMsgT must be a supported pose message type");
+
+class PoseFilteringNode : public rclcpp::Node {
+   public:
+    explicit PoseFilteringNode(const rclcpp::NodeOptions& options);
+
+    ~PoseFilteringNode() {}
+
+   private:
+    void setup_publishers_and_subscribers();
+
+    void setup_track_manager();
+
+    void create_pose_subscription(const std::string& topic_name,
+                                  const rmw_qos_profile_t& qos_profile);
+
+    void timer_callback();
+
+    void setup_debug_publishers();
+
+    void publish_meas_debug();
+
+    void publish_state_debug();
+
+    std::shared_ptr<message_filters::Subscriber<PoseMsgT>> subscriber_;
+
+    std::shared_ptr<tf2_ros::MessageFilter<PoseMsgT>> tf_filter_;
+
+    std::string target_frame_;
+    std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
+    std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+
+    rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pose_array_pub_;
+
+    rclcpp::Publisher<vortex_msgs::msg::LandmarkArray>::SharedPtr
+        landmark_array_pub_;
+
+    rclcpp::TimerBase::SharedPtr pub_timer_;
+    double filter_dt_seconds_{0.0};
+
+    std::unique_ptr<PoseTrackManager> track_manager_;
+
+    std::vector<Pose> measurements_;
+    bool enu_ned_rotation_{false};
+
+    bool debug_{false};
+    rclcpp::Publisher<vortex_msgs::msg::PoseEulerStamped>::SharedPtr
+        pose_meas_debug_pub_;
+    rclcpp::Publisher<vortex_msgs::msg::PoseEulerStamped>::SharedPtr
+        pose_state_debug_pub_;
+};
+
+}  // namespace vortex::filtering
+
+#endif  // POSE_FILTERING__ROS__POSE_FILTERING_ROS_HPP_
diff --git a/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros_conversions.hpp b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros_conversions.hpp
new file mode 100644
index 000000000..4ac2de9d1
--- /dev/null
+++ b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros_conversions.hpp
@@ -0,0 +1,36 @@
+#ifndef POSE_FILTERING__ROS__POSE_FILTERING_ROS_CONVERSIONS_HPP_
+#define POSE_FILTERING__ROS__POSE_FILTERING_ROS_CONVERSIONS_HPP_
+
+#include <geometry_msgs/msg/pose_with_covariance.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <vector>
+#include <vortex_msgs/msg/landmark_array.hpp>
+#include "pose_filtering/lib/typedefs.hpp"
+
+namespace vortex::filtering::ros_conversions {
+
+/**
+ * @brief Convert a Track object to a PoseWithCovariance ROS message.
+ * @param track The Track object to convert.
+ * @return geometry_msgs::msg::PoseWithCovariance message representing the
+ * track.
+ */
+geometry_msgs::msg::PoseWithCovariance track_to_pose_with_covariance(
+    const Track& track);
+
+/**
+ * @brief Convert a vector of Track objects to a LandmarkArray ROS message.
+ * @param tracks The vector of Track objects to convert.
+ * @param timestamp The timestamp to set in the LandmarkArray header.
+ * @param frame_id The frame ID to set in the LandmarkArray header.
+ * @return vortex_msgs::msg::LandmarkArray message representing the tracks.
+ */
+vortex_msgs::msg::LandmarkArray tracks_to_landmark_array_msg(
+    const std::vector<Track>& tracks,
+    const rclcpp::Time& timestamp,
+    const std::string& frame_id);
+
+}  // namespace vortex::filtering::ros_conversions
+
+#endif  // POSE_FILTERING__ROS__POSE_FILTERING_ROS_CONVERSIONS_HPP_
diff --git a/filtering/pose_filtering/launch/pose_filtering.launch.py b/filtering/pose_filtering/launch/pose_filtering.launch.py
new file mode 100644
index 000000000..b24870a6d
--- /dev/null
+++ b/filtering/pose_filtering/launch/pose_filtering.launch.py
@@ -0,0 +1,28 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    config = os.path.join(
+        get_package_share_directory('pose_filtering'),
+        'config',
+        'pose_filtering.yaml',
+    )
+
+    return LaunchDescription(
+        [
+            Node(
+                package='pose_filtering',
+                executable='pose_filtering_node',
+                name='pose_filtering',
+                output='screen',
+                parameters=[
+                    config,
+                    # {'use_sim_time': True} # If testing with rosbags sim_time might be preferred if bag is looped
+                ],
+            ),
+        ]
+    )
diff --git a/filtering/pose_filtering/package.xml b/filtering/pose_filtering/package.xml
new file mode 100644
index 000000000..bbd9167e8
--- /dev/null
+++ b/filtering/pose_filtering/package.xml
@@ -0,0 +1,29 @@
+<?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>pose_filtering</name>
+  <version>0.0.0</version>
+  <description>Pose filtering and tracking package, with a C++ track manager and ROS 2 interface node.</description>
+
+  <maintainer email="jorgen.fjermedal@hotmail.com">jorgen</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>vortex_utils</depend>
+  <depend>vortex_utils_ros</depend>
+  <depend>vortex_utils_ros_tf</depend>
+  <depend>vortex_filtering</depend>
+  <depend>vortex_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/filtering/pose_filtering/src/lib/orientation_filter.cpp b/filtering/pose_filtering/src/lib/orientation_filter.cpp
new file mode 100644
index 000000000..c8119c76c
--- /dev/null
+++ b/filtering/pose_filtering/src/lib/orientation_filter.cpp
@@ -0,0 +1,80 @@
+#include "pose_filtering/lib/orientation_filter.hpp"
+#include <cmath>
+
+namespace vortex::filtering {
+
+OrientationFilter::OrientationFilter(const OrientationFilterConfig& cfg)
+    : dyn_mod_(cfg.dyn_mod.std_dev),
+      sensor_mod_(cfg.sensor_mod.std_dev),
+      pdaf_config_(cfg.pdaf) {}
+
+void OrientationFilter::step(
+    const std::vector<Eigen::Quaterniond>& measurements,
+    double dt,
+    OrientationState& state) {
+    if (measurements.empty()) {
+        auto prediction_result =
+            PDAF::predict(dyn_mod_, sensor_mod_, dt, state.error_state);
+        state.error_state = prediction_result.x_pred;
+        return;
+    }
+
+    Eigen::Matrix<double, 3, Eigen::Dynamic> measurements_error_space(
+        3, measurements.size());
+    for (size_t i = 0; i < measurements.size(); ++i) {
+        measurements_error_space.col(i) =
+            so3_log_quat(measurements.at(i) * state.q.conjugate());
+    }
+
+    auto result = PDAF::step(dyn_mod_, sensor_mod_, dt, state.error_state,
+                             measurements_error_space, pdaf_config_);
+
+    Eigen::Vector3d delta = result.x_post.mean();
+    state.q = so3_exp_quat(delta) * state.q;
+    state.q.normalize();
+
+    state.error_state.mean().setZero();
+    state.error_state.cov() = result.x_post.cov();
+}
+
+Eigen::Vector3d OrientationFilter::so3_log_quat(
+    const Eigen::Quaterniond& q_in) {
+    Eigen::Quaterniond q = q_in.normalized();
+
+    if (q.w() < 0.0) {
+        // Quat sign ambiguity q == -q
+        // Enforce consistent sign convention
+        q.coeffs() *= -1.0;
+    }
+
+    double norm_v = q.vec().norm();
+
+    if (norm_v < 1e-6) {
+        // Small-angle approximation of log map:
+        // for θ → 0, log(q) ≈ 2 * v since q ≈ [1, v]
+        return 2.0 * q.vec();
+    }
+
+    double theta = 2.0 * std::atan2(norm_v, q.w());
+    return theta * q.vec() / norm_v;
+}
+
+Eigen::Quaterniond OrientationFilter::so3_exp_quat(
+    const Eigen::Vector3d& rvec) {
+    double theta = rvec.norm();
+
+    if (theta < 1e-6) {
+        return Eigen::Quaterniond(1.0, 0.5 * rvec.x(), 0.5 * rvec.y(),
+                                  0.5 * rvec.z())
+            .normalized();
+    }
+
+    Eigen::Vector3d axis = rvec / theta;
+    double half = 0.5 * theta;
+
+    return Eigen::Quaterniond(std::cos(half), axis.x() * std::sin(half),
+                              axis.y() * std::sin(half),
+                              axis.z() * std::sin(half));
+}
+
+}  // namespace vortex::filtering
diff --git a/filtering/pose_filtering/src/lib/pose_track_manager.cpp b/filtering/pose_filtering/src/lib/pose_track_manager.cpp
new file mode 100644
index 000000000..1fbfb4ed9
--- /dev/null
+++ b/filtering/pose_filtering/src/lib/pose_track_manager.cpp
@@ -0,0 +1,164 @@
+#include "pose_filtering/lib/pose_track_manager.hpp"
+#include <algorithm>
+#include <ranges>
+#include <vortex/utils/math.hpp>
+#include "pose_filtering/lib/orientation_filter.hpp"
+
+namespace vortex::filtering {
+
+PoseTrackManager::PoseTrackManager(const TrackManagerConfig& config)
+    : track_id_counter_(0),
+      dyn_mod_(config.dyn_mod.std_dev),
+      sensor_mod_(config.sensor_mod.std_dev),
+      orientation_filter_(OrientationFilter(config.ori)) {
+    initial_position_std_ = config.initial_position_std;
+    initial_orientation_std_ = config.initial_orientation_std;
+    ipda_config_ = config.ipda;
+    existence_config_ = config.existence;
+    max_angle_gate_threshold_ = config.max_angle_gate_threshold;
+}
+
+void PoseTrackManager::step(std::vector<Pose>& measurements, double dt) {
+    sort_tracks_by_priority();
+
+    for (Track& track : tracks_) {
+        auto angular_gate_indices =
+            angular_gate_measurements(track, measurements);
+
+        Eigen::Matrix<double, 3, Eigen::Dynamic> Z =
+            build_position_matrix(measurements, angular_gate_indices);
+
+        const IPDA::State state_est_prev{
+            .x_estimate = track.state_pos,
+            .existence_probability = track.existence_probability};
+
+        auto ipda_output = IPDA::step(dyn_mod_, sensor_mod_, dt, state_est_prev,
+                                      Z, ipda_config_);
+
+        track.state_pos = ipda_output.state.x_estimate;
+        track.existence_probability = ipda_output.state.existence_probability;
+
+        std::vector<Eigen::Index> consumed_indices;
+        auto used_quaternions = collect_used_quaternions(
+            measurements, angular_gate_indices, ipda_output.gated_measurements,
+            consumed_indices);
+
+        orientation_filter_.step(used_quaternions, dt,
+                                 track.orientation_filter);
+
+        erase_gated_measurements(measurements, consumed_indices);
+    }
+
+    confirm_tracks();
+    delete_tracks();
+    create_tracks(measurements);
+}
+
+std::vector<Eigen::Index> PoseTrackManager::angular_gate_measurements(
+    const Track& track,
+    const std::vector<Pose>& measurements) const {
+    std::vector<Eigen::Index> indices;
+    indices.reserve(measurements.size());
+
+    for (Eigen::Index i = 0; i < static_cast<Eigen::Index>(measurements.size());
+         ++i) {
+        if (track.angular_distance(measurements[i]) <
+            max_angle_gate_threshold_) {
+            indices.push_back(i);
+        }
+    }
+    return indices;
+}
+
+Eigen::Matrix<double, 3, Eigen::Dynamic>
+PoseTrackManager::build_position_matrix(
+    const std::vector<Pose>& measurements,
+    const std::vector<Eigen::Index>& indices) const {
+    Eigen::Matrix<double, 3, Eigen::Dynamic> Z(3, indices.size());
+    for (Eigen::Index k = 0; k < static_cast<Eigen::Index>(indices.size());
+         ++k) {
+        Z.col(k) = measurements[indices[k]].pos_vector();
+    }
+    return Z;
+}
+
+std::vector<Eigen::Quaterniond> PoseTrackManager::collect_used_quaternions(
+    const std::vector<Pose>& measurements,
+    const std::vector<Eigen::Index>& angular_gate_indices,
+    const Eigen::Array<bool, 1, Eigen::Dynamic>& gated_measurements,
+    std::vector<Eigen::Index>& consumed_indices_out) const {
+    std::vector<Eigen::Quaterniond> quats;
+    quats.reserve(gated_measurements.size());
+
+    for (Eigen::Index k = 0;
+         k < static_cast<Eigen::Index>(angular_gate_indices.size()); ++k) {
+        if (gated_measurements[k]) {
+            Eigen::Index original_idx = angular_gate_indices[k];
+            quats.push_back(measurements[original_idx].ori_quaternion());
+            consumed_indices_out.push_back(original_idx);
+        }
+    }
+    return quats;
+}
+
+void PoseTrackManager::erase_gated_measurements(
+    std::vector<Pose>& measurements,
+    std::vector<Eigen::Index>& indices) {
+    std::ranges::sort(indices, std::greater<>());
+    for (Eigen::Index idx : indices) {
+        measurements.erase(measurements.begin() + idx);
+    }
+}
+
+void PoseTrackManager::create_tracks(const std::vector<Pose>& measurements) {
+    tracks_.reserve(tracks_.size() + measurements.size());
+
+    auto make_track = [this](const Pose& measurement) {
+        return Track{.id = track_id_counter_++,
+                     .state_pos = vortex::prob::Gauss3d(
+                         measurement.pos_vector(), Eigen::Matrix3d::Identity() *
+                                                       initial_position_std_ *
+                                                       initial_position_std_),
+                     .orientation_filter =
+                         OrientationState{.q = measurement.ori_quaternion(),
+                                          .error_state = State3d(
+                                              Eigen::Vector3d::Zero(),
+                                              Eigen::Matrix3d::Identity() *
+                                                  initial_orientation_std_ *
+                                                  initial_orientation_std_)},
+                     .existence_probability =
+                         existence_config_.initial_existence_probability,
+                     .confirmed = false};
+    };
+
+    for (const Pose& m : measurements) {
+        tracks_.push_back(make_track(m));
+    }
+}
+
+void PoseTrackManager::confirm_tracks() {
+    for (Track& track : tracks_) {
+        if (!track.confirmed && track.existence_probability >=
+                                    existence_config_.confirmation_threshold) {
+            track.confirmed = true;
+        }
+    }
+}
+
+void PoseTrackManager::delete_tracks() {
+    auto new_end = std::ranges::remove_if(tracks_, [this](Track& track) {
+        return track.existence_probability <
+               existence_config_.deletion_threshold;
+    });
+    tracks_.erase(new_end.begin(), new_end.end());
+}
+
+void PoseTrackManager::sort_tracks_by_priority() {
+    std::ranges::sort(tracks_, [](const Track& a, const Track& b) {
+        if (a.confirmed != b.confirmed)
+            return a.confirmed > b.confirmed;
+        return a.existence_probability > b.existence_probability;
+    });
+}
+
+}  // namespace vortex::filtering
diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp
new file mode 100644
index 000000000..e12c8dd09
--- /dev/null
+++ b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp
@@ -0,0 +1,194 @@
+#include "pose_filtering/ros/pose_filtering_ros.hpp"
+#include <spdlog/spdlog.h>
+#include <rclcpp_components/register_node_macro.hpp>
+#include <vortex/utils/math.hpp>
+#include <vortex/utils/ros/qos_profiles.hpp>
+#include "pose_filtering/ros/pose_filtering_ros_conversions.hpp"
+#include "vortex/utils/ros/ros_conversions.hpp"
+#include "vortex/utils/ros/ros_transforms.hpp"
+
+namespace vortex::filtering {
+
+PoseFilteringNode::PoseFilteringNode(const rclcpp::NodeOptions& options)
+    : rclcpp::Node("pose_filtering_node", options) {
+    setup_publishers_and_subscribers();
+    setup_track_manager();
+}
+
+void PoseFilteringNode::setup_publishers_and_subscribers() {
+    enu_ned_rotation_ = this->declare_parameter<bool>("enu_ned_rotation");
+    const auto qos_sensor_data_pub{
+        vortex::utils::qos_profiles::sensor_data_profile(1)};
+    std::string pub_topic_name =
+        this->declare_parameter<std::string>("pose_array_pub_topic");
+
+    std::string landmark_pub_topic =
+        this->declare_parameter<std::string>("landmark_pub_topic");
+
+    pose_array_pub_ = this->create_publisher<geometry_msgs::msg::PoseArray>(
+        pub_topic_name, qos_sensor_data_pub);
+
+    landmark_array_pub_ =
+        this->create_publisher<vortex_msgs::msg::LandmarkArray>(
+            landmark_pub_topic, qos_sensor_data_pub);
+
+    int timer_rate_ms = this->declare_parameter<int>("timer_rate_ms");
+
+    filter_dt_seconds_ = static_cast<double>(timer_rate_ms) / 1000;
+
+    pub_timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(timer_rate_ms),
+        std::bind(&PoseFilteringNode::timer_callback, this));
+
+    target_frame_ = this->declare_parameter<std::string>("target_frame");
+
+    tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+
+    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+        this->get_node_base_interface(), this->get_node_timers_interface());
+
+    tf2_buffer_->setCreateTimerInterface(timer_interface);
+    tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
+
+    const std::string pose_sub_topic =
+        this->declare_parameter<std::string>("pose_sub_topic");
+
+    const auto qos_sensor_data_sub{
+        vortex::utils::qos_profiles::sensor_data_profile(10)};
+
+    create_pose_subscription(pose_sub_topic,
+                             qos_sensor_data_sub.get_rmw_qos_profile());
+
+    debug_ = this->declare_parameter<bool>("debug.enable");
+    if (debug_) {
+        setup_debug_publishers();
+    }
+}
+
+void PoseFilteringNode::create_pose_subscription(
+    const std::string& topic_name,
+    const rmw_qos_profile_t& qos_profile) {
+    auto sub = std::make_shared<message_filters::Subscriber<PoseMsgT>>(
+        this, topic_name, qos_profile);
+
+    auto filter = std::make_shared<tf2_ros::MessageFilter<PoseMsgT>>(
+        *sub, *tf2_buffer_, target_frame_, 10,
+        this->get_node_logging_interface(), this->get_node_clock_interface());
+
+    filter->registerCallback([this](
+                                 const typename PoseMsgT::ConstSharedPtr msg) {
+        PoseMsgT pose_tf;
+        try {
+            vortex::utils::ros_transforms::transform_pose(
+                *tf2_buffer_, *msg, target_frame_, pose_tf);
+        } catch (const tf2::TransformException& ex) {
+            RCLCPP_WARN(
+                this->get_logger(), "TF transform failed from '%s' to '%s': %s",
+                msg->header.frame_id.c_str(), target_frame_.c_str(), ex.what());
+            return;
+        }
+
+        this->measurements_ =
+            vortex::utils::ros_conversions::ros_to_pose_vec(pose_tf);
+        if (enu_ned_rotation_) {
+            std::ranges::for_each(this->measurements_, [](auto& m) {
+                m.set_ori(
+                    vortex::utils::math::enu_ned_rotation(m.ori_quaternion()));
+            });
+        }
+    });
+
+    subscriber_ = sub;
+    tf_filter_ = filter;
+}
+
+void PoseFilteringNode::setup_track_manager() {
+    TrackManagerConfig config;
+
+    config.ipda.ipda.prob_of_survival =
+        this->declare_parameter<double>("position.ipda.prob_of_survival");
+    config.ipda.ipda.estimate_clutter =
+        this->declare_parameter<bool>("position.ipda.estimate_clutter");
+    config.ipda.pdaf.prob_of_detection =
+        this->declare_parameter<double>("position.ipda.prob_of_detection");
+    config.ipda.pdaf.mahalanobis_threshold = this->declare_parameter<double>(
+        "position.ipda.mahalanobis_gate_threshold");
+    config.ipda.pdaf.min_gate_threshold =
+        this->declare_parameter<double>("position.ipda.min_gate_threshold");
+    config.ipda.pdaf.max_gate_threshold =
+        this->declare_parameter<double>("position.ipda.max_gate_threshold");
+    config.ipda.pdaf.clutter_intensity =
+        this->declare_parameter<double>("position.ipda.clutter_intensity");
+
+    config.initial_position_std =
+        this->declare_parameter<double>("position.initial_position_std");
+    config.initial_orientation_std =
+        this->declare_parameter<double>("orientation.initial_orientation_std");
+
+    config.dyn_mod.std_dev =
+        this->declare_parameter<double>("position.dyn_mod.std_dev");
+
+    config.sensor_mod.std_dev =
+        this->declare_parameter<double>("position.sensor_mod.std_dev");
+
+    config.max_angle_gate_threshold =
+        this->declare_parameter<double>("max_angle_gate_threshold");
+
+    config.existence.confirmation_threshold = this->declare_parameter<double>(
+        "position.existence.confirmation_threshold");
+    config.existence.deletion_threshold = this->declare_parameter<double>(
+        "position.existence.deletion_threshold");
+    config.existence.initial_existence_probability =
+        this->declare_parameter<double>(
+            "position.existence.initial_existence_probability");
+
+    config.ori.pdaf.pdaf.prob_of_detection =
+        this->declare_parameter<double>("orientation.pdaf.prob_of_detection");
+    config.ori.pdaf.pdaf.clutter_intensity =
+        this->declare_parameter<double>("orientation.pdaf.clutter_intensity");
+    config.ori.pdaf.pdaf.mahalanobis_threshold =
+        this->declare_parameter<double>(
+            "orientation.pdaf.mahalanobis_gate_threshold");
+
+    config.ori.dyn_mod.std_dev =
+        this->declare_parameter<double>("orientation.dyn_mod.std_dev");
+    config.ori.sensor_mod.std_dev =
+        this->declare_parameter<double>("orientation.sensor_mod.std_dev");
+    track_manager_ = std::make_unique<PoseTrackManager>(config);
+}
+
+void PoseFilteringNode::timer_callback() {
+    if (debug_) {
+        publish_meas_debug();
+    }
+    track_manager_->step(measurements_, filter_dt_seconds_);
+    measurements_.clear();
+    if (debug_) {
+        publish_state_debug();
+    }
+
+    geometry_msgs::msg::PoseArray pose_array;
+    pose_array.header.frame_id = target_frame_;
+    pose_array.header.stamp = this->get_clock()->now();
+    for (const Track& track : track_manager_->get_tracks()) {
+        if (!track.confirmed)
+            continue;
+        pose_array.poses.push_back(
+            vortex::utils::ros_conversions::to_pose_msg(track.to_pose()));
+    }
+
+    if (pose_array.poses.empty()) {
+        return;
+    }
+    pose_array_pub_->publish(pose_array);
+
+    vortex_msgs::msg::LandmarkArray landmark_array_msg =
+        vortex::filtering::ros_conversions::tracks_to_landmark_array_msg(
+            track_manager_->get_tracks(), this->get_clock()->now(),
+            target_frame_);
+    landmark_array_pub_->publish(landmark_array_msg);
+}
+
+RCLCPP_COMPONENTS_REGISTER_NODE(PoseFilteringNode);
+
+}  // namespace vortex::filtering
diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp
new file mode 100644
index 000000000..dd0eb0401
--- /dev/null
+++ b/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp
@@ -0,0 +1,66 @@
+#include "pose_filtering/ros/pose_filtering_ros_conversions.hpp"
+
+namespace vortex::filtering::ros_conversions {
+
+geometry_msgs::msg::PoseWithCovariance track_to_pose_with_covariance(
+    const Track& track) {
+    geometry_msgs::msg::PoseWithCovariance msg;
+
+    msg.pose.position.x = track.state_pos.mean()(0);
+    msg.pose.position.y = track.state_pos.mean()(1);
+    msg.pose.position.z = track.state_pos.mean()(2);
+
+    msg.pose.orientation.w = track.orientation_filter.q.w();
+    msg.pose.orientation.x = track.orientation_filter.q.x();
+    msg.pose.orientation.y = track.orientation_filter.q.y();
+    msg.pose.orientation.z = track.orientation_filter.q.z();
+
+    msg.covariance.fill(0.0);
+
+    const auto& Pp = track.state_pos.cov();
+    msg.covariance[0 * 6 + 0] = Pp(0, 0);  // xx
+    msg.covariance[0 * 6 + 1] = Pp(0, 1);  // xy
+    msg.covariance[0 * 6 + 2] = Pp(0, 2);  // xz
+
+    msg.covariance[1 * 6 + 0] = Pp(1, 0);  // yx
+    msg.covariance[1 * 6 + 1] = Pp(1, 1);  // yy
+    msg.covariance[1 * 6 + 2] = Pp(1, 2);  // yz
+
+    msg.covariance[2 * 6 + 0] = Pp(2, 0);  // zx
+    msg.covariance[2 * 6 + 1] = Pp(2, 1);  // zy
+    msg.covariance[2 * 6 + 2] = Pp(2, 2);  // zz
+
+    const auto& Po = track.orientation_filter.error_state.cov();
+    msg.covariance[3 * 6 + 3] = Po(0, 0);  // RR
+    msg.covariance[3 * 6 + 4] = Po(0, 1);  // RP
+    msg.covariance[3 * 6 + 5] = Po(0, 2);  // RY
+
+    msg.covariance[4 * 6 + 3] = Po(1, 0);  // PR
+    msg.covariance[4 * 6 + 4] = Po(1, 1);  // PP
+    msg.covariance[4 * 6 + 5] = Po(1, 2);  // PY
+
+    msg.covariance[5 * 6 + 3] = Po(2, 0);  // YR
+    msg.covariance[5 * 6 + 4] = Po(2, 1);  // YP
+    msg.covariance[5 * 6 + 5] = Po(2, 2);  // YY
+
+    return msg;
+}
+
+vortex_msgs::msg::LandmarkArray tracks_to_landmark_array_msg(
+    const std::vector<Track>& tracks,
+    const rclcpp::Time& timestamp,
+    const std::string& frame_id) {
+    vortex_msgs::msg::LandmarkArray landmark_array_msg;
+    landmark_array_msg.header.stamp = timestamp;
+    landmark_array_msg.header.frame_id = frame_id;
+
+    for (const auto& track : tracks) {
+        vortex_msgs::msg::Landmark landmark;
+        landmark.pose = track_to_pose_with_covariance(track);
+        landmark_array_msg.landmarks.push_back(landmark);
+    }
+
+    return landmark_array_msg;
+}
+
+}  // namespace vortex::filtering::ros_conversions
diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp
new file mode 100644
index 000000000..7f0f481d3
--- /dev/null
+++ b/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp
@@ -0,0 +1,70 @@
+#include <vortex/utils/math.hpp>
+#include <vortex/utils/ros/qos_profiles.hpp>
+#include "pose_filtering/ros/pose_filtering_ros.hpp"
+
+namespace vortex::filtering {
+
+void PoseFilteringNode::setup_debug_publishers() {
+    std::string meas_topic_name =
+        this->declare_parameter<std::string>("debug.topic_name_meas");
+    std::string state_topic_name =
+        this->declare_parameter<std::string>("debug.topic_name_state");
+    const auto qos_sensor_data{
+        vortex::utils::qos_profiles::sensor_data_profile(10)};
+    pose_meas_debug_pub_ =
+        this->create_publisher<vortex_msgs::msg::PoseEulerStamped>(
+            meas_topic_name, qos_sensor_data);
+    pose_state_debug_pub_ =
+        this->create_publisher<vortex_msgs::msg::PoseEulerStamped>(
+            state_topic_name, qos_sensor_data);
+}
+
+void PoseFilteringNode::publish_meas_debug() {
+    if (measurements_.empty()) {
+        return;
+    }
+    Pose meas = measurements_.at(0);
+
+    vortex_msgs::msg::PoseEulerStamped msg;
+    msg.header.frame_id = target_frame_;
+    msg.header.stamp = this->get_clock()->now();
+    msg.x = meas.x;
+    msg.y = meas.y;
+    msg.z = meas.z;
+
+    Eigen::Vector3d euler =
+        vortex::utils::math::quat_to_euler(meas.ori_quaternion());
+
+    msg.roll = euler(0);
+    msg.pitch = euler(1);
+    msg.yaw = euler(2);
+
+    pose_meas_debug_pub_->publish(msg);
+}
+
+void PoseFilteringNode::publish_state_debug() {
+    if (track_manager_->get_tracks().empty()) {
+        return;
+    }
+    Track track = track_manager_->get_tracks().at(0);
+    Eigen::Vector3d pos = track.state_pos.mean();
+    Eigen::Quaterniond quat = track.orientation_filter.q;
+
+    Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(quat);
+
+    vortex_msgs::msg::PoseEulerStamped msg;
+    msg.header.frame_id = target_frame_;
+    msg.header.stamp = this->get_clock()->now();
+
+    msg.x = pos.x();
+    msg.y = pos.y();
+    msg.z = pos.z();
+
+    msg.roll = euler(0);
+    msg.pitch = euler(1);
+    msg.yaw = euler(2);
+
+    pose_state_debug_pub_->publish(msg);
+}
+
+}  // namespace vortex::filtering
diff --git a/filtering/pose_filtering/test/CMakeLists.txt b/filtering/pose_filtering/test/CMakeLists.txt
new file mode 100644
index 000000000..1408297d9
--- /dev/null
+++ b/filtering/pose_filtering/test/CMakeLists.txt
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.8)
+
+find_package(ament_cmake REQUIRED)
+find_package(GTest REQUIRED)
+find_package(vortex_filtering REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(tf2 REQUIRED)
+include(GoogleTest)
+
+set(TEST_BINARY_NAME ${PROJECT_NAME}_test)
+add_executable(
+    ${TEST_BINARY_NAME}
+    test_pose_filtering.cpp
+)
+
+target_link_libraries(
+  ${TEST_BINARY_NAME}
+  ${PROJECT_NAME}
+  GTest::GTest
+  GTest::gtest_main
+)
+
+ament_target_dependencies(
+  ${TEST_BINARY_NAME}
+  vortex_filtering
+  vortex_utils
+  Eigen3
+  tf2
+)
+
+gtest_discover_tests(${TEST_BINARY_NAME})
diff --git a/filtering/pose_filtering/test/test_pose_filtering.cpp b/filtering/pose_filtering/test/test_pose_filtering.cpp
new file mode 100644
index 000000000..df5e25369
--- /dev/null
+++ b/filtering/pose_filtering/test/test_pose_filtering.cpp
@@ -0,0 +1,161 @@
+#include <gtest/gtest.h>
+
+#include <Eigen/Dense>
+
+#include "pose_filtering/lib/pose_track_manager.hpp"
+#include "vortex/utils/types.hpp"
+
+namespace vortex::filtering {
+using vortex::utils::types::Pose;
+
+class PoseTrackManagerTests : public ::testing::Test {
+   protected:
+    TrackManagerConfig make_default_config() {
+        TrackManagerConfig cfg{};
+        cfg.dyn_mod.std_dev = 0.1;
+        cfg.sensor_mod.std_dev = 0.1;
+
+        cfg.existence.initial_existence_probability = 0.5;
+        cfg.existence.confirmation_threshold = 0.6;
+        cfg.existence.deletion_threshold = 0.2;
+
+        cfg.max_angle_gate_threshold = 0.5;  // radians
+
+        cfg.ipda.pdaf.prob_of_detection = 0.5;
+        cfg.ipda.pdaf.clutter_intensity = 0.01;
+        cfg.ipda.pdaf.mahalanobis_threshold = 1.0;
+        cfg.ipda.pdaf.min_gate_threshold = 0.0;
+        cfg.ipda.pdaf.max_gate_threshold = 1.0;
+        cfg.ipda.ipda.estimate_clutter = false;
+
+        return cfg;
+    }
+
+    Pose make_pose(
+        const Eigen::Vector3d& pos,
+        const Eigen::Quaterniond& q = Eigen::Quaterniond::Identity()) {
+        return Pose::from_eigen(pos, q);
+    }
+};
+
+TEST_F(PoseTrackManagerTests, creates_tracks_from_measurements) {
+    PoseTrackManager mgr(make_default_config());
+
+    std::vector<Pose> measurements{
+        make_pose({0.0, 0.0, 0.0}),
+        make_pose({1.0, 0.0, 0.0}),
+        make_pose({0.0, 1.0, 0.0}),
+    };
+
+    mgr.step(measurements, 0.1);
+
+    const auto& tracks = mgr.get_tracks();
+    ASSERT_EQ(tracks.size(), 3);
+
+    for (const auto& t : tracks) {
+        EXPECT_FALSE(t.confirmed);
+        EXPECT_NEAR(t.existence_probability, 0.5, 1e-12);
+        EXPECT_NEAR(t.orientation_filter.q.w(), 1.0, 1e-12);
+    }
+}
+
+TEST_F(PoseTrackManagerTests, angular_gate_filters_measurements) {
+    auto cfg = make_default_config();
+    cfg.max_angle_gate_threshold = 0.2;  // strict gate
+    PoseTrackManager mgr(cfg);
+
+    Eigen::Quaterniond q_ref = Eigen::Quaterniond::Identity();
+
+    Eigen::Quaterniond q_far(Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitZ()));
+
+    std::vector<Pose> measurements{
+        make_pose({0, 0, 1}, q_ref),
+        make_pose({0, 0, 1}, q_far),
+    };
+
+    mgr.step(measurements, 0.1);
+
+    ASSERT_EQ(mgr.get_tracks().size(), 2);
+
+    double t1_exist_prob_step1 = mgr.get_tracks().at(0).existence_probability;
+    double t2_exist_prob_step1 = mgr.get_tracks().at(1).existence_probability;
+
+    measurements = {
+        make_pose({0, 0.5, 1}, q_ref),
+        make_pose({0, 0.5, 1}, q_far),
+    };
+
+    mgr.step(measurements, 0.1);
+
+    ASSERT_EQ(mgr.get_tracks().size(), 2);
+
+    double t1_exist_prob_step2 = mgr.get_tracks().at(0).existence_probability;
+    double t2_exist_prob_step2 = mgr.get_tracks().at(1).existence_probability;
+
+    ASSERT_GT(t1_exist_prob_step2, t1_exist_prob_step1);
+    ASSERT_GT(t2_exist_prob_step2, t2_exist_prob_step1);
+}
+
+TEST_F(PoseTrackManagerTests, single_target_existance_increase) {
+    auto cfg = make_default_config();
+
+    Eigen::Quaterniond q = Eigen::Quaterniond::Identity();
+
+    std::vector<Pose> z = {make_pose({0, 0, 0}, q)};
+
+    PoseTrackManager mgr(cfg);
+
+    mgr.step(z, 0.1);
+
+    double exist_prob1 = mgr.get_tracks().front().existence_probability;
+
+    ASSERT_EQ(mgr.get_tracks().size(), 1);
+    ASSERT_EQ(exist_prob1, 0.5);
+
+    z = {make_pose({0, 0, 0}, q)};
+
+    mgr.step(z, 0.1);
+
+    double exist_prob2 = mgr.get_tracks().front().existence_probability;
+
+    ASSERT_EQ(mgr.get_tracks().size(), 1);
+    ASSERT_GT(exist_prob2, exist_prob1);
+
+    z = {make_pose({0, 0, 0}, q)};
+
+    mgr.step(z, 0.1);
+
+    double exist_prob3 = mgr.get_tracks().front().existence_probability;
+
+    ASSERT_EQ(mgr.get_tracks().size(), 1);
+    ASSERT_GT(exist_prob3, exist_prob2);
+}
+
+TEST_F(PoseTrackManagerTests, existence_decrease_on_no_detection) {
+    auto cfg = make_default_config();
+
+    Eigen::Quaterniond q = Eigen::Quaterniond::Identity();
+
+    std::vector<Pose> z = {make_pose({0, 0, 0}, q)};
+
+    PoseTrackManager mgr(cfg);
+
+    mgr.step(z, 0.1);
+
+    double exist_prob1 = mgr.get_tracks().front().existence_probability;
+    ASSERT_EQ(mgr.get_tracks().size(), 1);
+
+    z = {};
+
+    mgr.step(z, 0.1);
+
+    const auto& tracks_after = mgr.get_tracks();
+    if (tracks_after.empty()) {
+        SUCCEED();
+    } else {
+        double exist_prob2 = tracks_after.front().existence_probability;
+        ASSERT_LT(exist_prob2, exist_prob1);
+    }
+}
+
+}  // namespace vortex::filtering
diff --git a/tests/ros_node_tests/lqr_autopilot_node_test.sh b/tests/ros_node_tests/lqr_autopilot_node_test.sh
old mode 100644
new mode 100755