# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: Python 3
#     language: python
#     name: python3
# ---

import numpy as np
import pandas as pd
import math


def load_day(day):
    header = ['timestamp', 'line_id', 'direction', 'jrny_patt_id', 'time_frame', 'journey_id', 'operator', 
              'congestion', 'lon', 'lat', 'delay', 'block_id', 'vehicle_id', 'stop_id', 'at_stop']
    types = {'timestamp': np.int64,
             'journey_id': np.int32,
             'congestion': np.int8,
             'lon': np.float64,
             'lat': np.float64,
             'delay': np.int8,
             'vehicle_id': np.int32,
             'at_stop': np.int8}
    file_name = 'data/siri.201301{0:02d}.csv'.format(day)
    df = pd.read_csv(file_name, header=None, names=header, dtype=types, parse_dates=['time_frame'], infer_datetime_format=True)
    null_replacements = {'line_id': 0, 'stop_id': 0}
    df = df.fillna(value=null_replacements)
    df['line_id'] = df['line_id'].astype(np.int32)
    df['stop_id'] = df['stop_id'].astype(np.int32)
    df['timestamp'] = pd.to_datetime(df['timestamp'], unit='us')
    return df


def haversine_np(lon1, lat1, lon2, lat2):
    """
    Calculate the great circle distance between two points
    on the earth (specified in decimal degrees)

    All args must be of equal length.    
    Taken from here: https://stackoverflow.com/questions/29545704/fast-haversine-approximation-python-pandas#29546836
    """
    lon1, lat1, lon2, lat2 = map(np.radians, [lon1, lat1, lon2, lat2])

    dlon = lon2 - lon1
    dlat = lat2 - lat1

    a = np.sin(dlat/2.0)**2 + np.cos(lat1) * np.cos(lat2) * np.sin(dlon/2.0)**2

    #c = 2 * np.arcsin(np.sqrt(a))
    c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1.0 - a))
    meters = 6378137.0 * c
    return meters


def calculate_durations(data_frame, vehicle_id):
    one_second = np.timedelta64(1000000000, 'ns')
    dv = data_frame[data_frame['vehicle_id']==vehicle_id]
    ts = dv.timestamp.values
    dtd = ts[1:] - ts[:-1]
    dt = np.zeros(len(dtd) + 1)
    dt[1:] = dtd / one_second
    return dt


def calculate_distances(data_frame, vehicle_id):
    dv = data_frame[data_frame['vehicle_id']==vehicle_id]
    lat = dv.lat.values
    lon = dv.lon.values
    dxm = haversine_np(lon[1:], lat[1:], lon[:-1], lat[:-1])
    dx = np.zeros(len(dxm) + 1)
    dx[1:] = dxm
    return dx


def delta_location(lat, lon, bearing, meters):
    delta = meters / 6378137.0
    theta = math.radians(bearing)
    lat_r = math.radians(lat)
    lon_r = math.radians(lon)
    lat_r2 = math.asin(math.sin(lat_r) * math.cos(delta) + math.cos(lat_r) * math.sin(delta) * math.cos(theta))
    lon_r2 = lon_r + math.atan2(math.sin(theta) * math.sin(delta) * math.cos(lat_r), 
                                math.cos(delta) - math.sin(lat_r) * math.sin(lat_r2))
    return math.degrees(lat_r2), math.degrees(lon_r2)


def x_meters_to_degrees(meters, lat, lon):
    lat2, lon2 = delta_location(lat, lon, 90, meters)
    return abs(lon - lon2)


def y_meters_to_degrees(meters, lat, lon):
    lat2, lon2 = delta_location(lat, lon, 0, meters)
    return abs(lat - lat2)


def calculate_Q(lat, lon, sigma_speed):
    Q = np.zeros((4,4), dtype=np.float)
    Q[2,2] = x_meters_to_degrees(sigma_speed, lat, lon) ** 2
    Q[3,3] = y_meters_to_degrees(sigma_speed, lat, lon) ** 2
    return Q


def calculate_R(lat, lon, sigma):
    R = np.zeros((2,2), dtype=np.float)
    R[0,0] = x_meters_to_degrees(lat, lon, sigma)
    R[1,1] = y_meters_to_degrees(lat, lon, sigma)
    return R


def calculate_P(lat, lon, sigma, sigma_speed):
    P = np.zeros((4,4), dtype=np.float)
    P[0,0] = x_meters_to_degrees(sigma, lat, lon) ** 2
    P[1,1] = y_meters_to_degrees(sigma, lat, lon) ** 2
    P[2,2] = x_meters_to_degrees(sigma_speed, lat, lon) ** 2
    P[3,3] = y_meters_to_degrees(sigma_speed, lat, lon) ** 2
    return P


def calculate_Kalman_gain(P, C, R):
    num = np.matmul(P, np.transpose(C))
    den = np.matmul(C, num) + R
    return np.matmul(num, np.linalg.pinv(den))


def predict_step(prev_x, prev_P):
    next_x = np.matmul(measurement, prev_x)
    next_P = np.matmul(np.matmul(measurement, prev_P), np.transpose(measurement)) +
             calculate_Q(prev_x[0,0], prev_x[0,1], sigma_s)
    return next_x, next_P


def update_step(predicted_x, predicted_P, C, y):
    lon = predicted_x[0,0]
    lat = predicted_x[0,1]
    R = calculate_R(lat, lon, sigma_x)
    K = calculate_Kalman_gain(predicted_P, C, R)
    updated_x = predicted_x + np.matmul(K, y - np.matmul(C, predicted_x))
    I = np.eye(4)
    updated_P = np.matmul(I - np.matmul(K, C), predicted_P)
    return updated_x, updated_P



lon = -6.236852
lat = 53.425327
delta_location(lat, lon, 90, 5)

calculate_Q(lat, lon, sigma_speed=1.0)

calculate_P(lat, lon, sigma=4, sigma_speed=1.0)



df = load_day(1)

# Measurement matrix
measurement = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])

state = np.array([[0], [0], [0], [0]])



sigma_s = 0.1
sigma_x = 4.0









StarcoderdataJupyterScriptsDedupFiltered