Estimate IMU lever arm (distance from vehicle center of gravity)

[adam-weber]

Problem

The device can be mounted anywhere on the vehicle - dashboard, transmission tunnel, rear parcel shelf. This offset from the vehicle's center of gravity (CoG) introduces errors:

       CoG (rotation center)
        ●────────────● Blackbox (1m behind)
                     ↑
              lever arm (r)

When the vehicle rotates, the IMU experiences additional accelerations:

• Centripetal: a_c = ω² × r (toward rotation center)
• Tangential: a_t = α × r (perpendicular, from yaw acceleration)

If we don't compensate, these get mixed into what we think is linear vehicle acceleration.

Research Summary

Academic literature confirms this is a known problem with established solutions:

"If the IMU is not located in the center of gravity, the accelerometers of the IMU are disturbed from additive accelerations such as centripetal acceleration and tangential acceleration."
— Fang et al., Korea Science

"The lever arm should be estimated and then the convective acceleration should be removed from the acceleration."
— IMU-Based Vehicle Slip Angle Estimation, MDPI Sensors

Commercial systems like SBG Systems offer automatic lever arm estimation, eliminating manual measurement.

Key Insight from Literature

"Changes in the angular velocity enhance the lever arm estimate. Both the motions of translation and constant angular velocity have no influence on the estimation of the lever arm."
— Springer, Lever Arm State Estimation

This means: steady-state cornering won't work. You need yaw acceleration (entering/exiting turns) to observe the lever arm.

Physics

The measured acceleration at the IMU includes lever arm effects:

a_measured = a_cog + ω × (ω × r) + α × r

Where:

• a_cog = true vehicle acceleration at center of gravity
• ω = yaw rate (rad/s)
• α = yaw acceleration (rad/s²)
• r = lever arm vector [x, y] from CoG to IMU

For a 2D case (car on flat ground):

a_x_measured = a_x_cog - ω² × r_x - α × r_y
a_y_measured = a_y_cog - ω² × r_y + α × r_x

Estimation Approaches

Option 1: Add lever arm to EKF state (recommended by literature)

Extend the state vector to include lever arm:

x = [x, y, ψ, vx, vy, bax, bay, r_x, r_y]
                                  ↑    ↑
                              lever arm

The EKF will converge on lever arm during normal driving with yaw dynamics.

Pros: No special calibration maneuver, converges during normal driving
Cons: More complex EKF, may take time to converge

Option 2: Dedicated calibration maneuver

Figure-eight or slalom at moderate speed. Provides high yaw acceleration in both directions.

Pros: Fast convergence, clear user action
Cons: Requires space, another calibration step

Option 3: User input

Dashboard UI: "Where is the device mounted?" → Front / Center / Rear + Left / Center / Right

Use typical values: ±1m longitudinal, ±0.3m lateral.

Pros: Simple, no math
Cons: Inaccurate

Proposed Solution

Phase 1: Option 3 (user input) - simple dropdown in UI with rough position

Phase 2: Option 1 (EKF estimation) - add r_x, r_y to state vector, converge during driving

Data Structure

pub struct MountCalibration {
    /// Yaw offset from device frame to vehicle frame (radians)
    pub yaw_offset: f32,

    /// Longitudinal offset from CoG (meters, + = forward of CoG)
    pub x_offset: f32,

    /// Lateral offset from CoG (meters, + = left of CoG)
    pub y_offset: f32,

    pub is_calibrated: bool,
}

Compensation

Before using accelerometer data in EKF:

fn compensate_lever_arm(ax: f32, ay: f32, wz: f32, alpha_z: f32, r: &MountCalibration) -> (f32, f32) {
    let ax_cog = ax + wz * wz * r.x_offset + alpha_z * r.y_offset;
    let ay_cog = ay + wz * wz * r.y_offset - alpha_z * r.x_offset;
    (ax_cog, ay_cog)
}

Note: alpha_z (yaw acceleration) can be computed as derivative of wz or estimated by the EKF.

References

• Attitude determination including lever arms - Springer
• Enhanced Attitude Determination with IMU - Korea Science
• IMU Extrinsic Calibration for Mass Production Vehicles - PMC
• IMU-Based Vehicle Slip Angle Estimation - MDPI
• SBG Systems Lever Arm Tool
• VectorNav IMU Calibration Primer