DEV Community

Cover image for Building an IMU Simulator from the Ground Up: A Journey Through Inertial Navigation
William Pederzoli
William Pederzoli

Posted on

Building an IMU Simulator from the Ground Up: A Journey Through Inertial Navigation

#robotics #softwareengineering #satellite #space

Introduction

In my previous articles, I've built a complete GNSS positioning engine. But for robust autonomous navigation, we need more than satellites... We need an Inertial Measurement Unit (IMU) that works when GNSS signals drop. However, testing fusion algorithms with real hardware is expensive and slow. So the best solution is to build a high-fidelity IMU simulator.

This article is the process of how to implement an IMU simulator from first principles, breaking down the problem into four logical levels of increasing complexity.

🎯 LEVEL 1: IMU Fundamentals - More Than Just Raw Data

An IMU doesn't measure what you might intuitively think. Understanding this distinction is crucial for anyone working with inertial systems.

1.1 What's Really in an IMU?

At its core, an IMU contains two fundamental sensors:

  • Accelerometers: Measure specific force (all reaction forces except gravity)
  • Gyroscopes: Measure angular rate (how fast the body is rotating in 3D space)

Think of it this way: if you're in a elevator accelerating upward, the accelerometer measures both the elevator's acceleration AND the gravity you're pushing against.

1.2 The Critical Insight: Specific Force ≠ Acceleration

This is one of the most fundamental concepts:

What accelerometers measure = (All real forces except gravity) / mass
Enter fullscreen mode Exit fullscreen mode

Or mathematically:

specificforce=trueacceleration−gravityvectorspecific force = true acceleration - gravity vector

This matter tremendously because when our rover is perfectly stationary on Earth, the accelerometers don't read zero, they measure approximately 9.8 m/s² upward (the normal force counteracting gravity). This is why we must "remove gravity" during inertial navigation.

1.3 Why Simulate Errors? The Realism vs. Theory Gap

Real IMUs are far from perfect. Simulation lets us bridge this gap:

  • Test algorithms under controlled, repeatable error conditions
  • Understand how different error types accumulate over time
  • Develop robust filters before investing in expensive hardware
  • Isolate specific error effects that would be impossible to separate in real data

🎯 LEVEL 2: Calculating the "Ground Truth" IMU Data

Before we can add realistic errors, we need perfect measurements derived from the rover's known trajectory. This reverse-engineering is surprisingly complex.

2.1 Rotation Matrices: The Language of 3D Orientation

While Euler angles (roll, pitch, yaw) are intuitive for humans, rotation matrices are computationally superior for navigation. Euler angles can suffer from "gimbal lock" (losing a degree of freedom when two axes align), while rotation matrices avoid this entirely.

A rotation matrix describes how to transform vectors from one coordinate frame to another. For the aerospace sequence (yaw → pitch → roll), we build the rotation from three elemental rotations:

The rotation from body to navigation frame (NED) is constructed as:

Rbn=Rx(ϕ)⋅Ry(θ)⋅Rz(ψ) R_b^{n} = R_x(\phi) \cdot R_y(\theta) \cdot R_z(\psi)

Where the elemental rotation matrices are:

Rz(ψ)=[cos⁡ψ−sin⁡ψ0sin⁡ψcos⁡ψ0001] R_z(\psi) = \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}
Ry(θ)=[cos⁡θ0sin⁡θ010−sin⁡θ0cos⁡θ] R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}
Rx(ϕ)=[1000cos⁡ϕ−sin⁡ϕ0sin⁡ϕcos⁡ϕ] R_x(\phi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix}

When expanded, the complete rotation matrix becomes:

Rbn=[cθcψcθsψ−sθsϕsθcψ−cϕsψsϕsθsψ+cϕcψsϕcθcϕsθcψ+sϕsψcϕsθsψ−sϕcψcϕcθ] R_b^{n} = \begin{bmatrix} c_\theta c_\psi & c_\theta s_\psi & -s_\theta \\ s_\phi s_\theta c_\psi - c_\phi s_\psi & s_\phi s_\theta s_\psi + c_\phi c_\psi & s_\phi c_\theta \\ c_\phi s_\theta c_\psi + s_\phi s_\psi & c_\phi s_\theta s_\psi - s_\phi c_\psi & c_\phi c_\theta \end{bmatrix}

where
c=cos, s=sin.

Geometric Interpretation:

  • Column 1: Direction of body X-axis in NED coordinates
  • Column 2: Direction of body Y-axis in NED coordinates
  • Column 3: Direction of body Z-axis in NED coordinates

Key Properties:

  • The matrix is orthonormal:
    R−1=RTR^-1 = R^T
  • Determinant is +1 (preserves orientation)
  • Composition of rotations is simple matrix multiplication

Pseudo-code Implementation 

FUNCTION create_rotation_matrix(roll_angle, pitch_angle, yaw_angle)

    // Precompute trigonometric functions
    cos_roll = COS(roll_angle)
    sin_roll = SIN(roll_angle)
    cos_pitch = COS(pitch_angle) 
    sin_pitch = SIN(pitch_angle)
    cos_yaw = COS(yaw_angle)
    sin_yaw = SIN(yaw_angle)

    // Build the combined rotation matrix (roll × pitch × yaw)
    rotation_matrix = [
        // Row 1
        [cos_pitch * cos_yaw, 
         cos_pitch * sin_yaw, 
         -sin_pitch],

        // Row 2  
        [sin_roll * sin_pitch * cos_yaw - cos_roll * sin_yaw,
         sin_roll * sin_pitch * sin_yaw + cos_roll * cos_yaw, 
         sin_roll * cos_pitch],

        // Row 3
        [cos_roll * sin_pitch * cos_yaw + sin_roll * sin_yaw,
         cos_roll * sin_pitch * sin_yaw - sin_roll * cos_yaw,
         cos_roll * cos_pitch]
    ]

    RETURN rotation_matrix

END FUNCTION

// Usage example:
body_to_ned_rotation = create_rotation_matrix(roll_rad, pitch_rad, yaw_rad)

// Transform a vector from body to NED frame:
vector_ned = MATRIX_MULTIPLY(body_to_ned_rotation, vector_body)
Enter fullscreen mode Exit fullscreen mode

2.2 The Complete Rotation Model: It's Not Just About the Vehicle

The orientation change between time steps comes from two sources:

  • Body rotation: The rover's own movement (turning, tilting)
  • Earth rotation: The Earth spinning under us at ~15 degrees per hour 
// Both rotations must be combined
body_rotation = calculate_from_angular_velocity(gyro_measurements)
earth_rotation = calculate_earth_rotation_at_current_latitude()

// The total rotation is the combination of both
total_rotation = combine_rotations(body_rotation, earth_rotation)
Enter fullscreen mode Exit fullscreen mode

2.3 Rodrigues' Rotation Formula: Efficient 3D Updates

For discrete time steps, we need an efficient way to update rotation matrices. Rodrigues' rotation formula gives us this:

R=I+sin⁡(θ)K+(1−cos⁡(θ))K2R = I + \sin(\theta)K + (1 - \cos(\theta))K^2

Where:

K=[0−kzky kz0−kx −kykx0]K = \begin{bmatrix} 0 & -k_z & k_y \ k_z & 0 & -k_x \ -k_y & k_x & 0 \end{bmatrix} 

// Given an angular velocity vector [wx, wy, wz] and time step dt
rotation_vector = angular_velocity × time_step
rotation_matrix = rodrigues_formula(rotation_vector)

// This efficiently computes the rotation matrix
// It's really useful for real-time applications
Enter fullscreen mode Exit fullscreen mode

2.4 The Specific Force Transformation

Now we can compute what ideal accelerometers would measure:

// Step 1: Get the true acceleration from position changes
true_acceleration_ecef = calculate_acceleration_from_trajectory()

// Step 2: Get gravity at current position  
gravity_vector_ecef = calculate_gravity_model(current_latitude, current_height)

// Step 3: Compute specific force in ECEF frame
specific_force_ecef = true_acceleration_ecef - gravity_vector_ecef

// Step 4: Transform to body frame (what the physical sensors would measure)
specific_force_body = transpose(rotation_matrix) × specific_force_ecef
Enter fullscreen mode Exit fullscreen mode

🎯 LEVEL 3: Simulating Realistic IMU Errors

Now we transform our ideal measurements into the noisy, imperfect readings that real sensors produce. This is where simulation meets reality.

3.1 The Zoo of IMU Errors

Real IMUs suffer from multiple error sources that interact in complex ways:

  • Biases: Constant offsets that slowly drift over time
    • Example: Always reading 0.1 m/s² even when stationary
  • Scale Factors: Gain errors in the measurement chain
    • Example: 1% error means 1 m/s² becomes 1.01 m/s²
  • Cross-Coupling: Axis misalignment and interference
    • Example: X-axis acceleration affecting Y-axis measurements
  • Noise: Random high-frequency variations
    • Example: Small random fluctuations around the true value
  • Quantization: Digital resolution limits
    • Example: Can only measure in 0.01 m/s² increments

3.2 Mathematical Error Model

Each error type has a specific mathematical representation:

// The complete error model for one axis
measured_value = (
    (true_value + constant_bias + time_varying_bias) 
    × (1 + scale_factor_error)
    + cross_coupling_from_other_axes
    + random_noise
    + quantization_error
)
Enter fullscreen mode Exit fullscreen mode

3.3 Implementing Realistic Error Simulation

Here's how we apply errors in practice:

FUNCTION simulate_imu_errors(ideal_measurements, error_parameters)

    // Initialize output measurements
    realistic_measurements = copy(ideal_measurements)

    FOR each time_step IN realistic_measurements:

        // Apply biases (constant + slow drift)
        realistic_measurements.gyro_x[time_step] = (
            ideal_measurements.gyro_x[time_step] 
            + error_parameters.gyro_bias_x
            + calculate_bias_drift(time_step)
        )

        // Apply scale factors
        realistic_measurements.gyro_x[time_step] = (
            realistic_measurements.gyro_x[time_step]
            × (1 + error_parameters.gyro_scale_x)
        )

        // Apply cross-coupling between axes
        realistic_measurements.gyro_x[time_step] = (
            realistic_measurements.gyro_x[time_step]
            + error_parameters.cross_coupling_xy × ideal_measurements.gyro_y[time_step]
            + error_parameters.cross_coupling_xz × ideal_measurements.gyro_z[time_step]
        )

        // Add random noise
        realistic_measurements.gyro_x[time_step] = (
            realistic_measurements.gyro_x[time_step]
            + generate_gaussian_noise(error_parameters.noise_std)
        )

        // Apply quantization based on sensor resolution
        realistic_measurements.gyro_x[time_step] = (
            round(realistic_measurements.gyro_x[time_step] / resolution) × resolution
        )

    END FOR

    RETURN realistic_measurements
Enter fullscreen mode Exit fullscreen mode

3.4 The Complete Measurement Pipeline

Putting it all together:

// Input: Perfect trajectory data
// Output: Realistic IMU measurements

ideal_angular_rates = calculate_ideal_gyro_measurements(true_trajectory)
ideal_specific_forces = calculate_ideal_accel_measurements(true_trajectory)

realistic_angular_rates = simulate_imu_errors(ideal_angular_rates, gyro_error_params)
realistic_specific_forces = simulate_imu_errors(ideal_specific_forces, accel_error_params)
Enter fullscreen mode Exit fullscreen mode

The Complete Simulation Pipeline

Putting all three levels together gives us a powerful IMU data generator:

// Input: Reference trajectory (ground truth)
// Output: Realistic IMU measurements

true_trajectory = load_reference_data()

// Level 2: Work backward from trajectory to ideal measurements
ideal_imu = calculate_ideal_imu_measurements(true_trajectory)

// Level 3: Add realistic errors
realistic_imu = simulate_imu_errors(ideal_imu, imu_error_parameters)

// Output: Ready-to-use IMU data for testing fusion algorithms
RETURN realistic_imu
Enter fullscreen mode Exit fullscreen mode

Validation and Critical Insights

The validation process revealed several key insights:

  • Visual Analysis: By plotting the simulated IMU data against the ground truth, I could immediately spot if the error models were behaving realistically. The noise should be random, biases should be constant, and scale factors should proportionally affect all measurements.
  • Error Correlation: Different error types create distinctive patterns in the data. White noise appears as high-frequency jitter, while bias drift creates low-frequency wander in the measurements.
  • The Gravity Test: A quick sanity check—when the rover is stationary and level, the accelerometers in the body frame should measure the gravity vector. If the rover is perfectly aligned with the NED frame, this would be [0, 0, 9.8] m/s², but in practice, the measured specific force depends on the rover's orientation relative to the local vertical.

Conclusion: Foundation for Sensor Fusion

Building this IMU simulator has been an incredible journey through the physics of inertial navigation. From understanding the fundamental concept of specific force to implementing sophisticated error models, each level built upon the previous one.

The most valuable lesson? Simulation forces you to understand the system at a deeper level. You can't simulate what you don't understand.

This IMU simulator now provides the perfect foundation for the next exciting phase: IMU Mechanization. In the next article, I'll show how we can take these simulated measurements and reconstruct the rover's trajectory through numerical integration and discover why IMUs alone can never provide long-term accurate navigation.

But that's a story for next time...

For those working with inertial systems: What aspects of IMU error modeling have you found most challenging? Are there particular error sources that surprised you with their impact on navigation performance?

If you've made it this long, thank you and hope this is useful content for your endeavors. See you in the next one!

Top comments (0)