0
$\begingroup$

I have an Ackermann steered robot with optical flow sensor mounted at an offset from base_link x = 0, y = 0.4 , z = 0.15 and robot_state_publisher publishes static transform between base_link as parent and optical_flow_link as child. which outputs odometry in x and y as distances and I am calculating the velocities from the distances. Both verified with manual measurements over known distances and reports reasonably good. Now for yaw, I don't have any other sensor and I am using steering feedback from the actuator (nearly linear relationship) and finding the angle and using to find the Angular velocity (Vyaw) and then delta_theta.

Vyaw = Vx*tan(current_angle)/wheelbase
delta_theta = Vyaw*delta_time
Absolute_theta +=delta_theta

Now, I visualised in rviz for a full 360 drive, it showed a circle so, I continued with this approach. Now,I have also read that this is not the direct approach and the error will be increasing. So, I tried to fuse the information of this odom message with twist fields Vx, Vy and Vyaw and added process_noise_covariance as well. As I said I do not believ fully rely on steering input so I added covariance value of 0.1 in Vyaw. I had good values for Vx abd Vy so added small covariance.

I believe it works according to ROS REP105 and REP103.(correct me if I am missing something here

Now I started slam_toolbox and in the map frame for straight motion the yaw from slam and from EKF starts differs by 2 degrees which sometimes increases and when I turn the robot, it sometimes stays same but some after reversing and turning bit sharper the robot the yaw differs more and NAV2 due to above enters into lethal zones.

In rviz2, I visualize the covariance circle keeps getting larger which I think means its growing without bounds.

Optical flow sensor output with angular velocity computed from steering feedback

  header:
   stamp:
   sec: 1711996544
   nanosec: 364230972
  frame_id: odom
 child_frame_id: base_link
pose:
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance:
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
twist:
  twist:
    linear:
      x: 0.1738306736874741
      y: 0.013906453894997927
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: -0.024044470724701637
  covariance:
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0

Here is the EKF odometry message:

header:
  stamp:
    sec: 1711996556
    nanosec: 323442581
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x: -1.6793282770947027
      y: 0.22047952088394343
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: -0.1492351199844369
      w: 0.9888017389564152
  covariance:
  - 23.880361414199236
  - 50.379086083150106
  - 0.0
  - 0.0
  - 0.0
  - 15.01899733839927
  - 50.3790860831498
  - 127.52964039306863
  - 0.0
  - 0.0
  - 0.0
  - 33.59412032164872
  - 0.0
  - 0.0
  - 2.0435926596699186e-07
  - -2.2435245497600878e-13
  - -4.900634829392862e-12
  - 0.0
  - 0.0
  - 0.0
  - -2.2435245497600872e-13
  - 9.991993723637299e-07
  - -1.3818676719951415e-18
  - 0.0
  - 0.0
  - 0.0
  - -4.900634829392863e-12
  - -1.3818676719950882e-18
  - 9.991993723336081e-07
  - 0.0
  - 15.01899733839927
  - 33.59412032164872
  - 0.0
  - 0.0
  - 0.0
  - 17.329188507970443
twist:
  twist:
    linear:
      x: 0.1546616554043215
      y: -0.021090222967886374
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.013265298939958113
  covariance:
  - 9.999995111518736e-10
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 9.999998026597455e-10
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 9.921069996240334e-07
  - -5.41735905038207e-19
  - -1.1682705572021648e-17
  - 0.0
  - 0.0
  - 0.0
  - -5.417359050382068e-19
  - 8.538481248084917e-07
  - -5.2559712026548233e-23
  - 0.0
  - 0.0
  - 0.0
  - -1.1682705572021646e-17
  - -5.255971202654596e-23
  - 8.538481248084905e-07
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 9.99999809068553e-10

Another thing, my sensor for slam is Intel D435 RGB-D and not lidar which might be not properly suitable for slam_toolbox but it should do basic moves and I tuned slam_toolbox a bit to reduce the jumping of the robot too much from previous runs as well.

I am currently looking to integrate the rtabmap rgb-d odomettry with this and tell the EKF that use rtab's odometry for yaw but when rtab's odometry is lost then use the steering angle odometry until the rgb-d odom is back in action. I tried this but then EKF just uses rtab's odom and when its lost there is sudden big jumps.

My current EKF params.

### ekf config file ###
ekf_filter_node:
    ros__parameters:
        
        frequency: 20.0
        sensor_timeout: 0.05       
        two_d_mode: true

        transform_time_offset: 0.2     
        transform_timeout: 0.1
        print_diagnostics: true

        debug: false
        permit_corrected_publication: false
        publish_acceleration: false
        publish_tf: true
        
        #map_frame: map              # Defaults to "map" if unspecified
        odom_frame: odom            # Defaults to "odom" if unspecified
        base_link_frame: base_link  # Defaults to "base_link" if unspecified
        world_frame: odom           # Defaults to the value of odom_frame if unspecified

        odom0: /optical_flow_odom

        odom0_config: [false,  false,  false,
                       false, false, false,
                       true, true, false,
                       false, false, true,
                       false, false, false]
        
        # odom1: /rtabmap_odom

        # odom1_config: [true,  true,  false,
        #                false, false, true,
        #                false, false, false,
        #                false, false, false,
        #                false, false, false]


        process_noise_covariance: [0.001, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.005, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.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,    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,    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,    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,    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,
                               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,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.04, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.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,    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,    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,    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,    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,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.015]

# dynamic_process_noise_covariance: true

This is the link to the bag file with ekf odometry topic , slam's pose topic and a navigation goal. There is some need to tweak controller params of navigation but this is prior thing I think needs to resolve for a reasonable estimate.

https://github.com/Rak-r/ROS1-ROS2-contents/tree/main/nav2_run_1

Any guidance will be much appreaciated.@automatom

Expected

To run slam with ekf generating the odom to base_link transform. and setup the NAV2 stack

Improve this question
$\endgroup$
3
  • $\begingroup$ Update: I have fused the full odom message (x, y, yaw, vx, vy, vyaw) and checked the slam now outputs the same distance values but the yaw is still hanging a similar situation. What to tune in process noise covariance. Also, I try to add measurement covariance in odometry message with fixed value for twist filed vx, vy, vyaw $\endgroup$
    – Rakshit Soni
    Commented Apr 8 at 11:44
  • $\begingroup$ I don't have the cycles to download your bags and analyse them, I'm afraid. Please add a sample message from every sensor input to the question itself. You should also back up a bit: what are you trying to do? Are you wanting to run SLAM and use the EKF to generate the odom->base_link transform? $\endgroup$
    – automatom
    Commented Apr 16 at 8:28
  • $\begingroup$ I have added the sensor message which I am using. @automatom. Yes, I am trying to setup slam and NAV2 stack. I have been trying with slam_toolbox and rtabmap. But this is the only bit I am getting wrong and trying to figure out. $\endgroup$
    – Rakshit Soni
    Commented Apr 18 at 15:44

1 Answer 1

Reset to default
0
$\begingroup$

OK, so a few things.

I have an Ackermann steered robot with optical flow sensor mounted at an offset from base_link x = 0, y = 0.4 , z = 0.15 and robot_state_publisher publishes static transform between base_link as parent and optical_flow_link as child.

Your optical flow sensor message has a child_frame_id of base_link, and the data reported from that sensor, according to you, is in the optical_flow_link frame. If the message reports velocities in the optical_flow_link frame, then the child_frame_id should be filled out with that frame.

Side note: your optical flow message has no covariance for the velocity data that you're fusing. That's a singular matrix, so the EKF handles that by introducing a very small covariance value for those dimensions. You should fill that data out.

Now I started slam_toolbox and in the map frame for straight motion the yaw from slam and from EKF starts differs by 2 degrees which sometimes increases and when I turn the robot, it sometimes stays same but some after reversing and turning bit sharper the robot the yaw differs more and NAV2 due to above enters into lethal zones.

This is exactly what I'd expect to see. If the odom frame state estimate (as generated by the EKF) exactly matched the map frame state estimate (as provided by the mapping software), there would be no reason to have different coordinate frames. This is specifically called out in REP-105. Your odom frame state estimate will drift from reality over time, but should be continuous. Your map frame state estimate will be globally accurate, but will suffer from discontinuities (e.g., during loop closure). They should separate from each other over time.

Also, the kinematic model used by r_l ends up just being the unicycle model when two_d_mode is true. If your vehicle has Ackermann kinematics, then the integrated velocity is obviously going to differ from reality.

In rviz2, I visualize the covariance circle keeps getting larger which I think means its growing without bounds.

Yes, that's expected for EKF's output in this case. You are fusing only velocity. Velocity gets integrated into pose, and the velocity error gets integrated into pose error.

I am currently looking to integrate the rtabmap rgb-d odomettry with this and tell the EKF that use rtab's odometry for yaw but when rtab's odometry is lost then use the steering angle odometry until the rgb-d odom is back in action. I tried this but then EKF just uses rtab's odom and when its lost there is sudden big jumps.

This isn't something the EKF can do for you. Filters are only as good as the data going into them. In this case, if you want the EKF to trust the optical flow odometry when rtabmap is struggling, then you need rtabmap to (a) know that it's failing, and (b) inflate the covariance in its measurements (or just stop producing estimates).

Improve this answer
$\endgroup$

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.