DOC 06AUTONOMY DEVELOPMENT
Localization and state estimation
The localization layer estimates where a simulated vehicle is from its canonical sensor measurements alone, never from the simulator's truth. Its estimate can drive the built-in controllers in place of true state, which closes the loop from sensors through estimation to control on the canonical World.
Validated in simulation The estimator has been tested across seeded scenarios with sensor outages, added noise and sensor faults, against simulated truth. There is no field or real-data evidence. Localization runs from the simulation service and the Python library; the application has no dedicated localization panel.
Where it fits
Localization sits between the sensors and control in LEVIATHAN's stack. It consumes the canonical sensor measurements described on Sensors, with their integer-nanosecond timestamps, declared frames and refusal states. Its output is a typed estimate that controllers and evaluation tools consume.
The estimator
The estimator is an error-state extended Kalman filter (ESKF) in the formulation of Solà (2017). Its nominal state is position and velocity in NED, attitude as a body-to-world rotation, and accelerometer and gyroscope biases. Its 15-element error state uses a local attitude error. Process noise comes from the IMU's declared noise and bias specification, never from ground truth.
| Sensor | Role | Required |
|---|---|---|
| IMU | Propagation: specific force and angular rate, held over each sample interval | Yes |
| DVL | Bottom-track velocity update; refused packets are logged, never applied | Yes |
| Pressure | Depth from absolute pressure, against a declared free-surface level | Optional |
| Magnetometer | Heading from the measured field vector, with disturbance rejection | Optional |
| USBL | Position fix with its covariance | Optional |
Each update is gated by a chi-square test on its innovation. Optional calibration states can estimate a DVL time offset and a DVL mount correction. The filter also monitors innovation consistency over a sliding window, so a model error it does not estimate, such as a wrong mount, shows up as a degraded status instead of passing silently.
What the sensors make observable
- With IMU and DVL only, heading is not observable: an initial heading error persists. A magnetometer bounds heading.
- Depth drifts without a pressure sensor; with one, depth is bounded.
- A DVL timing offset is estimated under strong motion and explicitly refused when the motion does not excite it. A DVL mount yaw error is unobservable on straight-line motion.
- A constant pressure bias is detectable but not estimable.
The estimate
| Field | Content |
|---|---|
| Time | Integer simulation time in nanoseconds |
| Pose and velocity | NED position and velocity; body-to-world orientation quaternion; bias-corrected body rate (absent before the first IMU sample, never zero-filled) |
| Biases | Accelerometer and gyroscope bias estimates |
| Covariance | Full error-state covariance with its declared ordering |
| Status and health | valid, degraded or invalid, with reasons, per-sensor bookkeeping and events |
| Aiding | The last applied time for each aiding sensor |
| Provenance | Estimator identity and a hash of its configuration, mounts and initialisation protocol |
Estimates are immutable and content-hashed, and they contain no ground truth. Initialisation is explicit. It uses either a state given in the request or the vehicle's declared mission start, at tick 0 only, and the chosen protocol is recorded in the provenance.
Estimator in the loop
A vehicle's controller can take its state from the estimate instead of from truth. The built-in controllers are unchanged: the autopilot, station keeping, waypoint following and trajectory tracking. They simply receive the estimated pose and velocity. When the estimate is invalid, out of date or not at the control time, the step stops with a typed failure. Truth is never substituted.
The loop advances one tick at a time: the World steps, each sensor due at the new tick is sampled, and the estimator ingests the packets. Batched stepping would hand the controller an estimate several ticks old, so it is refused when the estimate drives control.
Using it
Localization attaches to a live World through the simulation service.
Create a World
Create it with a vehicle whose catalogue model declares at least an IMU and a DVL, with their mounts and schedules.
Attach an estimator
Choose the vehicle, its sensors with their measurement configuration, and whether the controller follows the truth or the estimate. Add the free-surface level if a pressure sensor is used.
Step with sensing
Step the World with sensing and estimation for up to 5,000 ticks per request, and read the latest estimate summary for each attached vehicle.
- In the canonical catalogue, the vehicles that declare the navigation sensors the estimator needs are the reference navigation vehicles. These are labelled test and reference models. The built-in production vehicles declare a front camera only, and the catalogue cannot be extended from the application.
- Estimators are held in memory with their World and are discarded with it.
- Visual aiding of the filter, gyro-bias correction from camera rotation, exists in the estimator and its benchmarks but is not part of the runtime's sensor set.
- The navigation EKF on the analytic tier (Dashboard Nav filter) is a separate, older estimator; see Mapping and navigation.