Skip to content

IMU Processing with a 1D Kalman Filter

Overview

Convert raw IMU acceleration data into a stable estimate of the car's distance to a wall. You will implement a 1D Kalman filter that fuses a kinematic model with sporadic range measurements.

Prerequisites

Learning Objectives

  • Students can fuse and filter noisy sensor data to get a robust estimate of the car's state.
  • Parse raw IMU acceleration and convert to linear velocity and position
  • Model process and measurement noise for a 1D Kalman filter
  • Fuse IMU integration with range updates to suppress drift
  • Validate the filter against ground truth logs

1. Inspect Raw IMU Data

  1. Plot a short bag of /imu/data_raw using rqt_multiplot or Python: 
    ros2 bag record /imu/data_raw -o imu_wall
  2. Check axes alignment and bias. Expect ~9.81 m/s² on the gravity axis when stationary.

2. Bias Removal and Integration

Create a ROS 2 node imu_integrator.py:

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
from geometry_msgs.msg import PointStamped

class IMUIntegrator(Node):
    def __init__(self):
        super().__init__('imu_integrator')
        self.declare_parameter('acc_bias', 0.05)
        self.declare_parameter('dt', 0.01)
        self.bias = self.get_parameter('acc_bias').value
        self.dt = self.get_parameter('dt').value
        self.vel = 0.0
        self.pos = 0.0
        self.sub = self.create_subscription(Imu, '/imu/data_raw', self.callback, 10)
        self.pub = self.create_publisher(PointStamped, '/wall_estimate/raw_integrated', 10)

    def callback(self, msg: Imu):
        ax = msg.linear_acceleration.x - self.bias
        self.vel += ax * self.dt
        self.pos += self.vel * self.dt
        out = PointStamped()
        out.header = msg.header
        out.point.x = self.pos
        self.pub.publish(out)

rclpy.init()
node = IMUIntegrator()
rclpy.spin(node)

Run the node and monitor drift over 30 seconds. Note bias sensitivity.

3. Design the Kalman Filter

Let the state be [position, velocity]ᵀ. Define: - Process model xₖ = A xₖ₋₁ + B uₖ + wₖ, where A = [[1, dt], [0, 1]] - Control input uₖ from acceleration, B = [[0.5 dt²], [dt]] - Measurement zₖ from wall distance sensor observing position only - Tune Q (process covariance) to reflect IMU noise; start with diag([1e-4, 1e-3]) - Set R (measurement covariance) using the sensor datasheet variance

4. Implement the Filter

Add to imu_kf_node.py:

import numpy as np
from rclpy.qos import QoSProfile

class IMUKalman(Node):
    def __init__(self):
        super().__init__('imu_kalman')
        dt = self.declare_parameter('dt', 0.01).value
        self.A = np.array([[1.0, dt], [0.0, 1.0]])
        self.B = np.array([[0.5 * dt ** 2], [dt]])
        self.H = np.array([[1.0, 0.0]])
        self.Q = np.diag([1e-4, 1e-3])
        self.R = np.array([[0.02 ** 2]])
        self.x = np.zeros((2, 1))
        self.P = np.eye(2) * 0.1
        qos = QoSProfile(depth=10)
        self.sub_imu = self.create_subscription(Imu, '/imu/data_raw', self.imu_cb, qos)
        self.sub_range = self.create_subscription(PointStamped, '/wall_range', self.range_cb, qos)
        self.pub = self.create_publisher(PointStamped, '/wall_estimate/kalman', qos)
        self.latest_acc = 0.0

    def imu_cb(self, msg: Imu):
        acc = msg.linear_acceleration.x
        self.latest_acc = acc
        u = np.array([[acc]])
        self.x = self.A @ self.x + self.B @ u
        self.P = self.A @ self.P @ self.A.T + self.Q
        self.publish_estimate(msg.header)

    def range_cb(self, msg: PointStamped):
        z = np.array([[msg.point.x]])
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        y = z - (self.H @ self.x)
        self.x = self.x + K @ y
        self.P = (np.eye(2) - K @ self.H) @ self.P

    def publish_estimate(self, header):
        out = PointStamped()
        out.header = header
        out.point.x = float(self.x[0, 0])
        self.pub.publish(out)

Run colcon build, source the workspace, and launch both nodes.

5. Validate

  • Collect a rosbag while moving toward and away from a wall.
  • Plot /wall_estimate/raw_integrated vs /wall_estimate/kalman.
  • Compute RMS error relative to /wall_range using a notebook or rqt_plot.
  • Adjust Q and R until the filter is responsive but smooth.

6. Troubleshooting

  • Sudden spikes usually mean outlier range readings; add a median filter.
  • If the estimate lags, decrease R or increase Q to trust IMU more.
  • Bias drift requires recalibration; log static periods and recompute bias.

Wrap-Up

Commit the Kalman filter node, add launch file entries, and document tuning values. These estimates feed into the upcoming obstacle avoidance controller.