Develop Algorithm to Nudge Dead Reckoned LRAUV Segments to Match GPS Fixes

Read data from .nc4 files and examine performance of nudging method - needs to work for all cases

Executing this Notebook requires a personal STOQS server. Follow the steps to build your own development system — this will take a few hours and depends on a good connection to the Internet. Once your server is up log into it (after a cd ~/Vagrants/stoqsvm) and activate your virtual environment with the usual commands, e.g.:

vagrant ssh -- -X
export STOQS_HOME=/vagrant/dev/stoqsgit  # Use STOQS_HOME=/home/vagrant/dev/stoqsgit if not using NFS mount
cd $STOQS_HOME && source venv-stoqs/bin/activate
export DATABASE_URL=postgis://stoqsadm:CHANGEME@127.0.0.1:5438/stoqs

Launch Jupyter Notebook from this directory on your system with:

cd $STOQS_HOME/stoqs/loaders/CANON/toNetCDF/notebooks
../../../../manage.py shell_plus --notebook

A Firefox window should appear where you can open this file and execute it.

Test with various files that have special circumstances — encountered while reprocessing the archive


In [1]:
from netCDF4 import Dataset
import numpy as np

# 1. Initial daphne file from the https://stoqs.mbari.org/stoqs_canon_may2018 campaign, wget'ted from:
# http://dods.mbari.org/data/lrauv/daphne/missionlogs/2018/20180603_20180611/20180608T003220/201806080032_201806090421.nc4
#df = '/vagrant/dev/stoqsgit/201806080032_201806090421.nc4'

# 2. This file has dead reckoned positions before the first GPS fix
# http://dods.mbari.org/data/lrauv/daphne/missionlogs/2018/20180603_20180611/20180605T183835/201806051838_201806070507.nc4
#df = '/vagrant/dev/stoqsgit/201806051838_201806070507.nc4'

# 3. This file has a bogus coordinate _time value at index 832 that should be removed to improve performance
# http://dods.mbari.org/data/lrauv/daphne/missionlogs/2018/20180227_20180301/20180301T095515/201803010955_201803011725.nc4
df = '/vagrant/dev/stoqsgit/201803010955_201803011725.nc4'

ds = Dataset(df)

# Default is to not create the interactive data plots, which add a lot of data to the Notebook
bokeh_plots = False

Define function to remove bad time axis values and convert angles to degrees


In [2]:
import pandas as pd
from datetime import datetime
from time import time
def var_series(data_array, time_array, tmin=0, tmax=time(), angle=False, verbose=False):
    '''Return a Pandas series of the coordinate with invalid and out of range time values removed'''
    mt = np.ma.masked_invalid(time_array)
    mt = np.ma.masked_outside(mt, tmin, tmax)
    bad_times = [str(datetime.utcfromtimestamp(es)) for es in time_array[:][mt.mask]]
    if verbose and bad_times:
        print(f"Removing bad times in {data_array.name} ([index], [values]): {np.where(mt.mask)[0]}, {bad_times}")
    v_time = pd.to_datetime(mt.compressed(), unit='s',errors = 'coerce')
    da = pd.Series(data_array[:][~mt.mask], index=v_time)
    
    rad_to_deg = False
    if angle:
        # Some universal positions are in degrees, some are in radians - make a guess based on mean values
        if np.max(np.abs(da)) <= np.pi and np.max(np.abs(da)) <= np.pi:
            rad_to_deg = True
        if verbose:
            print(f"{data_array.name}: rad_to_deg = {rad_to_deg}")
        if rad_to_deg:
            da = da * 180.0 / np.pi

    return da

Examine the data from the file


In [3]:
%matplotlib inline
from pylab import plt
import numpy as np

# Make Pandas Series of the coordinate and yaw and depth variables
lon_fix = var_series(ds['longitude_fix'], ds['longitude_fix_time'], angle=True)
lat_fix = var_series(ds['latitude_fix'], ds['latitude_fix_time'], angle=True)
lon = var_series(ds['longitude'], ds['longitude_time'], angle=True)
lat = var_series(ds['latitude'], ds['latitude_time'], angle=True)
yaw = var_series(ds['platform_orientation'], ds['platform_orientation_time'], angle=True)
depth = var_series(ds['depth'], ds['depth_time'], angle=False)

# Make plots of the original data
plt.rcParams['figure.figsize'] = (15, 8);
fig, ax = plt.subplots(3,1)
ax[0].set_title('Dead Reckoned and GPS Navigation Time Series')
ax[1].set_xlabel('Time (GMT)')

ax[0].set_ylabel('Latitude (degrees_north)')
ax[0].plot(lat.index, lat, '-x', label='Dead Reckoned')
ax[0].plot(lat_fix.index, lat_fix, 'o', label='GPS')
ax[0].grid(True)

ax[1].set_ylabel('Longitude (degrees_east)')
ax[1].plot(lon.index, lon, '-x', label='Dead Reckoned')
ax[1].plot(lon_fix.index, lon_fix, 'o', label='GPS')
ax[1].grid(True)

ax[2].set_ylabel('Yaw (degrees)')
ax[2].plot(yaw.index, yaw)
ax[2].grid(True)

ax[0].legend()
_ = ax[1].legend()


Files 1 and 2 have interesting left turns during the underwater dead reckoned segments in the Latitude plots. These are correlated with significant changes in the Yaw (heading) data, so they appear to be real. The goal with the algorithm is to elimate the jumps in position when the LRAUV surfaces and receives new GPS positions.

Define function to make interactive Bokeh plot of dead reckoned and GPS latitude or longitude variables


In [4]:
def plot_vars(var_time, var, var_fix_time, var_fix, y_label, plot_depth=False):
    from bokeh.plotting import figure
    from bokeh.io import output_notebook, show
    from bokeh.models import LinearAxis, Range1d
    from bokeh.resources import INLINE
    output_notebook(resources=INLINE, hide_banner=True)
    p = figure(width = 900, height = 300,
                 title = 'Dead Reckoned and GPS Fix Positions',
                 x_axis_type="datetime",
                 x_axis_label='Time (GMT)',
                 y_range=(np.min(var), np.max(var)),
                 y_axis_label = y_label)

    p.extra_y_ranges = {"depth_axis": Range1d(start=2, end=-0.2)}  
    p.add_layout(LinearAxis(y_range_name="depth_axis"), 'right')

    p.line(var_time, var, line_width=1)
    p.cross(var_time, var)
    p.square(var_fix_time, var_fix, color="orange")

    if plot_depth:
        p.line(depth.index, depth, y_range_name="depth_axis", line_color="yellow")
        p.cross(depth.index, depth, y_range_name="depth_axis", line_color="yellow")

    _ = show(p)

Create interactive plots where we can zoom into areas of interest. (plot_vars() adds a lot of data to the Notebook, so don't commit rendered.)


In [5]:
if bokeh_plots:
    plot_vars(lat.index, lat, lat_fix.index, lat_fix, 'Latitude (degrees_north)', plot_depth=True)

In [6]:
if bokeh_plots:
    plot_vars(lon.index, lon, lon_fix.index, lon_fix, 'Longitude (degrees_east)', plot_depth=True)

After exploring the data, it appears that we can safely adjust all dead reckoned positions that are in between the GPS fixes.

Define a function to loop though pairs of GPS fixes and "nudge" the dead reckoned positions so that they match the position of the second GPS fix (acquired after surfacing) in the pair.


In [7]:
def nudge_coords(ds, verbose=False):
    '''Given a ds object to an LRAUV .nc4 file return adjusted longitude
    and latitude arrays that reconstruct the trajectory so that the dead
    reckoned positions are nudged so that they match the GPS fixes
    '''
    from math import cos
    
    # Produce Pandas time series from the NetCDF variables
    lon_fix = var_series(ds['longitude_fix'], ds['longitude_fix_time'], angle=True, verbose=verbose)
    lat_fix = var_series(ds['latitude_fix'], ds['latitude_fix_time'], angle=True, verbose=verbose)
    lon = var_series(ds['longitude'], ds['longitude_time'], angle=True, verbose=verbose)
    lat = var_series(ds['latitude'], ds['latitude_time'], angle=True, verbose=verbose)

    max_sec_diff_at_end = 10
    print(f"{'seg#':4s}  {'end_sec_diff':12s} {'end_lon_diff':12s} {'end_lat_diff':12s}", end='')
    print(f" {'len(segi)':9s} {'seg_min':7s} {'u_drift (cm/s)':14s} {'v_drift (cm/s)':14s}")
    
    # Any dead reckoned points before first GPS fix - usually empty as GPS fix happens before dive
    segi = np.where(lat.index < lat_fix.index[0])[0]
    if lon[:][segi].any():
        lon_nudged = lon[segi]
        lat_nudged = lat[segi]
        dt_nudged = lon.index[segi]
        print(f"{' ':4}  {'-':>12} {'-':>12} {'-':>12}", end='')
    else:
        lon_nudged = np.array([])
        lat_nudged = np.array([])
        dt_nudged = np.array([], dtype='datetime64[ns]')
        if segi.any():
            print(f"{' ':4}  {'nan':>12} {'nan':>12} {'nan':>12}", end='')
    if segi.any():
        seg_min = (lat.index[segi][-1] - lat.index[segi][0]).total_seconds() / 60
        print(f" {len(segi):-9d} {seg_min:7.2f} {'-':>14} {'-':>14}")
    else:
        seg_min = 0
    
    for i in range(len(lat_fix) - 1):
        # Segment of dead reckoned (under water) positions, each surrounded by GPS fixes
        segi = np.where(np.logical_and(lat.index > lat_fix.index[i], 
                                       lat.index < lat_fix.index[i+1]))[0]
        end_sec_diff = (lat_fix.index[i+1] - lat.index[segi[-1]]).total_seconds()
        assert(end_sec_diff < max_sec_diff_at_end)

        end_lon_diff = lon_fix[i+1] - lon[segi[-1]]
        end_lat_diff = lat_fix[i+1] - lat[segi[-1]]
        seg_min = (lat.index[segi][-1] - lat.index[segi][0]).total_seconds() / 60
        
        # Compute approximate horizontal drift rate as a sanity check
        u_drift = (end_lat_diff * cos(lat_fix[i+1]) * 60 * 185300
                    / (lat.index[segi][-1] - lat.index[segi][0]).total_seconds())
        v_drift = (end_lat_diff * 60 * 185300 
                    / (lat.index[segi][-1] - lat.index[segi][0]).total_seconds())
        print(f"{i:4d}: {end_sec_diff:12.3f} {end_lon_diff:12.7f} {end_lat_diff:12.7f}", end='')
        print(f" {len(segi):-9d} {seg_min:7.2f} {u_drift:14.2f} {v_drift:14.2f}")

        # Start with zero adjustment at begining and linearly ramp up to the diff at the end
        lon_nudge = np.interp( lon.index[segi].astype(np.int64), 
                              [lon.index[segi].astype(np.int64)[0], lon.index[segi].astype(np.int64)[-1]],
                              [0, end_lon_diff] )
        lat_nudge = np.interp( lat.index[segi].astype(np.int64), 
                              [lat.index[segi].astype(np.int64)[0], lat.index[segi].astype(np.int64)[-1]],
                              [0, end_lat_diff] )

        lon_nudged = np.append(lon_nudged, lon[segi] + lon_nudge)
        lat_nudged = np.append(lat_nudged, lat[segi] + lat_nudge)
        dt_nudged = np.append(dt_nudged, lon.index[segi])
    
    # Any dead reckoned points after first GPS fix - not possible to nudge, just copy in
    segi = np.where(lat.index > lat_fix.index[-1])[0]
    seg_min = 0
    if segi.any():
        lon_nudged = np.append(lon_nudged, lon[segi])
        lat_nudged = np.append(lat_nudged, lat[segi])
        dt_nudged = np.append(dt_nudged, lon.index[segi])
        seg_min = (lat.index[segi][-1] - lat.index[segi][0]).total_seconds() / 60
   
    print(f"{i:4d}: {'-':>12} {'-':>12} {'-':>12}", end='')
    print(f" {len(segi):-9d} {seg_min:7.2f} {'-':>14} {'-':>14}")

    return pd.Series(lon_nudged, index=dt_nudged), pd.Series(lat_nudged, index=dt_nudged)

In [8]:
lon_nudged, lat_nudged = nudge_coords(ds, verbose=True)


longitude_fix: rad_to_deg = False
latitude_fix: rad_to_deg = False
Removing bad times in longitude ([index], [values]): [832], ['2513-11-08 13:46:34.058369']
longitude: rad_to_deg = True
Removing bad times in latitude ([index], [values]): [832], ['2513-11-08 13:46:34.058369']
latitude: rad_to_deg = True
seg#  end_sec_diff end_lon_diff end_lat_diff len(segi) seg_min u_drift (cm/s) v_drift (cm/s)
   0:        0.313   -0.0005611    0.0015981      3295   24.47           7.48          12.10
   1:        0.325    0.0000057    0.0000405        24    0.39          12.01          19.41
   2:        0.326    0.0002171    0.0069561     15600  104.60           7.67          12.32
   3:        0.349    0.0019506    0.0091442      9563   65.63          16.21          25.82
   4:        0.313    0.0002268    0.0008398       732    5.59          17.49          27.83
   5:        0.327    0.0002402    0.0006763       589    4.53          17.41          27.68
   6:        0.330    0.0007208    0.0073278      7882   52.78          16.21          25.73
   7:        0.368   -0.0000182    0.0000730        24    0.38          22.69          36.01
   8:        0.335    0.0008541    0.0068540      7519   50.24          15.96          25.28
   9:        0.359   -0.0000190    0.0001218        25    0.39          36.96          58.54
  10:        0.296    0.0006757    0.0068363      8242   55.53          14.42          22.81
  10:            -            -            -      9856   66.83              -              -

In [9]:
if bokeh_plots:
    plot_vars(lat_nudged.index, lat_nudged, lat_fix.index, lat_fix, 'Latitude (degrees_north)')

In [10]:
if bokeh_plots:
    plot_vars(lon_nudged.index, lon_nudged, lon_fix.index, lon_fix, 'Longitude (degrees_east)')

In [11]:
plt.rcParams['figure.figsize'] = (15, 8);
fig, ax = plt.subplots(2,1)
ax[0].set_title('Dead Reckoned and GPS Navigation Positions After Nudging')
ax[1].set_xlabel('Time (GMT)')

ax[0].set_ylabel('Latitude (degrees_north)')
ax[0].plot(lat_nudged.index, lat_nudged, '-x', label='Nudged Dead Reckoned')
ax[0].plot(lat_fix.index, lat_fix, 'o', label='GPS')
ax[0].grid(True)

ax[1].set_ylabel('Longitude (degrees_east)')
ax[1].plot(lon_nudged.index, lon_nudged, '-x', label='Nudged Dead Reckoned')
ax[1].plot(lon_fix.index, lon_fix, 'o', label='GPS')
ax[1].grid(True)

ax[0].legend()
_ = ax[1].legend()



In [ ]: