A system and method for self and group location and tracking based on ultra wide band ranging among members of the group is presented. The system comprises an anchored station, a plurality of nodes, each node having at least knowledge of where the node is facing and heading, knowledge of all pair wise distances among all of the plurality of nodes, and ability to exchange information among the nodes and with the anchored station via relays. The system and method find a new position estimate of the group in accordance with an adaptive search process based on constraints of the ultra wide band ranging, and the search process enables extracting directional information and adaptively stabilizing orientation of the group. In one embodiment, adaptively stabilizing the orientation of the group is performed using an orientation-correcting polygon matching process.