ekf_localization AR-Drone

edit

Hello,

I'm trying to use the ekf_localization node with the AR-Drone and I'm facing some problems. Before using it with the real drone, I'm using a simulator ( https://github.com/dougvk/tum_simulator ), which publishes an /ardrone/imu topic. In order to use odometry data, I've created a node which publishes a Odometry message (nav_msgs) with the frame_id (odom), the child_frame_id (base_link) , the planar velocities vx and vy (obtained from optical flow onboard) and the altitude z. My launch file is:

<launch>

<param name="/use_sim_time" value="true"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
  <param name="frequency" value="30"/>
  <param name="sensor_timeout" value="0.1"/>
  <param name="map_frame" value="map"/>
  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_link"/>
  <param name="world_frame" value="odom"/>
  <param name="odom0" value="/ardrone/Odometry"/>
  <param name="imu0" value="/ardrone/imu"/>
  <rosparam param="odom0_config">[false, false,  true,
                                  false, false, false,
                                  true,  true,  false,
                                  false, false, false,
                                  false, false, false]</rosparam>
  <rosparam param="imu0_config">[false, false, false,
                                 true,  true,  true,
                                 false, false, false,
                                 true,  true,  true,
                                 false, false, false]</rosparam>
  <param name="odom0_queue_size" value="10"/>
  <param name="imu0_queue_size" value="10"/>
  <rosparam param="process_noise_covariance">[...]</rosparam>
  <param name="print_diagnosis" value="true"/>
</node>

</launch>

The filter estimates well the altitude, but the x and y position have huge errors. The drone isn't takeoff and the estimated planar position are growing a lot.

Any help or hint would be appreciated.

Thanks

EDIT: I've recorded a bag file: https://github.com/CesarAsturias/catk...

The drone only takeoff in this file. The topic /ardrone/Odometry is published by the following simple node:

    self.msg = Odometry()
    self.nav_subs = rospy.Subscriber("/ardrone/navdata", Navdata, self.Navdata_callback)
    self.msg_pub = rospy.Publisher('/ardrone/Odometry', Odometry, queue_size=10)
    self.rate = rate
    self.r = rospy.Rate(self.rate)

    while self.msg.header.stamp.secs == "":
        rospy.sleep(0.1)
    rospy.loginfo("Start publishing")

    while not rospy.is_shutdown():
        self.msg_pub.publish(self.msg)
        self.r.sleep()

def Navdata_callback(self, navdata):
    self.msg.header.stamp.secs = navdata.header.stamp.secs
    self.msg.twist.twist.linear.x = navdata.vx / 1000
    self.msg.twist.twist.linear.y = navdata.vy / 1000
    self.msg.pose.pose.position.z = navdata.altd 
    self.msg.header.frame_id = 'odom'
    self.msg.child_frame_id = 'base_link'

I've created this node because the simulator, unlike the ardrone_autonomy package, doesn't publish the odometry message.

I don't know if this is what you want, but here are the messages that the filter takes and the result, in two points of time. Before the quadrotor takeoff:

/ardrone/Odometry:

• header
  • seq: 19969
  • stamp
  • frame_id: odom
• child_frame_id: base_link
• pose
  • pose
    • position
      • x: 0.000000
      • y: 0.000000
      • z: 40.000000
    • orientation
      • x: 0.000000
      • y: 0.000000
      • z: 0.000000
      • w: 0.000000
  • covariance
• twist
  • twist
    • linear
      • x: -0.001639
      • y: 0.000001
      • z: 0.000000
    • angular
  • covariance

/ardrone/ imu_Raw

• header
  • seq: 42180
  • stamp
    • secs: 1081
    • nsecs: 546000000
  • frame_id: ardrone_base_link
• orientation
  • x: -0.000293
  • y: 0.001232
  • z: -0.000024
  • w: 0.999690
• orientation_covariance
  • [0]: 0.001276 ...