In [55]:
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
%matplotlib inline
In [56]:
data = pd.read_csv("logdata.csv") #loading datas
data
Out[56]:
In [57]:
# pandas -> ndarray
cols = data.columns
times = np.array(map(lambda x: float(x.replace(' ','')),data[cols[0]]))
accelX = np.array(data[cols[1]])
accelY = np.array(data[cols[2]])
accelZ = np.array(data[cols[3]])
magX = np.array(data[cols[5]])
magY = np.array(data[cols[6]])
magZ = np.array(data[cols[7]])
gyroX = np.array(data[cols[8]])
gyroY = np.array(data[cols[9]])
gyroZ = np.array(data[cols[10]])
press = np.array(data[cols[11]] * 100.0) # [hPa]->[Pa]
height = np.array(data[cols[12]] )
temperature = np.array(data[cols[13]])
flightmode = np.array(data[cols[14]],np.int)
In [58]:
def normalize(vec):
vec = np.array(vec)
norm = np.linalg.norm(vec)
if norm == 0.0:
return vec
else:
return (vec/norm)
In [59]:
# make the height before launching zero
height[ flightmode < 2] = 0.0
In [60]:
# The graph of height
plt.plot(times,height)
plt.xlabel("time[s]")
plt.ylabel("height[m]")
plt.title("The height of rocket")
Out[60]:
In [61]:
# The graph of atmospheric pressure
plt.plot(times,press)
plt.xlabel("time[s]")
plt.ylabel("atmospheric pressure[Pa]")
plt.title("The atmospheric pressure")
Out[61]:
In [62]:
# The graph of temperature
plt.plot(times,temperature)
plt.xlabel("time[s]")
plt.ylabel("temperature[C^o]")
plt.title("The temperature of atmosphpere")
Out[62]:
In [63]:
# The graph of acceleration
accelMag = map( lambda x: np.linalg.norm(x) , zip(accelX,accelY,accelZ) )
plt.plot(times,accelMag,label="accel mag")
plt.plot(times,accelX,label="accelX")
plt.plot(times,accelY,label="accelY")
plt.plot(times,accelZ,label="accelZ")
plt.legend()
plt.title("The acceleration of rocket")
plt.xlabel("time[s]")
plt.ylabel("accel[G]")
Out[63]:
In [64]:
# detect bias of the gyro sensor and eliminate
detect_bias = lambda xs: xs - np.average(xs[flightmode == 0])
gyroX = detect_bias(gyroX)
gyroY = detect_bias(gyroY)
gyroZ = detect_bias(gyroZ)
In [65]:
# The graph of angular velocity
gyroMag = map( lambda x: np.linalg.norm(x) , zip(gyroX,gyroY,gyroZ) )
plt.plot(times,gyroMag,label="gyro mag")
plt.plot(times,gyroX,label="gyroX")
plt.plot(times,gyroY,label="gyroY")
plt.plot(times,gyroZ,label="gyroZ")
plt.legend(loc='upper left')
plt.title("The angular velocity of rocket")
plt.xlabel("time[s]")
plt.ylabel("angular velocity[deg/s]")
Out[65]:
In [66]:
# detect bias of the magnetic sensor and eliminate
def calc_bias_mag(magXs,magYs,magZs):
# Least-square method
# make the norm of mag. constant
A = np.array([magXs,magYs,magZs,np.ones_like(magXs)]).T
b = map( lambda x: x[0]**2+x[1]**2+x[2]**2 , zip(magXs,magYs,magZs) )
a,b,c,d = np.linalg.solve(2*A.T.dot(A),A.T.dot(b))
return (a,b,c)
a,b,c = calc_bias_mag(magX,magY,magZ)
magX = magX[:] - a
magY = magY[:] - b
magZ = magZ[:] - c
In [67]:
# The graph of magnetic sensor
magMag = map( lambda x: np.linalg.norm(x) , zip(magX,magY,magZ) )
plt.plot(times,magMag,label="mag norm")
plt.plot(times,magX,label="magX")
plt.plot(times,magY,label="magY")
plt.plot(times,magZ,label="magZ")
plt.legend(loc='upper left')
plt.title("The geomagnetism ")
plt.xlabel("time[s]")
plt.ylabel("geomagnetism [gauss]")
Out[67]:
In [68]:
# The quaternion class from: http://d.hatena.ne.jp/yatt/20090920/1253452790
class Quaternion(object):
def __init__(self, t, v):
self.t = float(t)
self.v = np.array(map(float,v))
def __pos__(p): # +p
return p
def __neg__(p): # -p
return Quaternion(-p.t, -p.v)
def __add__(p, q): # p+q
return Quaternion(p.t + q.t, p.v + q.v)
def __sub__(p, q): # p-q
return Quaternion(p.t - q.t, p.v - q.v)
def __mul__(p, q): # p*q
return Quaternion(p.t*q.t - np.dot(p.v, q.v),
p.t*q.v + q.t*p.v + np.cross(p.v, q.v)
)
def inverse(p): # p*p_ = 1
# 逆４元数
n = p.abs2()
p_= p.conj()
return Quaternion(p_.t / n, p_.v / n)
def abs2(p):
return p.t**2 + sum(p.v**2)
def to_arr(p):
return [q.t,q.x,q.y,q.z]
def __abs__(p):
return p.abs2()**.5
def conj(p):
# 共役４元数
return Quaternion(p.t, -p.v)
def __repr__(p):
return "(%f; %f, %f, %f)" % (p.t, p.x, p.y, p.z)
@property
def x(self): return self.v[0]
@property
def y(self): return self.v[1]
@property
def z(self): return self.v[2]
In [69]:
def rotation(v,q_attitude):
v = Quaternion(0.0,v)
v = q_attitude * v * q_attitude.conj()
v = v.v
return np.array(v)
In [70]:
def gyro_to_quat(gx,gy,gz,dt,q_attitude,is_deg = True):
""" make the small rotation quaternion """
if is_deg:
gx = np.deg2rad(gx)
gy = np.deg2rad(gy)
gz = np.deg2rad(gz)
v = rotation([gx,gy,gz],q_attitude)
norm = np.linalg.norm(v)
v = normalize(v)
theta = norm * 0.5 * dt
return Quaternion(np.cos(theta),np.sin(theta)*v)
In [71]:
def to_euler(q):
""" quaternion -> euler angles """
q_arr = np.array([q.t,q.x,q.y,q.z])
norm = np.linalg.norm(q_arr)
if np.abs(norm) < 1e-8:
q_arr = [1,0,0,0]
else:
q_arr = q_arr / norm
q1,q2,q3,q4 = q_arr
# https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
# https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Math/quaternion.cpp
pitch = np.arcsin(2.0*(q1*q3 - q4*q2))
roll = (np.arctan2(2.0*(q1*q2 + q3*q4), 1.0 - 2.0*(q2*q2 + q3*q3)))
yaw = np.arctan2(2.0*(q1*q4 + q2*q3), 1.0 - 2.0*(q3*q3 + q4*q4))
return (roll,pitch,yaw)
In [72]:
def to_quat(euler):
""" euler angles -> quaternion """
# https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
# https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Math/quaternion.cpp
roll,pitch,yaw = euler
cr2 = np.cos(roll*0.5)
cp2 = np.cos(pitch*0.5)
cy2 = np.cos(yaw*0.5)
sr2 = np.sin(roll*0.5)
sp2 = np.sin(pitch*0.5)
sy2 = np.sin(yaw*0.5)
q1 = cr2*cp2*cy2 + sr2*sp2*sy2
q2 = sr2*cp2*cy2 - cr2*sp2*sy2
q3 = cr2*sp2*cy2 + sr2*cp2*sy2
q4 = cr2*cp2*sy2 - sr2*sp2*cy2
return Quaternion(q1,[q2,q3,q4])
In [73]:
# calculate the attitude of rocket from gyrosensor and convert quaternion and euler angles
q = Quaternion(1,[0,0,0])
quats = []
mag0 = np.array([magX[0],magY[0],magZ[0]])
mag0 = normalize(mag0)
for dt,gx,gy,gz,mx,my,mz in zip(np.diff(times),gyroX[1:],gyroY[1:],gyroZ[1:],magX[1:],magY[1:],magZ[1:]):
q_small = gyro_to_quat(gx,gy,gz,dt,q,True)
q = q_small * q
"""
# correction by magnetic sensor?????????
mag_v = rotation([mx,my,mz],q)
mag_v = normalize(mag_v)
theta = np.arccos(mag_v.dot(mag0))
v_rot = np.cross(mag_v,mag0)
v_rot = normalize(v_rot)
if theta <= 1e-8:
pass
else:
corr_ratio = 0.01
theta = theta * 0.5 * corr_ratio
q_cor = Quaternion(np.cos(theta),np.sin(theta)*v_rot)
q = q_cor * q
"""
quats.append(q)
angle_xyz = map(to_euler,quats)
quats = [q.to_arr() for q in quats]
attitudes = np.c_[times[1:],quats,angle_xyz,height[1:]]
attitudes
Out[73]:
In [74]:
np.savetxt("attitudes.csv",attitudes,delimiter=",",header="time,q0,q1,q2,q3,angle_x,angle_y,angle_z,height")
In [75]:
magXYZ = zip( magX[1:],magY[1:],magZ[1:])
for i in range(len(magXYZ)):
mag_vec = magXYZ[i]
q = quats[i]
q = Quaternion(q[0],q[1:])
magXYZ[i] = rotation(magXYZ[i],q)
magXYZ = np.array(magXYZ)
In [76]:
# The graph of magnetic sensor (r-frame)
magMagXYZ = map( lambda x: np.linalg.norm(x) , magXYZ )
plt.plot(times[1:],magMagXYZ,label="mag norm")
plt.plot(times[1:],magXYZ[:,0],label="magX")
plt.plot(times[1:],magXYZ[:,1],label="magY")
plt.plot(times[1:],magXYZ[:,2],label="magZ")
plt.legend(loc='upper left')
plt.title("The geomagnetism ")
plt.xlabel("time[s]")
plt.ylabel("geomagnetism [gauss]")
Out[76]: