Navigation Theory Guide

Comprehensive Foundation in Underwater Navigation Systems

Back to Project

Table of Contents

1. Inertial Navigation Systems 2. Acoustic Positioning 3. Sensor Fusion & Kalman Filtering 4. Error Propagation Models 5. Coordinate Systems & Transformations

1. Inertial Navigation Systems

1.1 Fundamentals of Inertial Navigation

Inertial Navigation Systems (INS) provide self-contained navigation by integrating measurements from accelerometers and gyroscopes. The fundamental principle is dead reckoning: measuring acceleration and rotation to compute position and orientation changes over time.

Core Principle: An INS measures specific force (acceleration minus gravity) and angular velocity, then integrates these measurements to compute velocity, position, and attitude.

1.2 Navigation Equations

The mechanization equations in the navigation frame (NED - North-East-Down) are:

$$\dot{\mathbf{v}}^n = \mathbf{C}_b^n \mathbf{f}^b - (2\boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \mathbf{v}^n + \mathbf{g}^n$$ $$\dot{\mathbf{p}}^n = \mathbf{v}^n$$ $$\dot{\mathbf{C}}_b^n = \mathbf{C}_b^n [\boldsymbol{\omega}_{ib}^b \times]$$

Where:

1.3 Error Propagation

INS errors grow unbounded without external aiding. Position error grows approximately linearly with distance traveled due to velocity errors, which themselves accumulate from attitude and sensor bias errors.

$$\sigma_{pos}(t) \approx \sigma_{vel,0} \cdot t + \frac{1}{2}\sigma_{accel} \cdot t^2 + \sigma_{attitude} \cdot g \cdot t^2$$ For practical underwater navigation: $$\text{Position Drift} \approx 0.1\% \text{ to } 2\% \text{ of distance traveled}$$

2. Acoustic Positioning Systems

2.1 Long Baseline (LBL)

LBL systems use a network of seafloor transponders to provide absolute position fixes. The vehicle ranges to multiple transponders and computes position via trilateration.

$$r_i = ||\mathbf{p}_{vehicle} - \mathbf{p}_{transponder,i}|| + n_i$$ Where \(r_i\) is measured range and \(n_i \sim \mathcal{N}(0, \sigma_r^2)\) is measurement noise. Position is found by minimizing: $$\min_{\mathbf{p}} \sum_{i=1}^{N} (r_i - ||\mathbf{p} - \mathbf{p}_i||)^2$$

2.2 Geometric Dilution of Precision (GDOP)

GDOP quantifies how transponder geometry affects positioning accuracy. It multiplies measurement error to give position error:

$$\sigma_{position} = GDOP \times \sigma_{range}$$ $$GDOP = \sqrt{trace[(H^T H)^{-1}]}$$ Where \(H\) is the observation matrix with rows: \(H_i = (\mathbf{p} - \mathbf{p}_i)^T / r_i\)

2.3 Ultra-Short Baseline (USBL)

USBL systems use a compact transceiver array on a surface vessel to track underwater vehicles. Range is measured via travel time, and bearing via phase interferometry.

USBL Accuracy: Typically 1-2% of slant range. Bearing accuracy improves with longer baselines: \(\sigma_\theta \approx \lambda / (2\pi d)\) where \(\lambda\) is wavelength and \(d\) is baseline.

3. Sensor Fusion & Kalman Filtering

3.1 Extended Kalman Filter (EKF)

The EKF fuses INS, DVL, LBL, and depth measurements to provide optimal state estimates. The state vector typically includes position, velocity, attitude, and sensor biases.

State vector: \(\mathbf{x} = [\mathbf{p}, \mathbf{v}, \boldsymbol{\psi}, \mathbf{b}_g, \mathbf{b}_a]^T\) Prediction: $$\hat{\mathbf{x}}^- = f(\hat{\mathbf{x}}^+, \mathbf{u})$$ $$\mathbf{P}^- = \mathbf{F}\mathbf{P}^+\mathbf{F}^T + \mathbf{Q}$$ Update: $$\mathbf{K} = \mathbf{P}^-\mathbf{H}^T(\mathbf{H}\mathbf{P}^-\mathbf{H}^T + \mathbf{R})^{-1}$$ $$\hat{\mathbf{x}}^+ = \hat{\mathbf{x}}^- + \mathbf{K}(\mathbf{z} - h(\hat{\mathbf{x}}^-))$$ $$\mathbf{P}^+ = (\mathbf{I} - \mathbf{K}\mathbf{H})\mathbf{P}^-$$

3.2 Process and Measurement Noise

Proper tuning of \(\mathbf{Q}\) (process noise) and \(\mathbf{R}\) (measurement noise) is critical for filter performance:

4. Error Propagation Models

4.1 Dead Reckoning Error Growth

Without external aiding, position uncertainty grows according to sensor characteristics:

For a mission of duration \(T\) and distance \(D\): $$\sigma_{DR}(T) = \sqrt{(\sigma_0)^2 + (k_{gyro} \cdot D)^2 + (k_{accel} \cdot T^2)^2}$$ Where: - \(\sigma_0\): Initial position uncertainty - \(k_{gyro}\): Gyro-induced drift rate (% of distance) - \(k_{accel}\): Accelerometer-induced drift

4.2 DVL-Aided Navigation

Doppler Velocity Log (DVL) provides velocity updates that bound error growth. The bounded error is:

$$\sigma_{DVL}(t) = \sqrt{\sigma_{DVL}^2 \cdot \Delta t_{DVL} + (\sigma_{INS} \cdot \Delta t_{DVL})^2}$$ Typical performance: Position error < 0.2% of distance with 5 Hz DVL updates

4.3 LBL-Aided Navigation

LBL fixes reset absolute position uncertainty. Between fixes, uncertainty grows according to INS drift:

$$\sigma(t) = \begin{cases} \sigma_{LBL} & \text{at fix} \\ \sqrt{\sigma_{LBL}^2 + (k_{drift} \cdot d(t))^2} & \text{between fixes} \end{cases}$$

5. Coordinate Systems & Transformations

5.1 Common Reference Frames

5.2 Rotation Representations

Attitude can be represented using:

Direction Cosine Matrix (DCM): $$\mathbf{C}_b^n = \begin{bmatrix} c_\psi c_\theta & c_\psi s_\theta s_\phi - s_\psi c_\phi & c_\psi s_\theta c_\phi + s_\psi s_\phi \\ s_\psi c_\theta & s_\psi s_\theta s_\phi + c_\psi c_\phi & s_\psi s_\theta c_\phi - c_\psi s_\phi \\ -s_\theta & c_\theta s_\phi & c_\theta c_\phi \end{bmatrix}$$ Quaternions: \(\mathbf{q} = [q_0, q_1, q_2, q_3]^T\), \(||\mathbf{q}|| = 1\)

5.3 Gravity Modeling

Accurate gravity modeling is essential for INS. The WGS84 ellipsoidal gravity model:

$$g(\phi, h) = g_e \cdot \frac{1 + k \sin^2\phi}{\sqrt{1 - e^2\sin^2\phi}} - 2g_e \cdot \frac{h}{a}$$ Where \(g_e = 9.7803267714\) m/s², \(k = 0.00193185138639\), \(e^2 = 0.00669437999013\)

Further Reading

Back to Project API Reference