Problem w/ Robot Localization Package (x,y,z): IMU & GPS.

edit

Hello,

I am using the robot localization package together (ekf and navsat_transform_node) to fuse IMU and GPS, where the GPS will only be used to estimate the x, y, z.

So, I have the following tf tree: odom_combined -> base_link -> sensor_frame.

My problem is that if I put the global frame as base_link on RViz, I can see the link changing its orientation correctly (as estimated by the ekf with the IMU input). The problem arises when I put the odom_combined as my global_frame, where the x_y_z is apparently wrong.

Here is my GPS:

Where:

1. the figure on the left is the /odometry/gps (output of navsat and input of ekf);

2. the one in the middle is the /fix topic received by mavros (ArduPilot);

3. and the one on the right is the /odometry/filtered (output of ekf and input of navsat).

I know this isn't of much help, but is does it seem ok? When I recorded this, I started at a fixed position, then I moved the ArduPilot to the left a little and then backwards. Although it makes those curves, which I do not know if it is normal. Is it possible that the problem is on the ArduPilot data and not with my ekf/navsat?

Thank you for your attention. I will leave my navsat/ekf launch file below:

<launch>     

<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
</node>
<param name="robot_description" textfile="$(find obstacle_detection)/description/tripod.urdf" />
<node name="modelvisualization" pkg="rviz" type="rviz" output="screen"/>

<!-- VR Brain Parameters -->
<arg name="fcu_url" default="/dev/ttyUSB0:57600" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<arg name="fcu_protocol" default="v2.0" />
<arg name="respawn_mavros" default="false" />   

<!-- Launch VR Brain -->
<include file="$(find mavros)/launch/apm2.launch">

<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
<arg name="fcu_protocol" value="$(arg fcu_protocol)" />
<arg name="respawn_mavros" default="$(arg respawn_mavros)" />
</include>

<!-- EKF -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_local" clear_params="true">
<param name="frequency" value="30 "/>
<param name="sensor_timeout" value="2"/>

<param name="two_d_mode" value="false"/>

<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom_combined"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom_combined"/>

<param name="transform_time_offset" value="0.0"/>

<param name="odom0" value="/odometry/gps"/>
<param name="imu0" value="/mavros/imu/data"/>

<rosparam param="odom0_config">[true, true, true,
                                false, false, false,
                                false, false, 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_differential" value="false"/> 
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>

<param name="debug"           value="false"/>
<param name="debug_out_file"  value="debug_ekf_localization.txt"/>
</node>

<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true ...