Doppler Velocity Logger Notes and Description

Both the UUV Simulator and the Woods Hole Oceanographic Institute’s (WHOI) Deep Submergence Laboratory (DSL) environment provide Doppler Velocity Logger (DVL) ROS plugins for use in Gazebo: DVLROSPlugin.cc and dsros_dvl_plugin.cc respectively. These notes are an attempt to describe both the plugin interfaces and the methods used in the implementation for both plugins. In addition, a summary of noted fidelity and data gaps between the plugins and the real-world sensors they are intended to emulate is provided.

See whn_dvl_examples for a complete description of the plugin’s use including working examples.

UUV Simulator

Implementation

The UUV Simulator DVL plugin is implemented as a Gazebo model plugin that interacts with the Gazebo world and publishes ROS messages corresponding to perceived readings. Velocity information is derived by adding Gaussian noise to the Gazebo-provided linear velocity of the model with which the plugin is associated. Altitude information is derived from 4 Gazebo RaySensors and RayPlugins corresponding to the individual sonar transducers. A Gazebo RaySensor can be configured to collect multiple samples with user-specified horizontal and vertical fields of view and resolutions. Since the sonar transducers associated with a DVL each provide range only along their centerlines, they should be configured to provide only a single range (e.g., the example below specifies 9 samples in 3.5 degree vertical and horizontal fields of view, but the specified resolution causes them to be combined into a single “return”).

Specification and Configuration

The plugin must be associated with a Gazebo model. The following example is excerpted from a URDF model generated by the Xacro templates discussed in the DVL Examples tutorial:

<!-- DVL physical link--visual, collision, and mass/inertial properties are based on
     vendor-provided specifications for the Teledyne Workhorse Navigator (WHN) 600 -->
<link name="dvl/dvl_link">
  <inertial>
    <mass value="8.8"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <inertia ixx="0.071732122" ixy="0" ixz="0"
	     iyy="0.071732122" iyz="0"
	     izz="0.055786544"/>
  </inertial>
  <visual>
    <geometry>
      <mesh filename="package://dvl_gazebo/meshes/teledyne_whn.dae"/>
    </geometry>
  </visual>
  <collision>
    <geometry>
      <cylinder length="0.2445" radius="0.1126"/>
    </geometry>
    <origin rpy="0 1.57079632679 0" xyz="0 0 0"/>
  </collision>
</link>

<!-- Attachment of the DVL link to the robot (forward bottom here) -->
<joint name="dvl/dvl_joint" type="fixed">
  <origin rpy="0 1.57079632679 0" xyz="1.5 0 -0.5"/>
  <parent link="robot_link"/>
  <child link="dvl/dvl_link"/>
</joint>

<!-- Gazebo model plugin specification with Teledyne WHN parameters -->
<gazebo>
  <plugin filename="libuuv_gazebo_ros_dvl_plugin.so" name="libuuv_gazebo_dvl_plugin">
    <robot_namespace>dvl</robot_namespace>	<!-- ROS topic & link namespace -->
    <link_name>robot_link</link_name>		<!-- link to which the IMU sensor is attached  -->
    <sensor_topic>dvl</sensor_topic> 		<!-- name of the sensor data ROS topic -->
    <update_rate>7</update_rate>		<!-- Hertz (should match beam plugin specs) -->
    <noise_sigma>0.005</noise_sigma>		<!-- Only used for coviariance matrix -->
    <noise_amplitude>0.005</noise_amplitude>	<!-- Gaussian noise added to velocity vector components -->
    <enable_gazebo_messages>false</enable_gazebo_messages>
    <enable_local_ned_frame>false</enable_local_ned_frame>  <!-- Can use if a global NED frame is available -->
    <static_reference_frame>world</static_reference_frame>
    <!-- Declaring the links associated with individual sonar beams (see below)-->
    <beam_link_name_0>dvl/dvl_sonar0_link</beam_link_name_0>
    <beam_link_name_1>dvl/dvl_sonar1_link</beam_link_name_1>
    <beam_link_name_2>dvl/dvl_sonar2_link</beam_link_name_2>
    <beam_link_name_3>dvl/dvl_sonar3_link</beam_link_name_3>
    <!-- Declaring the individual sonar beam output ROS range topics -->
    <beam_topic_0>dvl_sonar0</beam_topic_0>
    <beam_topic_1>dvl_sonar1</beam_topic_1>
    <beam_topic_2>dvl_sonar2</beam_topic_2>
    <beam_topic_3>dvl_sonar3</beam_topic_3>
  </plugin>
</gazebo>

In order to function correctly (i.e., at all), the plugin requires access to 4 Gazebo RaySensors with RayPlugins corresponding to the individual DVL sonar transducers (beams). Individual sonar placement on the DVL and plugin parameterization is not part of the DVL plugin, so their link, joint, sensor, and plugin nodes must be explicitly declared within the model as in the following URDF example:

<!-- Individual sonar beam link -->
<link name="dvl/dvl_sonar0_link">
  <inertial>
    <mass value="0.001"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <inertia ixx="0.000000017" ixy="0" ixz="0"
	     iyy="0.000000017" iyz="0"
	     izz="0.000000017"/>
  </inertial>
  <collision>
    <geometry>
      <cylinder length="1e-06" radius="1e-06"/>
    </geometry>
    <origin rpy="0 1.57079632679 0" xyz="0 0 0"/>
  </collision>
</link>

<!-- Attachment of the sonar beam to the DVL link -->
<joint name="dvl/dvl_sonar0_joint" type="fixed">
  <origin rpy="0 -0.52 0.0" xyz="0 0 0"/> <!-- Location & orientation on DVL link -->
  <parent link="dvl/dvl_link"/>
  <child link="dvl/dvl_sonar0_link"/>
</joint>

<!-- RaySensor and RayPlugin specification -->
<gazebo reference="dvl/dvl_sonar0_link">
  <sensor name="dvl_sonar0" type="ray">
    <pose>0 0 0 0 0 0</pose>
    <update_rate>7</update_rate>	<!-- Hertz -->
    <visualize>true</visualize>		<!-- Show beam in Gazebo GUI -->
    <ray>
      <scan>
        <horizontal>
          <samples>3</samples>		<!-- Number of samples (ray picks) per "slice" -->
          <resolution>1</resolution> 	<!-- Angle btwn reported ranges (only 1 for this sensor) -->
          <min_angle>-0.03</min_angle>	<!-- Beam width (radians) -->
          <max_angle>0.03</max_angle> 	<!-- specified around beam center -->
        </horizontal>
        <vertical>
          <samples>3</samples>   	<!-- Number of horizontal "slices" -->
          <resolution>1</resolution>
          <min_angle>-0.03</min_angle>
          <max_angle>0.03</max_angle>
        </vertical>
      </scan>
      <range>   <!-- Min range, max range, & precision (in meters) -->
        <min>0.7</min>
        <max>90</max>
        <resolution>0.01</resolution>
      </range>
    </ray>
    <!-- Plugin parameters repeat some of the RaySensor parameters -->
    <plugin filename="libgazebo_ros_range.so" name="dvl_sonar0">
      <gaussianNoise>0.005</gaussianNoise>	 <!-- Standard deviation -->
      <alwaysOn>true</alwaysOn>
      <updateRate>7</updateRate>
      <topicName>dvl/dvl_sonar0</topicName>	 <!-- ROS topic that range info is published to -->
      <frameName>dvl/dvl_sonar0_link</frameName> <!-- Ranges relative to this reference frame -->
      <fov>0.06</fov>
      <radiation>ultrasound</radiation>
    </plugin>
  </sensor>
</gazebo>

In addition, the plugin requires access to the ROS transorms associated with the individual sonar beams. These can be published by a separate Gazebo plugin (e.g., one controlling the robot) or by a ROS robot_state_publisher.

ROS Interface

Published Topics

The topic names can be described in the plugin SDF or URDF. For this example, we have the following configuration

    <!-- All DVL topics are published and subscribed to or from this namespace -->
    <robot_namespace>dvl</robot_namespace>
    <!-- Messages are published to this topic and with _ranges appended -->
    <sensor_topic>dvl</sensor_topic>
    <!-- These topics are published to by the individual beam RayPlugins -->
    <beam_topic_0>dvl_sonar0</beam_topic_0>
    <beam_topic_1>dvl_sonar1</beam_topic_1>
    <beam_topic_2>dvl_sonar2</beam_topic_2>
    <beam_topic_3>dvl_sonar3</beam_topic_3>

Custom ROS Messages

The following ROS messages from the uuv_sensor_ros_plugins_msgs package are published by the DVL plugin:

DVL

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id		# Link frame in which velocity is reported
geometry_msgs/Vector3 velocity	# Computed linear velocity vector (m/sec)
  float64 x
  float64 y
  float64 z
float64[9] velocity_covariance			# 3x3 linear velocity covariance matrix
float64 altitude				# Computed altitude above the bottom
uuv_sensor_ros_plugins_msgs/DVLBeam[] beams	# Individual sonar beam values

DVLBeam

float64 range			  # Range (meters) of a single sonar beam
float64 range_covariance	  # Variance of the range value
float64 velocity		  # Doppler velocity of the sonar beam
float64 velocity_covariance	  # Variance of the doppler velocity value
geometry_msgs/PoseStamped pose	  # Pose of the sonar transducer on the DVL
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w

Implementation Description

The UUV Simulator DVL plugin does not directly rely on a Gazebo sensor and does not extend an existing Gazebo plugin. The DVLROSPlugin inherits from the UUV Simulator ROSBaseModelPlugin class which inherits from the UUV Simulator ROSBasePlugin and the standard Gazebo ModelPlugin classes.

Upon load, the plugin subscribes to the individual sonar beam topics specified in the model (see example) and uses a ROS message_filter to force synchronized processing at the model-prescribed rate. It then advertises for publication to the required ROS topics and fills in fields for the ROS messages that will not change during execution (reference frame, velocity covariances).

The plugin attempts to retrieve the transforms for each of the individual sonar links at each model update until it successfully retrieves all four. Once it has obtained these transforms, they do not change.

Individual sonar beam returns are processed simultaneoulsy at the prescribed rate. DVL altitude is computed as the average of all individual beam ranges, and the individual values are applied to the beam-specific uuv_sensor_ros_plugins_msgs/DVLBeam messages that are included in the uuv_sensor_ros_plugins_msgs/DVL message. If the bean transforms are available, they are used to fill in the messages’ pose fields.

Each time the model is updated, the DVL velocity is derived by adding Gaussian noise to the Gazebo-provided linear velocities relative to the robot (link->RelativeLinearVel()). These values are included in the uuv_sensor_ros_plugins_msgs/DVL and sensor_msgs/TwistWithCovarianceStamped messages that are published to the dvl/dvl and dvl/dvl_twist topics respectively.

Fidelity Gaps and Suggestions for Improvement

• Bottom Tracking versus Water Tracking: DVLs can base their velocity solutions on either bottom tracking or water tracking (or mixed which uses bottom tracking if available and water tracking if not). These modes have different error values and current affects the water tracking solution as well. The plugin, however, does not differentiate the two modes, so the error reflects one or the other (or neither). Also, the plugin does not account for current.
• Fixed Noise (Error) Variance: The plugin adds Gaussian noise to the noise-free velocity values based on a fixed (model-specified) standard deviation. Vendor-provide error values for all modeled DVLs specify noise as a percentage of the velocity.
• Single Ping versus Long Term Velocity Solution: The plugin model computes only “single ping” velocities, whereas modeled DVLs provide both single ping and long term velocities. Implementation of a Kalman filter incorporating published single-ping and long-term solution error values will improve the model’s fidelity.
• Individual Sonar Beam Velocities Not Calculated: Despite the inclusion of velocity and velocity_covariance fields in the DVLBeam message, the values are not computed, and the fields will always contain 0s.
• Inaccurate Sensed Altitude: Altitude is computed as the average of the 4 sensor beam ranges and does not account for orientation of the beams on the sensor (“sensed” altitude is, thus, generally high). Further, sonar beams beyond maximum or within minimum range will report the maximum or minimum range rather than an “out-of-range” value. Unless all 4 beams are out of range, the maximum or minimum range values will be incorporated into the average. If all beams report maximum range or all beams report minimum range, an “out-of-range” altitude will be reported for the DVL’s altitude value.
• No Current Profiling: In addition to providing velocity information, DVLs are usually capable of calculating a current profile (i.e., current speed and direction for user-specified layer widths). The plugin, however, does not have this capability. This would probably be a difficult capability to add since it would require interaction with a current plugin that may or may not provide for variation with depth.

WHOI DSL Environment

Implementation

The WHOI DSL environment is externally maintained as a Bitbucket repository and requires both the ds_sim and ds_msgs repositories. NPS-maintained copies of these repositories are provided on the Field-Robotics-Lab Github site. The WHOI DSL environment DVL plugin is implemented as a Gazebo sensor plugin that is associated with a custom Gazebo sensor. Velocity information is derived by calculating a least squares solution (singular value decomposition) to a 4x3 matrix of noisy linear velocities for 4 DVL sonar beams. The plugin adds noise to noise-free beam-specific velocity values provided by the Gazebo DVL sensor.

See whn_dvl_examples for a complete description of the plugin’s use including working examples.

Specification and Configuration

The plugin must be associated with a Gazebo sensor. The following example is excerpted from a URDF model generated by the Xacro templates discussed in the tutorial.

The sensor with which the plugin is associated must be attached to a model link to which the sensor is assigned. The sensor link is typically attached to the robot.

<!-- DVL physical link--visual, collision, and mass/inertial properties are based on
     vendor-provided specifications for the Teledyne Workhorse Navigator (WHN) 600 -->
<link name="dvl_link">
  <inertial>
    <mass value="8.8"/>
    <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
    <inertia ixx="0.071732122" ixy="0" ixz="0"
	     iyy="0.071732122" iyz="0"
	     izz="0.055786544"/>
  </inertial>
  <visual>
    <geometry>
      <mesh filename="package://dvl_gazebo/meshes/teledyne_whn.dae"/>
    </geometry>
    <origin rpy="0 -1.57079632679 0" xyz="0 0 0"/>
  </visual>
  <collision>
    <geometry>
      <cylinder length="0.2445" radius="0.1126"/>
    </geometry>
    <origin rpy="0 -1.57079632679 0" xyz="0 0 0"/>
  </collision>
</link>

<!-- Attachment of the sensor link to the robot (forward bottom here) -->
<joint name="dvl_joint" type="fixed">
  <!-- Rotate around X axis so that positive Z is down -->
  <origin rpy="3.14159265359 0 0" xyz="1.5 0 -0.5"/>
  <parent link="robot_link"/>
  <child link="dvl_link"/>
</joint>

The sensor and plugins are specified in a sensor element that describes the DVL’s characteristics. The sensor implementation requires the plugin from the example—it will not function properly (i.e., at all) if any other plugin filename is specified.

<!-- Gazebo model plugin specification with Teledyne WHN parameters -->
<gazebo reference="dvl_link">
  <sensor name="dvl_sensor" type="dsros_dvl">
    <always_on>1</always_on>
    <update_rate>7.0</update_rate>
    <pose frame="">0 0 0 0 0 0</pose>

    <!-- This plugin specification--must be this type or sensor won't load -->
    <plugin filename="libdsros_ros_dvl.so" name="dvl_sensor_controller">
      <robotNamespace>dvl</robotNamespace>		    <!-- ROS topic namespace -->
      <topicName>dvl</topicName>			    <!-- Topic name of primary DVL sensor output -->
      <rangesTopicName>ranges</rangesTopicName>		    <!-- ROS topic for individual beam range information -->
      <frameName>dvl_link</frameName>			    <!-- Link frame in which velocity is reported -->
      <pointcloudFrame>robot_link</pointcloudFrame>	    <!-- Link frame in which beam point cloud is computed -->

      <!--  WHN 600 parameters (from vendor data sheet) -->
      <updateRateHZ>7.0</updateRateHZ>			    <!-- Hertz -->
      <gaussianNoiseBeamVel>0.005</gaussianNoiseBeamVel>    <!-- Individual sonar velocity Gaussian noise (meters) -->
      <gaussianNoiseBeamRange>0.1</gaussianNoiseBeamRange>  <!-- Individual sonar range Gaussian noise (meters) -->
      <minRange>0.7</minRange>				    <!-- Minimum altitude for bottom tracking -->
      <maxRange>90.0</maxRange>				    <!-- Maximum altitude for bottom tracking -->
      <maxRangeDiff>10</maxRangeDiff>			    <!-- Isn't actually used but is required anyway -->
      <beamAngleDeg>30.0</beamAngleDeg>			    <!-- Individual sonar orientation (degrees) from vertical -->
      <beamWidthDeg>4.0</beamWidthDeg>			    <!-- Individual sonar field of view (degrees) -->
      <beamAzimuthDeg1>-135</beamAzimuthDeg1>		    <!-- First sonar azimuth (degrees) -->
      <beamAzimuthDeg2>135</beamAzimuthDeg2>		    <!-- Second sonar azimuth (degrees) -->
      <beamAzimuthDeg3>45</beamAzimuthDeg3>		    <!-- Third sonar azimuth (degrees) -->
      <beamAzimuthDeg4>-45</beamAzimuthDeg4>		    <!-- Fourth sonar azimuth (degrees) -->
      <pos_z_down>true</pos_z_down>
    </plugin>
  </sensor>
</gazebo>

ROS Interface

Published Topics

The topic names can be described in the plugin SDF or URDF. For this example, we have the following configuration

    <!-- All DVL topics are published and subscribed to or from this namespace -->
    <robotNamespace>dvl</robotNamespace>
    <!-- Messages are published to this topic and with _cloud appended -->
    <topicName>dvl</topicName>
    <!-- Individual beam range information is published to this topic -->
    <rangesTopicName>ranges</rangesTopicName>

Custom ROS Messages

The following ROS messages from the ds_sensor_msgs package are published by the DVL plugin:

Dvl

# Enumerations for tracking modes, coordinate frames,
# and transducer types for use in applicable data fields
uint8 DVL_MODE_BOTTOM=1
uint8 DVL_MODE_WATER=2
uint8 DVL_COORD_BEAM=0
uint8 DVL_COORD_INSTRUMENT=1
uint8 DVL_COORD_SHIP=2
uint8 DVL_COORD_EARTH=3
uint8 DVL_TYPE_PISTON=0
uint8 DVL_TYPE_PHASED_ARRAY=1

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id			# Link frame in which velocity is reported
ds_core_msgs/DsHeader ds_header
  time io_time
  uint8[16] source_uuid
float64 dvl_time
geometry_msgs/Vector3 velocity		# Linear velocity in reported frame (m/sec)
  float64 x
  float64 y
  float64 z
float64[9] velocity_covar		# Linear velocity covariance matrix
float64 altitude			# Sensed altitude above the bottom (meters)
float64 course_gnd			# Course in the reporting frame (degrees) ( atan2(x', y') )
float64 speed_gnd			# Speed in the reporting frame (meters)
uint8 num_good_beams			# Number of sonar beams with valid data
float32 speed_sound			# Speed of sound used in velocity computation
geometry_msgs/Vector3[4] beam_unit_vec	# Orientation unit vector of each sonar beam
  float64 x
  float64 y
  float64 z
float64[4] range			# Raw range reported by each sonar (meters)
float32[4] range_covar			# Raw range variance for each sonar
float32[4] beam_quality			# Beam quality metric for each sonar
float32[4] raw_velocity			# Raw velocity for each sonar (m/sec)
float32[4] raw_velocity_covar		# Raw velocity variance for each sonar
uint8 velocity_mode			# Velocity mode (bottom or water)
uint8 coordinate_mode			# Coordinate mode (instrument, earth, etc.)
uint8 dvl_type				# Type of DVL (piston or phased array)

Ranges3D

# Enumeration for sound speed corrections types
# for use in the applicable data field
uint8 SOUNDSPEED_CORRECTION_NORMAL=0
uint8 SOUNDSPEED_CORRECTION_PHASEDARRAYDVL=1

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id		  # Link frame in which range is reported
ds_core_msgs/DsHeader ds_header
  time io_time
  uint8[16] source_uuid
ds_sensor_msgs/Range3D[] ranges	  # Range information for each sonar
uint8 soundspeed_correction_type  # Sound speed correction type

Range3D

# Enumerations for sensor value quality
uint8 RANGE_INDETERMINANT=0
uint8 RANGE_LOW=1
uint8 RANGE_HIGH=2
uint8 RANGE_VALID=3

geometry_msgs/PointStamped range
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id		# Link frame in which range collision is reported
  geometry_msgs/Point point	# Point where beam collides with the world
    float64 x
    float64 y
    float64 z
float32 range_quality
uint8 range_validity

Implementation Description

The WHOI DSL environment DVL plugin relies on a DsrosDvlSensor, a custom Gazebo sensor that is included in the ds_sim package. Since the DsrosDvlSensor sensor is a custom addition to Gazebo, the library must be loaded when Gazebo is started by adding -s libdsros_sensors.so to the command line (see tutorial for incorporation into a ROS launch file). The DsrosDvlSensor inherits directly from the Gazebo Sensor class. The sensor provides the Gazebo interface for the DVL ROS plugin (dsrosRosDvlSensor) which inherits directly from the Gazebo SensorPlugin class.

Upon load, the DsrosDvlSensor checks for the correct plugin specification (the sensor works only with the dsrosRosDvlSensor from the libdsros_ros_dvl.so library file), sets the minimum range, maximum range, beam angles, beam field of view, bean azimuths, and “pos_z_down” values as specified in the plugin definition. It also instantiates a DsrosDvlBeam object (also provided in the libdsros_sensors.so library) for each sonar transducer and orients it to align with the orientation and azimuth values from the plugin definition.

On each update, the DsrosDvlSensor computes the velocity of the sensor based on the linear and rotational velocities of the link on which it is mounted and its mounting position on the link. This value is passed to an update function for each of the individual sonar DsrosDvlBeam objects. The update function uses a Gazebo RayShape object corresponding to its beam and the physics engine to compute beam’s point of intersection with the world, range to intersection, and the coresponding link. The linear velocity of the point of intersection is computed from the linear and angular velocities of the link and the point of intersection position on the link. Finally, the beam-specific velocity is computed as the dot product of the beam’s unit vector and the relative linear velocity. The DsrosDvlSensor determines a perceived velocity by computing a least-squares solution of the 4x3 matrix of individual beam linear velocities. If any beams do not report valid values (i.e., beyond maximum or within minimum range), no overall velocity solution is provided.

Upon load, the dsrosRosDvlSensor plugin advertises for publication to the DVL sensor information, ranges, and point cloud topics. Upon each update, it obtains the individual sonar beam velocities from the DsrosDvlSensor and adds Gaussian noise as specified in the model. It then computes an “observed” linear velocity vector as a least squares solution of the 4x3 matrix of noise-inclusive individual beam linear velocities. Sensed altitude is computed as the average of z-component (sensor frame) of the valid individual beam ranges. If there are subscribers to the DVL sensor ROS topic, the ds_sensor_msgs/Dvl message is published. Similarly, if there are subscribers to the ranges and/or point cloud topics, the ds_sensor_msgs/Ranges3D and/or geometry_msgs/PointCloud messages are published as well.

Water Tracking

DVL water tracking has been implemented in the WHOI DSL Gazebo sensor and plugin and ROS plugin in the nps_dev branch of the NPS-maintained repository (origin repository pull request pending). Water tracking combines vehicle velocity, ocean current, and water-track-specific noise to provide a realistic water tracking solution when bottom tracking is not available.

Water tracking characteristics are specified in the following elements that are included in the URDF or SDF DVL plugin element. All elements are optional with default values as specified.

<enableWaterTrack>1</enableWaterTrack>  <!-- Default to false (0) -->
<ocean_current_topic>hydrodynamics/current_velocity</ocean_current_topic>  <!-- Gazebo topic--default shown -->
<gaussianNoiseBeamWtrVel>0.0075</gaussianNoiseBeamWtrVel> <!-- Default 2X bottom track noise -->

Water tracking is provided only when a bottom tracking solution is not available due to the sensor’s altitude above the bottom. The use of water tracking is indicated in the ROS ds_sensor_msgs/Dvl message velocity_mode field where 1 indicates bottom tracking and 2 indicates water tracking.

Current-aware water tracking is compatible with the UUV Simulator ocean current plugin (or any Gazebo plugin that publishes to the ocean current topic). The plugin subscribes to the specified ocean current topic and interprets received gazebo::msgs::Vector3d messages as a current velocity vector in Gazebo’s ENU frame.

The Gazebo sensor computes “velocity through the water” by subtracting ocean current velocity from the velocity of the link to which the DVL is attached. This velocity is used to compute individual velocities for all DVL beams as in the bottom tracking implementation. Error-free velocity through the water is computed as a least-squares solution of the 4x3 matrix of individual beam linear water velocities. Noise as specified in the plugin’s SDF element is added to the sensor’s water velocity solution by the ROS plugin prior to publication to the DVL ROS topic.

Fidelity Gaps and Suggestions for Improvement

• Single Ping versus Long Term Velocity Solution: The plugin model computes only “single ping” velocities, whereas modeled DVLs provide both single ping and long term velocities. Implementation of a Kalman filter incorporating published single-ping and long-term solution error values will improve the model’s fidelity.
• Fixed Noise (Error) Variance: The plugin adds Gaussian noise to the noise-free velocity values based on a fixed (model-specified) standard deviation. Vendor-provide error values for all modeled DVLs specify noise as a percentage of the velocity.
• Hard Coded Sensed Altitude Calculation: The calculated altitude is computed as the average of the valid individual beam-specific altitudes. Individual beam altitudes are computed as range * cos(pi/6) regardless of the actual vertical orientation. The induced error is fairly small (typical beam orientations range from 25 to 30 degrees from vertical), but the actual orientation is available making this a fairly straight forward fix.
• Reported Velocity Orientation: The course of the velocity computed velocity vector is calculated as atan2(x', y'). This makes sense for the Gazebo ENU-oriented frame. For a NED-oriented frame, it does not. Further, if the velocity is reported in sensor or robot body-fixed Forward-Right-Down frame it makes even less sense (in the tutorial examples, foward motion is reported as a course of 90 degrees).
• No Current Profiling: In addition to providing velocity information, DVLs are usually capable of calculating a current profile (i.e., current speed and direction for user-specified layer widths). The plugin, however, does not have this capability. This would probably be a difficult capability to add since it would require interaction with a current plugin that may or may not provide for variation with depth.