Improved algorithm with direct observation:

Debugging on why range-bearing measurements failed:
-
Attempted algorithm with range-bearing (perfect rnb observation, noisy time update):

-
Attempted algorithm with range-bearing (noisy rnb observation, perfect time update):

Still, problem arise when observation is too noisy.