diff --git a/fusioncore_core/include/fusioncore/fusioncore.hpp b/fusioncore_core/include/fusioncore/fusioncore.hpp index 3555ee7..295aec5 100644 --- a/fusioncore_core/include/fusioncore/fusioncore.hpp +++ b/fusioncore_core/include/fusioncore/fusioncore.hpp @@ -117,6 +117,11 @@ class FusionCore { void init(const State& initial_state, double timestamp_seconds); + // Runtime updater for the IMU lever arm — the ROS wrapper calls this + // after auto-resolving base_frame -> imu_frame from TF. Cheap (one + // struct copy) and only touches config_.imu.lever_arm. + void set_imu_lever_arm(const sensors::ImuLeverArm& lever_arm); + // IMU raw update (gyro + accel) void update_imu( double timestamp_seconds, diff --git a/fusioncore_core/include/fusioncore/sensors/gnss.hpp b/fusioncore_core/include/fusioncore/sensors/gnss.hpp index 683b46f..d6ae7e8 100644 --- a/fusioncore_core/include/fusioncore/sensors/gnss.hpp +++ b/fusioncore_core/include/fusioncore/sensors/gnss.hpp @@ -72,6 +72,17 @@ struct GnssParams { // Antenna offset from base_link in body frame GnssLeverArm lever_arm; + + // Normally the lever arm is only applied after heading_validated_ flips + // true (dock compass, dual-GNSS, or 5 m of straight GPS track). Setting + // this to true applies the lever arm from the very first fix — which lets + // GPS position innovations actively observe yaw from startup, instead of + // having to wait for the straight-line accumulation. Safe when Mahalanobis + // gating is on AND either (a) initial yaw is roughly known (dock compass + // available) OR (b) the receiver reports full-covariance RTK fixes that + // let the filter weight yaw corrections correctly. Off by default to + // preserve the original conservative behavior. + bool apply_lever_arm_pre_heading = false; }; // ─── GNSS fix ──────────────────────────────────────────────────────────────── diff --git a/fusioncore_core/include/fusioncore/sensors/imu.hpp b/fusioncore_core/include/fusioncore/sensors/imu.hpp index 5f3b3d4..d4fbe58 100644 --- a/fusioncore_core/include/fusioncore/sensors/imu.hpp +++ b/fusioncore_core/include/fusioncore/sensors/imu.hpp @@ -15,6 +15,24 @@ constexpr int IMU_DIM = 6; using ImuMeasurement = Eigen::Matrix; using ImuNoiseMatrix = Eigen::Matrix; +// ─── IMU lever arm ─────────────────────────────────────────────────────────── +// Offset from base_link origin to the IMU's sensing point, expressed in the +// body frame (after the rotation between base_link and the IMU chip has been +// removed). At a non-zero offset, the accelerometer reads +// a_imu = a_base + ω × (ω × r) + α × r + g_body +// The ω×(ω×r) term (centripetal) is exact and function of state; α (angular +// acceleration) is not in the state, so we drop the tangential term. For +// typical ground-robot angular velocities (|ω| ≤ 1 rad/s, r ≤ 0.5 m) the +// centripetal term dominates and is ≤ 0.5 m/s². +struct ImuLeverArm { + double x = 0.0; + double y = 0.0; + double z = 0.0; + bool is_zero() const { + return std::abs(x) < 1e-6 && std::abs(y) < 1e-6 && std::abs(z) < 1e-6; + } +}; + struct ImuParams { double gyro_noise_x = 0.005; double gyro_noise_y = 0.005; @@ -22,9 +40,13 @@ struct ImuParams { double accel_noise_x = 0.1; double accel_noise_y = 0.1; double accel_noise_z = 0.1; + + // IMU offset from base_link in body frame. Zero means "IMU at base_link" + // and the measurement function stays identical to the pre-lever-arm form. + ImuLeverArm lever_arm; }; -// h(x): state -> expected raw IMU measurement +// h(x): state -> expected raw IMU measurement (IMU colocated with base_link) // Accelerometer reads specific force = body acceleration + gravity in body frame. // Gravity world frame (ENU, z-up): g_world = [0, 0, g]. // Gravity body frame: g_body = R(q)^T * g_world (third column of R^T = third row of R). @@ -43,6 +65,41 @@ inline ImuMeasurement imu_measurement_function(const StateVector& x) { return z; } +// h(x): state -> expected raw IMU measurement accounting for the lever arm +// between base_link and the IMU sensing point. +// +// Gyro is unchanged (angular velocity is position-invariant on a rigid body). +// Accel adds the centripetal contribution ω × (ω × r), written out as +// ω × (ω × r) = (ω · r) ω − (ω · ω) r. +// The tangential component α × r is dropped because α is not part of the +// state; it is bounded by |α·r| and absorbed by accel_noise for typical +// ground-robot controls. +inline auto imu_measurement_function_with_lever_arm(const ImuLeverArm& lever_arm) +{ + return [lever_arm](const StateVector& x) -> ImuMeasurement { + ImuMeasurement z; + constexpr double g = 9.80665; + const double qw = x[QW], qx = x[QX], qy = x[QY], qz = x[QZ]; + const double wx = x[WX], wy = x[WY], wz = x[WZ]; + const double rx = lever_arm.x, ry = lever_arm.y, rz = lever_arm.z; + + // Centripetal: ω × (ω × r) in body frame + const double w_dot_r = wx*rx + wy*ry + wz*rz; + const double w_dot_w = wx*wx + wy*wy + wz*wz; + const double cen_x = w_dot_r * wx - w_dot_w * rx; + const double cen_y = w_dot_r * wy - w_dot_w * ry; + const double cen_z = w_dot_r * wz - w_dot_w * rz; + + z[0] = wx + x[B_GX]; + z[1] = wy + x[B_GY]; + z[2] = wz + x[B_GZ]; + z[3] = x[AX] + x[B_AX] + 2*(qx*qz - qw*qy) * g + cen_x; + z[4] = x[AY] + x[B_AY] + 2*(qy*qz + qw*qx) * g + cen_y; + z[5] = x[AZ] + x[B_AZ] + (1 - 2*(qx*qx + qy*qy)) * g + cen_z; + return z; + }; +} + inline ImuNoiseMatrix imu_noise_matrix(const ImuParams& p) { ImuNoiseMatrix R = ImuNoiseMatrix::Zero(); R(0,0) = p.gyro_noise_x * p.gyro_noise_x; diff --git a/fusioncore_core/src/fusioncore.cpp b/fusioncore_core/src/fusioncore.cpp index d6dcdba..19ed334 100644 --- a/fusioncore_core/src/fusioncore.cpp +++ b/fusioncore_core/src/fusioncore.cpp @@ -94,6 +94,10 @@ FusionCore::FusionCore(const FusionCoreConfig& config) : config_(config), ukf_(config.ukf) {} +void FusionCore::set_imu_lever_arm(const sensors::ImuLeverArm& lever_arm) { + config_.imu.lever_arm = lever_arm; +} + void FusionCore::init(const State& initial_state, double timestamp_seconds) { ukf_.init(initial_state); last_timestamp_ = timestamp_seconds; @@ -220,11 +224,18 @@ bool FusionCore::apply_delayed_measurement( ukf_.predict(dt); last_timestamp_ = imu.timestamp; - // Re-apply the IMU measurement so the filter sees the real dynamics + // Re-apply the IMU measurement so the filter sees the real dynamics. + // Pick the same measurement function as update_imu() to keep the + // replay consistent with the original update (lever-arm aware when + // the IMU is offset from base_link). sensors::ImuMeasurement z; z[0] = imu.wx; z[1] = imu.wy; z[2] = imu.wz; z[3] = imu.ax; z[4] = imu.ay; z[5] = imu.az; - ukf_.update(z, sensors::imu_measurement_function, imu.R); + auto h_imu_replay = !config_.imu.lever_arm.is_zero() + ? sensors::imu_measurement_function_with_lever_arm(config_.imu.lever_arm) + : std::function( + sensors::imu_measurement_function); + ukf_.update(z, h_imu_replay, imu.R); replayed_any = true; } } @@ -336,11 +347,22 @@ void FusionCore::update_imu( // Use adaptive R if initialized, else config default sensors::ImuNoiseMatrix R = adaptive_initialized_ ? R_imu_ : sensors::imu_noise_matrix(config_.imu); + // Pick the measurement function: plain if IMU is at base_link origin, + // else the lever-arm-aware variant that adds ω×(ω×r) centripetal to the + // predicted accel. Both produce identical output when lever_arm == 0, so + // we could always use the lambda; the explicit fork avoids the lambda + // allocation on the hot path when no lever arm is configured. + const bool use_imu_lever_arm = !config_.imu.lever_arm.is_zero(); + auto h_imu = use_imu_lever_arm + ? sensors::imu_measurement_function_with_lever_arm(config_.imu.lever_arm) + : std::function( + sensors::imu_measurement_function); + // Mahalanobis outlier rejection for IMU if (config_.outlier_rejection) { sensors::ImuMeasurement innovation_pre; sensors::ImuNoiseMatrix S; - ukf_.predict_measurement(z, sensors::imu_measurement_function, R, innovation_pre, S); + ukf_.predict_measurement(z, h_imu, R, innovation_pre, S); if (is_outlier(innovation_pre, S, config_.outlier_threshold_imu)) { ++imu_outliers_; last_imu_time_ = timestamp_seconds; @@ -348,7 +370,7 @@ void FusionCore::update_imu( } } - auto innovation = ukf_.update(z, sensors::imu_measurement_function, R); + auto innovation = ukf_.update(z, h_imu, R); // Track innovation for adaptive noise estimation adapt_R(R_imu_, R_imu_floor_, imu_innovations_, innovation, config_.adaptive_imu); @@ -637,7 +659,13 @@ bool FusionCore::apply_gnss_update( if (R(i,i) < 1e-6) R(i,i) = 1e-6; } - bool use_lever_arm = !fix.lever_arm.is_zero() && heading_validated_; + // Apply the antenna lever arm when it is known AND either the heading + // has been validated (the safe default) OR the user opted in to applying + // the lever arm from the first fix (config_.gnss.apply_lever_arm_pre_heading). + // The latter turns GPS into an active yaw observation from startup, which + // is safe when Mahalanobis gating is on and fixes are RTK-grade. + bool use_lever_arm = !fix.lever_arm.is_zero() + && (heading_validated_ || config_.gnss.apply_lever_arm_pre_heading); auto h_gnss = use_lever_arm ? sensors::gnss_pos_measurement_function_with_lever_arm(fix.lever_arm) diff --git a/fusioncore_ros/src/fusion_node.cpp b/fusioncore_ros/src/fusion_node.cpp index fbbcb55..0cce6ac 100644 --- a/fusioncore_ros/src/fusion_node.cpp +++ b/fusioncore_ros/src/fusion_node.cpp @@ -74,6 +74,15 @@ class FusionNode : public rclcpp_lifecycle::LifecycleNode // already subtracted gravity, enable this to add gravity back before fusing. declare_parameter("imu.remove_gravitational_acceleration", false); + // IMU lever arm (offset from base_link to IMU sensing point, body frame). + // Leave at 0 to auto-resolve from TF (base_frame -> imu_frame translation + // via the URDF). Set non-zero to override the TF value — useful when the + // URDF translation is missing/wrong or when you want to experiment + // without rebuilding robot_state_publisher. + declare_parameter("imu.lever_arm_x", 0.0); + declare_parameter("imu.lever_arm_y", 0.0); + declare_parameter("imu.lever_arm_z", 0.0); + declare_parameter("encoder.vel_noise", 0.05); declare_parameter("encoder.yaw_noise", 0.02); @@ -99,11 +108,20 @@ class FusionNode : public rclcpp_lifecycle::LifecycleNode // Set to empty string to disable (use sensor_msgs/Imu heading instead) declare_parameter("gnss.azimuth_topic", ""); - // Antenna lever arm params: primary receiver + // Antenna lever arm params: primary receiver. + // Leave at 0 to auto-resolve from TF (base_frame -> GNSS msg frame_id + // translation, via URDF); set non-zero to override. declare_parameter("gnss.lever_arm_x", 0.0); declare_parameter("gnss.lever_arm_y", 0.0); declare_parameter("gnss.lever_arm_z", 0.0); + // When true, the GNSS lever arm is applied from the very first fix, not + // only after heading_validated_. Let RTK-grade fixes observe yaw directly + // through the antenna-offset projection from startup rather than waiting + // for the heading_observable_distance integration. Safe with Mahalanobis + // gating on. + declare_parameter("gnss.apply_lever_arm_pre_heading", false); + // Antenna lever arm params: secondary receiver (gnss.fix2_topic) // Leave at 0.0 if second antenna is at the same position as the first, // or if fix2_topic is not used. @@ -174,6 +192,23 @@ class FusionNode : public rclcpp_lifecycle::LifecycleNode config.imu.accel_noise_z = config.imu.accel_noise_x; imu_remove_gravity_ = get_parameter("imu.remove_gravitational_acceleration").as_bool(); imu_frame_override_ = get_parameter("imu.frame_id").as_string(); + + // IMU lever arm: start from explicit params; if all zero, the ROS + // wrapper will auto-resolve from TF (base_frame -> imu_frame) on the + // first IMU message and call fc_->update_config_imu_lever_arm() with + // the translation it extracts from URDF. + config.imu.lever_arm.x = get_parameter("imu.lever_arm_x").as_double(); + config.imu.lever_arm.y = get_parameter("imu.lever_arm_y").as_double(); + config.imu.lever_arm.z = get_parameter("imu.lever_arm_z").as_double(); + imu_lever_arm_explicit_ = !config.imu.lever_arm.is_zero(); + if (imu_lever_arm_explicit_) { + RCLCPP_INFO(get_logger(), + "IMU lever arm (explicit): x=%.3f y=%.3f z=%.3f m", + config.imu.lever_arm.x, config.imu.lever_arm.y, config.imu.lever_arm.z); + } else { + RCLCPP_INFO(get_logger(), + "IMU lever arm: will auto-resolve from TF on first IMU message"); + } RCLCPP_INFO(get_logger(), "IMU gravity removal: %s", imu_remove_gravity_ ? "ENABLED" : "disabled"); if (!imu_frame_override_.empty()) @@ -197,6 +232,14 @@ class FusionNode : public rclcpp_lifecycle::LifecycleNode gnss_lever_arm_.x = get_parameter("gnss.lever_arm_x").as_double(); gnss_lever_arm_.y = get_parameter("gnss.lever_arm_y").as_double(); gnss_lever_arm_.z = get_parameter("gnss.lever_arm_z").as_double(); + gnss_lever_arm_explicit_ = !gnss_lever_arm_.is_zero(); + config.gnss.apply_lever_arm_pre_heading = + get_parameter("gnss.apply_lever_arm_pre_heading").as_bool(); + if (config.gnss.apply_lever_arm_pre_heading) { + RCLCPP_INFO(get_logger(), + "GNSS lever arm will be applied pre-heading-validation " + "(gnss.apply_lever_arm_pre_heading=true)"); + } gnss_lever_arm2_.x = get_parameter("gnss.lever_arm2_x").as_double(); gnss_lever_arm2_.y = get_parameter("gnss.lever_arm2_y").as_double(); @@ -562,6 +605,38 @@ class FusionNode : public rclcpp_lifecycle::LifecycleNode } } + // One-shot auto-resolve of the IMU lever arm from TF. Only runs when + // the user did NOT set imu.lever_arm_x/y/z explicitly (all zero). + // Picks up the translation from base_frame -> imu_frame published by + // robot_state_publisher from the URDF. + if (!imu_lever_arm_explicit_ && !imu_lever_arm_tf_resolved_ && + imu_frame != base_frame_) { + try { + auto tf = tf_buffer_->lookupTransform( + base_frame_, imu_frame, tf2::TimePointZero, + tf2::durationFromSec(0.2)); + fusioncore::sensors::ImuLeverArm la; + la.x = tf.transform.translation.x; + la.y = tf.transform.translation.y; + la.z = tf.transform.translation.z; + if (!la.is_zero()) { + fc_->set_imu_lever_arm(la); + RCLCPP_INFO(get_logger(), + "IMU lever arm auto-resolved from TF %s -> %s: x=%.3f y=%.3f z=%.3f m", + base_frame_.c_str(), imu_frame.c_str(), la.x, la.y, la.z); + } else { + RCLCPP_INFO(get_logger(), + "IMU lever arm auto-resolved to zero (IMU is at base_frame origin)"); + } + imu_lever_arm_tf_resolved_ = true; + } catch (const tf2::TransformException &ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "IMU lever arm auto-resolve failed (%s -> %s): %s. " + "Leaving lever arm at zero; set imu.lever_arm_x/y/z explicitly to override.", + base_frame_.c_str(), imu_frame.c_str(), ex.what()); + } + } + if (imu_frame == base_frame_) { double ax = msg->linear_acceleration.x; double ay = msg->linear_acceleration.y; @@ -741,6 +816,41 @@ class FusionNode : public rclcpp_lifecycle::LifecycleNode double t = rclcpp::Time(msg->header.stamp).seconds(); + // One-shot auto-resolve of the GNSS lever arm from TF, primary receiver + // only. Uses msg->header.frame_id (typically "gps" or "gnss_link") + // looked up against base_frame_. Only runs when the user did not set + // gnss.lever_arm_x/y/z explicitly. + if (source_id == 0 && !gnss_lever_arm_explicit_ && !gnss_lever_arm_tf_resolved_) { + if (!msg->header.frame_id.empty() && msg->header.frame_id != base_frame_) { + try { + auto tf = tf_buffer_->lookupTransform( + base_frame_, msg->header.frame_id, tf2::TimePointZero, + tf2::durationFromSec(0.2)); + gnss_lever_arm_.x = tf.transform.translation.x; + gnss_lever_arm_.y = tf.transform.translation.y; + gnss_lever_arm_.z = tf.transform.translation.z; + if (!gnss_lever_arm_.is_zero()) { + RCLCPP_INFO(get_logger(), + "GNSS lever arm auto-resolved from TF %s -> %s: x=%.3f y=%.3f z=%.3f m", + base_frame_.c_str(), msg->header.frame_id.c_str(), + gnss_lever_arm_.x, gnss_lever_arm_.y, gnss_lever_arm_.z); + } else { + RCLCPP_INFO(get_logger(), + "GNSS lever arm auto-resolved to zero (antenna at base_frame origin)"); + } + gnss_lever_arm_tf_resolved_ = true; + } catch (const tf2::TransformException &ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "GNSS lever arm auto-resolve failed (%s -> %s): %s. " + "Leaving lever arm at zero; set gnss.lever_arm_x/y/z explicitly to override.", + base_frame_.c_str(), msg->header.frame_id.c_str(), ex.what()); + } + } else { + // Empty frame_id or same as base: nothing to resolve, mark done. + gnss_lever_arm_tf_resolved_ = true; + } + } + fusioncore::sensors::LLAPoint lla; lla.lat_rad = msg->latitude * M_PI / 180.0; lla.lon_rad = msg->longitude * M_PI / 180.0; @@ -1317,6 +1427,14 @@ class FusionNode : public rclcpp_lifecycle::LifecycleNode fusioncore::sensors::GnssLeverArm gnss_lever_arm_; // primary receiver fusioncore::sensors::GnssLeverArm gnss_lever_arm2_; // secondary receiver (fix2_topic) + // Auto-resolve flags: true means a non-zero value was given in the YAML + // and we should NOT overwrite it from TF. Default (all zero) triggers + // one-shot TF resolution on the first matching message. + bool imu_lever_arm_explicit_ = false; + bool gnss_lever_arm_explicit_ = false; + bool imu_lever_arm_tf_resolved_ = false; + bool gnss_lever_arm_tf_resolved_ = false; + // ZUPT parameters bool zupt_enabled_ = true; double zupt_velocity_threshold_ = 0.05;