Lab 04-03: Sensor Fusion with Extended Kalman Filter

Objectives

By the end of this lab, you will be able to:

• Understand the principles of sensor fusion and state estimation
• Implement an Extended Kalman Filter (EKF) for orientation estimation
• Fuse accelerometer and gyroscope data to track robot orientation
• Evaluate filter performance and tune parameters

Prerequisites

• Completed Lab 04-01 (IMU data reading)
• Understanding of linear algebra (matrices, vectors)
• Basic probability concepts (Gaussian distributions)
• Familiarity with differential equations (helpful)

Materials

Type	Name	Tier	Notes
software	MuJoCo 3.0+	required	Physics simulation
software	Python 3.10+	required	Programming environment
software	NumPy, SciPy	required	Linear algebra
software	Matplotlib	required	Visualization
simulation	humanoid-sensors.xml	required	Model with IMU sensors

Background

The Sensor Fusion Problem

Individual sensors have limitations:

• Gyroscope: Accurate short-term, but drifts over time
• Accelerometer: Noisy but provides absolute reference (gravity)

By combining them, we get the best of both worlds: accurate orientation tracking without drift.

Extended Kalman Filter

The EKF is a recursive estimator for nonlinear systems:

1. Predict: Use system model to predict next state
2. Update: Correct prediction using sensor measurements

For orientation estimation, we track quaternions or Euler angles.

Instructions

Step 1: State Representation

Define the state space for orientation estimation:

import numpy as np
from scipy.spatial.transform import Rotation

class OrientationState:
    """
    State representation for orientation estimation.

    State vector: [roll, pitch, yaw, bias_gx, bias_gy, bias_gz]
    - roll, pitch, yaw: Euler angles (radians)
    - bias_g*: Gyroscope bias terms
    """

    def __init__(self):
        # State vector (6 elements)
        self.x = np.zeros(6)

        # State covariance (6x6)
        self.P = np.eye(6) * 0.1

        # Process noise covariance
        self.Q = np.diag([
            0.001,   # roll process noise
            0.001,   # pitch process noise
            0.001,   # yaw process noise
            0.0001,  # bias_gx drift
            0.0001,  # bias_gy drift
            0.0001   # bias_gz drift
        ])

        # Measurement noise covariance (accelerometer)
        self.R = np.diag([0.1, 0.1])  # roll, pitch from accel

    @property
    def euler(self):
        """Get Euler angles (roll, pitch, yaw)."""
        return self.x[:3]

    @property
    def gyro_bias(self):
        """Get gyroscope bias estimate."""
        return self.x[3:6]

    def rotation_matrix(self):
        """Get rotation matrix from current orientation."""
        return Rotation.from_euler('xyz', self.euler).as_matrix()

# Initialize state
state = OrientationState()
print(f"Initial state: {state.x}")
print(f"Initial covariance diagonal: {np.diag(state.P)}")

Expected: State initialized with zeros for angles and biases, identity-scaled covariance matrix.

Step 2: Prediction Step

Implement the EKF prediction using gyroscope data:

def predict(state, gyro_measurement, dt):
    """
    EKF prediction step using gyroscope.

    Args:
        state: OrientationState object
        gyro_measurement: [wx, wy, wz] angular velocity (rad/s)
        dt: Time step (seconds)

    Returns:
        Updated state (in-place modification)
    """
    # Extract current state
    roll, pitch, yaw = state.x[:3]
    bias = state.x[3:6]

    # Correct gyro measurement for bias
    gyro_corrected = gyro_measurement - bias
    wx, wy, wz = gyro_corrected

    # State transition (Euler angle rates from body angular velocity)
    # This is the nonlinear part that makes this an EKF
    cos_roll = np.cos(roll)
    sin_roll = np.sin(roll)
    cos_pitch = np.cos(pitch)
    tan_pitch = np.tan(pitch)

    # Euler angle derivatives
    roll_dot = wx + wy * sin_roll * tan_pitch + wz * cos_roll * tan_pitch
    pitch_dot = wy * cos_roll - wz * sin_roll
    yaw_dot = (wy * sin_roll + wz * cos_roll) / cos_pitch

    # Predict state (Euler integration)
    state.x[0] += roll_dot * dt
    state.x[1] += pitch_dot * dt
    state.x[2] += yaw_dot * dt
    # Bias assumed constant (random walk)

    # Jacobian of state transition (for covariance update)
    F = compute_jacobian_F(state, gyro_corrected, dt)

    # Predict covariance
    state.P = F @ state.P @ F.T + state.Q * dt

    return state

def compute_jacobian_F(state, gyro, dt):
    """Compute state transition Jacobian."""
    roll, pitch, yaw = state.x[:3]
    wx, wy, wz = gyro

    cos_roll = np.cos(roll)
    sin_roll = np.sin(roll)
    cos_pitch = np.cos(pitch)
    sin_pitch = np.sin(pitch)
    tan_pitch = np.tan(pitch)
    sec_pitch = 1.0 / cos_pitch

    # Jacobian matrix (partial derivatives)
    F = np.eye(6)

    # d(roll_dot)/d(roll)
    F[0, 0] += dt * (wy * cos_roll * tan_pitch - wz * sin_roll * tan_pitch)
    # d(roll_dot)/d(pitch)
    F[0, 1] += dt * (wy * sin_roll + wz * cos_roll) * sec_pitch**2

    # d(pitch_dot)/d(roll)
    F[1, 0] += dt * (-wy * sin_roll - wz * cos_roll)

    # d(yaw_dot)/d(roll)
    F[2, 0] += dt * (wy * cos_roll - wz * sin_roll) * sec_pitch
    # d(yaw_dot)/d(pitch)
    F[2, 1] += dt * (wy * sin_roll + wz * cos_roll) * tan_pitch * sec_pitch

    # Jacobian w.r.t. bias (negative effect on state)
    F[0:3, 3:6] = -np.eye(3) * dt

    return F

# Test prediction
test_gyro = np.array([0.1, 0.0, 0.05])  # rad/s
dt = 0.01
state = OrientationState()
state = predict(state, test_gyro, dt)
print(f"After prediction: roll={np.degrees(state.x[0]):.3f} deg")

Expected: Small positive roll angle after prediction with positive roll rate.

Step 3: Update Step

Implement the measurement update using accelerometer:

def update(state, accel_measurement, gravity=9.81):
    """
    EKF update step using accelerometer.

    Accelerometer provides roll and pitch through gravity vector.
    Note: Cannot estimate yaw from accelerometer alone.

    Args:
        state: OrientationState object
        accel_measurement: [ax, ay, az] acceleration (m/s²)
        gravity: Expected gravity magnitude

    Returns:
        Updated state (in-place modification)
    """
    ax, ay, az = accel_measurement

    # Normalize acceleration (assume stationary, measuring gravity)
    accel_norm = np.sqrt(ax**2 + ay**2 + az**2)

    # Skip update if acceleration is far from gravity (robot moving)
```python
```python
    if abs(accel_norm - gravity) > 0.5:

        return state

    ax_n, ay_n, az_n = ax/accel_norm, ay/accel_norm, az/accel_norm

    # Measurement: roll and pitch from gravity
    roll_meas = np.arctan2(ay_n, az_n)
    pitch_meas = np.arctan2(-ax_n, np.sqrt(ay_n**2 + az_n**2))

    z = np.array([roll_meas, pitch_meas])

    # Measurement model: h(x) = [roll, pitch]
    H = np.zeros((2, 6))
    H[0, 0] = 1  # d(roll_meas)/d(roll)
    H[1, 1] = 1  # d(pitch_meas)/d(pitch)

    # Innovation (measurement residual)
    z_pred = state.x[:2]
    y = z - z_pred

    # Wrap angles to [-pi, pi]
    y = np.arctan2(np.sin(y), np.cos(y))

    # Innovation covariance
    S = H @ state.P @ H.T + state.R

    # Kalman gain
    K = state.P @ H.T @ np.linalg.inv(S)

    # Update state
    state.x = state.x + K @ y

    # Update covariance (Joseph form for numerical stability)
    I_KH = np.eye(6) - K @ H
    state.P = I_KH @ state.P @ I_KH.T + K @ state.R @ K.T

    return state

# Test update
test_accel = np.array([0.0, 0.5, -9.8])  # Slight roll
state = OrientationState()
state = update(state, test_accel)
print(f"After update: roll={np.degrees(state.x[0]):.3f} deg")
print(f"Expected roll: {np.degrees(np.arctan2(0.5, 9.8)):.3f} deg")

Expected: Roll angle close to arctan2(0.5, 9.8) ≈ 2.9 degrees.

Step 4: Complete EKF Class

Combine prediction and update into a reusable class:

class OrientationEKF:
    """
    Extended Kalman Filter for IMU-based orientation estimation.
    """

    def __init__(self,
                 process_noise_gyro=0.001,
                 process_noise_bias=0.0001,
                 measurement_noise=0.1):
        """
        Initialize EKF.

        Args:
            process_noise_gyro: Variance for gyro-based prediction
            process_noise_bias: Variance for bias random walk
            measurement_noise: Variance for accelerometer measurements
        """
        self.state = OrientationState()
        self.state.Q = np.diag([
            process_noise_gyro, process_noise_gyro, process_noise_gyro,
            process_noise_bias, process_noise_bias, process_noise_bias
        ])
        self.state.R = np.diag([measurement_noise, measurement_noise])

        self.initialized = False

    def initialize(self, accel_measurement):
        """Initialize orientation from accelerometer."""
        ax, ay, az = accel_measurement
        norm = np.sqrt(ax**2 + ay**2 + az**2)
        ax, ay, az = ax/norm, ay/norm, az/norm

        self.state.x[0] = np.arctan2(ay, az)  # roll
        self.state.x[1] = np.arctan2(-ax, np.sqrt(ay**2 + az**2))  # pitch
        self.state.x[2] = 0  # yaw (cannot determine from accel)

        self.initialized = True

    def process(self, gyro, accel, dt):
        """
        Process one IMU sample.

        Args:
            gyro: [wx, wy, wz] angular velocity (rad/s)
            accel: [ax, ay, az] acceleration (m/s²)
            dt: Time step (seconds)

        Returns:
            Estimated Euler angles [roll, pitch, yaw]
        """
        if not self.initialized:
            self.initialize(accel)

        # Predict with gyro
        predict(self.state, gyro, dt)

        # Update with accelerometer
        update(self.state, accel)

        return self.state.euler.copy()

    def get_rotation_matrix(self):
        """Get current orientation as rotation matrix."""
        return self.state.rotation_matrix()

# Create EKF instance
ekf = OrientationEKF(
    process_noise_gyro=0.001,
    process_noise_bias=0.0001,
    measurement_noise=0.1
)
print("EKF initialized")

Expected: EKF class created with configurable noise parameters.

Step 5: Test with MuJoCo Simulation

Run the EKF on simulated IMU data:

import mujoco
import matplotlib.pyplot as plt

# Load model
model = mujoco.MjModel.from_xml_path("textbook/assets/robot-models/mujoco/humanoid-sensors.xml")
data = mujoco.MjData(model)

# Get sensor IDs
accel_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, "torso_accelerometer")
gyro_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, "torso_gyroscope")

def get_imu_readings(model, data, accel_id, gyro_id):
    """Extract IMU readings from sensor data."""
    accel_adr = model.sensor_adr[accel_id]
    gyro_adr = model.sensor_adr[gyro_id]

    accel = data.sensordata[accel_adr:accel_adr+3].copy()
    gyro = data.sensordata[gyro_adr:gyro_adr+3].copy()

    return accel, gyro

def get_true_orientation(data, body_name="torso"):
    """Get ground truth orientation from simulation."""
    body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, body_name)
    quat = data.xquat[body_id]  # [w, x, y, z]

    # Convert to Euler
    rot = Rotation.from_quat([quat[1], quat[2], quat[3], quat[0]])  # scipy uses [x,y,z,w]
    return rot.as_euler('xyz')

# Run simulation with EKF
ekf = OrientationEKF()
dt = model.opt.timestep
duration = 5.0
n_steps = int(duration / dt)

# Storage
times = np.zeros(n_steps)
estimated_euler = np.zeros((n_steps, 3))
true_euler = np.zeros((n_steps, 3))

# Apply perturbation
mujoco.mj_resetData(model, data)
data.qvel[3] = 0.5  # Angular velocity perturbation

for i in range(n_steps):
    mujoco.mj_step(model, data)

    # Get sensor readings
    accel, gyro = get_imu_readings(model, data, accel_id, gyro_id)

    # Run EKF
    euler = ekf.process(gyro, accel, dt)

    # Store results
    times[i] = data.time
    estimated_euler[i] = euler
    true_euler[i] = get_true_orientation(data)

print("Simulation complete")
print(f"Final estimated: {np.degrees(estimated_euler[-1])}")
print(f"Final true: {np.degrees(true_euler[-1])}")

Expected: EKF runs without errors, estimated and true orientations should be similar.

Step 6: Evaluate and Visualize Results

Compare EKF estimates to ground truth:

def plot_ekf_results(times, estimated, true):
    """Plot EKF performance comparison."""
    fig, axes = plt.subplots(3, 1, figsize=(12, 10), sharex=True)
    labels = ['Roll', 'Pitch', 'Yaw']

    for i, (ax, label) in enumerate(zip(axes, labels)):
        ax.plot(times, np.degrees(true[:, i]), 'b-', label='Ground Truth', linewidth=2)
        ax.plot(times, np.degrees(estimated[:, i]), 'r--', label='EKF Estimate', linewidth=1.5)
        ax.set_ylabel(f'{label} (degrees)')
        ax.legend()
        ax.grid(True)

    axes[-1].set_xlabel('Time (s)')
    axes[0].set_title('EKF Orientation Estimation Performance')

    plt.tight_layout()
    plt.savefig('ekf_results.png', dpi=150)
    plt.show()

def compute_rmse(estimated, true):
    """Compute RMSE for each Euler angle."""
    errors = estimated - true
    # Wrap angle errors
    errors = np.arctan2(np.sin(errors), np.cos(errors))
    rmse = np.sqrt(np.mean(errors**2, axis=0))
    return np.degrees(rmse)

# Plot results
plot_ekf_results(times, estimated_euler, true_euler)

# Compute error metrics
rmse = compute_rmse(estimated_euler, true_euler)
print(f"RMSE (degrees): Roll={rmse[0]:.3f}, Pitch={rmse[1]:.3f}, Yaw={rmse[2]:.3f}")

Expected: RMSE should be small (< 5 degrees) for roll and pitch. Yaw may drift without magnetometer.

Expected Outcomes

After completing this lab, you should have:

1. Code artifacts:

   • orientation_state.py: State representation class
   • ekf.py: Complete EKF implementation
   • imu_fusion.py: Integration with MuJoCo sensors

2. Visual outputs:

   • ekf_results.png: Comparison of estimated vs true orientation

3. Understanding:

   • State estimation principles
   • Kalman filter predict/update cycle
   • Sensor fusion concepts
   • Filter tuning strategies

Rubric

Criterion	Points	Description
State representation	15	Correct state vector and covariance initialization
Prediction step	25	Proper gyro integration with Jacobian computation
Update step	25	Correct accelerometer-based measurement update
Integration	15	Working fusion with MuJoCo simulation
Evaluation	10	RMSE computation and visualization
Code quality	10	Clear structure, comments, modularity
Total	100

Common Errors

Error: Filter diverges (values grow unbounded) Solution: Check Jacobian computation. Ensure covariance stays positive definite.

Error: Yaw drifts significantly Solution: This is expected without a magnetometer. Consider adding simulated magnetometer measurements.

Error: Poor tracking during fast motion Solution: Increase process noise Q or decrease measurement noise R to trust gyro more.

Extensions

For advanced students:

1. Unscented Kalman Filter: Implement UKF for better nonlinear handling
2. Quaternion EKF: Use quaternions instead of Euler angles to avoid gimbal lock
3. Magnetometer Integration: Add heading correction using magnetometer
4. AHRS Implementation: Build complete Attitude and Heading Reference System

Related Content

• Theory: Module 04 theory.md, Section 4.4 (Sensor Fusion)
• Previous Labs: Lab 04-01 (IMU), Lab 04-02 (Camera)
• Next Module: Module 05 (Dynamics and Control) uses orientation estimates