1、 Core Concepts: What is Observability in Robot State Estimation and Why is it Crucial for Robots?

When a Visual-Inertial Odometry (VIO) or SLAM system initializes, it faces a core challenge analogous to navigating a maze in the dark. Imagine a robot starting up in a completely dark room—it doesn’t know its position or the layout of its environment. It begins to collect data through its sensors (like cameras and IMUs) to gradually infer its own state and the structure of its surroundings. This process is called state estimation.

A critical problem arises from this process: certain changes in state cannot be perceived by the sensors. This phenomenon is known as “unobservability.”  For instance:

• If the robot and its entire environment move or rotate at a constant velocity, the sensor data will not change, making the shift in position undetectable.

• If the scale of the environment is uniformly enlarged or reduced, the sensors will also fail to recognize this change in scale.

These unobservable degrees of freedom are a central challenge in designing state estimation algorithms. If the system ignores these “undetectable” states, it may incorrectly “hallucinate” information, leading to problems like trajectory drift and localization failure.

2、Analysis of Unobservability in Different Sensor Configurations

△ Monocular Camera (Monocular SLAM): 7 Unobservable Degrees of Freedom

• Rotation: 3 degrees of freedom (rotation around the X, Y, and Z axes).

• Translation: 3 degrees of freedom (translation along the X, Y, and Z axes).

• Scale: 1 degree of freedom (the inability to determine the true distance to an object, e.g., whether it is 1 meter or 10 meters away).

△ Monocular Camera + Inertial Measurement Unit (VIO): Only 4 Unobservable Degrees of Freedom

• Rotation: Only the rotation around the vertical axis (the direction of gravity), known as the Yaw angle.

• Translation: 3 degrees of freedom of translation within the horizontal plane (the robot can move arbitrarily in the plane without being detected by the sensors).

✐ Why does adding an IMU improve observability?

• Gravity Perception: An IMU can detect the Pitch and Roll angles, making tilt in these two directions “observable.”

• Scale Estimation: The accelerometer in the IMU, combined with motion data from the images, can be used to calculate real-world scale information.

3、The Core Challenge of State Estimation: Marginalization and Information Matrix Conflicts

To improve computational efficiency, robots often employ sliding window optimization. This method retains only the most recent frames of data for optimization and “marginalizes” older frames. Marginalization compresses the information from old frames into a “residual r + information matrix Λ,” which serves as prior knowledge for subsequent optimization.

The Root of the Problem: The Jacobian matrix calculated during marginalization (which describes the relationship between an old frame and the current frame) is based on the old frame’s linearization point (e.g., position A). However, the Jacobian for a new frame during optimization is based on the current linearization point (e.g., position B). When Jacobians from different linearization points are fed into the optimizer simultaneously, it can cause the following issues:

• The system mistakenly treats unobservable translations or rotations as observable changes.

• Spurious constraints are introduced, leading to trajectory drift, incorrect scale estimation, or even divergence of the optimization.

4、First-Estimates Jacobian (FEJ) Technology: The Core Solution for Preserving Observability

△ The Core Principle of FEJ (First-Estimates Jacobian)

All Jacobians related to the same state variable must be calculated based on its first-estimated linearization point. In practice, this means:

• During marginalization, the Jacobian relating an old frame to a retained frame is based on the first-estimated position of the retained frame (e.g., point A).

• In later optimizations, even if the retained frame’s position is updated (e.g., to point B), the Jacobians for new residual terms are still forced to use the linearization point from A.

△ An Analogy: A Unified “Measurement Baseline”

Imagine a group of students collaboratively drawing a map of their classroom. FEJ is like a rule stating that whenever anyone describes the position of a desk, they must use the project lead’s initial standing position as the reference point. Even if the project lead moves later, all descriptions must stick to that original reference to ensure the map remains logically consistent and avoids contradictions caused by a changing baseline.

△ Understanding FEJ in a Nutshell

To prevent a robot from misinterpreting “unobservable” states as “observable,” all measurements related to the same state must be calculated from a single, unified initial baseline, thereby avoiding logical confusion in the optimization process.

5、Key Considerations for Engineering Implementation

△ Observability is the Foundation of Design:

• Be mindful of the 7 unobservable degrees of freedom in monocular SLAM and the 4 in VIO.

• If the system exhibits trajectory drift or scale anomalies, the first step should be to check if observability constraints have been violated.

△ The Double-Edged Sword of Marginalization:

• Advantage: It compresses old data to increase computational efficiency.

• Risk: Inconsistent linearization points can corrupt observability; this must be addressed by incorporating a mechanism like FEJ.

△ Key Implementation Points for FEJ:

• Enforce that Jacobians for the same state are based on the first linearization point.

• Ensure that sensor data (e.g., from images and the IMU) is time-synchronized with the linearization points.

6、RoboBaton: Powering the Deployment of Applied Robotics

In robot state estimation, the combined application of observability analysis and First-Estimates Jacobian (FEJ) technology is a core method for resolving consistency issues in systems like SLAM and VIO.

UAV Visual-Inertial Odometry (VIO)

During flight, there are unobservable dimensions in how a drone’s IMU and camera observe its motion state (such as absolute position and heading). Traditional methods are prone to trajectory drift due to inconsistent linearization points. In MSCKF (Multi-State Constraint Kalman Filter), the Jacobian matrices for all visual features are fixed to the state calculated at the time of their first observation. This prevents subsequent iterations from introducing false constraints due to updated linearization points, significantly enhancing the drone’s localization stability in complex environments.

Collaborative Mapping for Warehouse AGVs

In multi-AGV, vision-based collaborative SLAM, different vehicles will have different linearization points for the same landmark, which can lead to map tearing. By using FEJ technology, all AGVs are forced to use the landmark’s pose from when it was first identified to calculate their Jacobians. This ensures a consistent constraint baseline for the same landmark across multiple vehicles.

RoboBaton provides precise and stable 6-DOF spatial awareness for a wide range of smart devices, freeing users from the complex and time-consuming development of perception algorithms. Based on multi-source fusion perception and spatial computing, the system delivers real-time spatial awareness information, including depth maps and 6-DOF position and attitude. This empowers various devices with capabilities like autonomous localization, navigation, obstacle avoidance, and mapping.

Visual Spatial Computing Camera VIOBOT2 Binocular Fisheye Slam Camera – myrobotproject.com

• HM Localization: RoboBaton employs an advanced stereo fisheye VIO positioning system. Using dual fisheye cameras and an IMU, it provides continuous and stable decimeter-level navigation and localization for both indoor and outdoor environments, achieving full environmental adaptability. It can also be fused with the HM Mapping module to achieve high-precision, centimeter-level localization.

• HM Mapping: RoboBaton possesses powerful mapping capabilities, generating 3D reconstructed maps with accurate scale information. This helps robots reduce cumulative error when operating in unknown environments and enables global relocalization.

• HM Perception: Depth perception is achieved through stereo fisheye cameras, supporting various low-speed robots, yard robots, and drones moving at up to 2m/s. It provides robots with accurate obstacle avoidance capabilities. We also support the addition of an RGB camera for object recognition and semantic segmentation, such as grass segmentation.

• HM Planner: The RoboBaton planning module automatically generates paths to meet specific industry task requirements, enabling robots to complete missions efficiently and quickly in complex environments.