Tutorials - Undersea Navigation

Tutorial 1: Getting Started with INS Integration

Learn how to integrate IMU measurements and propagate navigation state.

1 Initialize the Navigation Filter

Set up initial state with known position and zero velocity:

const initialState = {
    position: [0, 0, 100],      // [x, y, z] meters
    velocity: [0, 0, 0],         // [vx, vy, vz] m/s
    attitude: [0, 0, 0],         // [roll, pitch, yaw] radians
    gyroBias: [0, 0, 0],
    accelBias: [0, 0, 0]
};

const initialP = diag([10, 10, 5, 1, 1, 1, ...]);  // Initial uncertainty
const filter = initializeFilter(initialState, initialP);
                

2 Process IMU Measurements

Read gyro and accelerometer data at high rate (100-1000 Hz):

// Read from IMU at 100 Hz
const gyro = readGyroscope();      // [wx, wy, wz] rad/s
const accel = readAccelerometer(); // [ax, ay, az] m/s²
const dt = 0.01;                   // 100 Hz = 0.01s

filter.predict(gyro, accel, dt);
                

3 Apply DVL Updates

Incorporate velocity measurements when available:

if (dvlDataAvailable()) {
    const velocity = readDVL();  // [vx, vy, vz] m/s
    const R_dvl = 0.01;          // DVL noise variance
    filter.updateDVL(velocity, R_dvl);
}
                

Tip: Always check DVL bottom lock before updating. Invalid measurements will degrade filter performance.

Tutorial 2: LBL Acoustic Positioning

Set up and use a Long Baseline acoustic positioning system.

1 Deploy Transponder Array

Position transponders to minimize GDOP in your operation area:

const transponders = [
    { id: 1, pos: [-500, -500, 0] },  // Southwest corner
    { id: 2, pos: [ 500, -500, 0] },  // Southeast corner
    { id: 3, pos: [ 500,  500, 0] },  // Northeast corner
    { id: 4, pos: [-500,  500, 0] }   // Northwest corner
];

// Verify GDOP < 3 throughout operation area
for (let x = -400; x <= 400; x += 50) {
    for (let y = -400; y <= 400; y += 50) {
        const gdop = computeGDOP([x, y, 100], transponders);
        if (gdop > 3) console.warn(`High GDOP at (${x}, ${y}): ${gdop}`);
    }
}
                

2 Range Measurements

Interrogate transponders and measure round-trip travel time:

const ranges = [];
for (const transponder of transponders) {
    const travelTime = pingTransponder(transponder.id);
    const soundSpeed = mackenzieEquation(temp, salinity, depth);
    const range = (travelTime / 2) * soundSpeed;
    ranges.push(range);
}
                

3 Position Fix

Solve for position and update filter:

const position = trilaterate(ranges, transponders);
const R_lbl = 6.25;  // LBL variance (2.5m std dev)²
filter.updateLBL(position, R_lbl);
                

Warning: Account for sound velocity variations. Use measured or modeled SVP for accurate ranging.

Tutorial 3: Kalman Filter Tuning

Optimize filter parameters for your specific sensor configuration.

1 Set Process Noise (Q)

Start conservative and tune based on innovation sequence:

const Q = {
    position: 0.1,      // Position process noise (m²)
    velocity: 0.05,     // Velocity process noise (m²/s²)
    attitude: 0.01,     // Attitude process noise (rad²)
    gyroBias: 1e-6,     // Gyro bias random walk
    accelBias: 1e-5     // Accel bias random walk
};
                

2 Set Measurement Noise (R)

Match to actual sensor specifications:

const R = {
    dvl: 0.01,      // DVL velocity variance (0.1 m/s std dev)²
    lbl: 6.25,      // LBL position variance (2.5 m std dev)²
    depth: 0.25,    // Depth sensor variance (0.5 m std dev)²
    heading: 0.01   // Compass variance (0.1 rad std dev)²
};
                

3 Check Consistency

Verify filter is consistent - errors should stay within predicted bounds:

// After mission, check consistency
const errors = actualPosition.map((p, i) => p - estimatedPosition[i]);
const uncertainties = filter.getUncertainty();

let consistent = 0;
for (let i = 0; i < errors.length; i++) {
    if (Math.abs(errors[i]) <= 2 * uncertainties[i]) {
        consistent++;
    }
}

const consistencyRatio = consistent / errors.length;
console.log(`Filter consistency: ${(consistencyRatio * 100).toFixed(1)}%`);
// Target: 95% for 2-sigma bounds
                

Tip: If consistency < 90%, increase Q. If innovation sequence shows bias, check R values against actual sensor performance.