It's been done.

Guess who?

Google DEVGRU

As Scott mentions, its been done.

However, the general problem that even this approach has is that the
dive flag is typically not directly above you, but some distance away
because you've put some scope on your line (ie, 100ft of line out if
50ft deep, etc), and unless the environment is very consistant with
which way the flag is relative to your position (wind, current, etc),
this will result in a circle of error on your indiciated position that
may not be suitable for your diving needs - - it depends on if your
waypoint is intended to find a big, obvious object (shipwreck) or
something small (particularly critter on an otherwise obscure
coralhead).

been integrated with inertial sensors.  The basic concept is that when
GPS isn't available, the inertial gyros take over to update your
position until such time that the GPS is again available.  There is
some information in the Open Literature on development efforts.

-hh