ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Applying yaw_offset and mag_declination in IMU driver

asked 2018-02-12 01:39:57 -0600

boost gravatar image

Hello,
I am writing an IMU driver (for an IMU that will be fused with GPS using robot_localization) and want to make sure I am applying yaw_offset and mag_declination the same way robot_localization does it so there is no need to set those parameters in robot_localization. The data from the IMU (called raw_imu) has been set to the right ROS convention but forward is North (therefore the need for yaw offset). Is the right way to create a yaw rotation quaternion with the two parameters and then apply that to the raw IMU data as follows?

YAW_OFFSET = 1.570796327
MAG_DECL = -0.1455605   #  at my location declination is 8° 17' E
yaw_transform = tf.transformations.quaternion_from_euler(0, 0, YAW_OFFSET+MAG_DECL)
imu_transformed = tf.transformations.quaternion_multiply(yaw_transform, raw_imu)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2018-02-19 06:50:10 -0600

Tom Moore gravatar image

Should be pretty easy to test using bpython:

>>> from tf import transformations
>>> raw = transformations.quaternion_from_euler(1.0, 1.0, 1.0)
>>> offset = transformations.quaternion_from_euler(0.0, 0.0, 0.3)
>>> print(transformations.euler_from_quaternion(transformations.quaternion_multiply(raw, offset
)))
(1.1668867925697146, 0.7335759265998189, 1.2166525164145243)
>>> print(transformations.euler_from_quaternion(transformations.quaternion_multiply(offset, raw
)))
(1.0000000000000002, 0.9999999999999999, 1.3000000000000003)

So the rotation you want is indeed yaw_transform * raw_imu. This is verified via Wikipedia:

image description

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2018-02-12 01:39:57 -0600

Seen: 344 times

Last updated: Feb 19 '18