<jupyter_start><jupyter_text>Transformation of aircraft trajectories to the ice floe reference frame Using on ice GPS and total station observations as keys<NAME>, February 2016, <EMAIL> On SIPEX-2 (2012 - see http://seaice.acecrc.org.au/sipex2012) airborne LiDAR was flown overa small area designated for intensive sampling, taking sea ice height measurements.An AUV swam underneath, taking observations of sea ice draft.To look at how well estimates of ice thickness using altimetry and some empirical models workwe need to coregister airborne and AUV data.How?On the ice floe a local coordinate system was deployed using at least one pair of GPS receiversand a robotic total station. Locating beacons for the AUV were registered in this localsystem, so UAV drafts are in the local 'floe north' system already.But LiDAR was captured in geographic coordinates! Using the on-ice GPS pair, we track ice driftand ice floe rotation, and ue these to adjust the aircraft trajectory. We know that one of the GPS stations (ice1) has a local coordinate of [0,0]. We also know that the azimuth between the two GPS stations gives us the rotation between 'map north' and 'floe north', since 'floe north' is the baseline between the reciever pair.We also know that we can work just on the aircraft trajectory, and then regenerate a LiDAR point cloud, rather than loading up and rotating N * 10^^6 points!So first the trajectory is translated to local coordinates, and then a rotated to align the aicraft trajectory 'north' to 'floe north'. It should probably be the other way (rotation -> translation), but it's not complex, operating only in two dimensions, so I think order doesn't matter here. Here's the code:<jupyter_code>import matplotlib.pyplot as plt
import numpy as np

#perform rotation of an array of 2D coordinates (x,y) about Z axis
def rotate_z(coord, angle):
    r_angle = angle * np.pi/180
    rotmatrix = np.array(((np.cos(r_angle), np.sin(r_angle)), (np.sin(-r_angle), np.cos(r_angle))))
    new_coord = coord * rotmatrix
    return new_coord

def trim_timeseries(t1, t2):
#trim a long timeseries to match a shoreter one.
#t1 is the long series, t2 is the short sries
#returns a set of indices to apply to the longer
#timeseries dataset
    start = np.min(t2)
    end = np.max(t2)
    idx = (t1>=start)*(t1<=end)
    the_inds = np.where(idx)
    return np.asarray(the_inds), start, end

def find_nearest_vector(array, value):
#http://stackoverflow.com/questions/2566412/find-nearest-value-in-numpy-array
  idx = np.array([np.linalg.norm(x+y) for (x,y) in array-value]).argmin()
  return idx<jupyter_output><empty_output><jupyter_text>OK, functions are declared. next is some messy data gathering (maybe implement using Pandas                                                              to tidy up next time):<jupyter_code>traj_dir = '/media/adam/data/is6_f11/lidar.matlab/trajectory/'
trajfile = 'is6_f11_pass1.3dp'

base_dir = '/media/adam/data/is6_f11/lidar.matlab/base/'
basefile = 'ice1_utm51s_grafnav_ppp_nohead.txt'

rots_dir = '/media/adam/data/is6_f11/lidar.matlab/base/'
baserots = 'ice1_ice2_relaz.txt'

outfilename = '/media/adam/data/is6_f11/lidar.matlab/trajectory/is6_f11_pass1_local_ice_rot.3dp'

#aim here  is to rotate the trajectory at each epoch by the azimuth
# between two GPS stations on the ice
# this *should* end up with a trajectory using 'ice floe' north.
# Also, translating such that the GPS on ice is point (0,0)

trajectory = np.loadtxt(traj_dir + trajfile)
base_pos = np.loadtxt(base_dir + basefile)
base_rots = np.loadtxt(rots_dir + baserots)<jupyter_output><empty_output><jupyter_text>So we have collected a number of data sources here:    - the '.3dp' file is the output of RT post-process, already reprojected to UTM. This has aircraft position and attitude from GPS+IMU,at 250 Hz.    - two ice base station files are used - first is the base position in space and time, and the second is the relative azimuth from base1 -> base2. This second file describes the rotation between UTM north and floe north.    The first job we do is collect the time vector from all three datasets<jupyter_code>t1 = base_pos[:,0]
t2 = trajectory[:,0]
t3 = base_rots[:,0]
In the next block, we make sure all the three data sources have the same time range
base_timeinds,start,end = trim_timeseries(t1,t2)
base_timeinds = base_timeinds[0,:]
base_pos = base_pos[base_timeinds,:]
t1 = base_pos[:,0]

rots_timeinds,start,end = trim_timeseries(t3,t2)
rots_timeinds = rots_timeinds[0,:]
base_rots = base_rots[rots_timeinds,:]
t3 = base_rots[:,0]
...and then we interpolate from GPS base frequency (2 Hz) to aicraft data frequency (250 Hz)

interp_base_x = np.interp(t2,t1,base_pos[:,1])
interp_base_y = np.interp(t2,t1,base_pos[:,2])
interp_rot = np.interp(t2,t3,base_rots[:,1])

local_x = trajectory[:,1] - interp_base_x
local_y = trajectory[:,2] - interp_base_y

local_xy = np.column_stack((local_x, local_y))<jupyter_output><empty_output><jupyter_text>Notice in the step above the planar motion of the ice floe was subtracted from the aircrafttrajectory. This is done for each timeseries step - not just one correction for the wholeposition vector, because the ice is always moving!Next, we find the closest point in our local coordinate vector to the origin. This indextells us where to find a rotation angle. Here, a single angle is applied as a sort of 'snapshot'. (why not a timeseries of angles? because it messes up aicraft heading required to make LiDAR)<jupyter_code>#find rotation angle closest to origin

angle_idx = find_nearest_vector(local_xy, [0, 0])
the_angle = interp_rot[angle_idx]
print('rotating by:')
print(the_angle)
print('degrees')

#rotate the xy array by the rotation closest to origin
rot_xy = rotate_z(np.matrix(local_xy), the_angle)

adjusted_heading = trajectory[:,6] - the_angle

out_trajectory = np.column_stack((t2, rot_xy[:,0],rot_xy[:,1], trajectory[:,3], trajectory[:,4], trajectory[:,5], adjusted_heading, \
                                  trajectory[:,7], trajectory[:,8], trajectory[:,9], trajectory[:,10], trajectory[:,11], \
                                  trajectory[:,12]))

np.savetxt(outfilename, out_trajectory, fmt='%.5f')<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.09064609450337512, 'fraction_numerical': 0.019768563162970106, 'mean_word_length': 4.631674208144797, 'pattern_counts': {'":': 0, '<': 20, '<?xml version=': 0, '>': 22, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '4', 'dir': 'jupyter-structured-clean-dedup', 'id': '312791', 'license': 'Apache-2.0', 'n_tokens_mistral': 2051, 'n_tokens_neox': 1959, 'n_words': 726, 'path': 'aircraft_trajectory_to_local.ipynb', 'repo_name': 'adamsteer/thesis-notebooks', 'size': '6241'}