def test_quat2rotmat(self):
result = quat.quat2rotmat(array([0, 0, 0.1]))
correct = array([[0.98, -0.19899749, 0.], [0.19899749, 0.98, 0.],
[0., 0., 1.]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.quat2rotmat([[0, 0, 0.1], [0, 0.1, 0]])
correct = array(
[[0.98, -0.19899749, 0., 0.19899749, 0.98, 0., 0., 0., 1.],
[0.98, 0., 0.19899749, 0., 1., 0., -0.19899749, 0., 0.98]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
def test_quat2rotmat(self):
result = quat.quat2rotmat(array([0, 0, 0.1]))
correct = array([[ 0.98 , -0.19899749, 0. ],
[ 0.19899749, 0.98 , 0. ],
[ 0. , 0. , 1. ]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
result = quat.quat2rotmat([[0, 0, 0.1], [0, 0.1, 0]])
correct = array([[ 0.98 , -0.19899749, 0. , 0.19899749, 0.98 ,
0. , 0. , 0. , 1. ],
[ 0.98 , 0. , 0.19899749, 0. , 1. ,
0. , -0.19899749, 0. , 0.98 ]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
def estimate_rot(data_num=1):
# Read the data
imu_data = read_data_imu(data_num)
curr_state = np.asarray([1, 0, 0, 0, 0, 0, 0])
ts = np.squeeze(imu_data['ts'])
real_measurement = np.squeeze(imu_data['vals']).transpose()
size_ts = ts.shape[0]
sigma = Sigma_pts()
params(sigma, data_num)
rpy = []
for i in range(1, size_ts):
# print(i)
dt = ts[i] - ts[i - 1]
sigma.find_points(curr_state, dt)
sigma.find_measurements()
corrected_measurements = convert_measurements(real_measurement[i, :3],
real_measurement[i, 3:])
curr_state = state.kalman_update(sigma, corrected_measurements)
# inv_curr_state = np.array([curr_state, -curr_state[1:4]]).flatten()
# rpy.append(quat.quat2rpy(curr_state[:4]))
rpy.append(
quat.rotmat2rpy(quat.quat2rotmat(curr_state[np.newaxis, :4])))
# plot vicon data
# if data_num in [6,5]:
# rpy = np.asarray(rpy)[-2600 - 700:-700, :]
return np.asarray(rpy)[:, 0], np.asarray(rpy)[:, 1], np.asarray(rpy)[:, 2]
def test_rotmat2quat(self):
result = quat.rotmat2quat(quat.quat2rotmat([0, 0, 0.1]))
correct = array([[0.99498744, 0., 0., 0.1]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
def test_rotmat2quat(self):
result = quat.rotmat2quat(quat.quat2rotmat([0, 0, 0.1]))
correct = array([[ 0.99498744, 0. , 0. , 0.1 ]])
error = norm(result - correct)
self.assertTrue(error < self.delta)