Navigation Algorithms

The following are key functions related to navigation algorithms.

MagNav Filter Model

MagNav.create_model — Function

create_model(dt = 0.1, lat1 = deg2rad(45);
             init_pos_sigma   = 3.0,
             init_alt_sigma   = 0.001,
             init_vel_sigma   = 0.01,
             init_att_sigma   = deg2rad(0.01),
             meas_var         = 3.0^2,
             VRW_sigma        = 0.000238,
             ARW_sigma        = 0.000000581,
             baro_sigma       = 1.0,
             ha_sigma         = 0.001,
             a_hat_sigma      = 0.01,
             acc_sigma        = 0.000245,
             gyro_sigma       = 0.00000000727,
             fogm_sigma       = 3.0,
             vec_sigma        = 1000.0,
             TL_sigma         = [],
             baro_tau         = 3600.0,
             acc_tau          = 3600.0,
             gyro_tau         = 3600.0,
             fogm_tau         = 600.0,
             vec_states::Bool = false,
             fogm_state::Bool = true,
             P0_TL            = [])

Create a magnetic navigation filter model for use in an EKF or a MPF.

Arguments:

dt: measurement time step [s]
lat1: initial approximate latitude [rad]
init_pos_sigma: (optional) initial position uncertainty [m]
init_alt_sigma: (optional) initial altitude uncertainty [m]
init_vel_sigma: (optional) initial velocity uncertainty [m/s]
init_att_sigma: (optional) initial attitude uncertainty [rad]
meas_var: (optional) measurement (white) noise variance [nT^2]
VRW_sigma: (optional) velocity random walk [m/s^2 /sqrt(Hz)]
ARW_sigma: (optional) angular random walk [rad/s /sqrt(Hz)]
baro_sigma: (optional) barometer bias [m]
ha_sigma: (optional) barometer aiding altitude bias [m]
a_hat_sigma: (optional) barometer aiding vertical accel bias [m/s^2]
acc_sigma: (optional) accelerometer bias [m/s^2]
gyro_sigma: (optional) gyroscope bias [rad/s]
fogm_sigma: (optional) FOGM catch-all bias [nT]
vec_sigma: (optional) vector magnetometer noise std dev
TL_sigma: (optional) Tolles-Lawson coefficients estimate std dev
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]
vec_states: (optional) if true, include vector magnetometer states
fogm_state: (optional) if true, include FOGM catch-all bias state
P0_TL: (optional) initial Tolles-Lawson covariance matrix

Returns:

P0: initial covariance matrix
Qd: discrete time process/system noise matrix
R: measurement (white) noise variance

source

MagNav filter model internals

MagNav.create_P0 — Function

create_P0(lat1 = deg2rad(45);
          init_pos_sigma   = 3.0,
          init_alt_sigma   = 0.001,
          init_vel_sigma   = 0.01,
          init_att_sigma   = deg2rad(0.01),
          ha_sigma         = 0.001,
          a_hat_sigma      = 0.01,
          acc_sigma        = 0.000245,
          gyro_sigma       = 0.00000000727,
          fogm_sigma       = 3.0,
          vec_sigma        = 1000.0,
          vec_states::Bool = false,
          fogm_state::Bool = true,
          P0_TL            = [])

Create initial covariance matrix P0.

Arguments:

lat1: initial approximate latitude [rad]
init_pos_sigma: (optional) initial position uncertainty [m]
init_alt_sigma: (optional) initial altitude uncertainty [m]
init_vel_sigma: (optional) initial velocity uncertainty [m/s]
init_att_sigma: (optional) initial attitude uncertainty [rad]
ha_sigma: (optional) barometer aiding altitude bias [m]
a_hat_sigma: (optional) barometer aiding vertical accel bias [m/s^2]
acc_sigma: (optional) accelerometer bias [m/s^2]
gyro_sigma: (optional) gyroscope bias [rad/s]
fogm_sigma: (optional) FOGM catch-all bias [nT]
vec_sigma: (optional) vector magnetometer noise std dev
vec_states: (optional) if true, include vector magnetometer states
fogm_state: (optional) if true, include FOGM catch-all bias state
P0_TL: (optional) initial Tolles-Lawson covariance matrix

Returns:

P0: initial covariance matrix

source

MagNav.create_Qd — Function

create_Qd(dt = 0.1;
          VRW_sigma        = 0.000238,
          ARW_sigma        = 0.000000581,
          baro_sigma       = 1.0,
          acc_sigma        = 0.000245,
          gyro_sigma       = 0.00000000727,
          fogm_sigma       = 3.0,
          vec_sigma        = 1000.0,
          TL_sigma         = [],
          baro_tau         = 3600.0,
          acc_tau          = 3600.0,
          gyro_tau         = 3600.0,
          fogm_tau         = 600.0,
          vec_states::Bool = false,
          fogm_state::Bool = true)

Create the discrete time process/system noise matrix Qd.

Arguments:

dt: measurement time step [s]
VRW_sigma: (optional) velocity random walk [m/s^2 /sqrt(Hz)]
ARW_sigma: (optional) angular random walk [rad/s /sqrt(Hz)]
baro_sigma: (optional) barometer bias [m]
acc_sigma: (optional) accelerometer bias [m/s^2]
gyro_sigma: (optional) gyroscope bias [rad/s]
fogm_sigma: (optional) FOGM catch-all bias [nT]
vec_sigma: (optional) vector magnetometer noise std dev
TL_sigma: (optional) Tolles-Lawson coefficients estimate std dev
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]
vec_states: (optional) if true, include vector magnetometer states
fogm_state: (optional) if true, include FOGM catch-all bias state

Returns:

Qd: discrete time process/system noise matrix

source

MagNav.get_pinson — Function

get_pinson(nx::Int, lat, vn, ve, vd, fn, fe, fd, Cnb;
           baro_tau         = 3600.0,
           acc_tau          = 3600.0,
           gyro_tau         = 3600.0,
           fogm_tau         = 600.0,
           vec_states::Bool = false,
           fogm_state::Bool = true,
           k1=3e-2, k2=3e-4, k3=1e-6)

Get the nx x nx Pinson dynamics matrix. States (errors) are:

Num	State	Units	Description
1	lat	rad	latitude
2	lon	rad	longitude
3	alt	m	altitude
4	vn	m/s	north velocity
5	ve	m/s	east velocity
6	vd	m/s	down velocity
7	tn	rad	north tilt (attitude)
8	te	rad	east tilt (attitude)
9	td	rad	down tilt (attitude)
10	ha	m	barometer aiding altitude
11	a_hat	m/s^2	barometer aiding vertical acceleration
12	ax	m/s^2	x accelerometer
13	ay	m/s^2	y accelerometer
14	az	m/s^2	z accelerometer
15	gx	rad/s	x gyroscope
16	gy	rad/s	y gyroscope
17	gz	rad/s	z gyroscope
end	S	nT	FOGM catch-all

Arguments:

nx: total state dimension
lat: latitude [rad]
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) [-]
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]
vec_states: (optional) if true, include vector magnetometer states
fogm_state: (optional) if true, include FOGM catch-all bias state
k1: (optional) barometer aiding constant [1/s]
k2: (optional) barometer aiding constant [1/s^2]
k3: (optional) barometer aiding constant [1/s^3]

Returns:

F: nx x nx Pinson matrix

source

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 (f(lat,lon) or f(lat,lon,alt))
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 (decimal year) for IGRF [yr]
core: (optional) if true, include core magnetic field in measurement

Returns:

P: non-linear covariance matrix

source

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 (f(lat,lon) or f(lat,lon,alt))
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 (decimal year) for IGRF [yr]
core: (optional) if true, include core magnetic field in measurement

Returns:

P: non-linear covariance matrix

source

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 (f(lat,lon) or f(lat,lon,alt))
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 (decimal year) for IGRF [yr]
core: (optional) if true, include core magnetic field in measurement
der_mapS: (optional) scalar map vertical derivative map interpolation function (f(lat,lon) or (f(lat,lon,alt))
map_alt: (optional) map altitude [m]

Returns:

filt_res: FILTres filter results struct

source

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 (f(lat,lon) or f(lat,lon,alt))
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 (decimal year) for IGRF [yr]
core: (optional) if true, include core magnetic field in measurement
der_mapS: (optional) scalar map vertical derivative map interpolation function (f(lat,lon) or (f(lat,lon,alt))
map_alt: (optional) map altitude [m]

Returns:

filt_res: FILTres filter results struct

source

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 (f(lat,lon) or f(lat,lon,alt))
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, only used for filt_type = :mpf
thresh: (optional) resampling threshold fraction {0:1}, only used for filt_type = :mpf
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 (decimal year) 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

source

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]

source