Kalman filter

Kalman filter – definition

A Kalman filter is a recursive state estimation algorithm used to fuse noisy measurements with a motion model. Its goal is to estimate variables that are not measured perfectly or not measured directly, such as robot position, velocity, heading, or sensor bias. In mobile robotics, the estimated state is typically represented as a vector and updated in two steps: prediction from the system model, then correction from sensor observations.

For UGV platforms, a Kalman filter is commonly used to combine wheel odometry, IMU, GNSS, visual odometry, and sometimes LiDAR-derived pose updates. The classical linear Kalman filter assumes linear dynamics and Gaussian noise. In practice, mobile robots usually use nonlinear variants such as the Extended Kalman Filter (EKF) or Unscented Kalman Filter (UKF), because vehicle kinematics, orientation, and sensor models are nonlinear. In ROS and ROS 2 deployments, this function is commonly provided by the robot_localization package, which publishes filtered odometry and map-frame estimates for navigation and autonomy.

On Leo Rover and Raph Rover, the Kalman filter is not a standalone autonomy feature. It is one layer in the localization pipeline. It improves the consistency of pose estimates used by SLAM, Nav2, waypoint following, teleoperation with odometry feedback, and outdoor navigation with GNSS. The exact result depends on sensor quality, calibration, frame conventions, timestamp accuracy, and filter tuning.

How a Kalman filter works in mobile robotics

In a UGV, the filter maintains an internal estimate of the robot state and its uncertainty. The state may include planar position, orientation, linear velocity, angular velocity, and inertial terms. At every cycle, the filter propagates the state using a motion model. When a new measurement arrives, it updates the estimate according to measurement uncertainty.

The standard linear form is:

Prediction:
x_k^- = F_k x_(k-1) + B_k u_k
P_k^- = F_k P_(k-1) F_k^T + Q_k

Correction:
K_k = P_k^- H_k^T (H_k P_k^- H_k^T + R_k)^-1
x_k = x_k^- + K_k (z_k - H_k x_k^-)
P_k = (I - K_k H_k) P_k^-

Where:

x – state vector
P – state covariance
F – state transition model
B – control input model
u – control input vector
Q – process noise covariance
z – measurement vector
H – measurement model
R – measurement noise covariance
K – Kalman gain

In differential-drive robots such as Leo Rover, wheel encoder odometry gives short-term motion information but drifts over time. IMU data gives angular velocity and linear acceleration at higher rates, but bias and vibration reduce long-term accuracy. GNSS gives global position outdoors, but with lower update rates and higher noise. A Kalman filter combines these sources so that each sensor contributes where it is strongest.

Kalman filter variants used with ROS 2

ROS 2 systems usually do not use the basic linear filter directly for full robot localization. They use nonlinear forms. One of the most common implementations in mobile robots is robot_localization, available for ROS 1 and ROS 2, with EKF and UKF nodes.

The practical variants are:

Kalman Filter – for linear systems with linear measurement models
Extended Kalman Filter – linearizes nonlinear models around the current estimate
Unscented Kalman Filter – propagates sigma points instead of local linearization

In ROS 2 Humble and newer, a typical setup uses:

ekf_node for local odometry fusion in the odom frame
navsat_transform_node to align GNSS with robot orientation and world frames
a second ekf_node or ukf_node for globally referenced map output

This architecture follows ROS frame conventions from REP 105, which defines the relationship of base_link, odom, map, and earth frames. Message and unit conventions are also aligned with REP 103. These REPs matter because incorrect frame semantics are a common source of filter instability.

ROS 2 interfaces, topics, and configuration

For a mobile platform, the filter consumes standard ROS messages. This makes it portable across hardware and software stacks. The most common input topics are IMU, wheel odometry, and GNSS-derived odometry.

Source	Typical ROS message	Typical rate	Role in filter
Wheel encoders	`nav_msgs/Odometry`	10-50 Hz	Short-term planar motion estimate
IMU	`sensor_msgs/Imu`	50-400 Hz	Angular velocity, orientation, acceleration
GNSS / RTK	`sensor_msgs/NavSatFix` or transformed `nav_msgs/Odometry`	1-20 Hz	Global position reference outdoors
Visual odometry	`nav_msgs/Odometry` or `geometry_msgs/PoseWithCovarianceStamped`	15-60 Hz	Drift-reduced local pose update
LiDAR localization	`geometry_msgs/PoseWithCovarianceStamped`	1-20 Hz	Absolute pose correction

A minimal ROS 2 EKF configuration can look like this:

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,  true,  false,
                   false, false, true,
                   false, false, false]

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

For a differential-drive rover operating mostly on a plane, two_d_mode is often enabled. This suppresses vertical states that are poorly observed and not needed by the navigation stack.

Use on Leo Rover and Raph Rover

On Leo Rover, the Kalman filter is typically used to fuse wheel odometry and IMU on the onboard Raspberry Pi-based compute unit, running ROS 1 or ROS 2 depending on the software setup. This improves local pose tracking for teleoperation, simple autonomy, and indoor or outdoor experiments. Since Leo Rover is not an autonomous platform out of the box, the filter must be integrated with a navigation stack, frame tree, and sensor drivers.

A practical Leo Rover setup may include:

wheel odometry from the differential drive base
IMU for yaw rate stabilization
optional depth camera or LiDAR odometry
optional GNSS or RTK for outdoor global reference

On Raph Rover, the same principle applies, but the platform is better suited to heavier payloads and higher-grade sensors. This allows more advanced fusion setups, such as dual GNSS antennas, industrial IMUs, 3D LiDAR localization, or vision-inertial odometry on an NVIDIA Jetson-class computer. The Kalman filter becomes more important as sensor count, baseline distances, and mission duration increase.

Key tuning parameters and limitations

Kalman filtering is only as good as the model and covariances. In practice, poor tuning produces lag, oscillation, overconfidence, or divergence. Time synchronization and correct covariance fields in ROS messages are critical.

The main parameters to validate are:

process noise covariance Q – how much the model is trusted between measurements
measurement covariance R – how much each sensor is trusted
update frequency and sensor timeout
frame IDs and TF consistency according to REP 105
2D versus 3D state formulation

Important limitations in UGV deployments:

The filter does not remove systematic bias caused by bad calibration.
It does not create a map. SLAM is a separate layer.
It cannot recover from gross frame errors or invalid timestamps.
Wheel slip on sand, mud, grass, or gravel can make encoder odometry unreliable.
Low-cost GNSS without RTK is often too noisy for precise waypoint tracking.

Normative references and standards

The term and method are grounded in estimation theory and widely documented in control and robotics literature. In ROS-based systems, the most relevant practical references are the ROS message standards and frame conventions.

R. E. Kalman, 1960, “A New Approach to Linear Filtering and Prediction Problems”
REP 103 – Standard Units of Measure and Coordinate Conventions
REP 105 – Coordinate Frames for Mobile Platforms
ROS / ROS 2 documentation for robot_localization, sensor_msgs/Imu, nav_msgs/Odometry, and sensor_msgs/NavSatFix