0
$\begingroup$

Rosanswers logo

Getting unexpected output from navsat_transform for /gps/filtered topic after being processed through the ekf filter. Coordinates diverge quickly from /gps/fix input topic very fast (likely due to imu drift) but divergence is mostly in negative z direction.

Project background info: Using robot_localization to fuse gps and imu referencing this Tom Moore answer: http://answers.ros.org/question/20007... My setup is the same with a full-size SUV vehicle, Beaglebone Black board, Phidgets imu w/mag, ublox gps, with no odometry sensors. I am running Ubuntu 14.04.4 LTS, deb 4.1.15-ti-rt-r40 armv7l, ros indigo, and robot_localization with the ekf filter. I've calibrated the magnetometer for hard and soft iron on the vehicle and incorporate the factors in the driver. I use an overall launch file that separately calls the devices and robot_localization. REP's 103 and 105 were adhered to with the imu (as far as I can tell), since the raw mag points (X) east at zero and the other checks (Y and Z) work out. Declination is accounted for in the navsat_transform launch file. Navsat and Ekf launch files:

<!-- Launch file for navsat_transform_node -->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">

  <!-- Frequency of the main run loop. -->
  <param name="frequency" value="32"/>
  <param name="delay" value="0"/>
  <param name="magnetic_declination_radians" value="-0.206996"/>
  <param name="yaw_offset" value="0.0"/>
  <param name="zero_altitude" value="false"/>
  <param name="broadcast_utm_transform" value="false"/>
  <param name="publish_filtered_gps" value="true"/>
  <param name="use_odometry_yaw" value="false"/>
  <param name="wait_for_datum" value="false"/>
[55.944904, -3.186693, 0.0, map, base_link] -->
  <!-- Placeholders for input remapping. Set your topic names as the "to" values.  -->
         <remap from="/imu/data" to="/imu/data"/>
         <remap from="/odometry/filtered" to="/odometry/filtered"/>
         <remap from="/gps/fix" to="/gps/fix"/>

  <!-- Placeholders for output remapping.        -->

         <remap from="/odometry/gps" to="/odometry/gps"/>
         <remap from="/gps/filtered" to="/gps/filtered"/>
</node>

`

ekf_localization launch file:

<!-- Launch file for ekf_localization_node -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

  <!-- ======== STANDARD PARAMETERS ======== -->

  <param name="frequency" value="32"/>
  <param name="sensor_timeout" value="0.05"/>
  <param name="two_d_mode" value="false"/>
  <param name="map_frame" value="map"/> 
  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_link"/>
  <param name="world_frame" value="odom"/>
  <param name="transform_time_offset" value="0.0"/>
  <param name="odom0" value="/odometry/gps"/>
  <param name="imu0" value="/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,
                                 true,  false,  true]</rosparam>
  <param name="odom0_differential" value="true"/>
  <param name="imu0_differential" value="false"/>
  <param name="odom0_relative" value="false"/>
  <param name="imu0_relative" value="true"/>
  <param name="imu0_remove_gravitational_acceleration" value="true"/>
  <param name="print_diagnostics" value="true"/>

  <!-- ======== ADVANCED PARAMETERS ======== -->

  <param name="odom0_queue_size" value="10"/>
  <param name="imu0_queue_size" value="10"/>

  <!-- If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to control
       how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
       numeric_limits<double>::max() if unspecified. -->
  <param name="odom0_pose_rejection_threshold" value="5"/>
  <param name="odom0_twist_rejection_threshold" value="1"/>
  <param name="pose0_rejection_threshold" value="2"/>
  <param name="twist0_rejection_threshold" value="1.2"/>
  <param name="imu0_pose_rejection_threshold" value="0.3"/>
  <param name="imu0_twist_rejection_threshold" value="0.1"/>
  <param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/>
  <param name="debug"           value="false"/>
  <!-- Defaults to "robot_localization_debug.txt" if unspecified. -->
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

  <!-- The process noise covariance matrix can be difficult to tune, and can vary for each application, so it
       is exposed as a configuration parameter. The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
       vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if unspecified. -->

[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.08, 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.25, 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]

  <!-- This represents the initial value for the state estimate error covariance matrix. Setting a diagonal value (a
       variance) to a large value will result in early measurements for that variable being accepted quickly. Users should
       take care not to use large values for variables that will not be measured directly. The values are ordered as x, y,
       z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if unspecified. -->

[16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 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, 0.035, 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, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.218e-7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.218e-7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6.5e-6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8.66e-4, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8.66e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0,8.66e4]

  <!--  Placeholder for output topic remapping
  <remap from="odometry/filtered" to=""/>
  -->

</node>

By playing Robot_localization bagfiles, raw (imu/data) data seems of reasonable magnitude and direction. Odometry (odometry/filtered topic) appears ok(??)

bagfiles and launch files are located at this Dropbox link: https://www.dropbox.com/sh/u3kznp4oiygbuan/AADAD_QmU_LieGK3y_sbxR65a?dl=0

But /gps/filtered topic vacillates and starts losing elevation quickly and orientation is off about +20 degrees. I attempted to run a similar run without the magnetic declination, with no change in orientation. I've recalibrated the magnetometer, varied runs with and without yaw offset, and arrived at no yaw offset. The source imu driver (imu_ros_i.cpp) was modified to arrive at the correct enu orientation for REP 103 and is therefore suspect, but looks correct to my eye. I've attempted to vary the noise and initial estimate covariances to no avail.

I cannot seem to modify the launch files nor any other code to reduce divergence of the filtered gps output that is critical to this project and am at a complete standstill.

I would greatly appreciate any insight into any of these issues.

B2256


Originally posted by b2256 on ROS Answers with karma: 162 on 2016-04-26

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Found error. Note ekf launch differential and relative params inadvertently modified. Revised back to false and wacked results go away. Noob mistake.


Originally posted by b2256 with karma: 162 on 2016-05-05

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.