Merge pull request #269 from MIT-AI-Accelerator/gnadt

Gnadt

Project.toml

@@ -1,6 +1,6 @@
 name = "MagNav"
 uuid = "f91b31a4-be4d-40e3-b767-4b8c09c10076"
-version = "1.2.1"
+version = "1.2.2"
 
 [deps]
 ArchGDAL = "c9ce4bd3-c3d5-55b8-8973-c0e20141b8c3"

examples/simple_magnav.jl

@@ -0,0 +1,88 @@
+##*  Simple MagNav example to use with YOUR data
+# 1) Install Julia (Juliaup is great): https://julialang.org/downloads/
+# 2) Start Julia & run this script (should show 7 m navigation errors)
+# 3) Modify files in simple_magnav_data folder with your data
+    # fields for traj (aka GPS) & ins structs:
+        # tt      [s] - time stamps
+        # lat     [deg]
+        # lon     [deg]
+        # alt     [m]
+        # roll    [deg]
+        # pitch   [deg]
+        # yaw     [deg]
+        # mag_1_c [nT] - compensated mag (only in traj)
+    # fields for map struct:
+        # map [nT]  - shape is yy by xx
+        # xx  [deg] - longitude here
+        # yy  [deg] - latitude here
+        # alt [m]
+# 4) Modify filter parameters in this script for your data
+# 5) Re-run this script
+
+##* Setup
+cd(@__DIR__)
+using Pkg
+# Pkg.add("MagNav") # as needed
+using MagNav
+using Random: seed!
+dir = "simple_magnav_data";
+
+##* Load data from CSV files
+seed!(33); # for reproducibility
+xyz_csv = "$dir/xyz.csv";
+map_csv = "$dir/map"; # folder with alt, map, xx, yy
+xyz1    = get_XYZ0(xyz_csv);
+mapS1   = get_map(map_csv);
+
+##* Load data from HDF5 files
+seed!(33); # for reproducibility
+xyz_h5  = "$dir/xyz.h5"
+map_h5  = "$dir/map.h5"
+xyz2    = get_XYZ0(xyz_h5);
+mapS2   = get_map(map_h5);
+
+##* Load data from MAT files
+seed!(33); # for reproducibility
+xyz_mat = "$dir/xyz.mat";
+map_mat = "$dir/map.mat";
+xyz3    = get_XYZ0(xyz_mat);
+mapS3   = get_map(map_mat);
+
+##* Choose data from one of above load options
+xyz  = xyz2;
+mapS = mapS2;
+
+##* Create, run, and plot navigation filter
+(P0,Qd,R) = create_model(xyz.traj.dt, xyz.traj.lat[1];
+                         init_pos_sigma = 3.0,           # initial position uncertainty [m]
+                         init_alt_sigma = 0.001,         # initial altitude uncertainty [m]
+                         init_vel_sigma = 0.01,          # initial velocity uncertainty [m/s]
+                         init_att_sigma = deg2rad(0.01), # initial attitude uncertainty [rad]
+                         meas_var       = 3.0^2,         # measurement (white) noise variance [nT^2]
+                         VRW_sigma      = 0.000238,      # velocity random walk [m/s^2 /sqrt(Hz)]
+                         ARW_sigma      = 0.000000581,   # angular  random walk [rad/s /sqrt(Hz)]
+                         baro_sigma     = 1.0,           # barometer bias [m]
+                         ha_sigma       = 0.001,         # barometer aiding altitude bias [m]
+                         a_hat_sigma    = 0.01,          # barometer aiding vertical accel bias [m/s^2]
+                         acc_sigma      = 0.000245,      # accelerometer bias [m/s^2]
+                         gyro_sigma     = 0.00000000727, # gyroscope bias [rad/s]
+                         fogm_sigma     = 3.0,           # FOGM catch-all bias [nT]
+                         baro_tau       = 3600.0,        # barometer time constant [s]
+                         acc_tau        = 3600.0,        # accelerometer time constant [s]
+                         gyro_tau       = 3600.0,        # gyroscope time constant [s]
+                         fogm_tau       = 600.0);        # FOGM catch-all time constant [s]
+
+itp_mapS = map_interpolate(mapS); # map interpolation function
+
+(crlb_out,ins_out,filt_out) = run_filt(xyz.traj, xyz.ins, xyz.mag_1_c, itp_mapS, :ekf;
+                                       P0       = P0,
+                                       Qd       = Qd,
+                                       R        = R,
+                                       baro_tau = 3600.0,
+                                       acc_tau  = 3600.0,
+                                       gyro_tau = 3600.0,
+                                       fogm_tau = 600.0,
+                                       date     = get_years(2024,25), # f(year, day of year), only needed if core = true
+                                       core     = false);             # core field contained within xyz.mag_1_c field
+
+p1 = plot_filt(xyz.traj,xyz.ins,filt_out;show_plot=false)[1]

examples/simple_magnav_data/map/alt.csv

@@ -0,0 +1 @@
+305.0

examples/simple_magnav_data/map/xx.csv

@@ -0,0 +1 @@
+-95.87,-95.86000000000001,-95.85,-95.84,-95.83000000000001,-95.82000000000001,-95.81,-95.80000000000001,-95.79,-95.78,-95.77000000000001,-95.76,-95.75,-95.74000000000001,-95.73,-95.72,-95.71000000000001,-95.7,-95.69,-95.68,-95.67,-95.66,-95.65,-95.64,-95.63,-95.62,-95.61000000000001,-95.6,-95.59,-95.58000000000001,-95.57000000000001,-95.56,-95.55000000000001,-95.54,-95.53,-95.52000000000001,-95.51,-95.5,-95.49000000000001,-95.48,-95.47,-95.46000000000001,-95.45,-95.44,-95.43,-95.42,-95.41,-95.4,-95.39,-95.38,-95.37,-95.36000000000001,-95.35,-95.34,-95.33000000000001,-95.32000000000001,-95.31,-95.30000000000001,-95.29,-95.28,-95.27000000000001,-95.26,-95.25,-95.24000000000001,-95.23,-95.22,-95.21000000000001,-95.2,-95.19,-95.18,-95.17,-95.16,-95.15,-95.14,-95.13,-95.12,-95.11000000000001,-95.1,-95.09,-95.08000000000001,-95.07000000000001,-95.06,-95.05000000000001,-95.04,-95.03,-95.02000000000001,-95.01,-95.0,-94.99000000000001,-94.98,-94.97,-94.96000000000001,-94.95,-94.94,-94.93,-94.92,-94.91,-94.9,-94.89,-94.88

examples/simple_magnav_data/map/yy.csv

@@ -0,0 +1 @@
+38.57000000000001,38.580000000000005,38.59,38.60000000000001,38.61000000000001,38.620000000000005,38.63,38.64,38.650000000000006,38.660000000000004,38.67,38.68000000000001,38.690000000000005,38.7,38.71000000000001,38.720000000000006,38.730000000000004,38.74000000000001,38.75000000000001,38.760000000000005,38.77,38.78,38.790000000000006,38.800000000000004,38.81,38.82000000000001,38.830000000000005,38.84,38.85000000000001,38.86000000000001,38.870000000000005,38.88,38.89,38.900000000000006,38.910000000000004,38.92,38.93000000000001,38.940000000000005,38.95,38.96000000000001,38.970000000000006,38.980000000000004,38.99000000000001,39.00000000000001,39.010000000000005,39.02,39.03,39.040000000000006,39.050000000000004,39.06,39.07000000000001,39.080000000000005,39.09,39.10000000000001,39.11000000000001,39.120000000000005,39.13,39.14,39.150000000000006,39.160000000000004,39.17,39.18000000000001,39.190000000000005,39.2,39.21000000000001,39.220000000000006,39.230000000000004,39.24000000000001,39.25000000000001,39.260000000000005,39.27,39.28,39.290000000000006,39.300000000000004,39.31,39.32000000000001,39.330000000000005,39.34,39.35000000000001,39.36000000000001,39.370000000000005,39.38,39.39,39.400000000000006,39.410000000000004,39.42,39.43000000000001,39.440000000000005,39.45,39.46000000000001,39.470000000000006,39.480000000000004,39.49000000000001,39.50000000000001,39.510000000000005,39.52,39.53,39.540000000000006,39.550000000000004,39.56