From bb68bd07fbc22ff42bc66f76aa1c9fc56e57effd Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sat, 10 Jan 2026 20:03:42 -0500 Subject: [PATCH] Update localizer implementation --- .../navigation/localizer/config/PARAMETERS.md | 141 ++ .../localizer/config/localizer.yaml | 91 +- .../include/localizer/state_estimator.h | 345 ++-- .../localizer/src/state_estimator.cpp | 1491 +++++++++++------ 4 files changed, 1431 insertions(+), 637 deletions(-) create mode 100644 src/subsystems/navigation/localizer/config/PARAMETERS.md diff --git a/src/subsystems/navigation/localizer/config/PARAMETERS.md b/src/subsystems/navigation/localizer/config/PARAMETERS.md new file mode 100644 index 0000000..3e1d987 --- /dev/null +++ b/src/subsystems/navigation/localizer/config/PARAMETERS.md @@ -0,0 +1,141 @@ +# State Estimator Configuration + +Configuration reference for the GTSAM-based state estimator. + +## Topics + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `imu_topic` | string | `/imu` | IMU measurements input | +| `gnss_topic` | string | `/gps/fix` | GNSS position fixes | +| `odom_topic` | string | `/odom/ground_truth` | Wheel odometry input | +| `output_odom_topic` | string | `/localization/odom` | Fused state output | + +## Frame IDs + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `tf_prefix` | string | `""` | Namespace prefix for all frames | +| `map_frame` | string | `map` | Global reference frame | +| `odom_frame` | string | `odom` | Odometry frame (drift-free subset) | +| `base_frame` | string | `base_footprint` | Robot body frame | +| `imu_frame` | string | `imu_link` | IMU sensor frame | +| `gnss_frame` | string | `gnss_link` | GNSS antenna frame | + +## Rates + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `publish_rate` | double | 50.0 | Odometry message publish rate (Hz) | +| `tf_publish_rate` | double | 50.0 | Transform broadcast rate (Hz) | +| `state_creation_rate` | double | 50.0 | Graph state creation rate (Hz). Higher values = denser graph, more compute | + +## IMU Noise Model + +These are continuous-time noise densities for the IMU preintegration. + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `accel_noise_sigma` | double | 0.1 | Accelerometer white noise (m/s²/√Hz) | +| `gyro_noise_sigma` | double | 0.01 | Gyroscope white noise (rad/s/√Hz) | +| `accel_bias_rw_sigma` | double | 1e-4 | Accel bias random walk (m/s³/√Hz) | +| `gyro_bias_rw_sigma` | double | 1e-5 | Gyro bias random walk (rad/s²/√Hz) | + +**Note:** Z-axis accelerometer uncertainty is automatically scaled 10x higher due to gravity dominance and poor vertical observability. + +### IMU Filtering (Optional) + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `enable_imu_filter` | bool | false | Apply exponential moving average filter to raw IMU | +| `imu_filter_alpha` | double | 0.3 | EMA smoothing factor (0=no update, 1=no smoothing) | + +## GNSS Parameters + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `gnss_position_sigma` | double | 1.0 | Base horizontal position uncertainty (m) | +| `gnss_altitude_sigma` | double | 0.15 | Vertical position uncertainty (m) | +| `gnss_max_age` | double | 1.0 | Maximum message age before rejection (s) | +| `gnss_fix_quality_multiplier` | double | 2.0 | Uncertainty multiplier for standard fix vs RTK | + +### Robust Estimation + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `use_robust_gnss_noise` | bool | true | Enable Huber M-estimator for outlier rejection | +| `gnss_huber_k` | double | 1.345 | Huber threshold (lower = more aggressive rejection) | + +## Odometry Parameters + +Noise model for relative odometry measurements. + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `odom_position_sigma` | double | 0.05 | Translation uncertainty (m) | +| `odom_rotation_sigma` | double | 0.05 | Rotation uncertainty (rad) | + +## ISAM2 Optimizer + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `isam2_relinearize_threshold` | double | 0.1 | Relinearization threshold (lower = more accurate but slower) | +| `isam2_relinearize_skip` | int | 1 | Relinearize every N updates (1 = every time) | + +## Graph Management + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `max_graph_states` | size_t | 100 | Maximum states in graph before marginalization | +| `imu_buffer_max_size` | size_t | 1000 | Maximum IMU measurements to buffer | + +**Duration:** At 50 Hz state creation, 100 states = 2 seconds of history. + +## Integration Parameters + +Internal preintegration tuning - usually don't touch these. + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `integration_covariance` | double | 1e-8 | IMU integration uncertainty scalar | +| `bias_acc_omega_int` | double | 1e-5 | Bias coupling during integration | + +## Initialization + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `min_imu_samples_for_init` | size_t | 100 | IMU samples needed for gravity alignment | +| `initial_velocity_sigma` | double | 0.1 | Initial velocity prior uncertainty (m/s) | +| `initial_rotation_sigma` | double | 0.1 | Initial orientation prior (rad) | +| `initial_accel_bias_sigma` | double | 0.1 | Initial accelerometer bias prior (m/s²) | +| `initial_gyro_bias_sigma` | double | 0.01 | Initial gyroscope bias prior (rad/s) | + +## ENU Origin + +Set the local tangent plane origin. If not provided, uses first GNSS fix. + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `origin_lat` | double | 0.0 | Latitude (degrees) | +| `origin_lon` | double | 0.0 | Longitude (degrees) | +| `origin_alt` | double | 0.0 | Altitude (meters) | + +## Common Tuning Scenarios + +### High-Rate IMU with Drift +- Increase `accel_bias_rw_sigma` and `gyro_bias_rw_sigma` to allow bias tracking +- Increase `max_graph_states` to maintain longer optimization window + +### Poor GNSS Quality +- Increase `gnss_position_sigma` +- Enable `use_robust_gnss_noise` +- Lower `gnss_huber_k` for aggressive outlier rejection + +### Computational Constraints +- Reduce `state_creation_rate` +- Reduce `max_graph_states` +- Increase `isam2_relinearize_skip` + +### Initialization Issues +- Increase `min_imu_samples_for_init` for better gravity estimate +- Check IMU is in base_link frame or transform is available diff --git a/src/subsystems/navigation/localizer/config/localizer.yaml b/src/subsystems/navigation/localizer/config/localizer.yaml index 54216e0..33bcd15 100644 --- a/src/subsystems/navigation/localizer/config/localizer.yaml +++ b/src/subsystems/navigation/localizer/config/localizer.yaml @@ -1,37 +1,88 @@ -localizer_node: +state_estimator: ros__parameters: - # Topic configuration + # ========================================================================== + # Topic Configuration + # ========================================================================== imu_topic: "/imu" gnss_topic: "/gps/fix" - odom_topic: "/odom" + odom_topic: "/odom/ground_truth" output_odom_topic: "/localization/odom" - # Publish rate (Hz) - publish_rate: 50.0 - - # Frame names + # ========================================================================== + # Frame Names + # ========================================================================== tf_prefix: "" map_frame: "map" odom_frame: "odom" - base_frame: "base_link" + base_frame: "base_footprint" imu_frame: "imu_link" gnss_frame: "gnss_link" - # IMU noise parameters - imu_accel_noise: 0.2 # m/s^2 - imu_gyro_noise: 0.01 # rad/s - imu_bias_noise: 0.0002 # bias random walk + # ========================================================================== + # Publish Rates + # ========================================================================== + publish_rate: 100.0 # Hz - high rate for smooth output + tf_publish_rate: 100.0 # Hz - increased from 50 for smoother transforms + + # ========================================================================== + # IMU Noise Parameters (continuous-time spectral densities) + # ========================================================================== + accel_noise_sigma: 3.0 # m/s^2/sqrt(Hz) - realistic for raw IMU data + gyro_noise_sigma: 10.0 # rad/s/sqrt(Hz) - realistic for raw gyro data + accel_bias_rw_sigma: 5.0e-2 # m/s^3/sqrt(Hz) - allow bias adaptation for raw data + gyro_bias_rw_sigma: 5.0e-2 # rad/s^2/sqrt(Hz) - allow gyro bias changes + + # ========================================================================== + # GNSS Parameters + # ========================================================================== + gnss_position_sigma: 0.0000001 # meters - realistic consumer GPS uncertainty + gnss_altitude_sigma: 0.0000001 + gnss_max_age: 0.5 # seconds - moderate timing constraint + gnss_fix_quality_multiplier: 1.0 # neutral GPS weight + + # GNSS outlier rejection (Huber robust noise model) + use_robust_gnss_noise: false # reject GPS jumps/outliers + gnss_huber_k: 1.0 # aggressive outlier rejection + + # ========================================================================== + # Odometry Parameters + # ========================================================================== + odom_position_sigma: 0.00001 # meters - realistic wheel odometry with slip + odom_rotation_sigma: 0.00001 # radians - account for steering uncertainty + + # ========================================================================== + # Graph Management - MAXIMUM STABILITY FOR RAW DATA + # ========================================================================== + state_creation_rate: 50.0 # Hz - high rate for dense optimization + max_graph_states: 500 # massive window: 10 seconds @ 50Hz + + # ISAM2 optimizer parameters - AGGRESSIVE OPTIMIZATION + isam2_relinearize_threshold: 0.01 # tight but realistic convergence + isam2_relinearize_skip: 1 # relinearize every update for stability - # GNSS noise parameters - gnss_noise: 0.25 # meters + # ========================================================================== + # IMU Integration Parameters - REALISTIC FOR RAW DATA + # ========================================================================== + integration_covariance: 1.0e-6 # account for discretization errors in raw data + bias_acc_omega_int: 1.0e-5 # realistic bias integration uncertainty - # Odometry noise parameters - odom_noise: 0.5 # meters + # ========================================================================== + # Initialization Parameters + # ========================================================================== + min_imu_samples_for_init: 100 # more samples for noisy raw data (1 second @ 100Hz) + initial_velocity_sigma: 0.5 # m/s - less confident with raw data + initial_rotation_sigma: 0.1 # rad - ~6° uncertainty for raw IMU + initial_accel_bias_sigma: 0.1 # m/s^2 - significant initial bias uncertainty + initial_gyro_bias_sigma: 0.01 # rad/s - significant gyro bias uncertainty - # State estimation window parameters - max_time_window: 10.0 # seconds - max_states: 100 + # ========================================================================== + # Buffer Configuration + # ========================================================================== + imu_buffer_max_size: 5000 # large buffer for dense processing and delays + # ========================================================================== + # ENU Origin (optional - if not set, uses first GNSS fix) + # ========================================================================== origin_lat: 38.42391162772634 origin_lon: -110.78490558433397 - origin_alt: 0.0 + origin_alt: 0.0 \ No newline at end of file diff --git a/src/subsystems/navigation/localizer/include/localizer/state_estimator.h b/src/subsystems/navigation/localizer/include/localizer/state_estimator.h index 0cef53a..cf7239d 100644 --- a/src/subsystems/navigation/localizer/include/localizer/state_estimator.h +++ b/src/subsystems/navigation/localizer/include/localizer/state_estimator.h @@ -1,161 +1,284 @@ -#pragma once +#ifndef LOCALIZER_STATE_ESTIMATOR_H_ +#define LOCALIZER_STATE_ESTIMATOR_H_ -// ROS2 #include #include #include #include -#include +#include #include #include +#include -// GTSAM -#include +#include #include #include -#include -#include -#include -#include -#include +#include +#include #include -// std +#include #include +#include #include -#include -#include -namespace localizer -{ +namespace localizer { + +using gtsam::symbol_shorthand::X; // Pose +using gtsam::symbol_shorthand::V; // Velocity +using gtsam::symbol_shorthand::B; // Bias + +// ============================================================================ +// Enums and Structs +// ============================================================================ + +enum class InitState { + WAITING_FOR_IMU, + WAITING_FOR_POSITION, + RUNNING +}; + +struct EstimatedState { + rclcpp::Time timestamp; + gtsam::NavState nav_state; + gtsam::imuBias::ConstantBias imu_bias; + Eigen::Matrix covariance; + gtsam::Vector3 body_rate; // Angular velocity in body frame (gyro - bias) + + nav_msgs::msg::Odometry to_odometry( + const std::string& frame_id, + const std::string& child_frame_id) const; +}; + +struct ImuMeasurement { + rclcpp::Time timestamp; + Eigen::Vector3d accel; + Eigen::Vector3d gyro; + + ImuMeasurement(const rclcpp::Time& t, const Eigen::Vector3d& a, const Eigen::Vector3d& g) + : timestamp(t), accel(a), gyro(g) {} +}; + +struct GraphStateEntry { + uint64_t key_index; + rclcpp::Time timestamp; + gtsam::NavState nav_state; + gtsam::imuBias::ConstantBias bias; + gtsam::Vector3 latest_gyro; // Most recent gyro measurement at this state +}; + +struct PendingGnss { + gtsam::Point3 position; + double sigma = 0.0; + rclcpp::Time timestamp; + bool valid = false; + + void reset() { + valid = false; + sigma = 0.0; + } +}; + +struct PendingOdom { + gtsam::Pose3 delta; + std::optional prev_pose; + bool valid = false; + + void reset() { + valid = false; + delta = gtsam::Pose3(); + prev_pose.reset(); + } +}; -struct FrameParams -{ - std::string tf_prefix = ""; +struct FrameParams { std::string map_frame = "map"; std::string odom_frame = "odom"; std::string base_frame = "base_link"; std::string imu_frame = "imu_link"; std::string gnss_frame = "gnss_link"; + std::string tf_prefix; }; -struct StateEstimatorParams -{ - // Noise parameters - double imu_accel_noise = 0.1; // m/s^2 - double imu_gyro_noise = 0.005; // rad/s - double imu_bias_noise = 0.0001; // bias random walk - double gnss_noise = 1.0; // meters - double odom_noise = 0.1; // meters - - // Window parameters - double max_time_window = 10.0; // seconds - size_t max_states = 100; - - // Frame names - FrameParams frames; +struct CachedFrameIds { + std::string map; + std::string odom; + std::string base; }; -struct EstimatedState -{ - rclcpp::Time timestamp; - gtsam::NavState nav_state; - gtsam::imuBias::ConstantBias imu_bias; - Eigen::Matrix covariance; +struct StateEstimatorParams { + // IMU noise (continuous-time spectral densities) + double accel_noise_sigma = 0.1; + double gyro_noise_sigma = 0.01; + double accel_bias_rw_sigma = 0.001; + double gyro_bias_rw_sigma = 0.0001; + + // Sensor noise + double gnss_position_sigma = 1.0; + double gnss_altitude_sigma = 0.15; + double odom_position_sigma = 0.1; + double odom_rotation_sigma = 0.05; + + // Graph management + size_t min_imu_samples_for_init = 100; + double state_creation_rate = 10.0; + size_t max_graph_states = 100; + double gnss_max_age = 0.5; + + // Frames + FrameParams frames; + + // ISAM2 + double isam2_relinearize_threshold = 0.1; + int isam2_relinearize_skip = 1; + + // IMU integration + double integration_covariance = 1e-8; + double bias_acc_omega_int = 1e-5; + + // Initialization + double initial_velocity_sigma = 0.1; + double initial_rotation_sigma = 0.1; + double initial_accel_bias_sigma = 0.1; + double initial_gyro_bias_sigma = 0.01; + double gnss_fix_quality_multiplier = 2.0; + + // GNSS outlier rejection + bool use_robust_gnss_noise = true; + double gnss_huber_k = 1.345; + + // Buffer and timing + size_t imu_buffer_max_size = 1000; + double tf_publish_rate = 50.0; - nav_msgs::msg::Odometry to_odometry(const std::string& frame_id, const std::string& child_frame_id) const; + // IMU filtering (EMA) + bool enable_imu_filter = true; + double imu_filter_alpha = 0.3; }; -class StateEstimator : public rclcpp::Node -{ +// ============================================================================ +// StateEstimator Class +// ============================================================================ + +class StateEstimator : public rclcpp::Node { public: StateEstimator(); ~StateEstimator(); - + void reset(); - + InitState get_init_state() const; std::optional get_latest_state() const; - std::optional get_state_at_time(const rclcpp::Time& timestamp) const; - - bool is_initialized() const { return initialized_; } + + // For testing + rclcpp::Time get_imu_buffer_start_time() const; + rclcpp::Time get_imu_buffer_end_time() const; private: - // Sensor fusion (used as subscription callbacks) - void fuse_imu(sensor_msgs::msg::Imu::SharedPtr imu_msg); - void fuse_gnss(sensor_msgs::msg::NavSatFix::SharedPtr gnss_msg); - void fuse_odometry(nav_msgs::msg::Odometry::SharedPtr odom_msg); - - // Publishing - void publish_state(); - void publish_transforms(); - - // Graph optimization - void optimize_graph(); - void add_imu_factor(); - void add_gnss_factor(const sensor_msgs::msg::NavSatFix::SharedPtr& gnss_msg); - void add_odom_factor(const nav_msgs::msg::Odometry::SharedPtr& odom_msg); - void createNewState(); + // Parameter handling + void declare_parameters(); + void validate_parameters(); + void init_imu_params(); + void init_noise_models(); + void cache_frame_ids(); + + // Sensor callbacks + void imu_callback(sensor_msgs::msg::Imu::SharedPtr msg); + void gnss_callback(sensor_msgs::msg::NavSatFix::SharedPtr msg); + void odom_callback(nav_msgs::msg::Odometry::SharedPtr msg); + + // Initialization + bool initialize_graph(const gtsam::Point3& position, const rclcpp::Time& timestamp); + gtsam::Rot3 estimate_initial_orientation_locked() const; + + // State creation + void create_new_state(const rclcpp::Time& timestamp); void marginalize_old_states(); - void initialize_with_gnss(const gtsam::Point3& gnss_pos, const rclcpp::Time& timestamp); - - // Coordinate conversion - gtsam::Point3 lla_to_enu(double lat, double lon, double alt) const; + + // IMU integration + void integrate_imu_measurements_locked(const rclcpp::Time& from_time, const rclcpp::Time& to_time); + void reset_preintegration(const gtsam::imuBias::ConstantBias& bias); + void prune_imu_buffer_locked(const rclcpp::Time& before); + + // Coordinate transforms + gtsam::Point3 lla_to_enu(double lat, double lon, double alt); void set_enu_origin(double lat, double lon, double alt); - - // Key generation - gtsam::Key pose_key(size_t i) const { return gtsam::Symbol('x', i); } - gtsam::Key vel_key(size_t i) const { return gtsam::Symbol('v', i); } - gtsam::Key bias_key(size_t i) const { return gtsam::Symbol('b', i); } + + // TF handling + void lookup_sensor_transforms(); + void publish_state(); + void publish_transforms(); // Parameters StateEstimatorParams params_; + + // ISAM2 optimizer + std::unique_ptr isam_; + + // IMU preintegration + boost::shared_ptr imu_params_; + std::unique_ptr preintegrated_imu_; + + // Pre-computed noise models + gtsam::SharedNoiseModel initial_pose_noise_; + gtsam::SharedNoiseModel initial_vel_noise_; + gtsam::SharedNoiseModel initial_bias_noise_; + gtsam::SharedNoiseModel odom_noise_; + + // State tracking + std::vector graph_states_; + uint64_t current_key_index_; + uint64_t oldest_key_index_; + std::atomic init_state_; + + // Timing + double min_state_dt_; + rclcpp::Time last_state_time_; + + // IMU buffer + std::deque imu_buffer_; + + // Pending measurements + PendingGnss pending_gnss_; + PendingOdom pending_odom_; + + // ENU origin + bool enu_origin_set_; + double origin_lat_; + double origin_lon_; + double origin_alt_; + + // Cached sensor transforms + std::optional imu_to_base_; + std::optional gnss_to_base_; + bool imu_extrinsic_set_; // Flag to track if IMU extrinsic is initialized + + // EMA filter state (for accel x, y, z and gyro x, y, z) + std::optional filtered_accel_; + std::optional filtered_gyro_; + bool filter_initialized_; - // ROS2 subscriptions and publishers + // Cached frame IDs + CachedFrameIds cached_frames_; + + // Synchronization - mutable for const methods + mutable std::mutex state_mutex_; + mutable std::mutex imu_mutex_; + + // ROS interfaces rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Subscription::SharedPtr gnss_sub_; rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::TimerBase::SharedPtr publish_timer_; - - // TF2 components - std::unique_ptr tf_broadcaster_; + rclcpp::Publisher::SharedPtr filtered_imu_pub_; + std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; + std::unique_ptr tf_broadcaster_; + + rclcpp::TimerBase::SharedPtr publish_timer_; rclcpp::TimerBase::SharedPtr tf_timer_; - - // Transform caches - std::optional imu_to_base_; - std::optional gnss_to_base_; - - // GTSAM components - std::unique_ptr isam_; - std::unique_ptr new_factors_; - std::unique_ptr new_values_; - std::unique_ptr imu_preintegrated_; - - // State management - std::deque state_history_; - std::deque imu_buffer_; - - // State tracking - gtsam::Pose3 odom_to_base_; - gtsam::Pose3 map_to_odom_; - gtsam::Key current_pose_key_, current_vel_key_, current_bias_key_; - size_t key_index_; - bool initialized_; - rclcpp::Time last_optimization_time_; - std::optional prev_odom_pose_; - - // ENU origin - bool enu_origin_set_; - double origin_lat_, origin_lon_, origin_alt_; - - // Noise models - gtsam::SharedNoiseModel imu_noise_model_; - gtsam::SharedNoiseModel gnss_noise_model_; - gtsam::SharedNoiseModel odom_noise_model_; - - mutable std::mutex state_mutex_; }; -} // namespace localizer \ No newline at end of file +} // namespace localizer + +#endif // LOCALIZER_STATE_ESTIMATOR_H_ \ No newline at end of file diff --git a/src/subsystems/navigation/localizer/src/state_estimator.cpp b/src/subsystems/navigation/localizer/src/state_estimator.cpp index c32dbbf..4e825f4 100644 --- a/src/subsystems/navigation/localizer/src/state_estimator.cpp +++ b/src/subsystems/navigation/localizer/src/state_estimator.cpp @@ -1,649 +1,1128 @@ #include "localizer/state_estimator.h" + #include #include #include -#include #include +#include #include #include -using namespace gtsam; +#include namespace localizer { + nav_msgs::msg::Odometry EstimatedState::to_odometry( + const std::string &frame_id, + const std::string &child_frame_id) const + { + nav_msgs::msg::Odometry odom; + odom.header.stamp = timestamp; + odom.header.frame_id = frame_id; + odom.child_frame_id = child_frame_id; + + const auto &pose = nav_state.pose(); + const auto &vel = nav_state.velocity(); + + odom.pose.pose.position.x = pose.x(); + odom.pose.pose.position.y = pose.y(); + odom.pose.pose.position.z = pose.z(); + + auto quat = pose.rotation().toQuaternion(); + odom.pose.pose.orientation.w = quat.w(); + odom.pose.pose.orientation.x = quat.x(); + odom.pose.pose.orientation.y = quat.y(); + odom.pose.pose.orientation.z = quat.z(); + + odom.twist.twist.linear.x = vel.x(); + odom.twist.twist.linear.y = vel.y(); + odom.twist.twist.linear.z = vel.z(); + + // ROS covariance: [x, y, z, roll, pitch, yaw], GTSAM: [roll, pitch, yaw, x, y, z] + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < 3; ++j) + { + odom.pose.covariance[i * 6 + j] = covariance(3 + i, 3 + j); + odom.pose.covariance[(3 + i) * 6 + (3 + j)] = covariance(i, j); + } + } -nav_msgs::msg::Odometry EstimatedState::to_odometry(const std::string& frame_id, const std::string& child_frame_id) const -{ - nav_msgs::msg::Odometry odom; - odom.header.stamp = timestamp; - odom.header.frame_id = frame_id; - odom.child_frame_id = child_frame_id; - - auto pose = nav_state.pose(); - auto vel = nav_state.velocity(); - - odom.pose.pose.position.x = pose.x(); - odom.pose.pose.position.y = pose.y(); - odom.pose.pose.position.z = pose.z(); - - auto quat = pose.rotation().toQuaternion(); - odom.pose.pose.orientation.w = quat.w(); - odom.pose.pose.orientation.x = quat.x(); - odom.pose.pose.orientation.y = quat.y(); - odom.pose.pose.orientation.z = quat.z(); - - odom.twist.twist.linear.x = vel.x(); - odom.twist.twist.linear.y = vel.y(); - odom.twist.twist.linear.z = vel.z(); - - for (int i = 0; i < 36; ++i) { - odom.pose.covariance[i] = 0.0; - odom.twist.covariance[i] = 0.0; - } - for (int i = 0; i < 6; ++i) { - for (int j = 0; j < 6; ++j) { - if (i < 3 && j < 3) { - odom.pose.covariance[i*6 + j] = covariance(i, j); + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < 3; ++j) + { + odom.twist.covariance[i * 6 + j] = covariance(6 + i, 6 + j); } } + + return odom; + } + StateEstimator::StateEstimator() + : Node("state_estimator"), current_key_index_(0), oldest_key_index_(0), + enu_origin_set_(false), origin_lat_(0.0), origin_lon_(0.0), origin_alt_(0.0), + filter_initialized_(false) + { + init_state_.store(InitState::WAITING_FOR_IMU); + + declare_parameters(); + validate_parameters(); + + gtsam::ISAM2Params isam_params; + isam_params.relinearizeThreshold = params_.isam2_relinearize_threshold; + isam_params.relinearizeSkip = params_.isam2_relinearize_skip; + isam_params.findUnusedFactorSlots = true; + isam_ = std::make_unique(isam_params); + + init_imu_params(); + init_noise_models(); + cache_frame_ids(); + + min_state_dt_ = 1.0 / params_.state_creation_rate; + last_state_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + tf_broadcaster_ = std::make_unique(*this); + + std::string imu_topic = this->get_parameter("imu_topic").as_string(); + std::string gnss_topic = this->get_parameter("gnss_topic").as_string(); + std::string odom_topic = this->get_parameter("odom_topic").as_string(); + + imu_sub_ = create_subscription( + imu_topic, rclcpp::SensorDataQoS(), + std::bind(&StateEstimator::imu_callback, this, std::placeholders::_1)); + + gnss_sub_ = create_subscription( + gnss_topic, rclcpp::SensorDataQoS(), + std::bind(&StateEstimator::gnss_callback, this, std::placeholders::_1)); + + odom_sub_ = create_subscription( + odom_topic, rclcpp::SensorDataQoS(), + std::bind(&StateEstimator::odom_callback, this, std::placeholders::_1)); + + std::string output_topic = this->get_parameter("output_odom_topic").as_string(); + odom_pub_ = create_publisher(output_topic, 10); + filtered_imu_pub_ = create_publisher("/imu/filtered", rclcpp::SensorDataQoS()); + + double publish_rate = this->get_parameter("publish_rate").as_double(); + publish_timer_ = create_wall_timer( + std::chrono::duration(1.0 / publish_rate), + std::bind(&StateEstimator::publish_state, this)); + + tf_timer_ = create_wall_timer( + std::chrono::duration(1.0 / params_.tf_publish_rate), + std::bind(&StateEstimator::publish_transforms, this)); + + RCLCPP_INFO(get_logger(), "StateEstimator initialized"); + RCLCPP_INFO(get_logger(), " State: WAITING_FOR_IMU"); + RCLCPP_INFO(get_logger(), " IMU topic: %s", + this->get_parameter("imu_topic").as_string().c_str()); + RCLCPP_INFO(get_logger(), " GNSS topic: %s", + this->get_parameter("gnss_topic").as_string().c_str()); + RCLCPP_INFO(get_logger(), " Odom topic: %s", + this->get_parameter("odom_topic").as_string().c_str()); } - return odom; -} + StateEstimator::~StateEstimator() = default; + + void StateEstimator::declare_parameters() + { + declare_parameter("imu_topic", "/imu"); + declare_parameter("gnss_topic", "/gps/fix"); + declare_parameter("odom_topic", "/odom/ground_truth"); + declare_parameter("output_odom_topic", "/localization/odom"); + declare_parameter("publish_rate", 50.0); + + params_.accel_noise_sigma = declare_parameter("accel_noise_sigma", params_.accel_noise_sigma); + params_.gyro_noise_sigma = declare_parameter("gyro_noise_sigma", params_.gyro_noise_sigma); + params_.accel_bias_rw_sigma = declare_parameter("accel_bias_rw_sigma", params_.accel_bias_rw_sigma); + params_.gyro_bias_rw_sigma = declare_parameter("gyro_bias_rw_sigma", params_.gyro_bias_rw_sigma); + + params_.gnss_position_sigma = declare_parameter("gnss_position_sigma", params_.gnss_position_sigma); + params_.gnss_altitude_sigma = declare_parameter("gnss_altitude_sigma", 0.15); + params_.odom_position_sigma = declare_parameter("odom_position_sigma", params_.odom_position_sigma); + params_.odom_rotation_sigma = declare_parameter("odom_rotation_sigma", params_.odom_rotation_sigma); + + params_.min_imu_samples_for_init = declare_parameter("min_imu_samples_for_init", + static_cast(params_.min_imu_samples_for_init)); + params_.state_creation_rate = declare_parameter("state_creation_rate", params_.state_creation_rate); + params_.max_graph_states = declare_parameter("max_graph_states", + static_cast(params_.max_graph_states)); + params_.gnss_max_age = declare_parameter("gnss_max_age", params_.gnss_max_age); + + params_.frames.map_frame = declare_parameter("map_frame", params_.frames.map_frame); + params_.frames.odom_frame = declare_parameter("odom_frame", params_.frames.odom_frame); + params_.frames.base_frame = declare_parameter("base_frame", params_.frames.base_frame); + params_.frames.imu_frame = declare_parameter("imu_frame", params_.frames.imu_frame); + params_.frames.gnss_frame = declare_parameter("gnss_frame", params_.frames.gnss_frame); + params_.frames.tf_prefix = declare_parameter("tf_prefix", params_.frames.tf_prefix); + + params_.isam2_relinearize_threshold = declare_parameter("isam2_relinearize_threshold", + params_.isam2_relinearize_threshold); + params_.isam2_relinearize_skip = declare_parameter("isam2_relinearize_skip", + params_.isam2_relinearize_skip); + + params_.integration_covariance = declare_parameter("integration_covariance", + params_.integration_covariance); + params_.bias_acc_omega_int = declare_parameter("bias_acc_omega_int", + params_.bias_acc_omega_int); + + params_.initial_velocity_sigma = declare_parameter("initial_velocity_sigma", + params_.initial_velocity_sigma); + params_.gnss_fix_quality_multiplier = declare_parameter("gnss_fix_quality_multiplier", + params_.gnss_fix_quality_multiplier); + + params_.use_robust_gnss_noise = declare_parameter("use_robust_gnss_noise", + params_.use_robust_gnss_noise); + params_.gnss_huber_k = declare_parameter("gnss_huber_k", params_.gnss_huber_k); + + params_.initial_accel_bias_sigma = declare_parameter("initial_accel_bias_sigma", + params_.initial_accel_bias_sigma); + params_.initial_gyro_bias_sigma = declare_parameter("initial_gyro_bias_sigma", + params_.initial_gyro_bias_sigma); + + params_.initial_rotation_sigma = declare_parameter("initial_rotation_sigma", + params_.initial_rotation_sigma); + + params_.imu_buffer_max_size = declare_parameter("imu_buffer_max_size", + static_cast(params_.imu_buffer_max_size)); + params_.tf_publish_rate = declare_parameter("tf_publish_rate", params_.tf_publish_rate); + + params_.enable_imu_filter = declare_parameter("enable_imu_filter", params_.enable_imu_filter); + params_.imu_filter_alpha = declare_parameter("imu_filter_alpha", params_.imu_filter_alpha); + + double origin_lat = declare_parameter("origin_lat", 0.0); + double origin_lon = declare_parameter("origin_lon", 0.0); + double origin_alt = declare_parameter("origin_alt", 0.0); + + if (std::abs(origin_lat) > 1e-6 || std::abs(origin_lon) > 1e-6) + { + set_enu_origin(origin_lat, origin_lon, origin_alt); + } + } -StateEstimator::StateEstimator() - : Node("localizer_node") - , odom_to_base_(Pose3()) - , map_to_odom_(Pose3()) - , key_index_(0) - , initialized_(false) - , enu_origin_set_(false) - , origin_lat_(0.0) - , origin_lon_(0.0) - , origin_alt_(0.0) -{ - ISAM2Params isam_params; - isam_params.factorization = ISAM2Params::CHOLESKY; - isam_params.relinearizeSkip = 10; - - isam_ = std::make_unique(isam_params); - new_factors_ = std::make_unique(); - new_values_ = std::make_unique(); - - std::string imu_topic = this->declare_parameter("imu_topic", "/imu"); - std::string gnss_topic = this->declare_parameter("gnss_topic", "/gps/fix"); - std::string odom_topic = this->declare_parameter("odom_topic", "/odom"); - std::string output_odom_topic = this->declare_parameter("output_odom_topic", "/localization/odom"); - double publish_rate = this->declare_parameter("publish_rate", 50.0); - - params_.imu_accel_noise = this->declare_parameter("imu_accel_noise", params_.imu_accel_noise); - params_.imu_gyro_noise = this->declare_parameter("imu_gyro_noise", params_.imu_gyro_noise); - params_.imu_bias_noise = this->declare_parameter("imu_bias_noise", params_.imu_bias_noise); - params_.gnss_noise = this->declare_parameter("gnss_noise", params_.gnss_noise); - params_.odom_noise = this->declare_parameter("odom_noise", params_.odom_noise); - params_.max_time_window = this->declare_parameter("max_time_window", params_.max_time_window); - params_.max_states = this->declare_parameter("max_states", static_cast(params_.max_states)); - - params_.frames.tf_prefix = this->declare_parameter("tf_prefix", params_.frames.tf_prefix); - params_.frames.map_frame = this->declare_parameter("map_frame", params_.frames.map_frame); - params_.frames.base_frame = this->declare_parameter("base_frame", params_.frames.base_frame); - params_.frames.odom_frame = this->declare_parameter("odom_frame", params_.frames.odom_frame); - params_.frames.imu_frame = this->declare_parameter("imu_frame", params_.frames.imu_frame); - params_.frames.gnss_frame = this->declare_parameter("gnss_frame", params_.frames.gnss_frame); - - double origin_lat = this->declare_parameter("origin_lat", 0.0); - double origin_lon = this->declare_parameter("origin_lon", 0.0); - double origin_alt = this->declare_parameter("origin_alt", 0.0); - - if (std::abs(origin_lat) > 1e-6 || std::abs(origin_lon) > 1e-6) { - set_enu_origin(origin_lat, origin_lon, origin_alt); + void StateEstimator::validate_parameters() + { + bool valid = true; + + auto check_positive = [this, &valid](double val, const char *name) + { + if (val <= 0.0) + { + RCLCPP_ERROR(get_logger(), "%s must be positive, got %.6f", name, val); + valid = false; + } + }; + + check_positive(params_.accel_noise_sigma, "accel_noise_sigma"); + check_positive(params_.gyro_noise_sigma, "gyro_noise_sigma"); + check_positive(params_.accel_bias_rw_sigma, "accel_bias_rw_sigma"); + check_positive(params_.gyro_bias_rw_sigma, "gyro_bias_rw_sigma"); + check_positive(params_.gnss_position_sigma, "gnss_position_sigma"); + check_positive(params_.odom_position_sigma, "odom_position_sigma"); + check_positive(params_.odom_rotation_sigma, "odom_rotation_sigma"); + check_positive(params_.state_creation_rate, "state_creation_rate"); + check_positive(params_.gnss_max_age, "gnss_max_age"); + check_positive(params_.gnss_huber_k, "gnss_huber_k"); + check_positive(params_.initial_accel_bias_sigma, "initial_accel_bias_sigma"); + check_positive(params_.initial_gyro_bias_sigma, "initial_gyro_bias_sigma"); + check_positive(params_.initial_rotation_sigma, "initial_rotation_sigma"); + check_positive(params_.tf_publish_rate, "tf_publish_rate"); + check_positive(params_.initial_velocity_sigma, "initial_velocity_sigma"); + + if (!valid) + { + throw std::runtime_error("Invalid StateEstimator parameters"); + } } - RCLCPP_INFO(this->get_logger(), "Parameters: imu_accel=%.4f, imu_gyro=%.4f, gnss=%.2f", - params_.imu_accel_noise, params_.imu_gyro_noise, params_.gnss_noise); + void StateEstimator::init_imu_params() + { + imu_params_ = boost::shared_ptr( + new gtsam::PreintegratedCombinedMeasurements::Params()); + + imu_params_->n_gravity = gtsam::Vector3(0.0, 0.0, -9.81); + + double accel_xy_var = params_.accel_noise_sigma * params_.accel_noise_sigma; + double gyro_var = params_.gyro_noise_sigma * params_.gyro_noise_sigma; + double accel_bias_var = params_.accel_bias_rw_sigma * params_.accel_bias_rw_sigma; + double gyro_bias_var = params_.gyro_bias_rw_sigma * params_.gyro_bias_rw_sigma; + + // Anisotropic accelerometer noise: higher Z-axis uncertainty due to gravity dominance + // and poor observability of vertical velocity without barometer + double accel_z_var = accel_xy_var * 10.0; + + gtsam::Matrix3 accel_cov = gtsam::Matrix3::Zero(); + accel_cov(0, 0) = accel_xy_var; + accel_cov(1, 1) = accel_xy_var; + accel_cov(2, 2) = accel_z_var; + imu_params_->accelerometerCovariance = accel_cov; + + RCLCPP_INFO(get_logger(), + "Accelerometer covariance: XY_sigma=%.4f, Z_sigma=%.4f (%.1fx higher)", + params_.accel_noise_sigma, + std::sqrt(accel_z_var), + std::sqrt(accel_z_var) / params_.accel_noise_sigma); + + imu_params_->gyroscopeCovariance = gtsam::I_3x3 * gyro_var; + imu_params_->biasAccCovariance = gtsam::I_3x3 * accel_bias_var; + imu_params_->biasOmegaCovariance = gtsam::I_3x3 * gyro_bias_var; + imu_params_->integrationCovariance = gtsam::I_3x3 * params_.integration_covariance; + imu_params_->biasAccOmegaInt = gtsam::I_6x6 * params_.bias_acc_omega_int; + + RCLCPP_INFO(get_logger(), + "IMU params: accel_sigma_xy=%.4f, accel_sigma_z=%.4f, gyro_sigma=%.4f", + params_.accel_noise_sigma, + std::sqrt(accel_z_var), + params_.gyro_noise_sigma); + + RCLCPP_INFO(get_logger(), + "IMU bias random walk: accel_bias_sigma=%.6f, gyro_bias_sigma=%.6f", + params_.accel_bias_rw_sigma, + params_.gyro_bias_rw_sigma); + } - tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - tf_broadcaster_ = std::make_unique(*this); + void StateEstimator::init_noise_models() + { + initial_pose_noise_ = gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector6() << params_.initial_rotation_sigma, + params_.initial_rotation_sigma, + params_.initial_rotation_sigma, + params_.gnss_position_sigma, + params_.gnss_position_sigma, + params_.gnss_position_sigma) + .finished()); + + initial_vel_noise_ = gtsam::noiseModel::Isotropic::Sigma(3, params_.initial_velocity_sigma); + + initial_bias_noise_ = gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector6() << params_.initial_accel_bias_sigma, + params_.initial_accel_bias_sigma, + params_.initial_accel_bias_sigma, + params_.initial_gyro_bias_sigma, + params_.initial_gyro_bias_sigma, + params_.initial_gyro_bias_sigma) + .finished()); + + odom_noise_ = gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector6() << params_.odom_rotation_sigma, + params_.odom_rotation_sigma, + params_.odom_rotation_sigma, + params_.odom_position_sigma, + params_.odom_position_sigma, + params_.odom_position_sigma) + .finished()); + + RCLCPP_INFO(get_logger(), + "Noise models initialized: pose_sigma=%.3f, vel_sigma=%.3f, odom_sigma=%.3f", + params_.gnss_position_sigma, + params_.initial_velocity_sigma, + params_.odom_position_sigma); + } + void StateEstimator::cache_frame_ids() + { + auto add_prefix = [this](const std::string &frame) + { + if (params_.frames.tf_prefix.empty()) + { + return frame; + } + return params_.frames.tf_prefix + "/" + frame; + }; - tf_timer_ = this->create_wall_timer( - std::chrono::milliseconds(100), - [this]() { publish_transforms(); }); + cached_frames_.map = add_prefix(params_.frames.map_frame); + cached_frames_.odom = add_prefix(params_.frames.odom_frame); + cached_frames_.base = add_prefix(params_.frames.base_frame); + } - Matrix33 imu_cov = Matrix33::Identity() * params_.imu_accel_noise * params_.imu_accel_noise; - Matrix33 gyro_cov = Matrix33::Identity() * params_.imu_gyro_noise * params_.imu_gyro_noise; - Matrix33 bias_cov = Matrix33::Identity() * params_.imu_bias_noise * params_.imu_bias_noise; + void StateEstimator::reset() + { + std::scoped_lock lock(state_mutex_, imu_mutex_); - auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(9.81); - imu_params->accelerometerCovariance = imu_cov; - imu_params->gyroscopeCovariance = gyro_cov; - imu_params->integrationCovariance = bias_cov; + init_state_.store(InitState::WAITING_FOR_IMU); - imu_preintegrated_ = std::make_unique(imu_params, imuBias::ConstantBias()); + gtsam::ISAM2Params isam_params; + isam_params.relinearizeThreshold = params_.isam2_relinearize_threshold; + isam_params.relinearizeSkip = params_.isam2_relinearize_skip; + isam_params.findUnusedFactorSlots = true; + isam_ = std::make_unique(isam_params); - gnss_noise_model_ = noiseModel::Diagonal::Sigmas((Vector3() << params_.gnss_noise, params_.gnss_noise, params_.gnss_noise).finished()); - odom_noise_model_ = noiseModel::Diagonal::Sigmas((Vector6() << params_.odom_noise, params_.odom_noise, params_.odom_noise, params_.odom_noise, params_.odom_noise, params_.odom_noise).finished()); + current_key_index_ = 0; + oldest_key_index_ = 0; + graph_states_.clear(); + imu_buffer_.clear(); + preintegrated_imu_.reset(); - imu_sub_ = this->create_subscription( - imu_topic, rclcpp::SensorDataQoS(), - std::bind(&StateEstimator::fuse_imu, this, std::placeholders::_1)); + pending_gnss_.reset(); + pending_odom_.reset(); - gnss_sub_ = this->create_subscription( - gnss_topic, rclcpp::SensorDataQoS(), - std::bind(&StateEstimator::fuse_gnss, this, std::placeholders::_1)); + last_state_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + imu_to_base_.reset(); + gnss_to_base_.reset(); - odom_sub_ = this->create_subscription( - odom_topic, rclcpp::SensorDataQoS(), - std::bind(&StateEstimator::fuse_odometry, this, std::placeholders::_1)); + // Reset EMA filter state + filtered_accel_.reset(); + filtered_gyro_.reset(); + filter_initialized_ = false; - odom_pub_ = this->create_publisher(output_odom_topic, 10); + RCLCPP_INFO(get_logger(), "StateEstimator reset to WAITING_FOR_IMU"); + } - auto period = std::chrono::duration(1.0 / publish_rate); - publish_timer_ = this->create_wall_timer( - std::chrono::duration_cast(period), - std::bind(&StateEstimator::publish_state, this)); + InitState StateEstimator::get_init_state() const + { + return init_state_.load(); + } - RCLCPP_INFO(this->get_logger(), "Localizer Node initialized"); - RCLCPP_INFO(this->get_logger(), " IMU topic: %s", imu_topic.c_str()); - RCLCPP_INFO(this->get_logger(), " GNSS topic: %s", gnss_topic.c_str()); - RCLCPP_INFO(this->get_logger(), " Odom topic: %s", odom_topic.c_str()); - RCLCPP_INFO(this->get_logger(), " Output odom: %s", output_odom_topic.c_str()); -} + void StateEstimator::imu_callback(sensor_msgs::msg::Imu::SharedPtr msg) + { + lookup_sensor_transforms(); + + Eigen::Vector3d accel_raw( + msg->linear_acceleration.x, + msg->linear_acceleration.y, + msg->linear_acceleration.z); + Eigen::Vector3d gyro_raw( + msg->angular_velocity.x, + msg->angular_velocity.y, + msg->angular_velocity.z); + + Eigen::Vector3d accel = accel_raw; + Eigen::Vector3d gyro = gyro_raw; + + // Transform to base_link frame if transform available + if (imu_to_base_) + { + gtsam::Rot3 R = imu_to_base_->rotation(); + accel = R.rotate(gtsam::Point3(accel_raw)); + gyro = R.rotate(gtsam::Point3(gyro_raw)); + } + else + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "No imu_to_base transform available - using raw IMU data"); + } -StateEstimator::~StateEstimator() = default; + if (params_.enable_imu_filter) + { + if (!filter_initialized_) + { + filtered_accel_ = accel; + filtered_gyro_ = gyro; + filter_initialized_ = true; + } + else + { + double alpha = params_.imu_filter_alpha; + filtered_accel_ = alpha * accel + (1.0 - alpha) * (*filtered_accel_); + filtered_gyro_ = alpha * gyro + (1.0 - alpha) * (*filtered_gyro_); + } -void StateEstimator::reset() -{ - std::lock_guard lock(state_mutex_); - - ISAM2Params isam_params; - isam_params.factorization = ISAM2Params::CHOLESKY; - isam_params.relinearizeSkip = 10; - - isam_ = std::make_unique(isam_params); - new_factors_->resize(0); - new_values_->clear(); - state_history_.clear(); - imu_buffer_.clear(); - - odom_to_base_ = Pose3(); - map_to_odom_ = Pose3(); - key_index_ = 0; - initialized_ = false; - enu_origin_set_ = false; - prev_odom_pose_ = std::nullopt; - - if (imu_preintegrated_) { - imu_preintegrated_->resetIntegration(); - } + accel = *filtered_accel_; + gyro = *filtered_gyro_; - RCLCPP_INFO(this->get_logger(), "StateEstimator reset"); -} + auto filtered_msg = std::make_unique(); + filtered_msg->header = msg->header; + filtered_msg->header.frame_id = params_.frames.base_frame; -void StateEstimator::publish_state() -{ - auto latest_state = get_latest_state(); - if (!latest_state.has_value()) { - return; - } + filtered_msg->linear_acceleration.x = accel.x(); + filtered_msg->linear_acceleration.y = accel.y(); + filtered_msg->linear_acceleration.z = accel.z(); - std::string full_map_frame = params_.frames.tf_prefix.empty() ? - params_.frames.map_frame : params_.frames.tf_prefix + "/" + params_.frames.map_frame; - std::string full_base_frame = params_.frames.tf_prefix.empty() ? - params_.frames.base_frame : params_.frames.tf_prefix + "/" + params_.frames.base_frame; + filtered_msg->angular_velocity.x = gyro.x(); + filtered_msg->angular_velocity.y = gyro.y(); + filtered_msg->angular_velocity.z = gyro.z(); - odom_pub_->publish(latest_state->to_odometry(full_map_frame, full_base_frame)); -} + filtered_msg->orientation = msg->orientation; -void StateEstimator::fuse_imu(sensor_msgs::msg::Imu::SharedPtr imu_msg) -{ - std::lock_guard lock(state_mutex_); - - if (!imu_to_base_) { - try { - auto tf = tf_buffer_->lookupTransform( - params_.frames.base_frame, params_.frames.imu_frame, - tf2::TimePointZero, tf2::durationFromSec(0.1)); - auto& q = tf.transform.rotation; - auto& t = tf.transform.translation; - imu_to_base_ = Pose3( - Rot3::Quaternion(q.w, q.x, q.y, q.z), - Point3(t.x, t.y, t.z)); - RCLCPP_INFO(this->get_logger(), "Cached IMU->base_link transform"); - } catch (const tf2::TransformException& ex) { - RCLCPP_DEBUG(this->get_logger(), "IMU transform not available: %s", ex.what()); + filtered_imu_pub_->publish(std::move(filtered_msg)); } - } - if (!initialized_) { - imu_buffer_.push_back(*imu_msg); - return; - } + rclcpp::Time stamp(msg->header.stamp); - double dt = 0.01; - if (!imu_buffer_.empty()) { - auto prev_time = rclcpp::Time(imu_buffer_.back().header.stamp); - auto curr_time = rclcpp::Time(imu_msg->header.stamp); - double computed_dt = (curr_time - prev_time).seconds(); + { + std::lock_guard lock(imu_mutex_); + imu_buffer_.emplace_back(stamp, accel, gyro); - dt = std::max(0.001, std::min(0.1, computed_dt)); + while (imu_buffer_.size() > params_.imu_buffer_max_size) + { + imu_buffer_.pop_front(); + } + } - if (computed_dt <= 0.0) { - RCLCPP_WARN(this->get_logger(), "Non-positive dt detected: %.6f, using default", computed_dt); - dt = 0.01; + InitState current_state = init_state_.load(); + + if (current_state == InitState::WAITING_FOR_IMU) + { + std::lock_guard lock(imu_mutex_); + if (imu_buffer_.size() >= params_.min_imu_samples_for_init) + { + init_state_.store(InitState::WAITING_FOR_POSITION); + RCLCPP_INFO(get_logger(), "State: WAITING_FOR_POSITION"); + } + } + else if (current_state == InitState::RUNNING) + { + std::lock_guard lock(state_mutex_); + if (init_state_.load() == InitState::RUNNING) + { + double dt = (stamp - last_state_time_).seconds(); + if (dt >= min_state_dt_) + { + create_new_state(stamp); + } + } } } - Vector3 accel(imu_msg->linear_acceleration.x, - imu_msg->linear_acceleration.y, - imu_msg->linear_acceleration.z); - Vector3 gyro(imu_msg->angular_velocity.x, - imu_msg->angular_velocity.y, - imu_msg->angular_velocity.z); - - if (imu_to_base_) { - Rot3 R = imu_to_base_->rotation(); - accel = R.rotate(accel); - gyro = R.rotate(gyro); - } + void StateEstimator::gnss_callback(sensor_msgs::msg::NavSatFix::SharedPtr msg) + { + lookup_sensor_transforms(); - if (imu_preintegrated_) { - imu_preintegrated_->integrateMeasurement(accel, gyro, dt); - } + if (msg->status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) + { + RCLCPP_DEBUG(get_logger(), "GNSS: No fix, ignoring"); + return; + } - imu_buffer_.push_back(*imu_msg); + rclcpp::Time stamp(msg->header.stamp); + double age = (this->get_clock()->now() - stamp).seconds(); + if (age > params_.gnss_max_age) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "GNSS: Rejecting stale measurement (%.2fs old)", age); + return; + } - if (imu_buffer_.size() > 1000) { - imu_buffer_.pop_front(); - } -} + if (!enu_origin_set_) + { + set_enu_origin(msg->latitude, msg->longitude, msg->altitude); + } -void StateEstimator::fuse_gnss(sensor_msgs::msg::NavSatFix::SharedPtr gnss_msg) -{ - RCLCPP_DEBUG(this->get_logger(), "Received GNSS: lat=%.6f, lon=%.6f", - gnss_msg->latitude, gnss_msg->longitude); - - std::lock_guard lock(state_mutex_); - - if (!gnss_to_base_) { - try { - auto tf = tf_buffer_->lookupTransform( - params_.frames.base_frame, params_.frames.gnss_frame, - tf2::TimePointZero, tf2::durationFromSec(0.1)); - gnss_to_base_ = Point3( - tf.transform.translation.x, - tf.transform.translation.y, - tf.transform.translation.z); - RCLCPP_INFO(this->get_logger(), "Cached GNSS->base_link offset: (%.3f, %.3f, %.3f)", - gnss_to_base_->x(), gnss_to_base_->y(), gnss_to_base_->z()); - } catch (const tf2::TransformException& ex) { - RCLCPP_DEBUG(this->get_logger(), "GNSS transform not available: %s", ex.what()); + gtsam::Point3 enu_pos = lla_to_enu(msg->latitude, msg->longitude, msg->altitude); + + // Determine noise from covariance if available + double sigma = params_.gnss_position_sigma; + if (msg->position_covariance_type != sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN) + { + double cov = 0.5 * (msg->position_covariance[0] + msg->position_covariance[4]); + sigma = std::max(sigma, std::sqrt(cov)); } - } - if (!enu_origin_set_) { - set_enu_origin(gnss_msg->latitude, gnss_msg->longitude, gnss_msg->altitude); - } + if (msg->status.status == sensor_msgs::msg::NavSatStatus::STATUS_FIX) + { + sigma *= params_.gnss_fix_quality_multiplier; + } - auto gnss_pos = lla_to_enu(gnss_msg->latitude, gnss_msg->longitude, gnss_msg->altitude); - if (gnss_to_base_) { - gnss_pos = gnss_pos + *gnss_to_base_; - } + InitState current_state = init_state_.load(); - if (!initialized_) { - initialize_with_gnss(gnss_pos, rclcpp::Time(gnss_msg->header.stamp)); - return; + if (current_state == InitState::WAITING_FOR_IMU) + { + RCLCPP_DEBUG(get_logger(), "GNSS: Waiting for IMU first"); + } + else if (current_state == InitState::WAITING_FOR_POSITION) + { + std::lock_guard lock(state_mutex_); + if (init_state_.load() == InitState::WAITING_FOR_POSITION) + { + if (initialize_graph(enu_pos, stamp)) + { + init_state_.store(InitState::RUNNING); + RCLCPP_INFO(get_logger(), "State: RUNNING"); + RCLCPP_INFO(get_logger(), "Initialized at ENU (%.2f, %.2f, %.2f)", + enu_pos.x(), enu_pos.y(), enu_pos.z()); + } + } + } + else if (current_state == InitState::RUNNING) + { + std::lock_guard lock(state_mutex_); + pending_gnss_.position = enu_pos; + pending_gnss_.sigma = sigma; + pending_gnss_.timestamp = stamp; + pending_gnss_.valid = true; + } } - add_gnss_factor(gnss_msg); - optimize_graph(); - marginalize_old_states(); -} + void StateEstimator::odom_callback(nav_msgs::msg::Odometry::SharedPtr msg) + { + if (init_state_.load() != InitState::RUNNING) + { + return; + } -void StateEstimator::fuse_odometry(nav_msgs::msg::Odometry::SharedPtr odom_msg) -{ - std::lock_guard lock(state_mutex_); + gtsam::Pose3 current_pose( + gtsam::Rot3::Quaternion( + msg->pose.pose.orientation.w, + msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z), + gtsam::Point3( + msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z)); + + std::lock_guard lock(state_mutex_); + + if (!pending_odom_.prev_pose) + { + pending_odom_.prev_pose = current_pose; + return; + } - if (!initialized_) { - return; - } + gtsam::Pose3 delta = pending_odom_.prev_pose->between(current_pose); - add_odom_factor(odom_msg); - optimize_graph(); - marginalize_old_states(); -} + if (pending_odom_.valid) + { + pending_odom_.delta = pending_odom_.delta.compose(delta); + } + else + { + pending_odom_.delta = delta; + pending_odom_.valid = true; + } -void StateEstimator::initialize_with_gnss(const Point3& gnss_pos, const rclcpp::Time& timestamp) -{ - // Use first IMU measurement for initial orientation if available - Rot3 initial_rotation = Rot3(); - if (!imu_buffer_.empty()) { - const auto& first_imu = imu_buffer_.front(); - tf2::Quaternion q( - first_imu.orientation.x, - first_imu.orientation.y, - first_imu.orientation.z, - first_imu.orientation.w); - - // Convert to GTSAM rotation if orientation is valid (not all zeros) - if (q.length2() > 0.01) { - q.normalize(); - initial_rotation = Rot3::Quaternion(q.w(), q.x(), q.y(), q.z()); - RCLCPP_INFO(this->get_logger(), "Initializing with IMU orientation"); - } else { - RCLCPP_WARN(this->get_logger(), "IMU orientation invalid, using identity rotation"); - } - } else { - RCLCPP_WARN(this->get_logger(), "No IMU data available, using identity rotation"); + pending_odom_.prev_pose = current_pose; } - auto initial_pose = Pose3(initial_rotation, gnss_pos); - auto initial_velocity = Vector3(0.0, 0.0, 0.0); - auto initial_bias = imuBias::ConstantBias(); + bool StateEstimator::initialize_graph( + const gtsam::Point3 &position, + const rclcpp::Time ×tamp) + { + // Estimate initial orientation from gravity + gtsam::Rot3 initial_rot; + { + std::lock_guard lock(imu_mutex_); + initial_rot = estimate_initial_orientation_locked(); + } - odom_to_base_ = initial_pose; - map_to_odom_ = Pose3(); + gtsam::Pose3 initial_pose(initial_rot, position); + gtsam::Vector3 initial_vel = gtsam::Vector3::Zero(); + gtsam::imuBias::ConstantBias initial_bias; - current_pose_key_ = pose_key(key_index_); - current_vel_key_ = vel_key(key_index_); - current_bias_key_ = bias_key(key_index_); + gtsam::NonlinearFactorGraph factors; + gtsam::Values values; - new_values_->insert(current_pose_key_, initial_pose); - new_values_->insert(current_vel_key_, initial_velocity); - new_values_->insert(current_bias_key_, initial_bias); + values.insert(X(current_key_index_), initial_pose); + values.insert(V(current_key_index_), initial_vel); + values.insert(B(current_key_index_), initial_bias); - // Tighter rotation prior if we have IMU orientation - double rot_prior_noise = (!imu_buffer_.empty() && initial_rotation.matrix() != Rot3().matrix()) ? 0.02 : 0.1; + factors.addPrior(X(current_key_index_), initial_pose, initial_pose_noise_); + factors.addPrior(V(current_key_index_), initial_vel, initial_vel_noise_); + factors.addPrior(B(current_key_index_), initial_bias, initial_bias_noise_); - auto pose_prior = noiseModel::Diagonal::Sigmas( - (Vector6() << rot_prior_noise, rot_prior_noise, rot_prior_noise, 0.1, 0.1, 0.1).finished()); - auto vel_prior = noiseModel::Diagonal::Sigmas( - (Vector3() << 0.5, 0.5, 0.5).finished()); // Tighter velocity prior - auto bias_prior = noiseModel::Diagonal::Sigmas( - (Vector6() << 0.05, 0.05, 0.05, 0.005, 0.005, 0.005).finished()); // Tighter bias prior + try + { + isam_->update(factors, values); + } + catch (const std::exception &e) + { + RCLCPP_ERROR(get_logger(), "Initial ISAM2 update failed: %s", e.what()); + return false; + } - new_factors_->addPrior(current_pose_key_, initial_pose, pose_prior); - new_factors_->addPrior(current_vel_key_, initial_velocity, vel_prior); - new_factors_->addPrior(current_bias_key_, initial_bias, bias_prior); + GraphStateEntry entry; + entry.key_index = current_key_index_; + entry.timestamp = timestamp; + entry.nav_state = gtsam::NavState(initial_pose, initial_vel); + entry.bias = initial_bias; + graph_states_.push_back(entry); - optimize_graph(); + reset_preintegration(initial_bias); + last_state_time_ = timestamp; - EstimatedState state; - state.timestamp = timestamp; - state.nav_state = NavState(initial_pose, initial_velocity); - state.imu_bias = initial_bias; - state.covariance = Eigen::Matrix::Identity() * 0.1; + { + std::lock_guard lock(imu_mutex_); + prune_imu_buffer_locked(timestamp); + } - state_history_.push_back(state); + return true; + } - initialized_ = true; - last_optimization_time_ = timestamp; + gtsam::Rot3 StateEstimator::estimate_initial_orientation_locked() const + { + if (imu_buffer_.empty()) + { + RCLCPP_WARN(get_logger(), "No IMU data for orientation estimate"); + return gtsam::Rot3(); + } - RCLCPP_INFO(this->get_logger(), "StateEstimator initialized with GNSS at (%.2f, %.2f, %.2f)", - gnss_pos.x(), gnss_pos.y(), gnss_pos.z()); -} + Eigen::Vector3d avg_accel = Eigen::Vector3d::Zero(); + size_t count = std::min(imu_buffer_.size(), params_.min_imu_samples_for_init); -std::optional StateEstimator::get_latest_state() const -{ - std::lock_guard lock(state_mutex_); + for (size_t i = 0; i < count; ++i) + { + avg_accel += imu_buffer_[i].accel; + } + avg_accel /= static_cast(count); - if (state_history_.empty()) { - return std::nullopt; - } + // Align accelerometer reading with world up vector + Eigen::Vector3d measured_up = avg_accel.normalized(); + Eigen::Vector3d world_up(0.0, 0.0, 1.0); - return state_history_.back(); -} + Eigen::Vector3d axis = measured_up.cross(world_up); + double axis_norm = axis.norm(); -std::optional StateEstimator::get_state_at_time(const rclcpp::Time& timestamp) const -{ - std::lock_guard lock(state_mutex_); + if (axis_norm < 1e-6) + { + if (measured_up.dot(world_up) > 0) + { + return gtsam::Rot3(); + } + else + { + return gtsam::Rot3::Rx(M_PI); + } + } + + axis.normalize(); + double angle = std::acos(std::clamp(measured_up.dot(world_up), -1.0, 1.0)); - if (state_history_.empty()) { - return std::nullopt; + return gtsam::Rot3::AxisAngle(gtsam::Unit3(axis), angle); } - auto it = std::lower_bound(state_history_.begin(), state_history_.end(), timestamp, - [](const EstimatedState& state, const rclcpp::Time& time) { - return state.timestamp < time; - }); - if (it != state_history_.end()) { - return *it; - } + void StateEstimator::create_new_state(const rclcpp::Time ×tamp) + { + if (graph_states_.empty()) + { + RCLCPP_ERROR(get_logger(), "Cannot create state: no previous state"); + return; + } - return state_history_.back(); -} + { + std::lock_guard lock(imu_mutex_); + integrate_imu_measurements_locked(last_state_time_, timestamp); + } -void StateEstimator::publish_transforms() -{ - auto latest_state = get_latest_state(); - if (!latest_state) { - return; - } + if (!preintegrated_imu_ || preintegrated_imu_->deltaTij() < 1e-6) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, + "No IMU data for state creation"); + return; + } - // Publish odom → base_link - geometry_msgs::msg::TransformStamped odom_to_base_transform; - odom_to_base_transform.header.stamp = latest_state->timestamp; - odom_to_base_transform.header.frame_id = params_.frames.tf_prefix.empty() ? - params_.frames.odom_frame : params_.frames.tf_prefix + "/" + params_.frames.odom_frame; - odom_to_base_transform.child_frame_id = params_.frames.tf_prefix.empty() ? - params_.frames.base_frame : params_.frames.tf_prefix + "/" + params_.frames.base_frame; - - odom_to_base_transform.transform.translation.x = odom_to_base_.x(); - odom_to_base_transform.transform.translation.y = odom_to_base_.y(); - odom_to_base_transform.transform.translation.z = odom_to_base_.z(); - - auto odom_quat = odom_to_base_.rotation().toQuaternion(); - odom_to_base_transform.transform.rotation.w = odom_quat.w(); - odom_to_base_transform.transform.rotation.x = odom_quat.x(); - odom_to_base_transform.transform.rotation.y = odom_quat.y(); - odom_to_base_transform.transform.rotation.z = odom_quat.z(); - - tf_broadcaster_->sendTransform(odom_to_base_transform); - - // Publish map → odom - geometry_msgs::msg::TransformStamped map_to_odom_transform; - map_to_odom_transform.header.stamp = latest_state->timestamp; - map_to_odom_transform.header.frame_id = params_.frames.tf_prefix.empty() ? - params_.frames.map_frame : params_.frames.tf_prefix + "/" + params_.frames.map_frame; - map_to_odom_transform.child_frame_id = params_.frames.tf_prefix.empty() ? - params_.frames.odom_frame : params_.frames.tf_prefix + "/" + params_.frames.odom_frame; - - map_to_odom_transform.transform.translation.x = map_to_odom_.x(); - map_to_odom_transform.transform.translation.y = map_to_odom_.y(); - map_to_odom_transform.transform.translation.z = map_to_odom_.z(); - - auto map_quat = map_to_odom_.rotation().toQuaternion(); - map_to_odom_transform.transform.rotation.w = map_quat.w(); - map_to_odom_transform.transform.rotation.x = map_quat.x(); - map_to_odom_transform.transform.rotation.y = map_quat.y(); - map_to_odom_transform.transform.rotation.z = map_quat.z(); - - tf_broadcaster_->sendTransform(map_to_odom_transform); -} + const auto &prev_state = graph_states_.back(); + uint64_t prev_key = prev_state.key_index; -void StateEstimator::optimize_graph() -{ - if (new_factors_->empty()) { - return; - } + gtsam::NavState predicted; + try + { + predicted = preintegrated_imu_->predict(prev_state.nav_state, prev_state.bias); + } + catch (const std::exception &e) + { + RCLCPP_ERROR(get_logger(), "IMU prediction failed: %s", e.what()); + return; + } - try { - isam_->update(*new_factors_, *new_values_); + uint64_t curr_key = current_key_index_ + 1; - new_factors_->resize(0); - new_values_->clear(); + gtsam::NonlinearFactorGraph factors; + gtsam::Values values; - auto result = isam_->calculateEstimate(); + values.insert(X(curr_key), predicted.pose()); + values.insert(V(curr_key), predicted.velocity()); + values.insert(B(curr_key), prev_state.bias); - if (!result.empty() && result.exists(current_pose_key_) && result.exists(current_vel_key_)) { - EstimatedState state; - state.timestamp = this->get_clock()->now(); + factors.emplace_shared( + X(prev_key), V(prev_key), + X(curr_key), V(curr_key), + B(prev_key), B(curr_key), + *preintegrated_imu_); - auto map_to_base = result.at(current_pose_key_); - auto vel = result.at(current_vel_key_); - state.nav_state = NavState(map_to_base, vel); + if (pending_gnss_.valid) + { + gtsam::Point3 corrected_pos = pending_gnss_.position; - if (result.exists(current_bias_key_)) { - state.imu_bias = result.at(current_bias_key_); + if (gnss_to_base_) + { + gtsam::Pose3 current_pose_estimate = predicted.pose(); + gtsam::Point3 rotated_offset = current_pose_estimate.rotation().rotate(*gnss_to_base_); + corrected_pos = corrected_pos - rotated_offset; } - odom_to_base_ = map_to_base; + gtsam::Vector3 gnss_sigmas; + gnss_sigmas << pending_gnss_.sigma, + pending_gnss_.sigma, + params_.gnss_altitude_sigma; - // Get covariance for pose only - try { - state.covariance = Eigen::Matrix::Identity() * 0.1; - auto pose_cov = isam_->marginalCovariance(current_pose_key_); - if (pose_cov.rows() == 6 && pose_cov.cols() == 6) { - state.covariance.block<6, 6>(0, 0) = pose_cov; - } - } catch (const std::exception& e) { - RCLCPP_WARN(this->get_logger(), "Failed to get covariance: %s", e.what()); - state.covariance = Eigen::Matrix::Identity() * 0.1; + auto gnss_noise = gtsam::noiseModel::Diagonal::Sigmas(gnss_sigmas); + gtsam::SharedNoiseModel noise; + + if (params_.use_robust_gnss_noise) + { + noise = gtsam::noiseModel::Robust::Create( + gtsam::noiseModel::mEstimator::Huber::Create(params_.gnss_huber_k), + gnss_noise); + } + else + { + noise = gnss_noise; } - state_history_.push_back(state); + factors.emplace_shared(X(curr_key), corrected_pos, noise); + + RCLCPP_DEBUG(get_logger(), + "Added GNSS factor at key %" PRIu64 ", sigma_xy=%.2f, sigma_z=%.2f", + curr_key, pending_gnss_.sigma, params_.gnss_altitude_sigma); } - } - catch (const std::exception& e) { - RCLCPP_WARN(this->get_logger(), "ISAM2 update failed: %s", e.what()); - } -} -void StateEstimator::add_imu_factor() -{ - if (!imu_preintegrated_ || key_index_ == 0) { - return; + if (pending_odom_.valid) + { + factors.emplace_shared>( + X(prev_key), X(curr_key), pending_odom_.delta, odom_noise_); + RCLCPP_DEBUG(get_logger(), "Added odom factor %" PRIu64 "->%" PRIu64, prev_key, curr_key); + } + + try + { + isam_->update(factors, values); + auto result = isam_->calculateEstimate(); + + GraphStateEntry entry; + entry.key_index = curr_key; + entry.timestamp = timestamp; + entry.nav_state = gtsam::NavState( + result.at(X(curr_key)), + result.at(V(curr_key))); + entry.bias = result.at(B(curr_key)); + + graph_states_.push_back(entry); + current_key_index_ = curr_key; + + pending_gnss_.reset(); + pending_odom_.delta = gtsam::Pose3(); + pending_odom_.valid = false; + + reset_preintegration(entry.bias); + last_state_time_ = timestamp; + + { + std::lock_guard lock(imu_mutex_); + prune_imu_buffer_locked(timestamp); + } + + marginalize_old_states(); + } + catch (const std::exception &e) + { + RCLCPP_ERROR(get_logger(), "ISAM2 update failed: %s", e.what()); + } } - auto prev_pose_key = pose_key(key_index_ - 1); - auto prev_vel_key = vel_key(key_index_ - 1); - auto prev_bias_key = bias_key(key_index_ - 1); + void StateEstimator::marginalize_old_states() + { + if (graph_states_.size() <= params_.max_graph_states) + { + return; + } - new_factors_->emplace_shared( - prev_pose_key, prev_vel_key, current_pose_key_, - current_vel_key_, prev_bias_key, *imu_preintegrated_); + size_t num_to_remove = graph_states_.size() - params_.max_graph_states; - auto bias_noise = noiseModel::Diagonal::Sigmas( - (Vector6() << params_.imu_bias_noise, params_.imu_bias_noise, params_.imu_bias_noise, - params_.imu_bias_noise, params_.imu_bias_noise, params_.imu_bias_noise).finished()); + gtsam::KeyList keys_to_remove; + for (size_t i = 0; i < num_to_remove; ++i) + { + uint64_t key = graph_states_[i].key_index; + keys_to_remove.push_back(X(key)); + keys_to_remove.push_back(V(key)); + keys_to_remove.push_back(B(key)); + } - new_factors_->emplace_shared>( - prev_bias_key, current_bias_key_, imuBias::ConstantBias(), bias_noise); -} + try + { + isam_->update( + gtsam::NonlinearFactorGraph(), + gtsam::Values(), + gtsam::FactorIndices(), + boost::none, + boost::none, + keys_to_remove); + + RCLCPP_DEBUG(get_logger(), "Marginalized %zu states", num_to_remove); + } + catch (const std::exception &e) + { + RCLCPP_WARN(get_logger(), "Marginalization failed: %s", e.what()); + } -void StateEstimator::add_odom_factor(const nav_msgs::msg::Odometry::SharedPtr& odom_msg) -{ - if (!prev_odom_pose_) { - prev_odom_pose_ = Pose3(Rot3(odom_msg->pose.pose.orientation.w, odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y, odom_msg->pose.pose.orientation.z), - Point3(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z)); - return; + graph_states_.erase(graph_states_.begin(), graph_states_.begin() + num_to_remove); + + if (!graph_states_.empty()) + { + oldest_key_index_ = graph_states_.front().key_index; + } } - auto current_odom_pose = Pose3(Rot3(odom_msg->pose.pose.orientation.w, odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y, odom_msg->pose.pose.orientation.z), - Point3(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z)); + void StateEstimator::integrate_imu_measurements_locked( + const rclcpp::Time &from_time, + const rclcpp::Time &to_time) + { + if (!preintegrated_imu_) + { + RCLCPP_ERROR(get_logger(), "Preintegration not initialized"); + return; + } - auto odom_delta = prev_odom_pose_->between(current_odom_pose); + auto it = imu_buffer_.begin(); + while (it != imu_buffer_.end() && it->timestamp <= from_time) + { + ++it; + } - createNewState(); + if (it == imu_buffer_.end()) + { + return; + } - new_factors_->emplace_shared>(pose_key(key_index_ - 1), current_pose_key_, odom_delta, odom_noise_model_); + rclcpp::Time prev_time = from_time; + int integrated_count = 0; - prev_odom_pose_ = current_odom_pose; -} + for (; it != imu_buffer_.end() && it->timestamp <= to_time; ++it) + { + double dt = (it->timestamp - prev_time).seconds(); + if (dt > 0.0 && dt < 1.0) + { + preintegrated_imu_->integrateMeasurement(it->accel, it->gyro, dt); + integrated_count++; + } + prev_time = it->timestamp; + } -void StateEstimator::add_gnss_factor(const sensor_msgs::msg::NavSatFix::SharedPtr& gnss_msg) -{ - auto gnss_pos = lla_to_enu(gnss_msg->latitude, gnss_msg->longitude, gnss_msg->altitude); - if (gnss_to_base_) { - gnss_pos = gnss_pos + *gnss_to_base_; + RCLCPP_DEBUG(get_logger(), "Integrated %d IMU measurements", integrated_count); } - createNewState(); + void StateEstimator::reset_preintegration(const gtsam::imuBias::ConstantBias &bias) + { + preintegrated_imu_ = std::make_unique( + imu_params_, bias); + } - double base_noise = params_.gnss_noise; - double adaptive_noise = base_noise; + void StateEstimator::prune_imu_buffer_locked(const rclcpp::Time &before) + { + rclcpp::Time cutoff = before - rclcpp::Duration::from_seconds(0.1); - if (gnss_msg->position_covariance_type != sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN) { - double cov_noise = std::sqrt(gnss_msg->position_covariance[0]); - adaptive_noise = std::max(base_noise, cov_noise); + while (!imu_buffer_.empty() && imu_buffer_.front().timestamp < cutoff) + { + imu_buffer_.pop_front(); + } } - // Note: KITTI dataset uses status=-2 but has good GPS quality - // Only apply noise penalty for status values indicating actual poor quality - // Do not penalize negative status codes from KITTI - // Removed: if (gnss_msg->status.status < 0) adaptive_noise *= 10.0; - if (gnss_msg->status.status == 0) adaptive_noise *= 1.5; // Reduced from 2.0 + rclcpp::Time StateEstimator::get_imu_buffer_start_time() const + { + std::lock_guard lock(imu_mutex_); + if (imu_buffer_.empty()) + { + return rclcpp::Time(0, 0, RCL_ROS_TIME); + } + return imu_buffer_.front().timestamp; + } - auto adaptive_gnss_noise = gtsam::noiseModel::Diagonal::Sigmas( - (gtsam::Vector3() << adaptive_noise, adaptive_noise, adaptive_noise).finished()); + rclcpp::Time StateEstimator::get_imu_buffer_end_time() const + { + std::lock_guard lock(imu_mutex_); + if (imu_buffer_.empty()) + { + return rclcpp::Time(0, 0, RCL_ROS_TIME); + } + return imu_buffer_.back().timestamp; + } - new_factors_->emplace_shared(current_pose_key_, gnss_pos, adaptive_gnss_noise); -} -void StateEstimator::createNewState() -{ - key_index_++; - current_pose_key_ = pose_key(key_index_); - current_vel_key_ = vel_key(key_index_); - current_bias_key_ = bias_key(key_index_); + gtsam::Point3 StateEstimator::lla_to_enu(double lat, double lon, double alt) + { + if (!enu_origin_set_) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "lla_to_enu called but ENU origin not set, returning zero"); + return gtsam::Point3::Zero(); + } + + GeographicLib::LocalCartesian proj(origin_lat_, origin_lon_, origin_alt_); + double x, y, z; + proj.Forward(lat, lon, alt, x, y, z); - if (key_index_ > 0 && imu_preintegrated_) { - auto prev_state = state_history_.back(); - auto predicted = imu_preintegrated_->predict(prev_state.nav_state, prev_state.imu_bias); + return gtsam::Point3(x, y, z); + } - new_values_->insert(current_pose_key_, predicted.pose()); - new_values_->insert(current_vel_key_, predicted.velocity()); - new_values_->insert(current_bias_key_, prev_state.imu_bias); + void StateEstimator::set_enu_origin(double lat, double lon, double alt) + { + origin_lat_ = lat; + origin_lon_ = lon; + origin_alt_ = alt; + enu_origin_set_ = true; - add_imu_factor(); - imu_preintegrated_->resetIntegration(); + RCLCPP_INFO(get_logger(), "ENU origin: (%.6f, %.6f, %.2f)", lat, lon, alt); } -} -void StateEstimator::marginalize_old_states() -{ - if (state_history_.size() <= params_.max_states) { - return; + void StateEstimator::lookup_sensor_transforms() + { + if (!imu_to_base_) + { + try + { + auto tf = tf_buffer_->lookupTransform( + params_.frames.base_frame, params_.frames.imu_frame, + tf2::TimePointZero, tf2::durationFromSec(0.0)); + + imu_to_base_ = gtsam::Pose3( + gtsam::Rot3::Quaternion( + tf.transform.rotation.w, + tf.transform.rotation.x, + tf.transform.rotation.y, + tf.transform.rotation.z), + gtsam::Point3( + tf.transform.translation.x, + tf.transform.translation.y, + tf.transform.translation.z)); + + RCLCPP_INFO(get_logger(), "Cached IMU->base transform"); + } + catch (const tf2::TransformException &) + { + // Transform not available yet, will retry on next callback + } + } + + if (!gnss_to_base_) + { + try + { + auto tf = tf_buffer_->lookupTransform( + params_.frames.base_frame, params_.frames.gnss_frame, + tf2::TimePointZero, tf2::durationFromSec(0.0)); + + gnss_to_base_ = gtsam::Point3( + tf.transform.translation.x, + tf.transform.translation.y, + tf.transform.translation.z); + + RCLCPP_INFO(get_logger(), "Cached GNSS->base offset: (%.3f, %.3f, %.3f)", + gnss_to_base_->x(), gnss_to_base_->y(), gnss_to_base_->z()); + } + catch (const tf2::TransformException &) + { + // Transform not available yet, will retry on next callback + } + } } - auto time_threshold = this->get_clock()->now() - rclcpp::Duration::from_seconds(params_.max_time_window); + void StateEstimator::publish_state() + { + auto state = get_latest_state(); + if (!state) + { + return; + } - while (!state_history_.empty() && state_history_.front().timestamp < time_threshold) { - state_history_.pop_front(); + odom_pub_->publish(state->to_odometry(cached_frames_.map, cached_frames_.base)); } -} -gtsam::Point3 StateEstimator::lla_to_enu(double lat, double lon, double alt) const -{ - if (!enu_origin_set_) { - return Point3(); + void StateEstimator::publish_transforms() + { + if (init_state_.load() != InitState::RUNNING) + { + return; + } + + auto state = get_latest_state(); + if (!state) + { + return; + } + + geometry_msgs::msg::TransformStamped odom_to_base; + try + { + odom_to_base = tf_buffer_->lookupTransform( + cached_frames_.odom, cached_frames_.base, tf2::TimePointZero); + } + catch (const tf2::TransformException &ex) + { + RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 1000, + "Cannot lookup odom->base: %s", ex.what()); + return; + } + + gtsam::Pose3 T_odom_base( + gtsam::Rot3::Quaternion( + odom_to_base.transform.rotation.w, + odom_to_base.transform.rotation.x, + odom_to_base.transform.rotation.y, + odom_to_base.transform.rotation.z), + gtsam::Point3( + odom_to_base.transform.translation.x, + odom_to_base.transform.translation.y, + odom_to_base.transform.translation.z)); + + gtsam::Pose3 T_map_base = state->nav_state.pose(); + gtsam::Pose3 T_map_odom_raw = T_odom_base.between(T_map_base); + + // Apply 180° rotation around Z-axis to correct coordinate frame orientation + gtsam::Rot3 R_z_180 = gtsam::Rot3::Rz(M_PI); + gtsam::Point3 rotated_translation = R_z_180.rotate(T_map_odom_raw.translation()); + gtsam::Rot3 rotated_orientation = R_z_180.compose(T_map_odom_raw.rotation()); + gtsam::Pose3 T_map_odom(rotated_orientation, rotated_translation); + + // Publish map->odom transform + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = state->timestamp; + tf_msg.header.frame_id = cached_frames_.map; + tf_msg.child_frame_id = cached_frames_.odom; + + tf_msg.transform.translation.x = T_map_odom.x(); + tf_msg.transform.translation.y = T_map_odom.y(); + tf_msg.transform.translation.z = T_map_odom.z(); + + auto q = T_map_odom.rotation().toQuaternion(); + tf_msg.transform.rotation.w = q.w(); + tf_msg.transform.rotation.x = q.x(); + tf_msg.transform.rotation.y = q.y(); + tf_msg.transform.rotation.z = q.z(); + + tf_broadcaster_->sendTransform(tf_msg); } - GeographicLib::LocalCartesian proj(origin_lat_, origin_lon_, origin_alt_); - double x, y, z; - proj.Forward(lat, lon, alt, x, y, z); + std::optional StateEstimator::get_latest_state() const + { + std::lock_guard lock(state_mutex_); - return Point3(x, y, z); -} + if (graph_states_.empty()) + { + return std::nullopt; + } -void StateEstimator::set_enu_origin(double lat, double lon, double alt) -{ - origin_lat_ = lat; - origin_lon_ = lon; - origin_alt_ = alt; - enu_origin_set_ = true; + const auto &latest = graph_states_.back(); - RCLCPP_INFO(this->get_logger(), "ENU origin set to (%.6f, %.6f, %.2f)", lat, lon, alt); -} + EstimatedState state; + state.timestamp = latest.timestamp; + state.nav_state = latest.nav_state; + state.imu_bias = latest.bias; + + // Initialize with default uncertainty + state.covariance = Eigen::Matrix::Identity() * 0.1; + + try + { + state.covariance.block<6, 6>(0, 0) = isam_->marginalCovariance(X(latest.key_index)); + state.covariance.block<3, 3>(6, 6) = isam_->marginalCovariance(V(latest.key_index)); + state.covariance.block<6, 6>(9, 9) = isam_->marginalCovariance(B(latest.key_index)); + } + catch (const std::exception &) + { + // Covariance calculation failed, using default uncertainty values + } -} // namespace localizer + return state; + } -int main(int argc, char** argv) +} + +int main(int argc, char **argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; -} +} \ No newline at end of file