Navsat_Transform + Dual EKF for Gps based autdoor navigation

Course Support Robot Localization With Sensor Data
, , ,
1

Hi,

I am trying to implement GPS based autonomous navigation using dual ekf and navsat transform. Also referred the construct materials but I am unable to refine the implementation
I urgently need help for my project. For weeks , I have been trying to improve , also tried adding amcl to the stack, tried changing yaml config for the fusion but. My dealine is in a week and I have to fix this .I would really appreciate if you someone can guide me asap please!!.

I am attaching a video here as to how the visualization on rviz looks like and the description of the problem is as follows

Issue:

1)The robot position drifts as I move it in its environment and the orientation of the robot on RVIZ is also not facing the correct way when compared to the actual orientation. There is a 180 degrees shift in its orientation.To help you understand the drift, on the video , the robot is actually moving in the direction indicated by the green arrow.

  1. The odometry/gps output from navsat_transform doesnt match odometry/filtered and odometry/filtered_map. While both the local and global odometry (i.e odometry/filtered and odometry/filtered_map match each other). I tried adding the magnetic declination and the yaw_offset (imu/data yaw value was not zero when facing magnetic north) in the navsat_transform parameters.

  2. On RVIZ, the odom frame position and orientaion on TF is not stable, it changes.

Map_frame: map
Odom_frame: odom
Base_link frame: Base_footprint

Please help me fix them.

Here are my file contents

  1. Dual_ekf_navsat.yaml
    ekf_se_odom:
      frequency: 30
      sensor_timeout: 0.1
      two_d_mode: true  #false
      transform_time_offset: 0.0
      transform_timeout: 0.0
      print_diagnostics: true
      debug: false
    
      map_frame: map
      odom_frame: odom
      base_link_frame: base_footprint   #base_link
      world_frame: odom
    
      odom0: odom    #odometry/wheel
      odom0_config: [false, false, false,
                     false, false, false,
                     true,  true,  true,
                     false, false, true,
                     false, false, false]
      odom0_queue_size: 10
      odom0_nodelay: true
      odom0_differential: false
      odom0_relative: false
    
      imu0: imu/data
      imu0_config: [false, false, false,
                    true,  true,  false,
                    false, false, false,
                    true,  true,  true,
                    true,  true,  true]
      imu0_nodelay: false
      imu0_differential: false
      imu0_relative: false
      imu0_queue_size: 10
      imu0_remove_gravitational_acceleration: true

  

    use_control: false
    
      process_noise_covariance: [1e-3, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                 0,    1e-3, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                 0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                 0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                 0,    0,    0,    0,    0.3,  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.5,   0,     0,    0,    0,    0,    0,    0,    0,
                                 0,    0,    0,    0,    0,    0,    0,     0.5,   0,    0,    0,    0,    0,    0,    0,
                                 0,    0,    0,    0,    0,    0,    0,     0,     0.1,  0,    0,    0,    0,    0,    0,
                                 0,    0,    0,    0,    0,    0,    0,     0,     0,    0.3,  0,    0,    0,    0,    0,
                                 0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.3,  0,    0,    0,    0,
                                 0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.3,  0,    0,    0,
                                 0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.3,  0,    0,
                                 0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.3,  0,
                                 0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.3]
    
      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,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                    0,    0,    0,    1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                    0,    0,    0,    0,    1.0,  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,    1.0,  0,    0,    0,     0,     0,     0,    0,    0,
                                    0,    0,    0,    0,    0,    0,    0,    1.0,  0,    0,     0,     0,     0,    0,    0,
                                    0,    0,    0,    0,    0,    0,    0,    0,    1.0,  0,     0,     0,     0,    0,    0,
                                    0,    0,    0,    0,    0,    0,    0,    0,    0,    1.0,   0,     0,     0,    0,    0,
                                    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1.0,   0,     0,    0,    0,
                                    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
                                    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1.0,  0,    0,
                                    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1.0,  0,
                                    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1.0]
    
      

  ekf_se_map:
      frequency: 30
      sensor_timeout: 0.1
      two_d_mode: true  #false
      transform_time_offset: 0.0
      transform_timeout: 0.0
      print_diagnostics: true
      debug: false
    
      map_frame: map
      odom_frame: odom
      base_link_frame: base_footprint    #base_link
      world_frame: map
    
      odom0: odom  #odometry/wheel
      odom0_config: [false, false, false,
                     false, false, false,
                     true,  true,  true,
                     false, false, true,
                     false, false, false]
      odom0_queue_size: 10
      odom0_nodelay: true
      odom0_differential: false
      odom0_relative: false
    
      odom1: odometry/gps
      odom1_config: [true,  true,  false,
                     false, false, false,
                     false, false, false,
                     false, false, false,
                     false, false, false]
      odom1_queue_size: 10
      odom1_nodelay: true
      odom1_differential: false
      odom1_relative: false
    
      imu0: imu/data
      imu0_config: [false, false, false,
                    true,  true,  false,
                    false, false, false,
                    true,  true,  true,
                    true,  true,  true]
      imu0_nodelay: true
      imu0_differential: false
      imu0_relative: false
      imu0_queue_size: 10
      imu0_remove_gravitational_acceleration: true
    
      use_control: false
    
      
    
        process_noise_covariance: [1.0,  0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                     0,    1.0,  0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                     0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                     0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                     0,    0,    0,    0,    0.3,  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.5,   0,     0,    0,    0,    0,    0,    0,    0,
                                     0,    0,    0,    0,    0,    0,    0,     0.5,   0,    0,    0,    0,    0,    0,    0,
                                     0,    0,    0,    0,    0,    0,    0,     0,     0.1,  0,    0,    0,    0,    0,    0,
                                     0,    0,    0,    0,    0,    0,    0,     0,     0,    0.3,  0,    0,    0,    0,    0,
                                     0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.3,  0,    0,    0,    0,
                                     0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.3,  0,    0,    0,
                                     0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.3,  0,    0,
                                     0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.3,  0,
                                     0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.3]
        
          initial_estimate_covariance: [1.0,  0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                        0,    1.0,  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,    1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                        0,    0,    0,    0,    1.0,  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,    1.0,  0,    0,    0,     0,     0,     0,    0,    0,
                                        0,    0,    0,    0,    0,    0,    0,    1.0,  0,    0,     0,     0,     0,    0,    0,
                                        0,    0,    0,    0,    0,    0,    0,    0,    1.0,  0,     0,     0,     0,    0,    0,
                                        0,    0,    0,    0,    0,    0,    0,    0,    0,    1.0,   0,     0,     0,    0,    0,
                                        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1.0,   0,     0,    0,    0,
                                        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
                                        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1.0,  0,    0,
                                        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1.0,  0,
                                        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1.0]
    
    navsat_transform:
      frequency: 30
      delay: 3.0
      magnetic_declination_radians: 0.081681408993  # For lat/long 55.944831, -3.186998
      yaw_offset: 0.795  #1.570796327   IMU reads 0 facing magnetic north, not east
      zero_altitude: true  #false
      broadcast_utm_transform: true
      publish_filtered_gps: true
      use_odometry_yaw: false
      wait_for_datum: false

2)Launch file:

dual_ekf_navsat.launch

<launch>

  <!-- Starting various launch files for starting up the robot --> 
  <include file="$(find innok_heros_driver)/launch/heros_all.launch" />
  <include file="$(find innok_heros_navigation)/launch/innok_AngularBoundsFilter.launch" />
  <include file="$(find innok_heros_navigation)/launch/innok_laserscan_multi_merger.launch" /> 
  
  <!-- Run the Map Server-->
  <include file="$(find gps_module)/launch/start_map_server.launch"/>

  <!-- Run Move Base-->
  <include file="$(find gps_module)/launch/with_map_move_base.launch" />


  <!--Run Navsat_transform and Move_base-->
  <rosparam command="load" file="$(find gps_module)/config/dual_ekf_navsat.yaml" />

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/>

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
    <remap from="odometry/filtered" to="odometry/filtered_map"/>
  </node>

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true">
    <remap from="odometry/filtered" to="odometry/filtered_map"/>
    <remap from="imu/data" to="imu/data"/>
    <remap from="gps/fix" to="fix"/>

  </node>

</launch>

Video link

video – Google Drive

TF_tree

frames
frames3969×1448 651 KB

Node_Graph

rosgraph
rosgraph3136×2400 715 KB

Please tell me how I can fix them

2

Can somebody pls guide me ?|

3

A detailed information of the issue can also be found here :Dual_ekf and navsat_transform for outdoor GPS based navigation - ROS Answers: Open Source Q&A Forum

4

I would really appreciate if anybody could guide me

6

Hi Abhijit,
I’ll try to help you. However, solving that problem through the Forum is almost impossible. It is too complex to be solved like this.

The best way would be to have a rosject that you can share with me, so I can test and check what is wrong. All those files that you posted, are useless if I cannot reproduce the situation, because the error is so complex, that trying to find the error by looking at the files is only for genius, and I’m far from it.

  • So, do you have a simulation for that or are you getting the data directly from the robot?
  • If you have the simulation, can you create a rosject containing the simulation, the instructions to launch everything, and how to reproduce the error. Then share it with me so I can have a look at it? Can you do that?

I promise to give you priority

7

Hi,

Thank you so much for taking time to help me out. Really appreciate it.

I am getting the data directly from the robot.
Please let me know what files or information you might need to reproduce this error and I will compile it and provide it to you.

Thanks again.
Regards,
Abhijit