I am working with a waterlinked DVL integrated into my blueROV and I am wondering if the rotational velocity of the vehicle is incorporated in any way when reporting vehicle velocity to the EKF onboard?

For example, if the vehicle is spinning, the translational velocity should ideally be zero if the coordinate system origin is the center of the vehicle. My DVL is offset by 6 inches or so, so rotational velocity produces significant velocity reading which is not technically correct to report to EKF

1 Like

After looking into this a bit deeper, it seems that even though waterlinked DVL obviously has a gyro, they do not expose that in their api. Would it be possible to add parameters for DVL offset that can factor into the EKF calculation?

How do we feel about this change? I donâ€™t have access to my vehicle today, but I can try it tomorrow. My goal is to be able to rotate the vehicle in yaw and have the position_delta be reported at zero. As the code was, rotating the vehicle would induce a surge/sway velocity, but this should account for that. I would like to remove hard-coded position offsets and add as params, but thatâ€™s not my concern at the moment

``````def handle_velocity(self, data: Dict[str, Any]) -> None:
# extract velocity data from the DVL JSON
vx, vy, vz, alt, valid, fom = (
data["vx"],
data["vy"],
data["vz"],
data["altitude"],
data["velocity_valid"],
data["fom"],
)
dt = data["time"] / 1000
dx = dt * vx
dy = dt * vy
dz = dt * vz

# fom is the standard deviation. scaling it to a confidence from 0-100%
# 0 is a very good measurement, 0.4 is considered a inaccurate measurement
_fom_max = 0.4
confidence = 100 * (1 - min(_fom_max, fom) / _fom_max) if valid else 0
# confidence = 100 if valid else 0

if not valid:
return

if self.rangefinder and alt > 0.05:
self.mav.send_rangefinder(alt)

####################### DVL OFFSET CORRECTION #######################
# Add correction for DVL offset
# extract the roll, pitch and yaw speed from the attitude message in radians
attitudeRates = np.array([attitude[axis] for axis in ("rollspeed", "pitchspeed", "yawspeed")])
# define DVL offset from center of vehicle in meters
dvlOffsetBODYCS = np.array([-0.1905, -0.1143, 0.1397])
####################### DVL OFFSET CORRECTION #######################

if self.should_send == MessageType.POSITION_DELTA:
dRoll, dPitch, dYaw = [
current_angle - last_angle
for (current_angle, last_angle) in zip(self.current_attitude, self.last_attitude)
]
if self.current_orientation == DVL_DOWN:
# position_delta = [dx, dy, dz]

# calculate the adjusted body coordinate system velocities
position_delta = list( (np.array((vx, vy, vz)) + np.cross(dvlOffsetBODYCS, attitudeRates)) * dt)

attitude_delta = [dRoll, dPitch, dYaw]
elif self.current_orientation == DVL_FORWARD:
position_delta = [dz, dy, -dx]
attitude_delta = [dYaw, dPitch, -dRoll]

self.mav.send_vision(position_delta, attitude_delta, dt=data["time"] * 1e3, confidence=confidence)
elif self.should_send == MessageType.SPEED_ESTIMATE:
velocity = [vx, vy, vz] if self.current_orientation == DVL_DOWN else [vz, vy, -vx]  # DVL_FORWARD

# calculate the adjusted body coordinate system velocities
if self.current_orientation == DVL_DOWN:
velocity = list( (np.array((vx, vy, vz)) + np.cross(dvlOffsetBODYCS, attitudeRates)) )

self.mav.send_vision_speed_estimate(velocity)

self.last_attitude = self.current_attitude
``````

I am struggling to add this to my waterlinked dvl dockerfile. Does anyone have any ideas on this?

Hi @cmarq â€“

I think that ArduSub already handles this offset via the `VISO_POS` parameters: ardupilot/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp at d7ae8b8bb3dbd3a076f832c753feabf38faf9a02 Â· ArduPilot/ardupilot Â· GitHub

Caveat: I havenâ€™t tested this. OTOH, if it doesnâ€™t work, then I would argue that itâ€™s a bug in EKF3.

1 Like

Hi Clyde,

That does look to be a promising. I will have to look at the code more closely and track exactly what is happening there.

Luckily, I was able to rebuilt a new dockerfile that does the rotation appropriately.

Would be nice to confirm for others that this exists in the ardupilot codebase already

1 Like

Ha! Hitting this same wall â€“ was getting ready to start doing my own screw transformations on the velocitiesâ€¦ this looks like a good solution. Going to try tomorrow and will report back. Was seeing about 2% drift error, mainly coming from pure yaw motions (offset DVL sees translations which then get integrated into EKF vs getting nulled out from screw transform).

Does anyone have any recommendation on more precise calibration processes to really dial in what the EKF origin is? I guess itâ€™s in the accel/gyro chips themselvesâ€¦ but the mounting is quite wigglyâ€¦ I see why the fancy FOG based INS payloads are very precise about thisâ€¦