def test_latlon_wgs84(self):
# Boldrewood
lat_ref = 50.936501
lon_ref = -1.404266
# Highfield B35
lat_p = 50.936870
lon_p = -1.396295
x, theta = latlon_to_metres(lat_p, lon_p, lat_ref, lon_ref)
self.assertLess(x, 600.0)
self.assertGreater(x, 500.0)
self.assertGreater(theta, 85.0)
self.assertLess(theta, 90.0)
print(x, theta)
easting = x * math.sin(math.radians(theta))
northing = x * math.cos(math.radians(theta))
print(northing, easting)
lat, lon = metres_to_latlon(lat_ref, lon_ref, easting, northing)
self.assertAlmostEqual(lat, lat_p, places=2)
self.assertAlmostEqual(lon, lon_p, places=2)
# Sydney
lat_ref = -33.8559799094
lon_ref = 151.20666584
# Tokyo
lat_p = 35.652832
lon_p = 139.839478
x, theta = latlon_to_metres(lat_p, lon_p, lat_ref, lon_ref)
self.assertLess(x, 8.5e6)
self.assertGreater(x, 7.5e6)
self.assertGreater(theta, -20)
self.assertLess(theta, 10)
easting = x * math.sin(math.radians(theta))
northing = x * math.cos(math.radians(theta))
lat, lon = metres_to_latlon(lat_ref, lon_ref, easting, northing)
self.assertAlmostEqual(lat, lat_p, places=2)
self.assertAlmostEqual(lon, lon_p, places=2)
def save_ekf_to_list(
ekf_states: List[EkfState],
mission: Mission,
vehicle: Vehicle,
dead_reckoning_dvl_list: List[SyncedOrientationBodyVelocity],
shift_to_origin: Optional[bool] = True,
) -> List[SyncedOrientationBodyVelocity]:
ekf_list = []
dr_idx = 1
for s in ekf_states:
b = s.toSyncedOrientationBodyVelocity()
if shift_to_origin:
# Offset the measurements from the DVL to the robot origin
[x_offset, y_offset, z_offset] = body_to_inertial(
b.roll,
b.pitch,
b.yaw,
vehicle.origin.surge - vehicle.dvl.surge,
vehicle.origin.sway - vehicle.dvl.sway,
vehicle.origin.heave - vehicle.dvl.heave,
)
b.northings += x_offset
b.eastings += y_offset
b.depth += z_offset
# Transform to lat lon using origins
b.latitude, b.longitude = metres_to_latlon(
mission.origin.latitude,
mission.origin.longitude,
b.eastings,
b.northings,
)
# Interpolate altitude from DVL
while (dr_idx < len(dead_reckoning_dvl_list)
and dead_reckoning_dvl_list[dr_idx].epoch_timestamp <
b.epoch_timestamp):
dr_idx += 1
b.altitude = interpolate(
b.epoch_timestamp,
dead_reckoning_dvl_list[dr_idx - 1].epoch_timestamp,
dead_reckoning_dvl_list[dr_idx].epoch_timestamp,
dead_reckoning_dvl_list[dr_idx - 1].altitude,
dead_reckoning_dvl_list[dr_idx].altitude,
)
ekf_list.append(b)
return ekf_list
def interpolate_sensor_list(
sensor_list,
sensor_name,
sensor_offsets,
origin_offsets,
latlon_reference,
_centre_list,
):
j = 0
[origin_x_offset, origin_y_offset, origin_z_offset] = origin_offsets
[latitude_reference, longitude_reference] = latlon_reference
# Check if camera activates before dvl and orientation sensors.
start_time = _centre_list[0].epoch_timestamp
end_time = _centre_list[-1].epoch_timestamp
if (sensor_list[0].epoch_timestamp > end_time
or sensor_list[-1].epoch_timestamp < start_time):
Console.warn(
"{} timestamps does not overlap with dead reckoning data, "
"check timestamp_history.pdf via -v option.".format(sensor_name))
else:
sensor_overlap_flag = 0
for i in range(len(sensor_list)):
if sensor_list[i].epoch_timestamp < start_time:
sensor_overlap_flag = 1
pass
else:
if i > 0:
Console.warn(
"Deleted",
i,
"entries from sensor",
sensor_name,
". Reason: data before start of mission",
)
del sensor_list[:i]
break
for i in range(len(sensor_list)):
if j >= len(_centre_list) - 1:
ii = len(sensor_list) - i
if ii > 0:
Console.warn(
"Deleted",
ii,
"entries from sensor",
sensor_name,
". Reason: data after end of mission",
)
del sensor_list[i:]
sensor_overlap_flag = 1
break
while _centre_list[j].epoch_timestamp < sensor_list[
i].epoch_timestamp:
if (j + 1 > len(_centre_list) - 1
or _centre_list[j + 1].epoch_timestamp >
sensor_list[-1].epoch_timestamp):
break
j += 1
# if j>=1: ?
sensor_list[i].roll = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].roll,
_centre_list[j].roll,
)
sensor_list[i].pitch = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].pitch,
_centre_list[j].pitch,
)
if abs(_centre_list[j].yaw - _centre_list[j - 1].yaw) > 180:
if _centre_list[j].yaw > _centre_list[j - 1].yaw:
sensor_list[i].yaw = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].yaw,
_centre_list[j].yaw - 360,
)
else:
sensor_list[i].yaw = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].yaw - 360,
_centre_list[j].yaw,
)
if sensor_list[i].yaw < 0:
sensor_list[i].yaw += 360
elif sensor_list[i].yaw > 360:
sensor_list[i].yaw -= 360
else:
sensor_list[i].yaw = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].yaw,
_centre_list[j].yaw,
)
sensor_list[i].x_velocity = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].x_velocity,
_centre_list[j].x_velocity,
)
sensor_list[i].y_velocity = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].y_velocity,
_centre_list[j].y_velocity,
)
sensor_list[i].z_velocity = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].z_velocity,
_centre_list[j].z_velocity,
)
sensor_list[i].altitude = interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].altitude,
_centre_list[j].altitude,
)
[x_offset, y_offset, z_offset] = body_to_inertial(
sensor_list[i].roll,
sensor_list[i].pitch,
sensor_list[i].yaw,
origin_x_offset - sensor_offsets[0],
origin_y_offset - sensor_offsets[1],
origin_z_offset - sensor_offsets[2],
)
sensor_list[i].northings = (interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].northings,
_centre_list[j].northings,
) - x_offset)
sensor_list[i].eastings = (interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].eastings,
_centre_list[j].eastings,
) - y_offset)
sensor_list[i].altitude = (interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].altitude,
_centre_list[j].altitude,
) - z_offset)
sensor_list[i].depth = (interpolate(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].depth,
_centre_list[j].depth,
) - z_offset)
[sensor_list[i].latitude,
sensor_list[i].longitude] = metres_to_latlon(
latitude_reference,
longitude_reference,
sensor_list[i].eastings,
sensor_list[i].northings,
)
sensor_list[i].northings_std = interpolate_property(
_centre_list, i, sensor_list, j, "northings_std")
sensor_list[i].eastings_std = interpolate_property(
_centre_list, i, sensor_list, j, "eastings_std")
sensor_list[i].depth_std = interpolate_property(
_centre_list, i, sensor_list, j, "depth_std")
sensor_list[i].roll_std = interpolate_property(
_centre_list, i, sensor_list, j, "roll_std")
sensor_list[i].pitch_std = interpolate_property(
_centre_list, i, sensor_list, j, "pitch_std")
sensor_list[i].yaw_std = interpolate_property(
_centre_list, i, sensor_list, j, "yaw_std")
sensor_list[i].x_velocity_std = interpolate_property(
_centre_list, i, sensor_list, j, "x_velocity_std")
sensor_list[i].y_velocity_std = interpolate_property(
_centre_list, i, sensor_list, j, "y_velocity_std")
sensor_list[i].z_velocity_std = interpolate_property(
_centre_list, i, sensor_list, j, "z_velocity_std")
sensor_list[i].vroll_std = interpolate_property(
_centre_list, i, sensor_list, j, "vroll_std")
sensor_list[i].vpitch_std = interpolate_property(
_centre_list, i, sensor_list, j, "vpitch_std")
sensor_list[i].vyaw_std = interpolate_property(
_centre_list, i, sensor_list, j, "vyaw_std")
if _centre_list[j].covariance is not None:
sensor_list[i].covariance = interpolate_covariance(
sensor_list[i].epoch_timestamp,
_centre_list[j - 1].epoch_timestamp,
_centre_list[j].epoch_timestamp,
_centre_list[j - 1].covariance,
_centre_list[j].covariance,
)
if sensor_overlap_flag == 1:
Console.warn(
"Sensor data from {} spans further than dead reckoning data."
" Data outside DR is ignored.".format(sensor_name))
Console.info("Complete interpolation and coordinate transfomations "
"for {}".format(sensor_name))
def parse_gaps(mission, vehicle, category, ftype, outpath):
Console.info(" Parsing GAPS data...")
# parser meta data
class_string = "measurement"
sensor_string = "gaps"
frame_string = "inertial"
timezone = mission.usbl.timezone
timeoffset = mission.usbl.timeoffset
filepath = mission.usbl.filepath
usbl_id = mission.usbl.label
latitude_reference = mission.origin.latitude
longitude_reference = mission.origin.longitude
# define headers used in phins
header_absolute = "$PTSAG" # '<< $PTSAG' #georeferenced strings
header_heading = "$HEHDT" # '<< $HEHDT'
# gaps std models
distance_std_factor = mission.usbl.std_factor
distance_std_offset = mission.usbl.std_offset
broken_packet_flag = False
# read in timezone
timezone_offset = read_timezone(timezone)
# convert to seconds from utc
# timeoffset = -timezone_offset*60*60 + timeoffset
# determine file paths
path = (outpath / ".." / filepath).absolute()
filepath = get_raw_folder(path)
all_list = os.listdir(str(filepath))
gaps_list = [line for line in all_list if ".dat" in line]
Console.info(" " + str(len(gaps_list)) + " GAPS file(s) found")
# extract data from files
data_list = []
if ftype == "acfr":
data_list = ""
for i in range(len(gaps_list)):
path_gaps = filepath / gaps_list[i]
with path_gaps.open("r", errors="ignore") as gaps:
# initialise flag
flag_got_time = 0
for line in gaps.readlines():
line_split = line.strip().split("*")
line_split_no_checksum = line_split[0].strip().split(",")
broken_packet_flag = False
# print(line_split_no_checksum)
# keep on upating ship position to find the prior interpolation
# value of ship coordinates
# line_split_no_checksum[0] == header_absolute:
if header_absolute in line_split_no_checksum[0]:
# start with a ship coordinate
if line_split_no_checksum[6] == str(
usbl_id) and flag_got_time == 2:
if (line_split_no_checksum[11] == "F"
and line_split_no_checksum[13] == "1"):
# read in date
yyyy = int(line_split_no_checksum[5])
mm = int(line_split_no_checksum[4])
dd = int(line_split_no_checksum[3])
# read in time
time_string = str(line_split_no_checksum[2])
try:
hour = int(time_string[0:2])
mins = int(time_string[2:4])
secs = int(time_string[4:6])
msec = int(time_string[7:10])
except ValueError:
broken_packet_flag = True
pass
if secs >= 60:
mins += 1
secs = 0
broken_packet_flag = True
if mins >= 60:
hour += 1
mins = 0
broken_packet_flag = True
if hour >= 24:
dd += 1
hour = 0
broken_packet_flag = True
epoch_time = date_time_to_epoch(
yyyy, mm, dd, hour, mins, secs,
timezone_offset)
# dt_obj = datetime(yyyy,mm,dd,hour,mins,secs)
# time_tuple = dt_obj.timetuple()
# epoch_time = time.mktime(time_tuple)
epoch_timestamp = epoch_time + msec / 1000 + timeoffset
# get position
latitude_negative_flag = False
longitude_negative_flag = False
latitude_string = line_split_no_checksum[7]
latitude_degrees = int(latitude_string[0:2])
latitude_minutes = float(latitude_string[2:10])
if line_split_no_checksum[8] == "S":
latitude_negative_flag = True
longitude_string = line_split_no_checksum[9]
longitude_degrees = int(longitude_string[0:3])
longitude_minutes = float(longitude_string[3:11])
if line_split_no_checksum[10] == "W":
longitude_negative_flag = True
depth = float(line_split_no_checksum[12])
latitude = latitude_degrees + latitude_minutes / 60.0
longitude = longitude_degrees + longitude_minutes / 60.0
if latitude_negative_flag:
latitude = -latitude
if longitude_negative_flag:
longitude = -longitude
# flag raised to proceed
flag_got_time = 3
else:
flag_got_time = 0
if line_split_no_checksum[6] == "0":
if flag_got_time < 3:
# read in date
yyyy = int(line_split_no_checksum[5])
mm = int(line_split_no_checksum[4])
dd = int(line_split_no_checksum[3])
# print(yyyy,mm,dd)
# read in time
time_string = str(line_split_no_checksum[2])
# print(time_string)
hour = int(time_string[0:2])
mins = int(time_string[2:4])
secs = int(time_string[4:6])
try:
msec = int(time_string[7:10])
except ValueError:
broken_packet_flag = True
pass
if secs >= 60:
mins += 1
secs = 0
broken_packet_flag = True
if mins >= 60:
hour += 1
mins = 0
broken_packet_flag = True
if hour >= 24:
dd += 1
hour = 0
broken_packet_flag = True
epoch_time = date_time_to_epoch(
yyyy, mm, dd, hour, mins, secs,
timezone_offset)
# dt_obj = datetime(yyyy,mm,dd,hour,mins,secs)
# time_tuple = dt_obj.timetuple()
# epoch_time = time.mktime(time_tuple)
epoch_timestamp_ship_prior = (epoch_time +
msec / 1000 +
timeoffset)
# get position
latitude_string = line_split_no_checksum[7]
latitude_degrees_ship_prior = int(
latitude_string[0:2])
latitude_minutes_ship_prior = float(
latitude_string[2:10])
latitude_prior = (
latitude_degrees_ship_prior +
latitude_minutes_ship_prior / 60.0)
if line_split_no_checksum[8] == "S":
latitude_prior = -latitude_prior
longitude_string = line_split_no_checksum[9]
longitude_degrees_ship_prior = int(
longitude_string[0:3])
longitude_minutes_ship_prior = float(
longitude_string[3:11])
longitude_prior = (
longitude_degrees_ship_prior +
longitude_minutes_ship_prior / 60.0)
if line_split_no_checksum[10] == "W":
longitude_prior = -longitude_prior
# flag raised to proceed
if flag_got_time < 2:
flag_got_time = flag_got_time + 1
elif flag_got_time >= 3:
if line_split_no_checksum[6] == "0":
# read in date
yyyy = int(line_split_no_checksum[5])
mm = int(line_split_no_checksum[4])
dd = int(line_split_no_checksum[3])
# read in time
time_string = str(line_split_no_checksum[2])
hour = int(time_string[0:2])
mins = int(time_string[2:4])
secs = int(time_string[4:6])
msec = int(time_string[7:10])
# calculate epoch time
epoch_time = date_time_to_epoch(
yyyy,
mm,
dd,
hour,
mins,
secs,
timezone_offset,
)
# dt_obj = datetime(yyyy,mm,dd,hour,mins,secs)
# time_tuple = dt_obj.timetuple()
# epoch_time = time.mktime(time_tuple)
epoch_timestamp_ship_posterior = (epoch_time +
msec / 1000 +
timeoffset)
# get position
latitude_string = line_split_no_checksum[7]
latitude_degrees_ship_posterior = int(
latitude_string[0:2])
latitude_minutes_ship_posterior = float(
latitude_string[2:10])
latitude_posterior = (
latitude_degrees_ship_posterior +
latitude_minutes_ship_posterior / 60.0)
if line_split_no_checksum[8] == "S":
latitude_posterior = -latitude_posterior
longitude_string = line_split_no_checksum[9]
longitude_degrees_ship_posterior = int(
longitude_string[0:3])
longitude_minutes_ship_posterior = float(
longitude_string[3:11])
longitude_posterior = (
longitude_degrees_ship_posterior +
longitude_minutes_ship_posterior / 60.0)
if line_split_no_checksum[10] == "W":
longitude_posterior = -longitude_posterior
# flag raised to proceed
flag_got_time = flag_got_time + 1
# line_split_no_checksum[0] == header_heading:
if header_heading in line_split_no_checksum[0]:
if flag_got_time < 3:
heading_ship_prior = float(line_split_no_checksum[1])
if flag_got_time < 2:
flag_got_time = flag_got_time + 1
else:
heading_ship_posterior = float(
line_split_no_checksum[1])
flag_got_time = flag_got_time + 1
if flag_got_time >= 5:
# interpolate for the ships location and heading
inter_time = (epoch_timestamp - epoch_timestamp_ship_prior
) / (epoch_timestamp_ship_posterior -
epoch_timestamp_ship_prior)
longitude_ship = (inter_time *
(longitude_posterior - longitude_prior) +
longitude_prior)
latitude_ship = (inter_time *
(latitude_posterior - latitude_prior) +
latitude_prior)
heading_ship = (
inter_time *
(heading_ship_posterior - heading_ship_prior) +
heading_ship_prior)
while heading_ship > 360:
heading_ship = heading_ship - 360
while heading_ship < 0:
heading_ship = heading_ship + 360
lateral_distance, bearing = latlon_to_metres(
latitude, longitude, latitude_ship, longitude_ship)
# determine range to input to uncertainty model
distance = math.sqrt(lateral_distance * lateral_distance +
depth * depth)
distance_std = distance_std_factor * distance + distance_std_offset
# determine uncertainty in terms of latitude and longitude
latitude_offset, longitude_offset = metres_to_latlon(
abs(latitude),
abs(longitude),
distance_std,
distance_std,
)
latitude_std = abs(abs(latitude) - latitude_offset)
longitude_std = abs(abs(longitude) - longitude_offset)
# calculate in metres from reference
lateral_distance_ship, bearing_ship = latlon_to_metres(
latitude_ship,
longitude_ship,
latitude_reference,
longitude_reference,
)
eastings_ship = (math.sin(bearing_ship * math.pi / 180.0) *
lateral_distance_ship)
northings_ship = (
math.cos(bearing_ship * math.pi / 180.0) *
lateral_distance_ship)
lateral_distance_target, bearing_target = latlon_to_metres(
latitude,
longitude,
latitude_reference,
longitude_reference,
)
eastings_target = (
math.sin(bearing_target * math.pi / 180.0) *
lateral_distance_target)
northings_target = (
math.cos(bearing_target * math.pi / 180.0) *
lateral_distance_target)
if not broken_packet_flag:
if ftype == "oplab" and category == Category.USBL:
data = {
"epoch_timestamp":
float(epoch_timestamp),
"class":
class_string,
"sensor":
sensor_string,
"frame":
frame_string,
"category":
category,
"data_ship": [
{
"latitude": float(latitude_ship),
"longitude": float(longitude_ship),
},
{
"northings": float(northings_ship),
"eastings": float(eastings_ship),
},
{
"heading": float(heading_ship)
},
],
"data_target": [
{
"latitude": float(latitude),
"latitude_std": float(latitude_std),
},
{
"longitude": float(longitude),
"longitude_std": float(longitude_std),
},
{
"northings": float(northings_target),
"northings_std": float(distance_std),
},
{
"eastings": float(eastings_target),
"eastings_std": float(distance_std),
},
{
"depth": float(depth),
"depth_std": float(distance_std),
},
{
"distance_to_ship": float(distance)
},
],
}
data_list.append(data)
elif ftype == "oplab" and category == Category.DEPTH:
data = {
"epoch_timestamp":
float(epoch_timestamp),
"epoch_timestamp_depth":
float(epoch_timestamp),
"class":
class_string,
"sensor":
sensor_string,
"frame":
"inertial",
"category":
Category.DEPTH,
"data": [{
"depth": float(depth),
"depth_std": float(distance_std),
}],
}
data_list.append(data)
if ftype == "acfr":
data = (
"SSBL_FIX: " + str(float(epoch_timestamp)) +
" ship_x: " + str(float(northings_ship)) +
" ship_y: " + str(float(eastings_ship)) +
" target_x: " + str(float(northings_target)) +
" target_y: " + str(float(eastings_target)) +
" target_z: " + str(float(depth)) +
" target_hr: " + str(float(lateral_distance)) +
" target_sr: " + str(float(distance)) +
" target_bearing: " + str(float(bearing)) +
"\n")
data_list += data
else:
Console.warn("Badly formatted packet (GAPS TIME)")
Console.warn(line)
# print(hour,mins,secs)
# reset flag
flag_got_time = 0
Console.info(" ...done parsing GAPS data.")
return data_list
def parse_usbl_dump(mission, vehicle, category, ftype, outpath):
# parser meta data
class_string = "measurement"
sensor_string = "jamstec_usbl"
frame_string = "inertial"
# gaps std models
distance_std_factor = mission.usbl.std_factor
distance_std_offset = mission.usbl.std_offset
timezone = mission.usbl.timezone
timeoffset = mission.usbl.timeoffset
filepath = mission.usbl.filepath
filename = mission.usbl.filename
label = mission.usbl.label
filepath = get_raw_folder(outpath / ".." / filepath)
latitude_reference = mission.origin.latitude
longitude_reference = mission.origin.longitude
# read in timezone
timezone_offset = read_timezone(timezone)
# convert to seconds from utc
# timeoffset = -timezone_offset*60*60 + timeoffset
# extract data from files
Console.info("... parsing usbl dump")
data_list = []
if ftype == "acfr":
data_list = ""
usbl_file = filepath / filename
with usbl_file.open("r", encoding="utf-8", errors="ignore") as filein:
for line in filein.readlines():
line_split = line.strip().split(",")
if line_split[2] == label:
date = line_split[0].split("-")
# read in date
yyyy = int(date[0])
mm = int(date[1])
dd = int(date[2])
timestamp = line_split[1].split(":")
# read in time
hour = int(timestamp[0])
mins = int(timestamp[1])
secs = int(timestamp[2])
msec = 0
epoch_time = date_time_to_epoch(
yyyy, mm, dd, hour, mins, secs, timezone_offset
)
# dt_obj = datetime(yyyy,mm,dd,hour,mins,secs)
# time_tuple = dt_obj.timetuple()
# epoch_time = time.mktime(time_tuple)
epoch_timestamp = epoch_time + msec / 1000 + timeoffset
if line_split[6] != "":
# get position
latitude_full = line_split[6].split(" ")
latitude_list = latitude_full[0].split("-")
latitude_degrees = int(latitude_list[0])
latitude_minutes = float(latitude_list[1])
if latitude_full[1] != "N":
latitude_degrees = latitude_degrees * -1 # southern hemisphere
latitude = latitude_degrees + latitude_minutes / 60
longitude_full = line_split[7].split(" ")
longitude_list = longitude_full[0].split("-")
longitude_degrees = int(longitude_list[0])
longitude_minutes = float(longitude_list[1])
if longitude_full[1] != "E":
longitude_degrees = (
longitude_degrees * -1
) # southern hemisphere
longitude = longitude_degrees + longitude_minutes / 60
depth_full = line_split[8].split("=")
depth = float(depth_full[1])
distance_full = line_split[11].split("=")
distance = float(distance_full[1])
distance_std = distance_std_factor * distance + distance_std_offset
lateral_distance_full = line_split[9].split("=")
lateral_distance = float(lateral_distance_full[1])
bearing_full = line_split[10].split("=")
bearing = float(bearing_full[1])
# determine uncertainty in terms of latitude and longitude
latitude_offset, longitude_offset = metres_to_latlon(
latitude, longitude, distance_std, distance_std
)
latitude_std = latitude - latitude_offset
longitude_std = longitude - longitude_offset
# calculate in metres from reference
eastings_ship = 0
northings_ship = 0
latitude_ship = 0
longitude_ship = 0
heading_ship = 0
lateral_distance_target, bearing_target = latlon_to_metres(
latitude, longitude, latitude_reference, longitude_reference,
)
eastings_target = (
math.sin(bearing_target * math.pi / 180.0)
* lateral_distance_target
)
northings_target = (
math.cos(bearing_target * math.pi / 180.0)
* lateral_distance_target
)
if ftype == "oplab":
data = {
"epoch_timestamp": float(epoch_timestamp),
"class": class_string,
"sensor": sensor_string,
"frame": frame_string,
"category": category,
"data_ship": [
{
"latitude": float(latitude_ship),
"longitude": float(longitude_ship),
},
{
"northings": float(northings_ship),
"eastings": float(eastings_ship),
},
{"heading": float(heading_ship)},
],
"data_target": [
{
"latitude": float(latitude),
"latitude_std": float(latitude_std),
},
{
"longitude": float(longitude),
"longitude_std": float(longitude_std),
},
{
"northings": float(northings_target),
"northings_std": float(distance_std),
},
{
"eastings": float(eastings_target),
"eastings_std": float(distance_std),
},
{
"depth": float(depth),
"depth_std": float(distance_std),
},
{"distance_to_ship": float(distance)},
],
}
data_list.append(data)
if ftype == "acfr":
data = (
"SSBL_FIX: "
+ str(float(epoch_timestamp))
+ " ship_x: "
+ str(float(northings_ship))
+ " ship_y: "
+ str(float(eastings_ship))
+ " target_x: "
+ str(float(northings_target))
+ " target_y: "
+ str(float(eastings_target))
+ " target_z: "
+ str(float(depth))
+ " target_hr: "
+ str(float(lateral_distance))
+ " target_sr: "
+ str(float(distance))
+ " target_bearing: "
+ str(float(bearing))
+ "\n"
)
data_list += data
return data_list