def setUp(self): self.c_sim = CINS() self.py_sim = PyINS() self.c_sim.prepare() self.py_sim.prepare()
def setUp(self): if C_IMP: self.sim = CINS() else: self.sim = PyINS() self.sim.prepare() import simulation self.model = simulation.Simulation()
class SimulatedFlightTests(unittest.TestCase): def setUp(self): if C_IMP: self.sim = CINS() else: self.sim = PyINS() self.sim.prepare() import simulation self.model = simulation.Simulation() def assertState(self, state, pos=[0, 0, 0], vel=[0, 0, 0], rpy=[0, 0, 0], bias=[0, 0, 0, 0, 0, 0]): """ check that the state is near a desired position """ # check position self.assertAlmostEqual(state[0], pos[0], places=0) self.assertAlmostEqual(state[1], pos[1], places=0) self.assertAlmostEqual(state[2], pos[2], places=0) # check velocity self.assertAlmostEqual(state[3], vel[0], places=1) self.assertAlmostEqual(state[4], vel[1], places=1) self.assertAlmostEqual(state[5], vel[2], places=1) # check attitude (in degrees) s_rpy = quat_rpy(state[6:10]) self.assertAlmostEqual(s_rpy[0], rpy[0], places=0) self.assertAlmostEqual(s_rpy[1], rpy[1], places=0) self.assertTrue(abs(math.fmod(s_rpy[2] - rpy[2], 360)) < 5) # check bias terms (gyros and accels) self.assertAlmostEqual(state[10], bias[0], places=0) self.assertAlmostEqual(state[11], bias[1], places=0) self.assertAlmostEqual(state[12], bias[2], places=0) self.assertAlmostEqual(state[13], bias[3], places=0) self.assertAlmostEqual(state[14], bias[4], places=0) self.assertAlmostEqual(state[15], bias[5], places=1) def plot(self, times, history, history_rpy, true_pos, true_vel, true_rpy): from numpy import cos, sin import matplotlib.pyplot as plt fig, ax = plt.subplots(2, 2, sharex=True) k = times.size ax[0][0].cla() ax[0][0].plot(times[0:k:4], true_pos[0:k:4, :], 'k--') ax[0][0].plot(times[0:k:4], 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], true_vel[0:k:4, :], 'k--') ax[0][1].plot(times[0:k:4], history[0:k:4, 3:6]) ax[0][1].set_title('Velocity') plt.sca(ax[0][1]) plt.ylabel('m/s') ax[1][0].cla() ax[1][0].plot(times[0:k:4], true_rpy[0:k:4, :], 'k--') ax[1][0].plot(times[0:k:4], history_rpy[0:k:4, :]) ax[1][0].set_title('Attitude') plt.sca(ax[1][0]) plt.ylabel('Angle (Deg)') plt.xlabel('Time (s)') ax[1][1].cla() ax[1][1].plot(times[0:k:4], history[0:k:4, 10:13]) ax[1][1].plot(times[0:k:4], history[0:k:4, -1]) 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() def test_circle(self, STEPS=50000): """ test that the INS gets a good fit for a simulated flight of circles """ sim = self.sim model = self.model dT = 1.0 / 666.0 numpy.random.seed(1) 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)) 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) if True and k % 60 == 59: sim.correction(pos=true_pos[k, :] + np) if True and k % 60 == 59: sim.correction(vel=true_vel[k, :] + nv) if k % 20 == 8: sim.correction(baro=-true_pos[k, 2] + np[2]) if True and 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 k > 100: numpy.testing.assert_almost_equal(sim.state[0:3], true_pos[k, :], decimal=0) numpy.testing.assert_almost_equal(sim.state[3:6], true_vel[k, :], decimal=0) # only test roll and pitch because of wraparound issues numpy.testing.assert_almost_equal(history_rpy[k, 0:2], true_rpy[k, 0:2], decimal=1) if VISUALIZE: self.plot(times, history, history_rpy, true_pos, true_vel, true_rpy) return sim.state, history, times def test_gyro_bias_circle(self): """ test that while flying a circle the location converges """ STEPS = 100000 sim = self.sim model = self.model dT = 1.0 / 666.0 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) 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() # add simulated bias gyro = gyro + numpy.array([0.1, -0.05, 0.15]) 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_accel_bias_circle(self): """ test that while flying a circle the location converges """ STEPS = 100000 sim = self.sim model = self.model dT = 1.0 / 666.0 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) 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() # add simulated bias accel = accel + numpy.array([0.0, 0, 0.2]) 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.2]) 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 rock_and_turn(self, STEPS=50000): """ test the biases work when rocking and turning. this tests the mag attitude compensation """ sim = self.sim model = self.model dT = 1.0 / 666.0 numpy.random.seed(1) 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)) for k in range(STEPS): model.rock_and_turn(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) if True and k % 60 == 59: sim.correction(pos=true_pos[k, :] + np) if True and k % 60 == 59: sim.correction(vel=true_vel[k, :] + nv) if k % 20 == 8: sim.correction(baro=-true_pos[k, 2] + np[2]) if True and 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 k > 100: numpy.testing.assert_almost_equal(sim.state[0:3], true_pos[k, :], decimal=0) numpy.testing.assert_almost_equal(sim.state[3:6], true_vel[k, :], decimal=0) # only test roll and pitch because of wraparound issues numpy.testing.assert_almost_equal(history_rpy[k, 0:2], true_rpy[k, 0:2], decimal=1) if VISUALIZE: self.plot(times, history, history_rpy, true_pos, true_vel, true_rpy) return sim.state, history, times
def setUp(self): if C_IMP: self.sim = CINS() else: self.sim = PyINS() self.sim.prepare()
class StepTestFunctions(unittest.TestCase): def setUp(self): if C_IMP: self.sim = CINS() else: self.sim = PyINS() self.sim.prepare() def run_step(self, accel=[0.0, 0.0, -CINS.GRAV], gyro=[0.0, 0.0, 0.0], mag=[400, 0, 1600], pos=[0, 0, 0], vel=[0, 0, 0], noise=False, STEPS=200000, CHANGE=6660 * 3): """ simulate a static set of inputs and measurements """ sim = self.sim dT = 1.0 / 666.0 #numpy.random.seed(1) history = numpy.zeros((STEPS, 16)) history_rpy = numpy.zeros((STEPS, 3)) times = numpy.zeros((STEPS, 1)) for k in range(STEPS): 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 if k < CHANGE: sim.predict(ng, numpy.array([0, 0, -CINS.GRAV]) + na, dT=dT) else: sim.predict(gyro + ng, accel + na, dT=dT) history[k, :] = sim.state history_rpy[k, :] = quat_rpy(sim.state[6:10]) times[k] = k * dT if True and k % 60 == 59: sim.correction(pos=pos + np) if True and k % 60 == 59: sim.correction(vel=vel + nv) if k % 20 == 8: sim.correction(baro=-pos[2] + np[2]) if True and k % 20 == 15: sim.correction(mag=mag + nm) if VISUALIZE: from numpy import cos, sin import matplotlib.pyplot as plt fig, ax = plt.subplots(2, 2, sharex=True) k = STEPS ax[0][0].cla() ax[0][0].plot(times[0:k:4], 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], 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], 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], history[0:k:4, 10:13], label="Gyro") ax[1][1].plot(times[0:k:4], history[0:k:4, -1], label="Accel") ax[1][1].set_title('Biases') plt.sca(ax[1][1]) plt.ylabel('Bias (rad/s)') plt.xlabel('Time (s)') plt.legend() plt.suptitle(unittest.TestCase.shortDescription(self)) plt.show() return sim.state, history, times def assertState(self, state, pos=[0, 0, 0], vel=[0, 0, 0], rpy=[0, 0, 0], bias=[0, 0, 0, 0, 0, 0]): """ check that the state is near a desired position """ # check position self.assertAlmostEqual(state[0], pos[0], places=1) self.assertAlmostEqual(state[1], pos[1], places=1) self.assertAlmostEqual(state[2], pos[2], places=1) # check velocity self.assertAlmostEqual(state[3], vel[0], places=1) self.assertAlmostEqual(state[4], vel[1], places=1) self.assertAlmostEqual(state[5], vel[2], places=1) # check attitude (in degrees) s_rpy = quat_rpy(state[6:10]) self.assertAlmostEqual(s_rpy[0], rpy[0], places=0) self.assertAlmostEqual(s_rpy[1], rpy[1], places=0) self.assertTrue(abs(math.fmod(s_rpy[2] - rpy[2], 360)) < 1) # check bias terms (gyros and accels) self.assertAlmostEqual(state[10], bias[0], places=2) self.assertAlmostEqual(state[11], bias[1], places=2) self.assertAlmostEqual(state[12], bias[2], places=2) self.assertAlmostEqual(state[13], bias[3], places=2) self.assertAlmostEqual(state[14], bias[4], places=2) self.assertAlmostEqual(state[15], bias[5], places=2) def test_accel_bias(self): """ test convergence with biased accelerometers """ bias = -0.20 state, history, times = self.run_step(accel=[0, 0, -PyINS.GRAV + bias], STEPS=90 * 666) self.assertState(state, bias=[0, 0, 0, 0, 0, bias]) def test_gyro_bias(self): """ test convergence with biased gyros """ state, history, times = self.run_step(gyro=[0.1, -0.05, 0.06], STEPS=90 * 666) self.assertState(state, bias=[0.1, -0.05, 0.06, 0, 0, 0]) def test_gyro_bias_xy_rate(self): """ test gyro xy bias converges within 10% in a fixed time """ TIME = 90 # seconds FS = 666 # sampling rate MAX_ERR = 0.1 BIAS = 10.0 * math.pi / 180.0 state, history, times = self.run_step(gyro=[BIAS, -BIAS, 0], STEPS=TIME * FS) self.assertAlmostEqual(state[10], BIAS, delta=BIAS * MAX_ERR) self.assertAlmostEqual(state[11], -BIAS, delta=BIAS * MAX_ERR) def test_gyro_bias_z_rate(self): """ test gyro z bias converges within 10% in a fixed time """ TIME = 90 # seconds FS = 666 # sampling rate MAX_ERR = 0.1 BIAS = 10.0 * math.pi / 180.0 state, history, times = self.run_step(gyro=[0, 0, BIAS], STEPS=TIME * FS) self.assertAlmostEqual(state[12], BIAS, delta=BIAS * MAX_ERR) def test_accel_bias_z_rate(self): """ test accel z bias converges within 10% in a fixed time """ TIME = 90 # seconds FS = 666 # sampling rate MAX_ERR = 0.1 BIAS = 1 state, history, times = self.run_step(accel=[0, 0, -PyINS.GRAV + BIAS], STEPS=TIME * FS) self.assertAlmostEqual(state[15], BIAS, delta=BIAS * MAX_ERR)
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 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 SimulatedFlightTests(unittest.TestCase): def setUp(self): if C_IMP: self.sim = CINS() else: self.sim = PyINS() self.sim.prepare() import simulation self.model = simulation.Simulation() def assertState(self, state, pos=[0, 0, 0], vel=[0, 0, 0], rpy=[0, 0, 0], bias=[0, 0, 0, 0, 0, 0]): """ check that the state is near a desired position """ # check position self.assertAlmostEqual(state[0], pos[0], places=0) self.assertAlmostEqual(state[1], pos[1], places=0) self.assertAlmostEqual(state[2], pos[2], places=0) # check velocity self.assertAlmostEqual(state[3], vel[0], places=1) self.assertAlmostEqual(state[4], vel[1], places=1) self.assertAlmostEqual(state[5], vel[2], places=1) # check attitude (in degrees) s_rpy = quat_rpy(state[6:10]) self.assertAlmostEqual(s_rpy[0], rpy[0], places=0) self.assertAlmostEqual(s_rpy[1], rpy[1], places=0) self.assertTrue(abs(math.fmod(s_rpy[2] - rpy[2], 360)) < 5) # check bias terms (gyros and accels) self.assertAlmostEqual(state[10], bias[0], places=0) self.assertAlmostEqual(state[11], bias[1], places=0) self.assertAlmostEqual(state[12], bias[2], places=0) self.assertAlmostEqual(state[13], bias[3], places=0) self.assertAlmostEqual(state[14], bias[4], places=0) self.assertAlmostEqual(state[15], bias[5], places=1) def plot(self, times, history, history_rpy, true_pos, true_vel, true_rpy): from numpy import cos, sin import matplotlib.pyplot as plt fig, ax = plt.subplots(2, 2, sharex=True) k = times.size ax[0][0].cla() ax[0][0].plot(times[0:k:4], true_pos[0:k:4, :], "k--") ax[0][0].plot(times[0:k:4], 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], true_vel[0:k:4, :], "k--") ax[0][1].plot(times[0:k:4], history[0:k:4, 3:6]) ax[0][1].set_title("Velocity") plt.sca(ax[0][1]) plt.ylabel("m/s") ax[1][0].cla() ax[1][0].plot(times[0:k:4], true_rpy[0:k:4, :], "k--") ax[1][0].plot(times[0:k:4], history_rpy[0:k:4, :]) ax[1][0].set_title("Attitude") plt.sca(ax[1][0]) plt.ylabel("Angle (Deg)") plt.xlabel("Time (s)") ax[1][1].cla() ax[1][1].plot(times[0:k:4], history[0:k:4, 10:13]) ax[1][1].plot(times[0:k:4], history[0:k:4, -1]) 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() def test_circle(self, STEPS=50000): """ test that the INS gets a good fit for a simulated flight of circles """ sim = self.sim model = self.model dT = 1.0 / 666.0 numpy.random.seed(1) 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)) 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) if True and k % 60 == 59: sim.correction(pos=true_pos[k, :] + np) if True and k % 60 == 59: sim.correction(vel=true_vel[k, :] + nv) if k % 20 == 8: sim.correction(baro=-true_pos[k, 2] + np[2]) if True and 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 k > 100: numpy.testing.assert_almost_equal(sim.state[0:3], true_pos[k, :], decimal=0) numpy.testing.assert_almost_equal(sim.state[3:6], true_vel[k, :], decimal=0) # only test roll and pitch because of wraparound issues numpy.testing.assert_almost_equal(history_rpy[k, 0:2], true_rpy[k, 0:2], decimal=1) if VISUALIZE: self.plot(times, history, history_rpy, true_pos, true_vel, true_rpy) return sim.state, history, times def test_gyro_bias_circle(self): """ test that while flying a circle the location converges """ STEPS = 100000 sim = self.sim model = self.model dT = 1.0 / 666.0 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) 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() # add simulated bias gyro = gyro + numpy.array([0.1, -0.05, 0.15]) 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_accel_bias_circle(self): """ test that while flying a circle the location converges """ STEPS = 100000 sim = self.sim model = self.model dT = 1.0 / 666.0 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) 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() # add simulated bias accel = accel + numpy.array([0.0, 0, 0.2]) 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.2] ) 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 rock_and_turn(self, STEPS=50000): """ test the biases work when rocking and turning. this tests the mag attitude compensation """ sim = self.sim model = self.model dT = 1.0 / 666.0 numpy.random.seed(1) 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)) for k in range(STEPS): model.rock_and_turn(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) if True and k % 60 == 59: sim.correction(pos=true_pos[k, :] + np) if True and k % 60 == 59: sim.correction(vel=true_vel[k, :] + nv) if k % 20 == 8: sim.correction(baro=-true_pos[k, 2] + np[2]) if True and 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 k > 100: numpy.testing.assert_almost_equal(sim.state[0:3], true_pos[k, :], decimal=0) numpy.testing.assert_almost_equal(sim.state[3:6], true_vel[k, :], decimal=0) # only test roll and pitch because of wraparound issues numpy.testing.assert_almost_equal(history_rpy[k, 0:2], true_rpy[k, 0:2], decimal=1) if VISUALIZE: self.plot(times, history, history_rpy, true_pos, true_vel, true_rpy) return sim.state, history, times
class StepTestFunctions(unittest.TestCase): def setUp(self): if C_IMP: self.sim = CINS() else: self.sim = PyINS() self.sim.prepare() def run_step( self, accel=[0.0, 0.0, -CINS.GRAV], gyro=[0.0, 0.0, 0.0], mag=[400, 0, 1600], pos=[0, 0, 0], vel=[0, 0, 0], noise=False, STEPS=200000, CHANGE=6660 * 3, ): """ simulate a static set of inputs and measurements """ sim = self.sim dT = 1.0 / 666.0 # numpy.random.seed(1) history = numpy.zeros((STEPS, 16)) history_rpy = numpy.zeros((STEPS, 3)) times = numpy.zeros((STEPS, 1)) for k in range(STEPS): 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 if k < CHANGE: sim.predict(ng, numpy.array([0, 0, -CINS.GRAV]) + na, dT=dT) else: sim.predict(gyro + ng, accel + na, dT=dT) history[k, :] = sim.state history_rpy[k, :] = quat_rpy(sim.state[6:10]) times[k] = k * dT if True and k % 60 == 59: sim.correction(pos=pos + np) if True and k % 60 == 59: sim.correction(vel=vel + nv) if k % 20 == 8: sim.correction(baro=-pos[2] + np[2]) if True and k % 20 == 15: sim.correction(mag=mag + nm) if VISUALIZE: from numpy import cos, sin import matplotlib.pyplot as plt fig, ax = plt.subplots(2, 2, sharex=True) k = STEPS ax[0][0].cla() ax[0][0].plot(times[0:k:4], 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], 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], 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], history[0:k:4, 10:13], label="Gyro") ax[1][1].plot(times[0:k:4], history[0:k:4, -1], label="Accel") ax[1][1].set_title("Biases") plt.sca(ax[1][1]) plt.ylabel("Bias (rad/s)") plt.xlabel("Time (s)") plt.legend() plt.suptitle(unittest.TestCase.shortDescription(self)) plt.show() return sim.state, history, times def assertState(self, state, pos=[0, 0, 0], vel=[0, 0, 0], rpy=[0, 0, 0], bias=[0, 0, 0, 0, 0, 0]): """ check that the state is near a desired position """ # check position self.assertAlmostEqual(state[0], pos[0], places=1) self.assertAlmostEqual(state[1], pos[1], places=1) self.assertAlmostEqual(state[2], pos[2], places=1) # check velocity self.assertAlmostEqual(state[3], vel[0], places=1) self.assertAlmostEqual(state[4], vel[1], places=1) self.assertAlmostEqual(state[5], vel[2], places=1) # check attitude (in degrees) s_rpy = quat_rpy(state[6:10]) self.assertAlmostEqual(s_rpy[0], rpy[0], places=0) self.assertAlmostEqual(s_rpy[1], rpy[1], places=0) self.assertTrue(abs(math.fmod(s_rpy[2] - rpy[2], 360)) < 1) # check bias terms (gyros and accels) self.assertAlmostEqual(state[10], bias[0], places=2) self.assertAlmostEqual(state[11], bias[1], places=2) self.assertAlmostEqual(state[12], bias[2], places=2) self.assertAlmostEqual(state[13], bias[3], places=2) self.assertAlmostEqual(state[14], bias[4], places=2) self.assertAlmostEqual(state[15], bias[5], places=2) def test_accel_bias(self): """ test convergence with biased accelerometers """ bias = -0.20 state, history, times = self.run_step(accel=[0, 0, -PyINS.GRAV + bias], STEPS=90 * 666) self.assertState(state, bias=[0, 0, 0, 0, 0, bias]) def test_gyro_bias(self): """ test convergence with biased gyros """ state, history, times = self.run_step(gyro=[0.1, -0.05, 0.06], STEPS=90 * 666) self.assertState(state, bias=[0.1, -0.05, 0.06, 0, 0, 0]) def test_gyro_bias_xy_rate(self): """ test gyro xy bias converges within 10% in a fixed time """ TIME = 90 # seconds FS = 666 # sampling rate MAX_ERR = 0.1 BIAS = 10.0 * math.pi / 180.0 state, history, times = self.run_step(gyro=[BIAS, -BIAS, 0], STEPS=TIME * FS) self.assertAlmostEqual(state[10], BIAS, delta=BIAS * MAX_ERR) self.assertAlmostEqual(state[11], -BIAS, delta=BIAS * MAX_ERR) def test_gyro_bias_z_rate(self): """ test gyro z bias converges within 10% in a fixed time """ TIME = 90 # seconds FS = 666 # sampling rate MAX_ERR = 0.1 BIAS = 10.0 * math.pi / 180.0 state, history, times = self.run_step(gyro=[0, 0, BIAS], STEPS=TIME * FS) self.assertAlmostEqual(state[12], BIAS, delta=BIAS * MAX_ERR) def test_accel_bias_z_rate(self): """ test accel z bias converges within 10% in a fixed time """ TIME = 90 # seconds FS = 666 # sampling rate MAX_ERR = 0.1 BIAS = 1 state, history, times = self.run_step(accel=[0, 0, -PyINS.GRAV + BIAS], STEPS=TIME * FS) self.assertAlmostEqual(state[15], BIAS, delta=BIAS * MAX_ERR)