Extended Kalman Filter (EKF)

Extended Kalman Filter (EKF) – definition

An Extended Kalman Filter (EKF) is a recursive state-estimation algorithm for nonlinear systems. In mobile robotics, it estimates the robot’s current state by combining a motion model with measurements from sensors such as wheel encoders, an IMU, GNSS/RTK receivers, LiDAR odometry, or visual odometry.

The original Kalman filter is designed for linear dynamic systems. An EKF extends this approach by locally linearising nonlinear process and measurement functions using Jacobian matrices. This makes it suitable for wheeled unmanned ground vehicles (UGVs), where robot motion, orientation, sensor observations, and coordinate transformations are generally nonlinear.

For a differential-drive platform such as Leo Rover, an EKF is commonly used to fuse wheel odometry and IMU data into a stable estimate of pose, velocity, and orientation. The resulting estimate can be published to ROS 2 as filtered odometry and used by navigation, mapping, perception, and control components. Leo Rover supports ROS 2 on Raspberry Pi-based compute hardware and requires a separately integrated navigation stack for autonomous operation. Source: Leo Rover technical documentation.

How an EKF works on a mobile UGV

An EKF alternates between prediction and correction. During prediction, it propagates the current robot state using a kinematic or dynamic motion model. During correction, it compares predicted sensor values with incoming measurements and adjusts the estimate according to measurement uncertainty.

A simplified EKF representation uses the following nonlinear system model:

x(k) = f(x(k-1), u(k)) + w(k)
z(k) = h(x(k)) + v(k)

In this model, x is the system state, u is the control or motion input, z is a sensor measurement, and w and v represent process and measurement noise. The EKF uses Jacobians of f and h to propagate covariance through these nonlinear functions.

For a ground rover, the state vector often contains position, orientation, linear velocity, angular velocity, and acceleration terms. The exact state definition depends on the filter configuration. The ROS robot_localization package uses a 15-state filter model that includes position, orientation, linear velocity, angular velocity, and linear acceleration. Source: robot_localization documentation, ROS.

EKF in ROS 2 and robot_localization

In ROS 2, EKF-based sensor fusion is commonly implemented with the ekf_node executable from the robot_localization package. The node subscribes to supported ROS messages, transforms measurements into the configured coordinate frame, and publishes filtered odometry.

The ekf_node supports, among others, the following message types:

nav_msgs/msg/Odometry for wheel, visual, LiDAR, or external odometry.
sensor_msgs/msg/Imu for angular velocity, orientation, and linear acceleration.
geometry_msgs/msg/PoseWithCovarianceStamped for externally estimated poses.

The package also provides navsat_transform_node, which consumes sensor_msgs/msg/NavSatFix GNSS measurements and produces odometry data that can be fused by ekf_node.

A typical ROS 2 configuration defines which variables from each sensor should be fused. For a differential-drive UGV operating on approximately planar terrain, the two_d_mode option can constrain vertical position, roll, and pitch variables. This should only be enabled when those constraints match the operational environment.

ekf_filter_node:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.1
    two_d_mode: true
    publish_tf: true
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    odom0: /wheel/odometry
    odom0_config: [false, false, false,
                   false, false, true,
                   true, false, false,
                   false, false, true,
                   false, false, false]

    imu0: /imu/data
    imu0_config: [false, false, false,
                  false, false, true,
                  false, false, false,
                  false, false, true,
                  false, false, false]

The values shown above are configuration examples, not universal parameters. Update rates, covariance values, frame names, and enabled variables must be selected from the actual sensor specifications and validated against recorded data.

Coordinate frames and REP 105

Correct frame semantics are essential for EKF integration. ROS mobile robots normally use the frame hierarchy described by REP 105: map, odom, and base_link. The odom frame provides locally continuous odometry. The map frame provides a globally referenced frame but may change discontinuously after localisation corrections.

For many UGV systems, the EKF publishes the transform between odom and base_link. A localisation system such as AMCL, SLAM Toolbox, or GNSS-based localisation may publish the transform between map and odom. This separation prevents global corrections from creating discontinuities in the local control frame.

Source: REP 105 – Coordinate Frames for Mobile Platforms, ROS.

Sensor fusion on Leo Rover and Raph Rover

On Leo Rover, an EKF can combine encoder-based odometry with onboard or externally integrated IMU data. Wheel odometry provides short-term motion information but accumulates error because of wheel slip, uneven ground, tyre deformation, and encoder resolution. An IMU can improve orientation and angular-rate estimation, but inertial measurements and orientation estimates can also drift over time. EKF fusion combines these complementary measurements according to their configured covariances.

A practical Leo Rover integration can use the following data flow:

A wheel-odometry source publishes odometry in the odom frame.
An IMU publishes sensor_msgs/msg/Imu with a valid frame ID and covariance fields.
ekf_node publishes /odometry/filtered.
The filter publishes an odom -> base_link transform.
Nav2, SLAM, or teleoperation nodes consume the filtered transform and odometry.

Raph Rover can follow the same architecture when carrying heavier sensor payloads, such as a 3D LiDAR, RTK GNSS receiver, industrial IMU, or depth camera. Larger payload capacity does not change EKF principles. It changes integration requirements, including sensor mounting rigidity, power distribution, time synchronisation, vibration isolation, and calibration.

Limitations and validation

An EKF does not remove systematic sensor errors automatically. Incorrect wheel radius, wheelbase, IMU axis alignment, timestamp errors, or unrealistic covariance values can produce a plausible but inaccurate pose estimate. The filter assumes that uncertainty models are meaningful and that sensor measurements are expressed in consistent coordinate frames.

Before using an EKF for autonomous navigation, validate the system with recorded ROS 2 bags. Compare filtered trajectories against known landmarks, surveyed GNSS/RTK positions, or motion-capture data where available. Monitor transform availability, message timestamps, covariance matrices, innovation behaviour, and drift during turns, acceleration, wheel slip, and temporary sensor loss.

Extended Kalman Filter (EKF)