Simple speed trap:
Tie a magnet at the end of a length of string.
Place a residential alarm switch inside the tail of the sub.
Apply a metal plate using tape over the switch.
Extend the plate beyond the prop to protect the string.
Monitor the switch from your PIC.
Then you have a simple repeatable way of calculating distance travelled over time while you tweak your formulas.

Edit: watertight, with no holes to plug up later either.

Robert