Branch to test trilateration

Too lazy/drunk to transfer manually.
Getting closer maybe?

Who knows.
Fixed some things

So there is still an issue with getting the actual routers but
everything should be working in the correct order.
Wait for signal list
Merge pull request #1 from bliu34/trilateration_test

Trilateration test