# robot_localization - IMU setup or other problem?

Hi ROS friends,

I'm building a custom ROS robot using the SenseHat (https://projects.raspberrypi.org/en/projects/getting-started-with-the-sense-hat) for the IMU.

I have followed REP-105 to define a set of frames published by TF2. All frames move consistently as seen in RViz (see image)

I'm now trying to use the EKF filter in robot_localization to fuse odometry and the IMU. After hours of hacking the EKF node doesnt output anything!!

I think my main issue is the IMU configuration and wondering if any expert out there can help me with this!

What the robot is providing at the moment:

• odometry : pose (x,y, theta) and velocity (vx,vy, w)

• TF: odom, baselink, trinibot_imu (imu frame) and depth_frame (for a ToF) (As shown in the image below)

Following transforms are being published:

The IMU frame, trinibot_imu provides a static transform of the IMU relative to base_link like so

imu_tr.header.frame_id = "base_link"
imu_tr.child_frame_id = "trinibot_imu"
imu_tr.transform.translation.x = 0.008
imu_tr.transform.translation.z = 0.084788
imu_tr.transform.translation.y = 0.0
imu_tr.transform.rotation.x = 0
imu_tr.transform.rotation.y = 0
imu_tr.transform.rotation.z = 0
imu_tr.transform.rotation.w = 1


The IMU (sensehat) seemed to be providing a NED transform. The sensehat API only provides Euler angles so I transformed these into a quaternion and then applied a ENU transformation like so:

 quat = tf.transformations.quaternion_from_euler(sense.get_orientation_degrees()['roll']*D2R, \
sense.get_orientation_degrees()['pitch']*D2R, \
sense.get_orientation_degrees()['yaw']*D2R)
quat= quat_NED2ENU(quat)

def quat_NED2ENU(iquat):
a  = np.array([0.707, 0.707, 0, 0 ])
b = np.array([[[iquat[3], -iquat[2], iquat[1], -iquat[0]], \
[iquat[2], iquat[3], -iquat[0], -iquat[1]], \
[-iquat[1], iquat[0], iquat[3], -iquat[2]], \
[iquat[0], iquat[1], iquat[2], iquat[3]]]])

c = np.array([[[0, 0, -0.707, 0.707], \
[0 , 0, 0.707, 0.707], \
[0.707, -0.707, 0, 0], \
[-0.707, -0.707, 0 ,0]]])
return (a.dot(b)).dot(c)


The above seems to transform the NED frame in an ENU frame. When aligning the rear of the robot against magnetic north (using my phone app), the quaternion looks like this and oriented as seen by the trinibot_frame in the image

orientation:
x: -0.00801062127965
y: 0.00182113633242
z: -0.973651144752
w: 0.226565780595


Pitching the robot (clock-wise base_link Y) results in CW rotation around the imu_frame RED axis

Rolling the robot positive (clock-wise base_link X) results in a Counter-clockwise rotation around the imu_frame GREEN axis

Yaw around base_link Y is aligned with the imu yaw rotation

and accelerations of the IMU correspond ;

+9.81 in the Z axis

+9.81 in the Y axis (base_link roll 90 deg)

-9.81 in the X axis (base_link pitch 90 deg)

QUESTION 1: The robot_localization docs states that the IMU can be arbitrarily oriented. However it suggests a body fixed frame for the acceleration vector!!!

Acceleration: Be careful with acceleration data. The state estimation nodes in robot_localization assume that an IMU that is placed in its neutral right-side-up position on a flat surface will: Measure +9.819.81 meters per second squared for the ZZ axis. If the sensor is rolled +9090 degrees (left side up), the acceleration should be +9.819.81 meters per second squared for the YY axis. If the sensor is pitched +9090 degrees (front side down), it should read -9.819.81 meters per second

The config file for the EKF is below:

frequency: 10
sensor_timeout: 2
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: true
debug_out_file: ~/sandbox/external-rospacks/src/trinibot_core/scripts/ekf_debug.txt
publish_tf: true
publish_acceleration: false
map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified

odom0: /trinibot/odometry
odom0_config: [true,  true,  false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]

odom0_queue_size: 100
odom0_nodelay: false
odom0_differential: false
odom0_relative: true

odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1

imu0: /trinibot_sensors/imu
imu0_config: [false, false, false,
true,  true,  true,
false, false , false,
true,  true,  true,
true,  true,  true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 100
imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8                #
imu0_linear_acceleration_rejection_threshold: 0.8  #
imu0_remove_gravitational_acceleration: true

use_control: true
stamped_control: false
control_timeout: 0.2
control_config: [true, false, false, false, false, true]
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]

process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]

initial_estimate_covariance: [1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]


Launch file:

<launch>
<node pkg="trinibot_core" type="tbmotioncontroller.py" args="/dev/ttyACM0 200 1 1" name="robotcontroller" machine="$$(arg machine_bot)" respawn="false" output="screen">    (arg machine_bot)" respawn="false" />
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" machine="$$(arg machine_c2i)" clear_params="true"/> (find trinibot_core)/params/tb_ekf.yaml" />
<node pkg="trinibot_core" type="tbteleopgui.py" name="teleoperation_gui" machine="\$(arg machine_c2i)" respawn="false" output="screen" />

</launch>


Echoing the /diagnostics topic provides;

header:
seq: 1856
stamp:
secs: 1508590386
nsecs: 474118459
frame_id: ''
status:
-
level: 2
name: ekf_se: Filter diagnostic updater
message: Erroneous data or settings detected for a robot_localization state estimation node.
hardware_id: none
values:
-
key: PITCH_configuration
value: Neither PITCH nor its velocity is being measured. This will result in unbounded error growth and erratic filter behavior.
-
key: ROLL_configuration
value: Neither ROLL nor its velocity is being measured. This will result in unbounded error growth and erratic filter behavior.
-
key: X_configuration
value: Neither X nor its velocity is being measured. This will result in unbounded error growth and erratic filter behavior.
-
key: YAW_configuration
value: Neither YAW nor its velocity is being measured. This will result in unbounded error growth and erratic filter behavior.
-
key: Y_configuration
value: Neither Y nor its velocity is being measured. This will result in unbounded error growth and erratic filter behavior.
-
key: Z_configuration
value: Neither Z nor its velocity is being measured. This will result in unbounded error growth and erratic filter behavior.
-
level: 2
name: ekf_se: odometry/filtered topic status
message: No events recorded.
hardware_id: none
values:
-
key: Events in window
value: 0
-
key: Events since startup
value: 0
-
key: Duration of window (s)
value: 10.166661
-
key: Actual frequency (Hz)
value: 0.000000
-
key: Minimum acceptable frequency (Hz)
value: 25.200000
-
key: Maximum acceptable frequency (Hz)
value: 35.200000


Is there something missing here? e.g. do I need to use /set_pose or some other params to get this working. I've read the docs over and over again, but cant progress!

Originally posted by fastestindian on ROS Answers with karma: 131 on 2017-10-21

Post score: 1

Some quick thoughts:

1. Turn off debug mode.
2. Turn off your rejection thresholds, all of them (and then make sure you clear the parameter server before running again).
3. This won't hurt anything, but you are trying to fuse 3D variables like Z, roll, and pitch, but you have two_d_mode on. Again, that data will just be ignored, but it's not helping you.

My general feedback is that you need to really simplify your configuration. You're using a lot of advanced settings and parameters, but you should really just be starting with the basics. It looks like you've taken the time to get your data streams working, which is great, but I'd start by fusing just wheel odometry until that's working, and then add the IMU.

Also, please post sample messages from each sensor input (wheel odometry too).

Originally posted by Tom Moore with karma: 13689 on 2018-01-18

This answer was ACCEPTED on the original site

Post score: 0