Initialization, failure and recovery

1. Engineering problem

Visual-inertial and LiDAR-inertial estimators must estimate gravity, scale (monocular) and biases at start. Without enough motion these are unobservable, and the estimator starts wrong and may never recover. Estimators can also diverge in operation; the robot needs a defined response.

2. When is this needed?

• Starting an estimator on a robot at rest.
• After a tracking or sensor failure.
• Designing fail-safe behaviour.

3. Selection and design decisions

Initialisation needs excitation: rotation and acceleration large enough to separate gravity and bias from noise. Some packages start from rest by assuming a stationary period for gravity alignment (FAST-LIO’s documentation discusses initialisation and a separate LiDAR–IMU initialisation package); others need a moving start (monocular VIO needs parallax). Read the initialisation section of your package and check its requirements on a recording.

4. Advantages and disadvantages

• Strict initialisation delays start and reduces bad starts.
• Fast restart resumes sooner and can restart on a bad estimate.
• Fallback sensors add integration and keep the robot safe.

5. Requirements and constraints

• A defined health signal that other nodes can read.
• The robot has a safe behaviour when the health is bad.
• Initialisation is tested from several start conditions.

6. Architecture

start ─▶ excitation check ─▶ initialise ─▶ run (monitor health) ─▶ failure ─▶ safe stop/fallback ─▶ re-initialise

7. Implementation

1. Read the package’s initialisation requirements.
2. Implement an excitation check (below) before declaring the estimator ready.
3. Monitor features, covariance and innovations in operation.
4. On failure, stop or fall back and report.
5. Test failure injection: cover the camera, spin quickly, disconnect the IMU.

import numpy as np
def ready(gyro, accel, min_gyro=0.2, min_accel=0.3):
    gyro_exc = float(np.max(np.linalg.norm(gyro, axis=1)))
    accel_var = float(np.max(np.std(accel, axis=0)))
    msgs = []
    if gyro_exc < min_gyro:
        msgs.append(f"rotation too small ({gyro_exc:.2f} rad/s < {min_gyro})")
    if accel_var < min_accel:
        msgs.append(f"acceleration too small (std {accel_var:.2f} m/s^2 < {min_accel})")
    return msgs or ["excitation sufficient: initialise"]
rng = np.random.default_rng(3)
still_g, still_a = rng.normal(0, 0.003, (100, 3)), rng.normal([0, 0, 9.81], 0.02, (100, 3))
move_g = np.column_stack([np.sin(np.linspace(0, 6, 100)) * 0.6, np.zeros(100), np.zeros(100)])
move_a = still_a + np.column_stack([np.sin(np.linspace(0, 6, 100)) * 1.0, np.zeros(100), np.zeros(100)])
print("stationary:", ready(still_g, still_a))
print("moving:", ready(move_g, move_a))

Printed (checked by the site’s tests):

stationary: ['rotation too small (0.01 rad/s < 0.2)', 'acceleration too small (std 0.02 m/s^2 < 0.3)']
moving: ['excitation sufficient: initialise']

Reading it: the stationary recording has too little rotation and acceleration to initialise (the check says so and gives the numbers), while the moving recording passes. The thresholds (0.2 rad/s, 0.3 m/s²) are illustrative: take yours from the package’s documentation and tests.

8. Real examples

• AMR start-up: a short forward-and-turn manoeuvre before enabling VIO-based navigation.

9. Failure scenarios

• Initialising while stationary with a monocular VIO.
• No divergence monitoring, so the robot drives on a wrong pose.
• Re-initialising in a place with no features.

10. Troubleshooting

1. Record the first 10 seconds of failing starts and run the excitation check.
2. Look at the first estimated gravity direction and biases.
3. Check the time sync before initialisation.

11. Validation

• The checker prints the lines shown (executed by the site tests).
• Failure injection leads to the defined safe behaviour every time.

12. Engineering checklist

• Initialisation requirements read
• Excitation check
• Health monitoring
• Safe behaviour on failure
• Re-initialisation plan
• Failure injection tested

13. Further references

• FAST-LIO and LIO-SAM READMEs, read 2026-11-06: FAST_LIO, LIO-SAM.
• VINS-Fusion README, read 2026-11-06: HKUST-Aerial-Robotics/VINS-Fusion.

Related on FixTheRobot