Skip to content

feat(imu,gnss): IMU lever arm centripetal, GPS pre-heading option, TF auto-resolve #17

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
cedbossneo wants to merge 1 commit into
base: main
Choose a base branch
from
Open

feat(imu,gnss): IMU lever arm centripetal, GPS pre-heading option, TF auto-resolve #17

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions fusioncore_core/include/fusioncore/fusioncore.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
11 changes: 11 additions & 0 deletions fusioncore_core/include/fusioncore/sensors/gnss.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 ────────────────────────────────────────────────────────────────
Expand Down
59 changes: 58 additions & 1 deletion fusioncore_core/include/fusioncore/sensors/imu.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,16 +15,38 @@ constexpr int IMU_DIM = 6;
using ImuMeasurement = Eigen::Matrix<double, IMU_DIM, 1>;
using ImuNoiseMatrix = Eigen::Matrix<double, IMU_DIM, IMU_DIM>;

// ─── 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;
double gyro_noise_z = 0.005;
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).
Expand All @@ -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;
Expand Down
38 changes: 33 additions & 5 deletions fusioncore_core/src/fusioncore.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<sensors::IMU_DIM>(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::ImuMeasurement(const StateVector&)>(
sensors::imu_measurement_function);
ukf_.update<sensors::IMU_DIM>(z, h_imu_replay, imu.R);
replayed_any = true;
}
}
Expand Down Expand Up @@ -336,19 +347,30 @@ 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::ImuMeasurement(const StateVector&)>(
sensors::imu_measurement_function);

// Mahalanobis outlier rejection for IMU
if (config_.outlier_rejection) {
sensors::ImuMeasurement innovation_pre;
sensors::ImuNoiseMatrix S;
ukf_.predict_measurement<sensors::IMU_DIM>(z, sensors::imu_measurement_function, R, innovation_pre, S);
ukf_.predict_measurement<sensors::IMU_DIM>(z, h_imu, R, innovation_pre, S);
if (is_outlier<sensors::IMU_DIM>(innovation_pre, S, config_.outlier_threshold_imu)) {
++imu_outliers_;
last_imu_time_ = timestamp_seconds;
return;
}
}

auto innovation = ukf_.update<sensors::IMU_DIM>(z, sensors::imu_measurement_function, R);
auto innovation = ukf_.update<sensors::IMU_DIM>(z, h_imu, R);

// Track innovation for adaptive noise estimation
adapt_R<sensors::IMU_DIM>(R_imu_, R_imu_floor_, imu_innovations_, innovation, config_.adaptive_imu);
Expand Down Expand Up @@ -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)
Expand Down
120 changes: 119 additions & 1 deletion fusioncore_ros/src/fusion_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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.
Expand Down Expand Up @@ -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())
Expand All @@ -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();
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down