Hello, I have a problem with navigation p3dx. As You can see on a video below. My model moving in random direction. P3dx has a problem with a little teleportation as see on video.

p3dx error video

I have Ubuntu 16.04 and ROS Kinetic

My config files:

amcl.launch

<?xml version="1.0"?>

<launch>
  <arg name="map_file" default="$(find p3dx_control)/map2.yaml"/>
  <arg name="use_map_topic" default="true"/>
  <arg name="scan_topic" default="p3dx/laser/scan" />


  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />


  <node pkg="amcl" type="amcl" name="amcl">
    <remap from="scan" to="mybot/laser/scan"/>
    <param name="odom_frame_id" value="odom"/>
    <param name="odom_model_type" value="diff-corrected"/>
    <param name="base_frame_id" value="base_link"/>
    <param name="update_min_d" value="0.5"/>
    <param name="update_min_a" value="1.0"/>
  </node>

</launch>

tf.launch

<?xml version="1.0"?>

<launch>
<node name="joint_state_publisher" pkg="joint_state_publisher"
type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

</launch>

move_base.launch

<?xml version="1.0"?>

<launch>

  <arg name="no_static_map" default="false"/>
  <arg name="base_global_planner" default="navfn/NavfnROS"/>
<!-- <arg name="base_local_planner" default="dwa_local_planner/DWAPlannerROS"/> -->
  <arg name="base_local_planner" default="base_local_planner/TrajectoryPlannerROS"/>


<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

    <param name="base_global_planner" value="$(arg base_global_planner)"/>
    <param name="base_local_planner" value="$(arg base_local_planner)"/> 
    <rosparam file="$(find p3dx_control)/config/planner.yaml" command="load"/>

    <rosparam file="$(find p3dx_control)/config/costmap_common.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find p3dx_control)/config/costmap_common.yaml" command="load" ns="local_costmap" />


    <rosparam file="$(find p3dx_control)/config/costmap_local.yaml" command="load" ns="local_costmap" />
    <param name="local_costmap/width" value="10.0"/>
    <param name="local_costmap/height" value="10.0"/>


    <rosparam file="$(find p3dx_control)/config/global_costmap.yaml" command="load" ns="global_costmap" unless="$(arg no_static_map)"/>

    <rosparam file="$(find p3dx_control)/config/global_costmap_static.yaml" command="load" ns="global_costmap" if="$(arg no_static_map)"/>
    <param name="global_costmap/width" value="100.0" if="$(arg no_static_map)"/>
    <param name="global_costmap/height" value="100.0" if="$(arg no_static_map)"/>

</node>
</launch>

planner.yaml

controller_frequency: 2.0
recovery_behaviour_enabled: true

NavfnROS:
  allow_unknown: true # Specifies whether or not to allow navfn to create plans that traverse unknown space.
  default_tolerance: 0.1 # A tolerance on the goal point for the planner.

TrajectoryPlannerROS:
  # Robot Configuration Parameters
  acc_lim_x: 2.5
  acc_lim_theta:  3.2

  max_vel_x: 1.0
  min_vel_x: 0.0

  max_vel_theta: 1.0
  min_vel_theta: -1.0
  min_in_place_vel_theta: 0.2

  holonomic_robot: false
  escape_vel: -0.1

  # Goal Tolerance Parameters
  yaw_goal_tolerance: 0.1
  xy_goal_tolerance: 0.1
  latch_xy_goal_tolerance: false

  # Forward Simulation Parameters
  sim_time: 2.0
  sim_granularity: 0.02
  angular_sim_granularity: 0.02
  vx_samples: 6
  vtheta_samples: 20
  controller_frequency: 2.0

  # Trajectory scoring parameters
  meter_scoring: true # Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default (false).
  occdist_scale:  0.1 #The weighting for how much the controller should attempt to avoid obstacles. default 0.01
  pdist_scale: 0.75  #     The weighting for how much the controller should stay close to the path it was given . default 0.6
  gdist_scale: 1.0 #     The weighting for how much the controller should attempt to reach its local goal, also controls speed  default 0.8

  heading_lookahead: 0.325  #How far to look ahead in meters when scoring different in-place-rotation trajectories
  heading_scoring: false  #Whether to score based on the robot's heading to the path or its distance from the path. default false
  heading_scoring_timestep: 0.8   
  dwa: true #Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout
  simple_attractor: false
  publish_cost_grid_pc: true  

  # Oscillation Prevention Parameters
  oscillation_reset_dist: 0.25 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05)
  escape_reset_dist: 0.1
  escape_reset_theta: 0.1

DWAPlannerROS:
  # Robot configuration parameters  
  acc_lim_x: 2.5
  acc_lim_y: 0
  acc_lim_th: 3.2

  max_vel_x: 0.5
  min_vel_x: 0.0
  max_vel_y: 0
  min_vel_y: 0

  max_vel_trans: 0.5
  max_vel_trans: 0.1
  max_vel_theta: 1.0
  max_vel_theta: 0.2

  # Goal Tolerance Parameters
  yaw_goal_tolerance: 0.1
  xy_goal_tolerance: 0.2
  latch_xy_goal_tolerance: false

costmap_common.yaml

footprint: [[0.07, 0.19], [0.17, 0.08], [0.17, -0.08], [0.07, -0.19], [-0.07, -0.19], [-0.09, -0.16], [-0.19, -0.16], [-0.26, -0.05], [-0.26, 0.05], [-0.19, 0.16], [-0.09, 0.16], [-0.07, 0.19]]
footprint_padding: 0.01

robot_base_frame: base_link
update_frequency: 2.0
publish_frequency: 2.0
transform_tolerance: 0.5

resolution: 0.05

obstacle_range: 5.5
raytrace_range: 6.0

#layer definitions
static:
    map_topic: /map
    subscribe_to_updates: true

obstacles_laser:
    observation_sources: laser
    laser: {data_type: LaserScan, clearing: true, marking: true, topic: /p3dx/laser/scan, inf_is_valid: true}

,
inflation: inflation_radius: 1.0

costmap_local.yaml

global_frame: map
rolling_window: true

plugins:
  - {name: obstacles_laser,           type: "costmap_2d::ObstacleLayer"}
  - {name: inflation,                 type: "costmap_2d::InflationLayer"}

global-costmap.yaml

global_frame: map
rolling_window: false
track_unknown_space: true
static_map: true
plugins:
  - {name: static,                  type: "costmap_2d::StaticLayer"}
  - {name: inflation,               type: "costmap_2d::InflationLayer"}

I knwo that can be less launch files.