State Estimation

A legged robot's state estimator ingests data from a suite of proprioceptive sensors. Key sensors include:

• Inertial Measurement Unit (IMU): Provides body-frame angular velocity and linear acceleration.
• Joint Encoders: Measure motor positions and, via differentiation, velocities.
• Force/Torque Sensors: Located in the feet or joints to measure Ground Reaction Forces (GRF).
• Joint Current Sensors: Provide an indirect measure of applied torque. This raw, asynchronous, and noisy data is the primary input for any estimation algorithm.

The estimator uses models of the robot's physical structure and motion to predict state evolution and interpret sensor data.

• Kinematic Model: Defines the geometric relationship between joint angles and the pose of links (like the base and feet). It's used for forward kinematics to predict foot positions.
• Dynamic Model: Describes how forces and torques cause motion. A floating base dynamics model is essential, as the robot's base is not fixed to the world. These models allow the estimator to predict how the state should change between measurements.

Determining which feet are in stable contact with the ground is critical. Misclassifying a swing foot as in stance introduces catastrophic error. Detection methods fuse:

• Thresholded Force/Torque Readings: A direct measurement if available.
• Expected Foot Velocity: From the kinematic model; a foot predicted to be moving but with near-zero measured velocity indicates contact.
• Acceleration Consistency: Checking if IMU acceleration is consistent with the assumption of a stationary contact point. Accurate contact state is a prerequisite for correct pose and velocity estimation.

This is the mathematical engine that fuses sensor data with model predictions. For real-time robotics, two families dominate:

• Kalman Filter Variants (EKF, UKF): Maintain a Gaussian belief over the state (mean and covariance). They perform a predict-update cycle: predict using the model, then update (correct) with the latest sensor data, weighting each by its estimated uncertainty.
• Complementary Filter: A simpler, highly efficient filter that fuses high-frequency data (like gyroscopes for orientation) with low-frequency, drift-free data (like accelerometers for gravity direction). It's less optimal but computationally lightweight. The choice depends on required accuracy, model linearity, and available compute.

Not all state variables can be directly inferred from sensors; some are unobservable. For example, a robot's global x-y position drifts without an external reference like GPS or vision (absolute positioning). Estimators manage this by:

• Prioritizing Observable States: Focusing estimation bandwidth on velocity and orientation, which are more directly observable from IMU and kinematics.
• Zero-Velocity Updates (ZUPTs): When a foot is detected to be stationary on the ground, its velocity is known to be zero. This observation is injected to correct drift in velocity and orientation.
• Loop Closure: In systems with exteroceptive sensors (e.g., LiDAR, cameras), detecting a previously visited location provides a strong correction to eliminate accumulated position drift.

The final, time-synchronized estimate produced at the control loop rate (typically 100-1000 Hz). For a legged robot, this minimally includes:

• Base Pose: 3D position and 3D orientation (often as a quaternion) of the robot's trunk relative to a world frame.
• Base Velocity: 3D linear and 3D angular velocity.
• Contact States: Boolean flags for each foot.
• Estimated Sensor Biases: Online calibration of IMU gyroscope and accelerometer biases. This state vector is the definitive input to the robot's balance controller and motion planner, closing the perception-action loop.

The Inertial Measurement Unit (IMU) is the core proprioceptive sensor, providing high-frequency measurements of the robot's rotational and linear motion. It typically contains:

• Gyroscopes: Measure angular velocity (ω) about the robot's body axes.
• Accelerometers: Measure proper acceleration (a), which is the sum of kinematic acceleration and gravity.

An IMU provides fast, drift-free orientation changes via the gyro but suffers from long-term drift. The accelerometer gives an absolute gravity vector for tilt estimation but is corrupted by robot motion. Sensor fusion algorithms, like complementary filters or Kalman filters, are required to combine these signals into a stable orientation estimate (attitude).

Joint encoders (absolute or incremental) provide precise measurements of each joint's angle (position) and, by differentiation, its velocity. This kinematic data is crucial for:

• Computing the robot's forward kinematics to estimate where its feet are relative to its body.
• Performing leg odometry, which estimates base motion by assuming fixed foot contacts with the ground.

Motor current sensors measure the electrical current drawn by each joint actuator. This current is proportional to the motor's output torque, providing a direct measurement of joint torque. This is a key input for estimating external contact forces and for whole-body controllers that manage internal forces.

Six-axis force/torque (F/T) sensors are often mounted in a robot's feet or between the leg linkage and the foot. They directly measure the three-dimensional Ground Reaction Force (GRF) vector and the moment (torque) acting at the foot.

These measurements are critical for:

• Contact State Detection: Determining if a foot is in firm contact, slipping, or in the air.
• Center of Pressure (CoP) Calculation: The point where the net GRF acts, fundamental for stability analysis.
• Momentum Observers: Estimating external disturbances and the robot's centroidal momentum by comparing expected dynamics (from commanded torques) with measured reaction forces.

Leg odometry is a velocity estimation technique that uses joint encoder data and a model of the robot's kinematics. When a foot is judged to be in stationary contact with the ground, the motion of the robot's base is inferred from the reverse motion of the leg joints.

The core challenge is that leg odometry is accurate only during perfect, non-slipping contact and accumulates error over time. Therefore, it is fused with IMU data in a kinematic-inertial estimator (often an Extended Kalman Filter). The IMU provides a high-frequency, short-term accurate velocity estimate that corrects for slip, while the leg odometry provides a low-drift position estimate during good contact, creating a robust velocity and position state.

Accurate contact state estimation—knowing which feet are firmly on the ground, slipping, or in swing phase—is not a given; it must be inferred from sensor data. Incorrect contact assumptions are a primary source of state estimation failure.

Methods for detecting contact include:

• Thresholding Force/Torque Sensor Data: A measured normal force above a threshold indicates contact.
• Current-Based Detection: A spike in motor current when a foot touches down.
• Kinematic Consistency Checks: Comparing the expected foot velocity (from leg kinematics) with a near-zero velocity assumption. A significant discrepancy suggests slip.
• Model-Based Observers: Using the full-body dynamics model to predict contact forces and comparing them to measurements.

While proprioceptive sensors (IMU, encoders) are primary for high-frequency state estimation, exteroceptive sensors like cameras and LiDAR provide absolute, drift-correction. They are used in a hierarchical or tightly-coupled manner:

• Hierarchical Correction: A separate Visual-Inertial Odometry (VIO) or LiDAR-Inertial Odometry (LIO) module runs in parallel, providing a slow-updating, globally-referenced pose estimate. This is fused at a lower frequency to correct the drift in the proprioceptive estimator.
• Tightly-Coupled Fusion: Visual features or LiDAR points are incorporated directly into the same filter as the IMU and joint data, providing a single, optimal estimate. This is more complex but robust.

For dynamic legged locomotion, exteroceptive data is often downweighted or used intermittently due to motion blur, high dynamics, and the critical need for low-latency estimates from proprioception.

Sensor fusion is the algorithmic core of state estimation, combining asynchronous, noisy data from multiple sources into a single, coherent state estimate. For legged robots, this typically involves:

• Proprioceptive sensors: Inertial Measurement Units (IMUs) for angular velocity and linear acceleration, and joint encoders for limb positions.
• Exteroceptive sensors: Cameras, LiDAR, or depth sensors for correcting drift by observing the external world.
• Kinematic models: Using the robot's own geometry and joint angles to predict limb and base motion. Common fusion frameworks include the Kalman Filter and its non-linear variants (Extended Kalman Filter, Unscented Kalman Filter) and optimization-based approaches like Factor Graphs.

An Inertial Measurement Unit (IMU) is the primary proprioceptive sensor for state estimation, providing high-frequency measurements of a robot's rotational and linear motion. A typical IMU contains:

• A 3-axis gyroscope: Measures angular velocity (ω).
• A 3-axis accelerometer: Measures proper acceleration (the sum of kinematic acceleration and gravity). For legged robots, the IMU is rigidly mounted to the main body (the 'base link'). Its data is fundamental but problematic: accelerometer readings are contaminated by gravity, and both sensors suffer from bias and noise. State estimation algorithms must separate gravity from motion acceleration and integrate gyro data to estimate orientation, a process prone to drift over time.

Contact detection (or contact state estimation) is the process of determining when and with what force a robot's feet are in contact with the ground. This is critical for legged state estimation because:

• Stance legs provide an absolute velocity reference: When a foot is firmly on the ground (assuming no slip), the velocity of that foot point in the world frame is zero. This observation is used to correct drifting velocity estimates from the IMU.
• Defines the kinematic chain: A leg in stance creates a closed kinematic chain from the ground to the robot's base, allowing precise calculation of base position from joint encoder data. Detection methods range from simple thresholding on measured joint torque or current to more sophisticated classifiers using Unscented Kalman Filters that estimate contact probability and force simultaneously.

Kinematic odometry (or leg odometry) estimates the robot's base motion by analyzing the geometry of its legs during stance phases. When a foot is detected to be in stationary contact with the ground:

1. The foot's position in the world frame is calculated from the previous base pose estimate and the leg's joint angles.
2. As the robot moves, new joint angles are read.
3. The change in the leg's geometry is used to compute the new base pose that keeps the stationary foot's world position constant. This method provides low-drift positional estimates during walking but is only available when legs are in stance and assumes no foot slip. It is typically fused with IMU data, which provides orientation and high-frequency velocity updates but drifts in position.

A Factor Graph is a probabilistic graphical model used for optimization-based state estimation, increasingly popular in modern legged robotics over filter-based methods. It represents the state estimation problem as:

• Nodes: Variables representing the robot's state (pose, velocity) at different points in time.
• Factors: Constraints that connect nodes, representing sensor measurements or prior knowledge (e.g., an IMU measurement factor between two states, a leg kinematics factor from a contact event). The graph is solved by finding the set of states that maximizes the probability of all measurements (maximum a posteriori estimation). This approach naturally handles asynchronous sensors, allows for easy incorporation of loop closures (if using vision/LiDAR), and maintains a history of states for smoothing. Libraries like GTSAM implement these solvers for real-time use.

Floating base estimation is the specific problem of estimating the 6-degree-of-freedom pose and velocity of a robot's base link, which is not fixed to the world. This is distinct from wheeled robot odometry where the base is always connected via rolling constraints. The state vector typically includes:

• Orientation (3 DOF): Represented as a quaternion.
• Position (3 DOF): In world coordinates.
• Linear and Angular Velocity (6 DOF). The challenge arises from the underactuation of the base; no motor directly controls its world position. It must be inferred entirely from the motion of the legs and IMU. The estimator must also handle sudden jumps in observability—when no feet are in contact (during a jump or flight phase of running), position becomes unobservable and drifts rapidly until the next footfall.