A GPS inspired Terrain Referenced Navigation algorithm