PhoneAxis_VehicleAxis_Projection

import pandas as pd
import numpy as np
import math
from pandas.errors import EmptyDataError

from geopy.distance import great_circle, geodesic
import matplotlib.pyplot as plt
plt.rcParams["figure.figsize"] = (20, 10)
%matplotlib inline  
from datetime import timedelta
import statistics
import os
import re
from zipfile import ZipFile
import plotly.express as px

def make_loc_plots(path):
    loc = pd.read_csv(f"{path}/Location.csv")
    print(f"Recorded for { ((loc['time'].iloc[-1] - loc['time'].iloc[0]) / 1e+9) /60 } mins")
    print(f"Avg speed {np.mean(loc['speed'])*3.60} km/h")
    print('Latitude Vs Longitude')
    plt.scatter(loc['latitude'],loc['longitude'],color = ['r']*len(loc))
    plt.show()
    print('Headings vs time')
    plt.plot(loc['bearing'],loc['time'])
    plt.show()

    fig = plt.figure()
    ax = plt.axes(projection='3d')
    ax.scatter3D(loc['time'], loc['latitude'], loc['longitude'], c=loc['longitude'], cmap='Greens')
    return None

def make_openstreetmap(path):
    df = pd.read_csv(f"{path}/Location.csv")
    df.dropna(
        axis=0,
        how='any',
        # thresh=None,
        subset=None,
        inplace=True
    )

    color_scale = [(0, 'orange'), (1,'red')]

    fig = px.scatter_mapbox(df, 
                            lat="latitude", 
                            lon="longitude", 
                            # hover_name="Address", 
                            # hover_data=["Address", "Listed"],
                            # color="Listed",
                            color_continuous_scale=color_scale,
                            # size="Listed",
                            zoom=8, 
                            height=800,
                            width=800)

    fig.update_layout(mapbox_style="open-street-map")
    fig.update_layout(margin={"r":0,"t":0,"l":0,"b":0})
    fig.show()
 
 # from: https://www.ncbi.nlm.nih.gov/pmc/articles/PMC8621449/ (20)
def transformed_axes(acc ,q0,q1,q2,q3):
    trans_matrix = np.array([
        [q0*q0 + q1*q1 - q2*q2 - q3*q3, 2*(q1*q2 - q3*q0),             2*(q1*q3 + q2*q0)],
        [2*(q1*q2 + q3*q0),             q0*q0 - q1*q1 + q2*q2 - q3*q3, 2*(q2*q3 - q1*q0)],
        [2*(q1*q3 - q2*q0),             2*(q2*q3 + q1*q0),             q0*q0 - q1*q1 - q2*q2 + q3*q3]
        ])
    return trans_matrix.dot(acc) # x,y,z => North, East, Down

def rotate_bearing(x,y,thetha):
    # rotate NE axis by bearing angle
    X = x*np.cos(thetha) + y*np.sin(thetha)
    Y = -x*np.sin(thetha) + y* np.cos(thetha)  
    return X,Y 
    
def low_pass_filter(data, alpha):

    filtered_data = [data.iloc[0]]
    for i in range(1, len(data)):

        filtered_data.append(alpha*data.iloc[i] + (1-alpha)*filtered_data[i-1])
    print(alpha, np.mean(filtered_data))
    return filtered_data

# Test on Long curve left

# Long curve left
path1 = '/Users/atufasheen/Downloads/projects/meiro_workspace/metering_workflow/datasets/drive-download-20230306T085040Z-001/Long curve left and right_4W_OPPO A74_2023-03-02_07-09-59'
path2= '/Users/atufasheen/Downloads/projects/meiro_workspace/metering_workflow/datasets/drive-download-20230306T085307Z-001/Home to office_4W-2023-03-02_05-19-08'

dfo = pd.read_csv(f"{path1}/Location.csv")
accel_north = []
accel_east = []
accel_down = []

df1 = pd.read_csv(f"{path1}/Accelerometer.csv")
df2 = pd.read_csv(f"{path1}/Orientation.csv")
for i in range(len(df1)): # assuming they r reported at the same time, which it is only in this recording
    acc1 = df1.iloc[i][['x','y','z']]
    acc = np.array([acc1]).T
    quat1 =  df2.iloc[i]
    q0 = quat1['qw']
    q1 = quat1['qx']
    q2 = quat1['qy']
    q3 = quat1['qz']
    n,e,d  = transformed_axes(acc,q0,q1,q2,q3)
    accel_north.append(n)
    accel_east.append(e)
    accel_down.append(d)
    
 dfa = pd.DataFrame({'time':df1['time'],'n':np.squeeze(accel_north),'e':np.squeeze(accel_east),'d':np.squeeze(accel_down)})
 
 thetha = 0
sens_i = 0
meas_i = 1
n_rotated = []
e_rotated = []
while sens_i < len(dfa):
    if (meas_i < len(dfo)):
        if (dfo.iloc[meas_i]['time'] > dfa.iloc[sens_i]['time'] > dfo.iloc[meas_i -1]['time']):
            thetha = dfo.iloc[meas_i-1]['bearing']
            meas_i+=1
    n,e = (rotate_bearing(dfa.iloc[sens_i]['n'],dfa.iloc[sens_i]['e'],thetha))
    n_rotated.append(n)
    e_rotated.append(e)
    sens_i+=1
dfa_rotated = pd.DataFrame({'time':df1['time'],'n':n_rotated,'e':e_rotated})

fig,ax = plt.subplots(1,3,figsize=(21,10))
ax[0].plot(df1['y'])
ax[0].set_title('raw y axis acc')
ax[1].plot(dfa['n'])
ax[1].set_title('fixed N axis acc')
ax[2].plot(dfa_rotated['n'])
ax[2].set_title('rotated N axis acc')
ax[0].set_ylim(-8,8)
ax[1].set_ylim(-8,8)
ax[2].set_ylim(-8,8)
plt.plot(low_pass_filter(dfa_rotated['n'],0.01))
plt.ylim(-4,6)