class CompareFunctions(unittest.TestCase):
def setUp(self):
self.c_sim = CINS()
self.py_sim = PyINS()
self.c_sim.prepare()
self.py_sim.prepare()
def run_static(self,
accel=[0.0, 0.0, -PyINS.GRAV],
gyro=[0.0, 0.0, 0.0],
mag=[400, 0, 1600],
pos=[0, 0, 0],
vel=[0, 0, 0],
noise=False,
STEPS=200000):
""" simulate a static set of inputs and measurements
"""
c_sim = self.c_sim
py_sim = self.py_sim
dT = 1.0 / 666.0
numpy.random.seed(1)
c_history = numpy.zeros((STEPS, 16))
c_history_rpy = numpy.zeros((STEPS, 3))
py_history = numpy.zeros((STEPS, 16))
py_history_rpy = numpy.zeros((STEPS, 3))
times = numpy.zeros((STEPS, 1))
for k in range(STEPS):
print ` k `
ng = numpy.zeros(3, )
na = numpy.zeros(3, )
np = numpy.zeros(3, )
nv = numpy.zeros(3, )
nm = numpy.zeros(3, )
if noise:
ng = numpy.random.randn(3, ) * 1e-3
na = numpy.random.randn(3, ) * 1e-3
np = numpy.random.randn(3, ) * 1e-3
nv = numpy.random.randn(3, ) * 1e-3
nm = numpy.random.randn(3, ) * 10.0
c_sim.predict(gyro + ng, accel + na, dT=dT)
py_sim.predict(gyro + ng, accel + na, dT=dT)
times[k] = k * dT
c_history[k, :] = c_sim.state
c_history_rpy[k, :] = quat_rpy(c_sim.state[6:10])
py_history[k, :] = py_sim.state
py_history_rpy[k, :] = quat_rpy(py_sim.state[6:10])
if False and k % 60 == 59:
c_sim.correction(pos=pos + np)
py_sim.correction(pos=pos + np)
if False and k % 60 == 59:
c_sim.correction(vel=vel + nv)
py_sim.correction(vel=vel + nv)
if True and k % 20 == 8:
c_sim.correction(baro=-pos[2] + np[2])
py_sim.correction(baro=-pos[2] + np[2])
if True and k % 20 == 15:
c_sim.correction(mag=mag + nm)
py_sim.correction(mag=mag + nm)
self.assertState(c_sim.state, py_sim.state)
if VISUALIZE:
from numpy import cos, sin
import matplotlib.pyplot as plt
fig, ax = plt.subplots(2, 2)
k = STEPS
ax[0][0].cla()
ax[0][0].plot(times[0:k:4], c_history[0:k:4, 0:3])
ax[0][0].set_title('Position')
plt.sca(ax[0][0])
plt.ylabel('m')
ax[0][1].cla()
ax[0][1].plot(times[0:k:4], c_history[0:k:4, 3:6])
ax[0][1].set_title('Velocity')
plt.sca(ax[0][1])
plt.ylabel('m/s')
#plt.ylim(-2,2)
ax[1][0].cla()
ax[1][0].plot(times[0:k:4], c_history_rpy[0:k:4, :])
ax[1][0].set_title('Attitude')
plt.sca(ax[1][0])
plt.ylabel('Angle (Deg)')
plt.xlabel('Time (s)')
#plt.ylim(-1.1,1.1)
ax[1][1].cla()
ax[1][1].plot(times[0:k:4], c_history[0:k:4, 10:])
ax[1][1].set_title('Biases')
plt.sca(ax[1][1])
plt.ylabel('Bias (rad/s)')
plt.xlabel('Time (s)')
plt.suptitle(unittest.TestCase.shortDescription(self))
plt.show()
return sim.state, history, times
def assertState(self, c_state, py_state):
""" check that the state is near a desired position
"""
# check position
self.assertAlmostEqual(c_state[0], py_state[0], places=1)
self.assertAlmostEqual(c_state[1], py_state[1], places=1)
self.assertAlmostEqual(c_state[2], py_state[2], places=1)
# check velocity
self.assertAlmostEqual(c_state[3], py_state[3], places=1)
self.assertAlmostEqual(c_state[4], py_state[4], places=1)
self.assertAlmostEqual(c_state[5], py_state[5], places=1)
# check attitude
self.assertAlmostEqual(c_state[0], py_state[0], places=0)
self.assertAlmostEqual(c_state[1], py_state[1], places=0)
self.assertAlmostEqual(c_state[2], py_state[2], places=0)
self.assertAlmostEqual(c_state[3], py_state[3], places=0)
# check bias terms (gyros and accels)
self.assertAlmostEqual(c_state[10], py_state[10], places=2)
self.assertAlmostEqual(c_state[11], py_state[11], places=2)
self.assertAlmostEqual(c_state[12], py_state[12], places=2)
self.assertAlmostEqual(c_state[13], py_state[13], places=2)
self.assertAlmostEqual(c_state[14], py_state[14], places=2)
self.assertAlmostEqual(c_state[15], py_state[15], places=2)
def test_face_west(self):
""" test convergence to face west
"""
mag = [0, -400, 1600]
state, history, times = self.run_static(mag=mag, STEPS=50000)
self.assertState(state, rpy=[0, 0, 90])
class ReplayFlightFunctions():
def __init__(self):
self.uavolist = []
def load(self, filename):
self.sim = CINS()
self.sim.prepare()
# add python directory to search path so the modules can be loaded
import sys,os
sys.path.insert(1, os.path.dirname(sys.path[0]))
import taulabs
import cPickle
# load data from file
handle = open(filename, 'rb')
githash = cPickle.load(handle)
print "Attempting to load data with githash of " + `githash`
uavo_parsed = cPickle.load(handle)
handle.close()
# prepare the parser
uavo_defs = taulabs.uavo_collection.UAVOCollection()
uavo_defs.from_git_hash(githash)
parser = taulabs.uavtalk.UavTalk(uavo_defs)
print "Converting log records into python objects"
uavo_list = taulabs.uavo_list.UAVOList(uavo_defs)
for obj_id, data, timestamp in uavo_parsed:
if obj_id in uavo_defs:
obj = uavo_defs[obj_id]
u = obj.instance_from_bytes(data, timestamp)
uavo_list.append(u)
else:
print "Missing key: " + `obj_id`
# We're done with this (potentially very large) variable, delete it.
del uavo_parsed
self.uavo_list = uavo_list
def run_uavo_list(self):
import taulabs
import math
uavo_list = self.uavo_list
print "Replaying log file"
attitude = uavo_list.as_numpy_array(taulabs.uavo.UAVO_AttitudeActual)
gyros = uavo_list.as_numpy_array(taulabs.uavo.UAVO_Gyros)
accels = uavo_list.as_numpy_array(taulabs.uavo.UAVO_Accels)
#ned = uavo_list.as_numpy_array(taulabs.uavo.UAVO_NEDPosition)
gps = uavo_list.as_numpy_array(taulabs.uavo.UAVO_GPSPosition)
vel = uavo_list.as_numpy_array(taulabs.uavo.UAVO_GPSVelocity)
mag = uavo_list.as_numpy_array(taulabs.uavo.UAVO_Magnetometer)
baro = uavo_list.as_numpy_array(taulabs.uavo.UAVO_BaroAltitude)
if gps.size == 0:
print "Unable to process flight. No GPS data"
return
# set home location as first sample and linearize around that
lat0 = gps['Latitude'][0,0]
lon0 = gps['Longitude'][0,0]
alt0 = gps['Altitude'][0,0]
T = [alt0+6.378137E6, cos(lat0 / 10e6 * math.pi / 180.0)*(alt0+6.378137E6)]
STEPS = gyros['time'].size
history = numpy.zeros((STEPS,16))
history_rpy = numpy.zeros((STEPS,3))
times = numpy.zeros((STEPS,1))
ned_history = numpy.zeros((gps['Latitude'].size,3))
steps = 0
t = gyros['time'][0]
gyro_idx = 0
accel_idx = 0
gps_idx = 0
vel_idx = 0
mag_idx = 0
baro_idx = 0
dT = numpy.mean(numpy.diff(gyros['time']))
for gyro_idx in numpy.arange(STEPS):
t = gyros['time'][gyro_idx]
steps = gyro_idx
accel_idx = (numpy.abs(accels['time']-t)).argmin()
gyros_dat = numpy.array([gyros['x'][gyro_idx],gyros['y'][gyro_idx],gyros['z'][gyro_idx]]).T[0]
accels_dat = numpy.array([accels['x'][accel_idx],accels['y'][accel_idx],accels['z'][accel_idx]]).T[0]
gyros_dat = gyros_dat / 180 * math.pi
self.sim.predict(gyros_dat,accels_dat,dT)
if (gps_idx < gps['time'].size) and (gps['time'][gps_idx] < t):
pos = [(gps['Latitude'][gps_idx,0] - lat0) / 10e6 * math.pi / 180.0 * T[0], (gps['Longitude'][gps_idx,0] - lon0) / 10e6 * math.pi / 180.0 * T[1], -(gps['Altitude'][gps_idx,0]-alt0)]
self.sim.correction(pos=pos)
ned_history[gps_idx,0] = pos[0]
ned_history[gps_idx,1] = pos[1]
ned_history[gps_idx,2] = pos[2]
gps_idx = gps_idx + 1
if (vel_idx < vel['time'].size) and (vel['time'][vel_idx] < t):
self.sim.correction(vel=[vel['North'][vel_idx,0], vel['East'][vel_idx,0], vel['Down'][vel_idx,0]])
vel_idx = vel_idx + 1
if (mag_idx < mag['time'].size) and (mag['time'][mag_idx] < t):
self.sim.correction(mag=[mag['x'][mag_idx,0], mag['y'][mag_idx,0], mag['z'][mag_idx,0]])
mag_idx = mag_idx + 1
if (baro_idx < baro['time'].size) and (baro['time'][baro_idx] < t):
self.sim.correction(baro=baro['Altitude'][baro_idx,0])
baro_idx = baro_idx + 1
history[steps,:] = self.sim.state
history_rpy[steps,:] = quat_rpy(self.sim.state[6:10])
times[steps] = t
print "Plotting results"
import matplotlib.pyplot as plt
fig, ax = plt.subplots(2,2,sharex=True)
ax[0][0].cla()
ax[0][0].plot(gps['time'],ned_history[:,0],'b--',label="North")
ax[0][0].plot(gps['time'],ned_history[:,1],'g--',label="East")
ax[0][0].plot(gps['time'],ned_history[:,2],'r--',label="Down"),
ax[0][0].plot(baro['time'], -baro['Altitude'],'--',label="Baro")
ax[0][0].plot(times,history[:,0],'b',label="Replay")
ax[0][0].plot(times,history[:,1],'g',label="Replay")
ax[0][0].plot(times,history[:,2],'r',label="Replay")
ax[0][0].set_title('Position')
plt.sca(ax[0][0])
plt.ylabel('m')
plt.legend()
ax[0][1].cla()
ax[0][1].plot(vel['time'],vel['North'],'b--',label="North")
ax[0][1].plot(vel['time'],vel['East'],'g--',label="East")
ax[0][1].plot(vel['time'],vel['Down'],'r--',label="Down")
ax[0][1].plot(times,history[:,3],'b',label="Replay")
ax[0][1].plot(times,history[:,4],'g',label="Replay")
ax[0][1].plot(times,history[:,5],'r',label="Replay")
ax[0][1].set_title('Velocity')
plt.sca(ax[0][1])
plt.ylabel('m/s')
plt.legend()
ax[1][0].cla()
ax[1][0].plot(attitude['time'],attitude['Roll'],'--',label="Roll")
ax[1][0].plot(attitude['time'],attitude['Pitch'],'--',label="Pitch")
ax[1][0].plot(attitude['time'],attitude['Yaw'],'--',label="Yaw")
ax[1][0].plot(times,history_rpy[:,:], label="Replay")
ax[1][0].set_title('Attitude')
plt.sca(ax[1][0])
plt.ylabel('Angle (Deg)')
plt.xlabel('Time (s)')
plt.legend()
ax[1][1].cla()
ax[1][1].plot(times,history[:,10:13]*180/3.1415,label="Gyro Bias")
ax[1][1].plot(times,history[:,-1],label="Z Bias")
ax[1][1].set_title('Biases')
plt.sca(ax[1][1])
plt.ylabel('Bias (rad/s)')
plt.xlabel('Time (s)')
plt.legend()
plt.draw()
plt.show()
return ins
class CompareFunctions(unittest.TestCase):
def setUp(self):
self.c_sim = CINS()
self.py_sim = PyINS()
self.c_sim.prepare()
self.py_sim.prepare()
def run_static(self, accel=[0.0,0.0,-PyINS.GRAV],
gyro=[0.0,0.0,0.0], mag=[400,0,1600],
pos=[0,0,0], vel=[0,0,0],
noise=False, STEPS=200000):
""" simulate a static set of inputs and measurements
"""
c_sim = self.c_sim
py_sim = self.py_sim
dT = 1.0 / 666.0
numpy.random.seed(1)
c_history = numpy.zeros((STEPS,16))
c_history_rpy = numpy.zeros((STEPS,3))
py_history = numpy.zeros((STEPS,16))
py_history_rpy = numpy.zeros((STEPS,3))
times = numpy.zeros((STEPS,1))
for k in range(STEPS):
print `k`
ng = numpy.zeros(3,)
na = numpy.zeros(3,)
np = numpy.zeros(3,)
nv = numpy.zeros(3,)
nm = numpy.zeros(3,)
if noise:
ng = numpy.random.randn(3,) * 1e-3
na = numpy.random.randn(3,) * 1e-3
np = numpy.random.randn(3,) * 1e-3
nv = numpy.random.randn(3,) * 1e-3
nm = numpy.random.randn(3,) * 10.0
c_sim.predict(gyro+ng, accel+na, dT=dT)
py_sim.predict(gyro+ng, accel+na, dT=dT)
times[k] = k * dT
c_history[k,:] = c_sim.state
c_history_rpy[k,:] = quat_rpy(c_sim.state[6:10])
py_history[k,:] = py_sim.state
py_history_rpy[k,:] = quat_rpy(py_sim.state[6:10])
if False and k % 60 == 59:
c_sim.correction(pos=pos+np)
py_sim.correction(pos=pos+np)
if False and k % 60 == 59:
c_sim.correction(vel=vel+nv)
py_sim.correction(vel=vel+nv)
if True and k % 20 == 8:
c_sim.correction(baro=-pos[2]+np[2])
py_sim.correction(baro=-pos[2]+np[2])
if True and k % 20 == 15:
c_sim.correction(mag=mag+nm)
py_sim.correction(mag=mag+nm)
self.assertState(c_sim.state, py_sim.state)
if VISUALIZE:
from numpy import cos, sin
import matplotlib.pyplot as plt
fig, ax = plt.subplots(2,2)
k = STEPS
ax[0][0].cla()
ax[0][0].plot(times[0:k:4],c_history[0:k:4,0:3])
ax[0][0].set_title('Position')
plt.sca(ax[0][0])
plt.ylabel('m')
ax[0][1].cla()
ax[0][1].plot(times[0:k:4],c_history[0:k:4,3:6])
ax[0][1].set_title('Velocity')
plt.sca(ax[0][1])
plt.ylabel('m/s')
#plt.ylim(-2,2)
ax[1][0].cla()
ax[1][0].plot(times[0:k:4],c_history_rpy[0:k:4,:])
ax[1][0].set_title('Attitude')
plt.sca(ax[1][0])
plt.ylabel('Angle (Deg)')
plt.xlabel('Time (s)')
#plt.ylim(-1.1,1.1)
ax[1][1].cla()
ax[1][1].plot(times[0:k:4],c_history[0:k:4,10:])
ax[1][1].set_title('Biases')
plt.sca(ax[1][1])
plt.ylabel('Bias (rad/s)')
plt.xlabel('Time (s)')
plt.suptitle(unittest.TestCase.shortDescription(self))
plt.show()
return sim.state, history, times
def assertState(self, c_state, py_state):
""" check that the state is near a desired position
"""
# check position
self.assertAlmostEqual(c_state[0],py_state[0],places=1)
self.assertAlmostEqual(c_state[1],py_state[1],places=1)
self.assertAlmostEqual(c_state[2],py_state[2],places=1)
# check velocity
self.assertAlmostEqual(c_state[3],py_state[3],places=1)
self.assertAlmostEqual(c_state[4],py_state[4],places=1)
self.assertAlmostEqual(c_state[5],py_state[5],places=1)
# check attitude
self.assertAlmostEqual(c_state[0],py_state[0],places=0)
self.assertAlmostEqual(c_state[1],py_state[1],places=0)
self.assertAlmostEqual(c_state[2],py_state[2],places=0)
self.assertAlmostEqual(c_state[3],py_state[3],places=0)
# check bias terms (gyros and accels)
self.assertAlmostEqual(c_state[10],py_state[10],places=2)
self.assertAlmostEqual(c_state[11],py_state[11],places=2)
self.assertAlmostEqual(c_state[12],py_state[12],places=2)
self.assertAlmostEqual(c_state[13],py_state[13],places=2)
self.assertAlmostEqual(c_state[14],py_state[14],places=2)
self.assertAlmostEqual(c_state[15],py_state[15],places=2)
def test_face_west(self):
""" test convergence to face west
"""
mag = [0,-400,1600]
state, history, times = self.run_static(mag=mag, STEPS=50000)
self.assertState(state,rpy=[0,0,90])