navigation based on gps not working properly!?

edit

hi

I am usin rtabmap as map provider and using two instances of robot-localization pkg, in order to do navigation based on gps. but as you can see in the following video, there are a lot of jumps in the output. would you please guide me why? I cannot find the problem. I have specified a number of gps waypoints, the vehicle will do autonomous navigation based on gps to catch the specified goals. but the result is not good!

https://www.youtube.com/watch?v=hIvZe...

<arg name="navigation"   default="true"/>
  <arg name="localization" default="false"/>
  <arg name="icp_odometry" default="true"/>
  <arg name="rtabmapviz"   default="true"/>
  <arg name="camera"       default="false"/>
  <arg name="lidar2d"      default="false"/>
  <arg name="lidar3d"      default="true"/>
  <arg name="lidar3d_ray_tracing" default="true"/>
  <arg name="slam2d"       default="true"/>        <!--vaghti active mishe map charkhunee az map 2d joda mishe-->
  <arg name="depth_from_lidar" default="false"/>

  <arg     if="$(arg lidar3d)" name="cell_size" default="0.3"/>
  <arg unless="$(arg lidar3d)" name="cell_size" default="0.05"/>

   <!-- 0=Visual, 1=ICP, 2=Visual+ICP Reg-->
   <!-- 0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) Odom/Strategy-->
   <!--RGBD/ProximityBySpace" type="string" value="true"/> is for Local loop closure detection (using estimated position) with locations in WM-->
  <arg if="$(arg lidar2d)" name="lidar_args" default="
      --Reg/Strategy 1 
      --RGBD/NeighborLinkRefining true 
      --Grid/CellSize $(arg cell_size) 
      --Icp/PointToPlaneRadius 0 
      --Icp/MaxTranslation 1"/>

  <arg if="$(arg lidar3d)" name="lidar_args" default="
      --Reg/Strategy 1 
      --RGBD/NeighborLinkRefining true 
      --RGBD/ProximityBySpace true
      --RGBD/ProximityPathMaxNeighbors 1
      --ICP/PM true 
      --Icp/PMOutlierRatio 0.7 
      --Icp/VoxelSize $(arg cell_size) 
      --Icp/MaxCorrespondenceDistance 1 
      --Icp/PointToPlaneGroundNormalsUp 0.9 
      --Icp/Iterations 10 
      --Icp/Epsilon 0.001 
      --OdomF2M/ScanSubtractRadius $(arg cell_size) 
      --OdomF2M/ScanMaxSize 15000 
      --Grid/ClusterRadius 1 
      --Grid/RangeMax 20 
      --Grid/RayTracing $(arg lidar3d_ray_tracing) 
      --Grid/CellSize $(arg cell_size) 
      --Icp/PointToPlaneRadius 0 
      --Icp/PointToPlaneNormalK 10 
      --Icp/MaxTranslation 1"/>

  <!--- Run rtabmap -->
  <remap from="/rtabmap/grid_map" to="/map"/>
  <include file="$(find rtabmap_ros)/launch/mainrtabmap_stereo.launch">
    <arg     if="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args)" />
    <arg unless="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args) -d" /> <!-- create new map -->
    <arg name="localization"    value="$(arg localization)" />
    <arg name="visual_odometry" value="false" />
    <arg name="approx_sync"     value="$(eval camera or not icp_odometry)" />
    <arg name="imu_topic"       value="/imu/data" />
    <arg unless="$(arg icp_odometry)" name="odom_topic" value="/odometry/filtered"/>    <!--/odometry/filtered_map--><!--for navigation based on gps-->
    <arg name="frame_id"        value="base_link" />
    <arg name="rtabmapviz"      value="$(arg rtabmapviz)" />
    <arg name="gps_topic"       value="/gps/fix"/>

    <!-- 2D LiDAR -->
    <arg name="subscribe_scan"  value="$(arg lidar2d)" />
    <arg if="$(arg lidar2d)" name="scan_topic"  value="/scan" />
    <arg unless="$(arg lidar2d)" name="scan_topic"  value="/scan_not_used" />    


    <!-- 3D LiDAR -->
    <arg name="subscribe_scan_cloud"  value="$(arg lidar3d)" />
    <arg if="$(arg lidar3d)" name="scan_cloud_topic" value="/velodyne_points" />
    <arg unless="$(arg lidar3d)" name="scan_cloud_topic" value="/scan_cloud_not_used" />

    <!-- If camera is used ...