Output of robot_pose_ekf jumps uncontrollably [closed]

edit

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 ... (more)