def test_init_bad_q(self):
""" test convergence with bad initial attitude
"""
ins.set_state(q=numpy.array([0.7, 0.7, 0, 0]))
state, history, times = self.run_static() #STEPS=5000)
self.assertState(state)
def test_init_bad_bias(self):
""" test convergence with bad initial gyro biases
"""
ins.set_state(gyro_bias=numpy.array([0.05, 0.05, -0.05]))
state, history, times = self.run_static(STEPS=100000)
self.assertState(state)
def test_init_100m(self):
""" test convergence to origin when initialized 100m away
"""
ins.set_state(pos=numpy.array([100.0, 100.0, 50]))
state, history, times = self.run_static(STEPS=50000)
self.assertState(state)
def test_init_bad_q_bias(self):
""" test convergence with bad initial attitude and biases
"""
ins.set_state(q=numpy.array([0.7, 0.7, 0, 0]), gyro_bias=numpy.array([0.05, 0.05, -0.05]))
state, history, times = self.run_static(STEPS=100000)
self.assertState(state)
def test_init_bad_q(self):
""" test convergence with bad initial attitude
"""
ins.set_state(q=numpy.array([0.7, 0.7, 0, 0]))
state, history, times = self.run_static() # STEPS=5000)
self.assertState(state)
def test_init_100m(self):
""" test convergence to origin when initialized 100m away
"""
ins.set_state(pos=numpy.array([100.0, 100.0, 50]))
state, history, times = self.run_static(STEPS=50000)
self.assertState(state)
def test_bad_init_q(self):
""" test that while flying a circle the location converges with a bad initial attitude
"""
sim = self.sim
model = self.model
dT = 1.0 / 666.0
STEPS = 60 * 666
history = numpy.zeros((STEPS, 16))
history_rpy = numpy.zeros((STEPS, 3))
true_pos = numpy.zeros((STEPS, 3))
true_vel = numpy.zeros((STEPS, 3))
true_rpy = numpy.zeros((STEPS, 3))
times = numpy.zeros((STEPS, 1))
numpy.random.seed(1)
ins.set_state(q=numpy.array([math.sqrt(2), math.sqrt(2), 0, 0]))
for k in range(STEPS):
model.fly_circle(dT=dT)
true_pos[k, :] = model.get_pos()
true_vel[k, :] = model.get_vel()
true_rpy[k, :] = model.get_rpy()
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
# convert from rad/s to deg/s
gyro = model.get_gyro() / 180.0 * math.pi
accel = model.get_accel()
sim.predict(gyro + ng, accel + na, dT=dT)
pos = model.get_pos()
if k % 60 == 59:
sim.correction(pos=pos + np)
if k % 60 == 59:
sim.correction(vel=model.get_vel() + nv)
if k % 20 == 8:
sim.correction(baro=-pos[2] + np[2])
if k % 20 == 15:
sim.correction(mag=model.get_mag() + nm)
history[k, :] = sim.state
history_rpy[k, :] = quat_rpy(sim.state[6:10])
times[k] = k * dT
if VISUALIZE:
self.plot(times, history, history_rpy, true_pos, true_vel,
true_rpy)
self.assertState(sim.state,
pos=model.get_pos(),
vel=model.get_vel(),
rpy=model.get_rpy(),
bias=[0, 0, 0, 0, 0, 0])
def test_bad_init_q(self):
""" test that while flying a circle the location converges with a bad initial attitude
"""
sim = self.sim
model = self.model
dT = 1.0 / 666.0
STEPS = 60 * 666
history = numpy.zeros((STEPS, 16))
history_rpy = numpy.zeros((STEPS, 3))
true_pos = numpy.zeros((STEPS, 3))
true_vel = numpy.zeros((STEPS, 3))
true_rpy = numpy.zeros((STEPS, 3))
times = numpy.zeros((STEPS, 1))
numpy.random.seed(1)
ins.set_state(q=numpy.array([math.sqrt(2), math.sqrt(2), 0, 0]))
for k in range(STEPS):
model.fly_circle(dT=dT)
true_pos[k, :] = model.get_pos()
true_vel[k, :] = model.get_vel()
true_rpy[k, :] = model.get_rpy()
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
# convert from rad/s to deg/s
gyro = model.get_gyro() / 180.0 * math.pi
accel = model.get_accel()
sim.predict(gyro + ng, accel + na, dT=dT)
pos = model.get_pos()
if k % 60 == 59:
sim.correction(pos=pos + np)
if k % 60 == 59:
sim.correction(vel=model.get_vel() + nv)
if k % 20 == 8:
sim.correction(baro=-pos[2] + np[2])
if k % 20 == 15:
sim.correction(mag=model.get_mag() + nm)
history[k, :] = sim.state
history_rpy[k, :] = quat_rpy(sim.state[6:10])
times[k] = k * dT
if VISUALIZE:
self.plot(times, history, history_rpy, true_pos, true_vel, true_rpy)
self.assertState(
sim.state, pos=model.get_pos(), vel=model.get_vel(), rpy=model.get_rpy(), bias=[0, 0, 0, 0, 0, 0]
)
