Finished position_est algorithm, missing interface.

TODO: clean it up, provide a command line interface. Data output?