0
$\begingroup$

Rosanswers logo

Picking up where I left ekf_localization-node-not-responding, I am trying to fuse position in x and y from 2 sonars (Tritech Micron MK III) with (double integrated accelerometer) position from an IMU (xsens MTi 30).

This happens inside a rectangular pool, so each sonar

  • will track a side wall and measure the absolute distance to it (one sonar looking at the short wall, the other at the long wall),
  • we have a distance and a heading attached to it
  • and report x and/or y position either to use as
  • absolute position with x axis || long side of pool, y || short side of pool
  • or relative position to the starting point (differential_integration == true)

(The point being trying to get a quicker update rate compared to doing machine vision on a full scan)

The problem now is: How can I account for the sonar's offset from base_link since the measurements aren't really in the body frame?

I could either

  1. report the data with frame_id odom or map, but how could I account for the mounting offset especially when the sub is not aligned with the side walls.
  2. would that be possible with a single instance of robot_localization, or should then tread them as 'GPS'?
  3. report the data in a Sonar1 and Sonar2 frame and use the current heading of the sonar head (plus the fixed translation) to publish a transform from Sonar1 to base_link
  • This means that I will have to use the attitude solution to track the wall, but that's outside the scope of robot_localization.

I believe the latter is the better approach. Does this make sense or how could it be improved?

Thanks,

Rapha

Wrap up on how to fuse the sonars

I am still working through implementing some of these steps, but I will document the process here.

What are the frames and transforms needed for fusing the sonars:

  • Frames:
  • odom
  • pool
  • sonar1 (will be fused for x only)
  • sonar2 (will be fused for y only)
  • base_link
  • imu
  • Transforms:
  • odom->base_link (generated by ekf_localization)
  • odom->pool (static transform, accounting for rotational offset of pool walls/axes with ENU)
  • pool->sonar1 (broadcasts a translation of the sonar mounting position relative to base_link rotated/trigonometried by the yaw angle of pool->base_link transform)
  • pool->sonar2
  • base_link->imu
  • Data:
  • /sonar/position/x in frame sonar1
  • /sonar/position/y in frame sonar2
  • /imu/data in frame imu
  • /svs/depth in frame svs

WORK IN PROGRESS

EDIT (regarding 1st edit in Answer):

I have set it up the way you recommended and I publish a transform from odom-->sonar every time I publish a new sonar reading (just before). The rotational part of that transform uses the current attitude as published by the robot_localization node.

Robot_localization sees the following input at the moment:

  • attitude from the IMU (imu/data)
  • position from the sonars (sonar/position0 and sonar/position1)

However, after a few seconds the robot_localization node slows down to about 1 update every 5 seconds... EDIT: By 'slows down' I mean the message broadcast rate slows down (rostopic hz /odometry/filtered)

I wasn't sure if using the odom attitude was the right thing to use, so I tried it with the direct attitude solution from the imu.

The robot_localization node still slows down.

My Launchfile is below and I've uploaded a rosbag here. I tried to create a debug file, but no file was created anywhere. Any ideas? Thank you so much,

Rapha

EDIT 2: I am running branch master 3c3da5a537

EDIT 3: I have investigated a little further. You said it might be to do with missing/broken transforms.

When initially posting this question I was publishing the transforms (odom->sonar) upon every execution of the sonar processing node (whenever a raw message from the sonar arrived) --> immediately before publishing the sonar position (in frame sonar) itself. I wasn't sure how ROS handles timestamps and if robot_localization would receive and process the transform before the position update. So I decided to publish the transform whenever the IMU attitude gets published. This happens at 400 Hz and is thus much more often than then sonar position (25-30Hz).

This improved the problem: Instead of slowing down virtually instantly it now runs for a while before slowing down and eventually stopping altogether. Also if I now stop the ekf_localization node and restart it, it will start running again for about ~45s. Stopping the process by ctrl+c takes about 10s to do. When the node is running normally, this is virtually instant.

Sometimes (1/5 tries perhaps) the ekf_localization complains:

 at line 240 in /tmp/buildd/ros-indigo-tf2-0.5.6-0trusty-20141014-0329/src/buffer_core.cpp Error: TF_NAN_INPUT: Ignoring transform for child_frame_id "base_link" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (-nan -nan -nan -nan)

I believe there might be a connection with sometimes erratic data from the sonars, but I can't quite pinpoint it. I'll try improving their accuracy in the meantime.

EDIT 4: After changing the covariance matrix robot_localization does not crash any more. But...

I have looked at more data sources working together and I came across another problem. This very much relates to the initial question: ekf_localization-node-not-responding.

I am fusing the absolute orientation from the IMU differentially. However, when I am looking at the attitude solution generated (and I've done this for the 1st time in degrees... now) the value does not track the change in attitude.

I.e. a 90° change in yaw gets registered as ~3 to 9° and its quite sluggish.

I've tried this with only the IMU publishing data and robot_localization running and without robot_localization.

Would you mind looking to the attached bag files? The files show a -90° rotation about yaw followed by +90 ° about yaw. (Ending up in same orientation)

IMU and robot_localization

IMU by itself

  • The launchfile below is still up to date
  • the IMU publishes in the xsens frame and (as discussed in the other thread)
  • there is a static transform base_link->xsens to account for the mounting.

Thank you very much for your help, Rapha

EDIT 5: Since fixing the covariance matrix robot_localization hasn't crashed anymore.

I am a little bit confused as of how the depth sensor (aka absolute z in odom) and the sonars (absolute x,y in odom) get fused. As per your suggestion I am publishing a transform with the offset of the sensor mounts to base_link and the current attitude solution from robot_localization as the rotation. (I hope I understood you correctly here). I have set ''differential integration = true'' for sonars (x,y in odom), but I don't want to do this for the depth sensor (z in odom).

When only fusing the depth sensor (and only using z in the ekf) I also get small values in the fused x,y. To me this odd, as I thought only the even z measure gets fused, but I can kind of make sense of it: The ekf fuses the untransformed z, then transforms it to base_link which produces x,y distances.

My question is: If I'm trying to fuse the x, y data ''differentially'' integrated, won't the fused x, y position be locked to the value generated from fusing the z measurements?

I could differentially fuse it, which should solve this, but the depth sensor does make for a very nice absolute measurement.

EDIT 5b: 2015-02-11-01-37-30.bag I updated the ekf launchfile below, but the rosbag is without the ekf.

I can currently only move along heave (z) (and due to issues with the buoyancy not that much):

  • We started off slightly submerged,
  • went down along negative heave / z (no change in attitude intended)
  • and up to the surface.

Z aka/svs/depth represents it exactly.

X aka sonar/positionBMT and Y aka sonar/positionUWE should not have changed, but there is a jump in X when surfacing (from ~4m to ~2m).

Thank you very much for your help.

@EDIT 6: Below is the diagram. For the time being the sub's orientation is fixed in relation to the pool walls, but that will change. The sonars are each tracking one of the pool walls.

I also realised 2 things I am doing wrong:

  • I assumed my odom frame is aligned with ENU as well as the pool. Since the pool isn't pointing north this is not true.
  • The sonars measure distance in relation to the long and short side of the pool.
  • I therefore need a pool frame that is rotated from odom, but shares the same origin.
  • The odom->sonarBMT/sonarUWE/SVS transforms I am publishing are wrong I believe:
  • I think I misunderstood Toms instructions in the initial answer. At the moment my transform looks like this:
    • Rotational component has the rotation of odom->base_link
    • Translational component has the distance of base_link to sonarBMT/sonarUWE/SVS measured in base_link
  • I think it should be the a pure translational transform, where the translation is computed from the current robot heading (aka the rotation in pool->base_link) and the mounting position offset in base_link.

Pool Experiment V1

EDIT 6b: I have these new frames and transformations running and the pool->SONAR_UWE as well as pool->SONAR_BMT transforms look good. The Depth data seems to get fused correctly, but neither does the X or Y position from the sonars.

Might the problem be that my X and Y data coming from the sonars is fused separately? After being rotated, each individual measurement is not 'just' x or y. Once in odom it has a non-zero x or y component respectively and is then fused absolute... I might be totally misunderstanding part of the process here though.

EDIT 6c: 6b might have some truth to it. My SONAR_BMTaka the x axis (pool frame) sonar wasn't outputing data. While that is the case rosrun tf tf_echo pool base_link returns a fitting solution and so does rosrun tf tf_echo odom base_link. As soon as the sonar outputs data again the ekf solution outputs rubbish again.

I will try to put the sonar measurements together in one message and try to fuse that.

Launchfile on Github

Originally posted by Raphael Nagel on ROS Answers with karma: 15 on 2015-01-16

Post score: 1

Original comments

Comment by Tom Moore on 2015-01-20:
Any chance you can record a bag file without running robot_localization? I need the tf tree to not have the odom->base_link transform in it.

Comment by Raphael Nagel on 2015-01-20:
No problem: 2015-01-20-19-10-36.bag

Most of the dynamic and static transforms (intentionally) have got a translation of (0,0,0) for the time being

Comment by Tom Moore on 2015-02-02:
Re: edit 4, it sounds like it's going to be a covariance issue. I'll check it out. Any reason you're using the differential setting?

Comment by Tom Moore on 2015-02-11:
Sorry, when you say that X should not have changed when surfacing, do you mean the raw data should not have changed, or that you think the EKF jumped? In the bag file, the /sonar/positionBMT topic changes from ~4 meters to ~2 meters at around 34 seconds into the bag file.

Comment by Raphael Nagel on 2015-02-12:
Sorry, that was a little unclear. It is a sonar processing artifact.The sonar has a vertical beamwidth of 30° and unless the robot is perfectly horizontal it will get a reflection of the water surface if the sub is not fully submerged.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

This is an issue that I have been thinking about for navsat_transform_node. In your case, one option would be to define a transform from odom to sonar that is actually the offset from base_link, but rotated by the robot's current heading. This means the transform could not come from a static transform, but would have to originate from some other node that manages the rotation of the offset and the transform broadcast. Then you can simply report each sonar position measurement in the sonar frame, and ekf_localization_node will transform it before fusing it.

EDIT in response to comments: ekf_localization_node will try to transform all absolute pose data into the frame specified by world_frame parameter. If you make the sonar transform relative to base_link, it will transform the position from sonar to base_link, and then apply the transform to the world_frame. For example, if your robot is at (10, 5) and your sonar is mounted at (1, 1) with respect to the base_link frame, and you get a sonar-based position estimate of (12, 7), then your measurement might get transformed to, say (11, 6) (after transforming to base_link), and then to (21, 11) when the transform to the world_frame is applied. '

EDIT in response to question edit: interesting. I've not seen slowdown occur before. Do you mean the message broadcast rate slows down (rostopic hz /odometry/filtered), or that it only produces an updated state at a slower rate? When I get a chance, I'll check out the bag file. My first guess is that a required transform is failing most of the time, so ekf_localization_node is ignoring the measurement.

EDIT 2: That is strange. I'll check it out and let you know what I find.

EDIT 3: Figured out the issue. Your pose2 topic has an incorrect covariance. You're measuring Y, so the covariance value should be non-zero in the (Y, Y) position, which, in a pose message, is position 7 in the array, not position 2 (position 2 is the covariance between X and Y). As your pose2 data is being reported in a different frame, when the covariance gets rotated into the odom frame, the final measurement covariance matrix that gets fed into the filter is broken. This eventually breaks the covariance for the entire state, and the filter locks up, presumably when trying to invert a singular matrix.

EDIT 4: Yeah, it's a covariance issue. Your yaw variance is ~9 degrees. That won't play well with the differential setting, at least as it is now. It's partially a bug, and partially an issue with unnecessary use of the differential setting. If you have multiple measurement sources for one variable, it's usually best to let one of them be integrated normally (i.e., with differential set to false), and let the others be handled differentially. This is more important for orientation data than for pose data. As of the next release, all differential measurements will be converted to velocities, and the covariances will be handled in a more numerically correct manner, but it will also mean that users should really only use the differential setting when necessary.

EDIT 5: I'd have to see an updated bag file, but if I had to guess, what you're seeing is the inevitable result of variables being correlated in the state transition function and the Jacobian matrix [UPDATE: I no longer think this is true. See edit 6 below]. Z is correlated with Z velocity, and Z velocity is correlated with X and Y position (due to your orientation). The real issue (again, without seeing a bag file) is that for whatever reason, the covariances between your variables are growing to a point where a measurement in one will affect the other. One recommendation I would make in general would be to only use the differential setting when it's absolutely necessary. For example, once I merge my current working branch back into the -devel branches, using the differential setting on absolute orientation data will not be a good idea (unless you have more than one source of that information, and one of them is fused absolutely). In any case, I would advise you to post an updated bag file with all of the data sources running except ekf_localization_node, and then drive the robot around a bit, and then let me know what exactly you did.

EDIT 6: After looking at your data, there are a couple things that I noticed:

  • First, your svs/depth data is being reported in the SVS frame. I ran tf_echo from odom->SVS, and here's a snapshot of what I saw:

     At time 1423618656.761 - Translation: [-0.575, -0.277, 0.033] - Rotation: in Quaternion [-0.015, -0.101, 0.946, 0.308] in RPY [-0.202, -0.034, 2.515] At time 1423618657.766 - Translation: [-0.575, -0.277, 0.033] - Rotation: in Quaternion [-0.015, -0.102, 0.946, 0.308] in RPY [-0.203, -0.035, 2.515] At time 1423618658.769 - Translation: [-0.575, -0.277, 0.033] - Rotation: in Quaternion [-0.015, -0.103, 0.946, 0.308] in RPY [-0.205, -0.035, 2.516] At time 1423618659.741 - Translation: [-0.575, -0.277, 0.033] - Rotation: in Quaternion [-0.015, -0.103, 0.946, 0.308] in RPY [-0.206, -0.035, 2.516]

The issue here is that you have a rotation in that transform (and it appears to be moving). The roll and pitch angles are non-zero, which is why you see X and Y motion when all you do is integrate the depth data. Where is this sensor mounted on your vehicle?

  • As I noted in the comment for your question, your raw sonar data did have a jump from 4 meters to 2 meters when you surfaced. Not sure if you were trying to determine if that was a result of the raw data or ekf_localization_node.

Perhaps I can learn a bit more about your setup. Is the robot always at a fixed orientation with respect to the sides of the pool, or can its heading vary? Where is the (0, 0) point in your pool world frame? Perhaps a diagram would be good to place in your original question.

EDIT 7: Whew, nice diagram. To answer your question, the filter itself doesn't care if you fuse them separately. It's built to let you measure as many variables in each measurement as you want, and they don't need to be synchronized in any way. However, I think what you're getting at is that there are implications to independently rotating each component, because what you really want to do is rotate the coordinate in the pool frame so that it's in the odom frame.

In any case, I want to make sure of a couple things.

  • As your diagram is, I would expect your initial position in the pool frame to be approximately (-4, -2). Is that correct?
  • If you turned off differential integration for your IMU data, then I would expect your odom frame's X-axis to align with whatever your IMU's zero yaw position is (likely magnetic north). I think this is what you've shown. However, I don't see why your odom frame's origin is the same as your pool frame. I would expect your origin in the odom frame to match your robot's start position. In other words, when you start, your position in the odom frame would be (0, 0) and with a heading of whatever your IMU is currently reading.

What you're effectively doing is taking range measurements and trying to use them as absolute position information. If I were you, I would do something like this:

  1. Define transforms from base_link->sonar1 and base_link->sonar2. I am considering base_link to be rigidly affixed to your vehicle's origin. One of the transforms will have just have an +X translation, and the other (it appears) will have a -X translation, and a rotation of +90 degrees yaw.
  2. Now, define a virtual sensor that takes in the raw range measurements, and then applies those transforms. You should now have the two ranges in your base_link frame, and one will be +X (from the long edge sonar), and the other +Y. You can easily test to make sure that the transformed values make sense.
  3. Now, in the event that your robot is not perfectly aligned with the pool, you need to correct those ranges to get the "true" (straight-line) ranges to the edges.
  4. At this point, you have the two straight-line distances from your vehicle's origin to the pool edges. You need to negate both of them.
  5. Now, apply your static pool-to-north rotation.
  6. Set the measurement's frame to odom
  7. Feed it to ekf_localization_node, and do it differentially.

I haven't thought through this in too much detail, but I think it's close to what you want. What you're attempting may work as well, though. Good luck!

Originally posted by Tom Moore with karma: 13689 on 2015-01-16

This answer was ACCEPTED on the original site

Post score: 2

Original comments

Comment by Raphael Nagel on 2015-01-16:
So we need the rotation by the heading to align the sonar axis with the base_link axis, because odom would have the axis at ENU?

Comment by Raphael Nagel on 2015-01-16:
I was thinking of attaching a frame to the sonar beam with a dynamic transform from base_link to sonar, based on the yaw angle between the heading of the sonar beam and the heading of the robot. Would there be a difference in reporting it through base_link->sonar rather than via odom->sonar?

Comment by Raphael Nagel on 2015-01-27:
@EDIT 3: Ahh that makes sense, I confused the position in the covariance matrix with the measurement s to take into account matrix position in the ekf_**.launch file...

I will double check this in the next few days. Thank you so much for your help.

Comment by Tom Moore on 2015-01-27:
No problem! Let me know if you need anything else.

Comment by Raphael Nagel on 2015-02-05:
@EDIT 4: The differential setting was a relict from our previous thread. My IMU does not output the covariances and the driver was assuming some datasheet values. I am now calculating them myself at startup and both differential on/off work fine. Thank you very much.

Comment by Tom Moore on 2015-02-05:
No worries! Let me know if you have more questions.

Comment by Raphael Nagel on 2015-02-13:
@EDIT 7: Most of your assumptions are correct:

  • Yes, the independent rotations were worrying me.
  • approx. (-4,-2) is correct
  • Odom X axis == IMU 0 yaw == magnetic north
  • With differential on that makes sense.

Comment by Raphael Nagel on 2015-02-13:\

  • I am basically doing 1-3 (separate x and y as of this post).
    • The transform in 1 is relative to the pool->base_link angle to account for the robot turning
    • I am trying my luck with a message_filter as my virtual sensor, transforming and repackaging both sonars into one message

Comment by Raphael Nagel on 2015-02-13:
@EDIT 7: It all seems to work!

  • Transforming both sonars by their distance to base_link (given the current base_link heading)
  • stitching them together with a message_filter
  • publishing in the pool frame
  • having a static pool->odom transform Proper testing tomorrow. THANK YOU SO MUCH!!

Comment by Tom Moore on 2015-02-15:
Great! Sounds like a cool robot, by the way. If you find it's behaving after some more testing, would you mind accepting the answer? Thanks!

Comment by Raphael Nagel on 2015-02-15:
Of course I will, thanks for your help!

Comment by Raphael Nagel on 2015-02-17:
All looks good. I will do a little wrap up in my question once I've got some more time. Thank you for your help again!

Comment by Raphael Nagel on 2015-02-22:
The above still stands, but in the meantime here are some videos of the sub working in the pool: Position holding Position holding with disturbances

Improve this answer
$\endgroup$