Zigzag ground track from eight shape flight path

edit

Hi all, I tried to using ukf template recursively with navsat_transform node following http://answers.ros.org/question/20007...

With my eight flight path it seem zigzag ground track as shown in link below.

https://onedrive.live.com/?authkey=%2...

This is my setup launch file.

<launch>
    <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
      <param name="frequency" value="30"/>
      <param name="delay" value="0"/>
      <param name="magnetic_declination_radians" value="0"/>
      <param name="yaw_offset" value="0"/>
      <param name="zero_altitude" value="true"/>
      <param name="broadcast_utm_transform" value="false"/>
      <param name="publish_filtered_gps" value="false"/>
      <param name="use_odometry_yaw" value="false"/>
      <param name="wait_for_datum" value="false"/>

      <remap from="/imu/data" to="/mavros/imu/data"/>
      <remap from="/gps/fix" to="/mavros/global_position/raw/fix"/>
    </node>

  <node pkg="robot_localization" type="ukf_localization_node" name="ukf_se" clear_params="true">
    <rosparam command="load" file="$(find robot_localization)/params/ukf_max.yaml" />
  </node>

</launch>

and this is my paramerter

frequency: 30
sensor_timeout: 0.1
transform_time_offset: 0.0
two_d_mode: false
print_diagnostics: true

# map_frame: map
odom_frame: odom
base_link_frame: fcu
world_frame: odom

alpha: 0.001
kappa: 0
beta: 2

odom0: /odometry/gps
odom0_config: [true, true, true,
               false, false, false,
               false, false, false,
               false, false, false,
               false, false, false]
odom0_differential: false
odom0_relative: false
odom0_queue_size: 2
odom0_nodelay: false

imu0: /mavros/imu/data
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]
imu0_nodelay: false
imu0_queue_size: 5

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 ...