test_imu_state.py 文件源码

def test_F(self):
        w_hat = np.array([1.0, 2.0, 3.0])
        q_hat = np.array([0.0, 0.0, 0.0, 1.0])
        a_hat = np.array([1.0, 2.0, 3.0])
        w_G = np.array([0.1, 0.1, 0.1])

        F = self.imu_state.F(w_hat, q_hat, a_hat, w_G)

        # -- First row --
        self.assertTrue(np_equal(F[0:3, 0:3], -skew(w_hat)))
        self.assertTrue(np_equal(F[0:3, 3:6], -np.ones((3, 3))))
        # -- Third Row --
        self.assertTrue(np_equal(F[6:9, 0:3], dot(-C(q_hat).T, skew(a_hat))))
        self.assertTrue(np_equal(F[6:9, 6:9], -2.0 * skew(w_G)))
        self.assertTrue(np_equal(F[6:9, 9:12], -C(q_hat).T))
        self.assertTrue(np_equal(F[6:9, 12:15], -skewsq(w_G)))
        # -- Fifth Row --
        self.assertTrue(np_equal(F[12:15, 6:9], np.ones((3, 3))))

        # Plot matrix
        if self.debug:
            ax = plt.subplot(111)
            ax.matshow(F)
            plt.show()