pos_estimator plugin for MADS

This plugin implements a robust 3D state estimator for autonomous mobile robots within the MADS framework. It leverages an Extended Kalman Filter (EKF) to fuse IMU data, differential drive encoders, and external positioning systems (e.g., RealSense). This is a Filter plugin for MADS.

Required MADS version: 1.4.0.

Supported platforms

Currently, the supported platforms are:

• Linux
• MacOS
• Windows

Installation

Linux and MacOS:

cmake -Bbuild -DCMAKE_INSTALL_PREFIX="$(mads -p)"
cmake --build build -j4
sudo cmake --install build

Windows:

cmake -Bbuild -DCMAKE_INSTALL_PREFIX="$(mads -p)"
cmake --build build --config Release
cmake --install build --config Release

INI settings

The plugin supports the following settings in the INI file:

[pos_estimator]
sub_topic = ["replay_encoders", "replay_imu", "replay_htc", "replay_realsense"]
pub_topic = "estimated"
dev = true
Rr = 0.08726
Rl = 0.08582
b = 0.86

All settings are optional; if omitted, the default values are used.

Executable demo

---

Plugin Structure

System Workflow

The plugin operates as a synchronous MADS component following this execution sequence:

• Parsing Layer: The Wrapper class receives input buffers (via JSON or CSV) and extracts raw kinematics: wheel ticks, angular velocities, and linear accelerations.
• IMU Pre-processing: Raw IMU data is converted to $m/s^2$, biased corrected, and compensated for the lever arm effect (offset between IMU and robot center).
• Prediction Step (EKF): At every clock "tick", the EKF uses a constant acceleration model to project the state ($x, y, \theta, v_x, v_y, \omega$) forward.
• Correction Step (Update):
  • Encoders: Updates the state using differential odometry.
  • External Pose: Updates absolute position when external sensors (RealSense) provide a valid global fix.
• State Output: The refined 12-dimensional expanded state is made available for control and telemetry.

Component Architecture

• Wrapper class is the data abstraction layer. It acts as the parent class for the plugin, managing the raw interface with sensors.

  • Sensor Parsing: Specialized methods like parse_enc() and parse_imu() handle JSON inputs from the MADS bus.
  • Offline Support: Includes robust CSV parsers (load_realsense_csv, load_imu_csv) for hardware-in-the-loop testing and log replay.
  • State Buffering: Stores the current and previous sensor snapshots to calculate $\Delta t$ and numerical derivatives.

• Localizer class provides the physical context and geometric constants for the robot.

  • Kinematic Configuration: Stores calibrated parameters such as wheel radii ($R_r, R_l$) and the track width ($b$).
  • Coordinate Management: Handles angle normalization and maintains the basic 3D pose $(x, y, \theta)$.
  • Abstraction: It wraps the Wrapper data into a coherent robot state, serving as the bridge between raw data and the filter logic.

• EKF class (Extended Kalman Filter) is the computational heart of the plugin. It implements a discrete-time Extended Kalman Filter to estimate a 12-dimensional expanded state vector $\mathbf{\hat{x}}$:

$$\mathbf{\hat{x}} = [x, y, \theta, v_x, v_y, \omega, a_x, a_y, \alpha, \Delta x, \Delta y, \Delta \theta]^T$$

A. Prediction Step (Dead Reckoning)

The filter projects the state forward using a constant acceleration kinematic model. The time step $\Delta t$ is dynamically calculated from the MADS sensor timestamps.

• Lever Arm & Dynamics Correction: Before prediction, raw IMU accelerations are compensated for the offset $(MP_x, MP_y)$ from the robot's center to remove centrifugal and tangential components: $$a_{m_x} = a_{imu_x} - (-\omega^2 MP_x - \alpha MP_y)$$ $$a_{m_y} = a_{imu_y} - (-\omega^2 MP_y + \alpha MP_x)$$

• State Transition: The non-linear function $f(\mathbf{\hat{x}}, \mathbf{u})$ updates the pose based on the current velocity and the compensated acceleration: $$x_{k+1} = x_k + (v_x \Delta t + \frac{1}{2} a_{m_x} \Delta t^2) \cos(\theta) - (v_y \Delta t + \frac{1}{2} a_{m_y} \Delta t^2) \sin(\theta)$$

• Covariance Extrapolation: The uncertainty matrix $P$ is propagated using the Jacobian matrix $F = \frac{\partial f}{\partial \mathbf{x}}$: $$P_{k+1} = F_k P_k F_k^T + Q$$

B. Multi-Sensor Update (Correction)

The EKF handles asynchronous updates from different sources by calculating the innovation $\mathbf{y}$ and the Kalman Gain $K$.

1. External Pose (RealSense): Direct observation of $[x, y, \theta]$. The implementation includes an outlier rejection gate: $$\text{if } ||\mathbf{z}_{rs} - \mathbf{H}\mathbf{\hat{x}}|| > \text{threshold} \implies \text{reject measurement}$$

2. Differential Encoders: The encoder update uses a specialized observation model that maps wheel displacements to global pose and velocities. The innovation for orientation and displacement is always normalized: $$y_\theta = \text{atan2}(\sin(\theta_{obs} - \theta_{pred}), \cos(\theta_{obs} - \theta_{pred}))$$

C. Covariance Management & Reset

To prevent long-term instability and cross-correlation issues in the delta states $(\Delta x, \Delta y, \Delta \theta)$, the implementation performs a block-reset of the covariance matrix $P$ after each encoder update:

• The blocks $P(9:11, 0:11)$ and $P(0:11, 9:11)$ are zeroed.
• The diagonal elements for deltas are reset to a small epsilon ($10^{-9}$), effectively "refreshing" the integration for the next cycle.