1. further implement/copy the functions in geo.h and geo.cpp
2. sync the state machine in quad_swarm and GCS side.
3. add update_ned_coor_by_ecef_coor() in Ground_Station

TODO:
1. test whether the state machine work properly
2. check whethe the update of position work properly
3. check whether the geodetic functions work properly