an o ( log n ) algorithm for simultaneous localization and mapping of mobile robots in indoor environments .
Numerical Recipes in C, 2nd Edition.