Design and implementation of sensing and estimation software in AggieNav, a small UAV navigation platform