Navigation Algorithms

The following are key functions related to navigation algorithms.

Cramér–Rao Lower Bound

MagNav.crlb — Function

crlb(lat, lon, alt, vn, ve, vd, fn, fe, fd, Cnb, dt, itp_mapS;
     P0         = create_P0(),
     Qd         = create_Qd(),
     R          = 1.0,
     baro_tau   = 3600.0,
     acc_tau    = 3600.0,
     gyro_tau   = 3600.0,
     fogm_tau   = 600.0,
     date       = get_years(2020,185),
     core::Bool = false)

Cramér–Rao lower bound (CRLB) computed with classic Kalman Filter. Equations evaluated about true trajectory.

Arguments:

lat: latitude [rad]
lon: longitude [rad]
alt: altitude [m]
vn: north velocity [m/s]
ve: east velocity [m/s]
vd: down velocity [m/s]
fn: north specific force [m/s^2]
fe: east specific force [m/s^2]
fd: down specific force [m/s^2]
Cnb: direction cosine matrix (body to navigation) [-]
dt: measurement time step [s]
itp_mapS: scalar map interpolation function
P0: (optional) initial covariance matrix
Qd: (optional) discrete time process/system noise matrix
R: (optional) measurement (white) noise variance
baro_tau: (optional) barometer time constant [s]
acc_tau: (optional) accelerometer time constant [s]
gyro_tau: (optional) gyroscope time constant [s]
fogm_tau: (optional) FOGM catch-all time constant [s]
date: (optional) measurement date for IGRF [yr]
core: (optional) if true, include core magnetic field in measurement

Returns:

P: non-linear covariance matrix

crlb(traj::Traj, itp_mapS;
     P0         = create_P0(),
     Qd         = create_Qd(),
     R          = 1.0,
     baro_tau   = 3600.0,
     acc_tau    = 3600.0,
     gyro_tau   = 3600.0,
     fogm_tau   = 600.0,
     date       = get_years(2020,185),
     core::Bool = false)

Cramér–Rao lower bound (CRLB) computed with classic Kalman Filter. Equations evaluated about true trajectory.

Arguments:

traj: Traj trajectory struct
itp_mapS: scalar map interpolation function
P0: (optional) initial covariance matrix
Qd: (optional) discrete time process/system noise matrix
R: (optional) measurement (white) noise variance
baro_tau: (optional) barometer time constant [s]
acc_tau: (optional) accelerometer time constant [s]
gyro_tau: (optional) gyroscope time constant [s]
fogm_tau: (optional) FOGM catch-all time constant [s]
date: (optional) measurement date for IGRF [yr]
core: (optional) if true, include core magnetic field in measurement

Returns:

P: non-linear covariance matrix

Extended Kalman Filter

MagNav.ekf — Function

ekf(lat, lon, alt, vn, ve, vd, fn, fe, fd, Cnb, meas, dt, itp_mapS;
    P0         = create_P0(),
    Qd         = create_Qd(),
    R          = 1.0,
    baro_tau   = 3600.0,
    acc_tau    = 3600.0,
    gyro_tau   = 3600.0,
    fogm_tau   = 600.0,
    date       = get_years(2020,185),
    core::Bool = false,
    der_mapS   = nothing,
    map_alt    = 0)

Extended Kalman filter (EKF) for airborne magnetic anomaly navigation.

Arguments:

lat: latitude [rad]
lon: longitude [rad]
alt: altitude [m]
vn: north velocity [m/s]
ve: east velocity [m/s]
vd: down velocity [m/s]
fn: north specific force [m/s^2]
fe: east specific force [m/s^2]
fd: down specific force [m/s^2]
Cnb: direction cosine matrix (body to navigation) [-]
meas: scalar magnetometer measurement [nT]
dt: measurement time step [s]
itp_mapS: scalar map interpolation function
P0: (optional) initial covariance matrix
Qd: (optional) discrete time process/system noise matrix
R: (optional) measurement (white) noise variance
baro_tau: (optional) barometer time constant [s]
acc_tau: (optional) accelerometer time constant [s]
gyro_tau: (optional) gyroscope time constant [s]
fogm_tau: (optional) FOGM catch-all time constant [s]
date: (optional) measurement date for IGRF [yr]
core: (optional) if true, include core magnetic field in measurement
der_mapS: (optional) scalar map vertical derivative grid interpolation
map_alt: (optional) map altitude [m]

Returns:

filt_res: FILTres filter results struct

ekf(ins::INS, meas, itp_mapS;
    P0         = create_P0(),
    Qd         = create_Qd(),
    R          = 1.0,
    baro_tau   = 3600.0,
    acc_tau    = 3600.0,
    gyro_tau   = 3600.0,
    fogm_tau   = 600.0,
    date       = get_years(2020,185),
    core::Bool = false,
    der_mapS   = map_itp(zeros(2,2),[-pi,pi],[-pi/2,pi/2]),
    map_alt    = 0)

Extended Kalman filter (EKF) for airborne magnetic anomaly navigation.

Arguments:

ins: INS inertial navigation system struct
meas: scalar magnetometer measurement [nT]
itp_mapS: scalar map interpolation function
P0: (optional) initial covariance matrix
Qd: (optional) discrete time process/system noise matrix
R: (optional) measurement (white) noise variance
baro_tau: (optional) barometer time constant [s]
acc_tau: (optional) accelerometer time constant [s]
gyro_tau: (optional) gyroscope time constant [s]
fogm_tau: (optional) FOGM catch-all time constant [s]
date: (optional) measurement date for IGRF [yr]
core: (optional) if true, include core magnetic field in measurement
der_mapS: (optional) scalar map vertical derivative grid interpolation
map_alt: (optional) map altitude [m]

Returns:

filt_res: FILTres filter results struct

Rao-Blackwellized (Marginalized) Particle Filter

MagNav.mpf — Function

mpf(lat, lon, alt, vn, ve, vd, fn, fe, fd, Cnb, meas, dt, itp_mapS;
    P0         = create_P0(),
    Qd         = create_Qd(),
    R          = 1.0,
    num_part   = 1000,
    thresh     = 0.8,
    baro_tau   = 3600.0,
    acc_tau    = 3600.0,
    gyro_tau   = 3600.0,
    fogm_tau   = 600.0,
    date       = get_years(2020,185),
    core::Bool = false)

Rao-Blackwellized (marginalized) particle filter (MPF) for airborne magnetic anomaly navigation. This simplified MPF works only with LINEAR dynamics. This allows the same Kalman filter covariance matrices to be used with each particle, simplifying the filter and reducing the computational load. It is especially suited for map-matching navigation in which there is a highly non-linear, non-Gaussian MEASUREMENT, but NOT non-linear dynamics. The filter also assumes NON-correlated measurements to speed up computation.

Arguments:

lat: latitude [rad]
lon: longitude [rad]
alt: altitude [m]
vn: north velocity [m/s]
ve: east velocity [m/s]
vd: down velocity [m/s]
fn: north specific force [m/s^2]
fe: east specific force [m/s^2]
fd: down specific force [m/s^2]
Cnb: direction cosine matrix (body to navigation) [-]
meas: scalar magnetometer measurement [nT]
dt: measurement time step [s]
itp_mapS: scalar map interpolation function
P0: (optional) initial covariance matrix
Qd: (optional) discrete time process/system noise matrix
R: (optional) measurement (white) noise variance
num_part: (optional) number of particles
thresh: (optional) resampling threshold fraction {0:1}
baro_tau: (optional) barometer time constant [s]
acc_tau: (optional) accelerometer time constant [s]
gyro_tau: (optional) gyroscope time constant [s]
fogm_tau: (optional) FOGM catch-all time constant [s]
date: (optional) measurement date for IGRF [yr]
core: (optional) if true, include core magnetic field in measurement

Returns:

filt_res: FILTres filter results struct

mpf(ins::INS, meas, itp_mapS;
    P0         = create_P0(),
    Qd         = create_Qd(),
    R          = 1.0,
    num_part   = 1000,
    thresh     = 0.8,
    baro_tau   = 3600.0,
    acc_tau    = 3600.0,
    gyro_tau   = 3600.0,
    fogm_tau   = 600.0,
    date       = get_years(2020,185),
    core::Bool = false)

Arguments:

ins: INS inertial navigation system struct
meas: scalar magnetometer measurement [nT]
itp_mapS: scalar map interpolation function
P0: (optional) initial covariance matrix
Qd: (optional) discrete time process/system noise matrix
R: (optional) measurement (white) noise variance
num_part: (optional) number of particles
thresh: (optional) resampling threshold fraction {0:1}
baro_tau: (optional) barometer time constant [s]
acc_tau: (optional) accelerometer time constant [s]
gyro_tau: (optional) gyroscope time constant [s]
fogm_tau: (optional) FOGM catch-all time constant [s]
date: (optional) measurement date for IGRF [yr]
core: (optional) if true, include core magnetic field in measurement

Returns:

filt_res: FILTres filter results struct

Run Filter (with additional options)

MagNav.run_filt — Function

run_filt(traj::Traj, ins::INS, meas, itp_mapS, filt_type::Symbol = :ekf;
         P0             = create_P0(),
         Qd             = create_Qd(),
         R              = 1.0,
         num_part       = 1000,
         thresh         = 0.8,
         baro_tau       = 3600.0,
         acc_tau        = 3600.0,
         gyro_tau       = 3600.0,
         fogm_tau       = 600.0,
         date           = get_years(2020,185),
         core::Bool     = false,
         map_alt        = 0,
         x_nn           = nothing,
         m              = nothing,
         y_norms        = nothing,
         terms          = [:permanent,:induced,:eddy,:bias],
         flux::MagV     = MagV([0.0],[0.0],[0.0],[0.0]),
         x0_TL          = ones(eltype(P0),19),
         extract::Bool  = true,
         run_crlb::Bool = true)

Run navigation filter and optionally compute Cramér–Rao lower bound (CRLB).

Arguments:

traj: Traj trajectory struct
ins: INS inertial navigation system struct
meas: scalar magnetometer measurement [nT]
itp_mapS: scalar map interpolation function
filt_type: (optional) filter type {:ekf,:mpf}
P0: (optional) initial covariance matrix
Qd: (optional) discrete time process/system noise matrix
R: (optional) measurement (white) noise variance
num_part: (optional) number of particles (:mpf only)
thresh: (optional) resampling threshold fraction {0:1} (:mpf only)
baro_tau: (optional) barometer time constant [s]
acc_tau: (optional) accelerometer time constant [s]
gyro_tau: (optional) gyroscope time constant [s]
fogm_tau: (optional) FOGM catch-all time constant [s]
date: (optional) measurement date for IGRF [yr]
core: (optional) if true, include core magnetic field in measurement
map_alt: (optional) map altitude [m]
x_nn: (optional) N x Nf data matrix for neural network (Nf is number of features)
m: (optional) neural network model
y_norms: (optional) tuple of y normalizations, i.e., (y_bias,y_scale)
terms: (optional) Tolles-Lawson terms to use {:permanent,:induced,:eddy,:bias}
flux: (optional) MagV vector magnetometer measurement struct
x0_TL: (optional) initial Tolles-Lawson coefficient states
extract: (optional) if true, extract output structs
run_crlb: (optional) if true, compute the Cramér–Rao lower bound (CRLB)

Returns:

if extract = true & run_crlb = true
- crlb_out: CRLBout Cramér–Rao lower bound extracted output struct
- ins_out: INSout inertial navigation system extracted output struct
- filt_out: FILTout filter extracted output struct
if extract = true & run_crlb = false
- filt_out: FILTout filter extracted output struct
if extract = false & run_crlb = true
- filt_res: FILTres filter results struct
- crlb_P: Cramér–Rao lower bound non-linear covariance matrix
if extract = false & run_crlb = false
- filt_res: FILTres filter results struct

run_filt(traj::Traj, ins::INS, meas, itp_mapS,
         filt_type::Vector{Symbol}; ...)

Run multiple filter models and print results (nothing returned).

Arguments:

filt_type: multiple filter types, e.g., [:ekf,:ekf_online_nn]