📊 Robot State Estimation

1. Course Introduction

Multi-sensor Extended Kalman Filter (EKF) fusion using IMU (Inertial Measurement Unit) and chassis motor encoder data to obtain high-precision filtered odometry, accurately estimating the spatial pose and motion state of the A1 Pro Ackermann robot.

🎯 Course Objectives

1. Understand the basic principles of EKF (Extended Kalman Filter) multi-sensor data fusion

2. Master the methods for launching the robot state estimation node and RViz visualization comparison

3. Deeply understand the characteristic differences between /odom (fused filtered odometry) and /odom_raw (wheel odometry)

4. Master the 15-dimensional state vector configuration rules for EKF fusion parameters (odom0_config and imu0_config)

2. Prerequisites

2.1 Launch Chassis Driver and Visualization Model

Run the following command in the terminal to launch the chassis hardware driver (driver_node), IMU attitude solution (imu_attitude_converter), EKF filter node, and RViz visualization interface:

ros2 launch dofbot_pro_description display_A1pro.launch.py pose_source:=ekf

2.2 Launch Keyboard Control

Launch the keyboard control node in a new terminal for manually controlling the robot's movement:

xxxxxxxxxx
ros2 run yahboomcar_ctrl yahboom_keyboard

3. Demonstration and Effect Comparison

Control the robot to move using the keyboard (recommended: trace a closed square loop), and observe the comparative performance of the two odometry trajectories in RViz:

• /odom (EKF fused odometry): Fuses IMU attitude yaw angle with wheel odometry velocity. The trajectory is smooth during turns and acceleration/deceleration, without sideslip discontinuities, and has extremely strong drift resistance.

• /odom_raw (raw wheel odometry): Calculated by the chassis driver node solely through midpoint integration of wheel velocities. Heading accumulates significant drift after wheel slippage or prolonged turning.

4. Computation Graph and Data Flow

Use rqt_graph to view the data interaction relationships between nodes:

xxxxxxxxxx
rqt_graph

4.1 Core Data Flow Diagram

4.2 Core responsibilities of Each Node

5. EKF Parameter Configuration Details

Configuration file path:

xxxxxxxxxx
$HOME/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_bringup/param/ekf_A1.yaml

5.1 Basic Filter Parameters

5.2 Sensor Fusion Matrix (15-Dimensional State Vector)

EKF configures a 15-element boolean array for each sensor input, where each position corresponds to the following state vector:

xxxxxxxxxx
[ X,  Y,  Z,  Roll,  Pitch,  Yaw,  Vx,  Vy,  Vz,  Vroll,  Vpitch,  Vyaw,  Ax,  Ay,  Az ]

1. Wheel Odometry Configuration (odom0: /odom_raw)

xxxxxxxxxx
odom0: /odom_raw
odom0_config: [false, false, false,
               false, false, false,
               true,  true,  false,
               false, false, true,
               false, false, false]
odom0_queue_size: 10
odom0_differential: false
odom0_relative: false

• Enabled dimensions: Vx (linear velocity), Vy (lateral velocity), Vyaw (angular velocity)

• Design Principles:

  • Ackermann non-holonomic kinematic constraint: Ackermann chassis theoretically has no sideslip lateral displacement. Enabling Vy measurement input (reported as 0 by the low-level) provides a strong sideslip suppression constraint to the filter.

  • Avoid pose double accumulation: Only fuse linear velocity and angular velocity to avoid negative effects from cumulative drift of raw pose on the filter.

2. IMU Attitude Configuration (imu0: /imu/data)

xxxxxxxxxx
imu0: /imu/data
imu0_config: [false, false, false,
              false, false, true,
              false, false, false,
              false, false, false,
              false, false, false]
imu0_queue_size: 10
imu0_differential: false
imu0_relative: true

• Enabled dimensions: Yaw (yaw angle)

• Design Principles:

  • IMU gyroscope has extremely high high-frequency response speed for rotation angle measurement, and is completely unaffected by wheel slip or changes in ground traction. Use IMU's Yaw angle to correct wheel odometry heading drift in real-time.

  • imu0_relative: true ensures that at node startup, the current initial heading direction is used as the $0$ reference point, avoiding sudden jumps caused by geomagnetic or uncalibrated absolute heading.

6. Source Code and Configuration File Reference