I would like to fuse data from a magnetometer, wheel odometry and a GPS sensor using the robot_pose_ekf. Since I would like to take advantage of the fact that both magnetometer and GPS are defined within an absolute reference frame, I switched to a fork by clearpath robotics which is publicly available.
I publish my wheel odometry on topic /odom:
dat = tf.transformations.quaternion_from_euler(0, 0,self.th)
quaternion = Quaternion(dat[0],dat[1],dat[2],dat[3])
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = "odom_combined"
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.pose.covariance= [0.000001, 0, 0, 0, 0, 0,
0, 0.000001, 0, 0, 0, 0,
0, 0, 0.000001, 0, 0, 0,
0, 0, 0, 0.000001, 0, 0,
0, 0, 0, 0, 0.000001, 0,
0, 0, 0, 0, 0, 0.000001]
odom.twist.covariance = [99999, 0, 0, 0, 0, 0,
0, 99999, 0, 0, 0, 0,
0, 0, 99999, 0, 0, 0,
0, 0, 0, 99999, 0, 0,
0, 0, 0, 0, 99999, 0,
0, 0, 0, 0, 0, 99999]
odom.child_frame_id = "base_footprint"
self.odomPub.publish(odom)
My gps in topic /gps
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom_combined"
(x, y) = self.converter.LatLong2UTM(self.lat, self.lon)
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
dat = tf.transformations.quaternion_from_euler(0, 0, 0)
odom.pose.pose.orientation = Quaternion(dat[0],dat[1],dat[2],dat[3])
odom.child_frame_id = "base_footprint"
odom.pose.covariance =[1, 0, 0, 0, 0, 0, # covariance on gps_x
0, 1, 0, 0, 0, 0, # covariance on gps_y
0, 0, 1, 0, 0, 0, # covariance on gps_z
0, 0, 0, 99999, 0, 0, # large covariance on rot x
0, 0, 0, 0, 99999, 0, # large covariance on rot y
0, 0, 0, 0, 0, 99999] # large covariance on rot z
odom.twist.covariance = [99999, 0, 0, 0, 0, 0,
0, 99999, 0, 0, 0, 0,
0, 0, 99999, 0, 0, 0,
0, 0, 0, 99999, 0, 0,
0, 0, 0, 0, 99999, 0,
0, 0, 0, 0, 0, 99999]
self.pub.publish(odom)
and my compass data in /imu_data
im = Imu()
ori = tf.transformations.quaternion_from_euler(0, 0, math.radians(bearing))
qua = Quaternion(ori[0],ori[1],ori[2],ori[3])
im.header.frame_id = "imu_data"
im.header.stamp = rospy.Time.now()
im.orientation = qua
im.orientation_covariance = [0.000001, 0, 0,
0, 0.000001, 0,
0, 0, 0.000001]
# Switch others off for now.
im.angular_velocity_covariance = [99999, 0, 0,
0, 99999, 0,
0, 0, 99999]
im.linear_acceleration_covariance = [99999, 0, 0,
0, 99999, 0,
0, 0, 99999]
self.imupub.publish(im)
I've been toying around with the covariance values a lot, but they don't seem to have any influence on my particular problem..
Furthermore, I've had problems with robot_pose_ekf giving me transformation errors, even when I set the header.frame_id of the imu-message to base_footprint:
Could not transform imu message from base_footprint to base_footprint.
Therefore I created a static transform publisher relating the imu_data directly to base_footprint.
Now the EKF is giving me some output, and at the beginning /odom, /odom_combined and /imu converge nicely. As soon as I turn one of the wheels however, the pose output of the EKF begins jumping around uncontrollably (with the heading being updated correctly). It settles on some rather arbitrary looking point when I stop turning the wheel.
Furthermore the EKF reports that diagnostics has discovered a potential problem, which according to the docs is due to /odom and /imu being too far apart. They however are just fine and nicely aligned; it's only the kalman filter that misbehaves.
I've had the same problem with the official github version when attaching the GPS to the /vo topic, which is in fact the reason I switched to another fork.
Any ideas? I've been at this all day.
EDIT: The problem is definitely related to the /odom/pose/orientation calculated from the wheel encoders. If I switch off the compass entirely in the launch file, the filtered position still jumps. If I however set the odom/pose/orientation to a constant (0,0,0) and thus not include my odometry direction measurements, the jumping subsides (both with compass on and off).
Originally posted by Simon Harst on ROS Answers with karma: 35 on 2014-04-03
Post score: 3
