normalize data doing division by 32768

euler_ekf.py

@@ -1,25 +1,17 @@
-'''
- Filename: 12_EulerEKF.py
- Created on: April,10, 2021
- Author: dhpark
-'''
 import numpy as np
 from numpy.linalg import inv
 import matplotlib.pyplot as plt
 from math import cos, sin, tan, asin, pi
-from scipy import io
 import pandas as pd
 
-def GetData(i, data):
+def get_data(i, data):
     x = data[0][i]  # (41500, 1)
     y = data[1][i]  # (41500, 1)
     z = data[2][i]  # (41500, 1)
     return x, y, z
 
-def EulerAccel(ax, ay, az):
-    g = 90000.8 # 9.8
-    print(ax)
-    print(g)
+def Euler_accel(ax, ay, az):
+    g = 9.8 # 9.8
     theta = asin(ax / g)
     phi = asin(-ay / (g * cos(theta)))
     return phi, theta
@@ -69,7 +61,7 @@ def fx(xhat, rates, dt):
     xp = xhat.reshape(-1,1) + xdot*dt # xhat : (3,) --> (3,1)
     return xp
 
-def EulerEKF(z, rates, dt):
+def Euler_EKF(z, rates, dt):
     global firstRun
     global Q, H, R
     global x, P
@@ -95,25 +87,37 @@ def EulerEKF(z, rates, dt):
     psi   = x[2]
     return phi, theta, psi
 
+def normalize_data_vector(data, division):
+    return [i/division for i in list(data)]
+
 H, Q, R = None, None, None
 x, P = None, None
 firstRun = True
+division_param = 32768
 
 df = pd.read_csv('raw_data_6d.xls', sep='\t')
 
-gyro_data = [list(df['%GyroX'].values), list(df['GyroY'].values), list(df['GyroZ'].values)]
-acce_data = [list(df['AcceX'].values), list(df['AcceY'].values), list(df['AcceZ'].values)]
+gyroX = normalize_data_vector(df['%GyroX'].values, division_param)
+gyroY = normalize_data_vector(df['%GyroX'].values, division_param)
+gyroZ = normalize_data_vector(df['%GyroX'].values, division_param)
+
+acceX = normalize_data_vector(df['AcceX'].values, division_param)
+acceY = normalize_data_vector(df['AcceY'].values, division_param)
+acceZ = normalize_data_vector(df['AcceZ'].values, division_param)
+
+gyro_data = [gyroX, gyroY, gyroZ]
+acce_data = [acceX, acceY, acceZ]
 
 Nsamples = len(df)
 EulerSaved = np.zeros([Nsamples,3])
 dt = 0.01
 
 for k in range(Nsamples):
-    p, q, r = GetData(k, gyro_data)
-    ax, ay, az = GetData(k, acce_data)
-    phi_a, theta_a = EulerAccel(ax, ay, az)
+    p, q, r = get_data(k, gyro_data)
+    ax, ay, az = get_data(k, acce_data)
+    phi_a, theta_a = Euler_accel(ax, ay, az)
 
-    phi, theta, psi = EulerEKF(np.array([phi_a, theta_a]).T, [p,q,r], dt)
+    phi, theta, psi = Euler_EKF(np.array([phi_a, theta_a]).T, [p,q,r], dt)
     if type(phi) == type(np.array([])):
         EulerSaved[k] = [phi[0], theta[0], psi[0]]
     else: