test_imu_state.py 文件源码

def test_G(self):
        q_hat = np.array([0.0, 0.0, 0.0, 1.0]).reshape((4, 1))
        G = self.imu_state.G(q_hat)

        # -- First row --
        self.assertTrue(np_equal(G[0:3, 0:3], -np.ones((3, 3))))
        # -- Second row --
        self.assertTrue(np_equal(G[3:6, 3:6], np.ones((3, 3))))
        # -- Third row --
        self.assertTrue(np_equal(G[6:9, 6:9], -C(q_hat).T))
        # -- Fourth row --
        self.assertTrue(np_equal(G[9:12, 9:12], np.ones((3, 3))))

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