distance_calculation.py

# %% [markdown]
# ### pattern recognition

# %%
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

# %%
path1 = '../datasets/pocom3/10s_hold_between_two_20s_laps_of_30kmph_in_hand_0.35km-2023-02-28_16-35-30'
path2 = '../datasets/pocom3/10s_hold_between_two_20s_laps_of_30kmph_in_pocket_0.35km-2023-02-28_16-37-40'

# %%
# 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

# %%
dfa = pd.read_csv(f"{path1}/Accelerometer.csv")
dfq = pd.read_csv(f"{path1}/Orientation.csv")

# %%
accel_north = []
accel_east = []
accel_down = []

for i in range(len(dfa)): # assuming they r reported at the same time, which it is only in this recording
    acc1 = dfa.iloc[i][['x','y','z']]
    acc = np.array([acc1]).T
    quat1 =  dfq.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)

# %%
plt.plot(accel_north)
plt.show()
plt.plot(accel_east)
plt.show()
plt.plot(accel_down)
plt.show()

# %%
dfa = pd.read_csv(f"{path2}/Accelerometer.csv")
dfq = pd.read_csv(f"{path2}/Orientation.csv")
accel_north = []
accel_east = []
accel_down = []

for i in range(len(dfa)): # assuming they r reported at the same time, which it is only in this recording
    acc1 = dfa.iloc[i][['x','y','z']]
    acc = np.array([acc1]).T
    quat1 =  dfq.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)

plt.plot(accel_north)
plt.show()
plt.plot(accel_east)
plt.show()
plt.plot(accel_down)
plt.show()

# %% [markdown]
# * calibrated shouldn't have went up by this range.

# %% [markdown]
# ### Error rate calculation

# %%

def calc_dist_gps_acc(df_a,df_m=None):
    dist = 0.0 # meters
    ini_vel = 0.0

    for i in range(1,len(df_a)):
        interval  = (df_a.iloc[i]['time'] - df_a.iloc[i-1]['time']) / 1e+9
        fin_vel  = ini_vel + (df_a.iloc[i]['total']*interval) #u+at
        dist += (ini_vel*interval) + 0.5*df_a.iloc[i]['total']*interval*interval #ut+0.5*at^2
        ini_vel= fin_vel

    print("dist with acc in kmeters",dist/1000)
    dist_g = 0
    if df_m: 
        for i in range(1,len(df_m)):
            dz = great_circle(df_m.iloc[i-1]['coordinate'],df_m.iloc[i]['coordinate']).meters
            dist_g+=dz
        print("dist with gps in kmeters",dist_g/1000)
    return dist, dist_g

# %%
dfa = pd.read_csv('../datasets/pocom3/10s_hold_between_two_20s_laps_of_30kmph_in_hand_0.35km-2023-02-28_16-35-30/Accelerometer.csv')
dfa['total'] = (dfa['x']**2 + dfa['y']**2 + dfa['z']**2)**(1/2)
print(calc_dist_gps_acc(dfa[:101],None),'for first 1 sec')
print(calc_dist_gps_acc(dfa,None))


# %%
dfq = pd.read_csv('../datasets/pocom3/10s_hold_between_two_20s_laps_of_30kmph_in_hand_0.35km-2023-02-28_16-35-30/Orientation.csv')
dfa = pd.read_csv('../datasets/pocom3/10s_hold_between_two_20s_laps_of_30kmph_in_hand_0.35km-2023-02-28_16-35-30/Accelerometer.csv')

accel_north = []
accel_east = []
accel_down = []

for i in range(len(dfa)): # assuming they r reported at the same time, which it is for pocom3 recordings
    acc1 = dfa.iloc[i][['x','y','z']]
    acc = np.array([acc1]).T
    quat1 =  dfq.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['total'] = (np.squeeze(accel_north)**2 + np.squeeze(accel_east)**2) **(1/2)
print(calc_dist_gps_acc(dfa,None))
print(calc_dist_gps_acc(dfa[:101],None),'for first 1 sec')


# %%
dfq = pd.read_csv('../datasets/pocom3/10s_hold_between_two_20s_laps_of_30kmph_in_hand_0.35km-2023-02-28_16-35-30/Orientation.csv')
dfa = pd.read_csv('../datasets/pocom3/10s_hold_between_two_20s_laps_of_30kmph_in_hand_0.35km-2023-02-28_16-35-30/Accelerometer.csv')

accel_north = []
accel_east = []
accel_down = []

for i in range(len(dfa)): # assuming they r reported at the same time, which it is for pocom3 recordings
    acc1 = dfa.iloc[i][['x','y','z']]
    acc = np.array([acc1]).T
    quat1 =  dfq.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['total'] = (np.squeeze(accel_north)**2 + np.squeeze(accel_east)**2) **(1/2)
print(calc_dist_gps_acc(dfa,None))
print(calc_dist_gps_acc(dfa[:101],None),'for first 1 sec')


# %%
dfq = pd.read_csv('../datasets/pocom3/30kmph_in_hand_20s_0.15km-2023-02-28_16-15-27/Orientation.csv')
dfa = pd.read_csv('../datasets/pocom3/30kmph_in_hand_20s_0.15km-2023-02-28_16-15-27/Accelerometer.csv')

accel_north = []
accel_east = []
accel_down = []

for i in range(len(dfa)): # assuming they r reported at the same time, which it is for pocom3 recordings
    acc1 = dfa.iloc[i][['x','y','z']]
    acc = np.array([acc1]).T
    quat1 =  dfq.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['total'] = (np.squeeze(accel_north)**2 + np.squeeze(accel_east)**2) **(1/2)
print(calc_dist_gps_acc(dfa,None))
print(calc_dist_gps_acc(dfa[:101],None),'for first 1 sec')


# %%
df = pd.read_csv('../datasets/pocom3/30kmph_in_hand_20s_0.15km-2023-02-28_16-15-27/Accelerometer.csv')
df['total'] = (df['x']**2 + df['y']**2 + df['z']**2)**(1/2)
print(calc_dist_gps_acc(df[:101],None),'for first 1 sec')
print(calc_dist_gps_acc(df,None))

# %%
df = pd.read_csv('../datasets/pocom3/Right_turn_in_pocket_0.21km-2023-02-28_16-31-40/Accelerometer.csv')
df['total'] = (df['x']**2 + df['y']**2 + df['z']**2)**(1/2)
print(calc_dist_gps_acc(df[:101],None),'for first 1 sec')
print(calc_dist_gps_acc(df,None))

# %% [markdown]
# ### orintentation of acceleration with uneven/diff sample rate.  

# %%