Bar-Shalom, Yaakov Estimation with Applications to Tracking and Navigation: Theory Algorithms and Software