This document contains code for parsing GPS data, road network data, and ground truth data to perform hidden Markov modeling (HMM) for vehicle trajectory estimation. It defines functions for calculating spherical distances and probabilities. It parses the input data files, initializes the HMM variables and road list states. It then performs analysis by running the Viterbi algorithm to estimate the most likely trajectory through the road network given the observed GPS points.

1. import re
import time
import math
# --- Parameter ---
FREQUENCY = 60 # (Hz)
DECIMAL = 3 # 2nd decimal place (About 70km)
ALPHA = 4.07 # Standard deviation of GPS measurement
'''
BETA value
'''
BETA = 1 # Exponential distribution parameter
# --- Structure ---
gps_data = [] # [(Latitude, Longitude) ...]
road_network = {} # {Edge ID: (From Node ID, To Node ID, Two Way, Speed
(m/s))}
road_list = [] # [(Edge ID, Segment ID, Latitude, Longitude, Status) ...] Status
0 - skip, 1 - do HMM
road_map = {} # {(Latitude(2nd decimal place), Longitude(2nd decimal
place)): [Edge ID, Segment ID, Latitude, Longitude ...])
ground_true = [] # (Edge, Traverse)
# --- Function ---
def deg2rad(d): # degree to radian
return d * math.pi / 180.0
EARTH_RADIUS_METER = 6378137.0
def spherical_distance(f, t): # caculate the spherical distance of two points
flat = deg2rad(f[0])
flon = deg2rad(f[1])
tlat = deg2rad(t[0])
tlon = deg2rad(t[1])
con = math.sin(flat) * math.sin(tlat)
con += math.cos(flat) * math.cos(tlat) * math.cos(flon - tlon)
return math.acos(con) * EARTH_RADIUS_METER # (m)

2. def route_distance(f, t):
'''
route algo
'''
return 0
def measure_probability(f, t):
return 1 / (math.sqrt(2 * math.pi) * ALPHA) * math.exp(-0.5 *
math.pow(spherical_distance(f, t) / ALPHA, 2))
def transition_probability(z1, z2, x1, x2):
return 1 / BETA * math.exp(-1 * (spherical_distance(z1, z2) - route_distance(x1,
x2)) / BETA)
'''
def viterbi(obs, states, start_p, trans_p, emit_p):
V = [{}]
path = {}
# Initialize base cases (t == 0)
for y in states:
V[0][y] = start_p[y] * emit_p[y][obs[0]]
path[y] = [y]
# Run Viterbi for t > 0
for t in range(1, len(obs)):
V.append({})
newpath = {}
for y in states:
(prob, state) = max((V[t-1][y0] * trans_p[y0][y] * emit_p[y][obs[t]], y0)
for y0 in states)
V[t][y] = prob
newpath[y] = path[state] + [y]
# Don't need to remember the old paths
path = newpath
n = 0 # if only one element is observed max is sought in the

3. initialization values
if len(obs)!=1:
n = t
print_dptable(V)
(prob, state) = max((V[n][y], y) for y in states)
return (prob, path[state])
'''
#====================================================================
=========================================#
# --- Parsing ---
print('Parsing...')
tStart = time.time() # (ms)
# [GPS data]
gps_data_file = open('gps_data.txt', 'r', encoding = 'UTF-8')
info = gps_data_file.readline() # [(Date (UTC), Time (UTC), Latitude, Longitude)
counter = 0
for line in gps_data_file:
counter += 1
if(counter == 60 / FREQUENCY):
temp = line.split()
gps_data.append((float(temp[2]), float(temp[3])))
counter = 0
print("GPS data (number): ", len(gps_data))
gps_data_file.close()
# [Road network data]
road_network_file = open('road_network.txt', 'r', encoding = 'UTF-8')
info = road_network_file.readline()
for line in road_network_file:
temp = line.split()
temp_dic_network = {int(temp[0]): ((int(temp[1]), int(temp[2]), int(temp[3]),
float(temp[4])))}
road_network.update(temp_dic_network) # (Edge ID, From Node ID, To

4. Node ID, Two Way, Speed (m/s))
for i in range(int(temp[5])):
temp_num_lati = float(re.sub('[^-.0-9]', '', temp[(i * 2 + 1) + 6])) #
Latitude
temp_num_longi = float(re.sub('[^-.0-9]', '', temp[(i * 2) + 6])) #
Longitude
# [Road Map]
if((round(temp_num_lati, DECIMAL), round(temp_num_longi, DECIMAL))
not in road_map):
temp_dic_map = {(round(temp_num_lati, DECIMAL),
round(temp_num_longi, DECIMAL)): [(int(temp[0]), i, temp_num_lati,
temp_num_longi)]}
road_map.update(temp_dic_map)
else:
road_map.get((round(temp_num_lati, DECIMAL),
round(temp_num_longi, DECIMAL))).append([int(temp[0]), i, temp_num_lati,
temp_num_longi])
# [Road List]
road_list.append([int(temp[0]), i, temp_num_lati, temp_num_longi], 0)
print("Road segment (number): ", len(road_list))
road_network_file.close()
# [Ground true data]
ground_true_file = open('ground_truth_route.txt', 'r', encoding = 'UTF-8')
info = ground_true_file.readline() # (Edge ID, Traversed From to To)
for line in ground_true_file:
temp = line.split()
ground_true.append((int(temp[0]), temp[1]))
print("Ground true (number): ", len(ground_true))
ground_true_file.close()
print('Elapsed time = ', round(time.time() - tStart, 2))
# --- Analysising ---
print('Analysising...')
tStart = time.time() # (ms)

5. V = [{}]
path = {}
# Need valid HMM states
# Initialize base cases (t == 0)
for y in road_list:
# Search road map
for i in range(3):
for j in range(3):
if ((round(y[0], DECIMAL) + i, round(y[1], DECIMAL) + j) in road_map):
temp = road_map.get(round(y[0], DECIMAL) + i, round(y[1],
DECIMAL) + j)
'''
Set status and count propability
'''
prev = (0, 0)
for t in range(len(gps_data)):
if(t == 0):
prev = gps_data[t]
elif(spherical_distance(prev, gps_data[t]) > 2 * ALPHA):
'''
do something
'''
prev = gps_data[t]
'''
1. Removing points that are within 2 * Standard Deviation of the previous point
2. HMM Breaks
2.1 More than (200m)
2.2 Route more than great circle (2000m)
2.3 Vehicle speed 3x in route or more than 50 m/s
'''
print('Elapsed time = ', round(time.time() - tStart, 2))
# --- Evaluation ---

6. '''
print('Evaluation...')
tStart = time.time() # (ms)
print('Elapsed time = ', round(time.time() - tStart, 2))
'''