An extended Kalman filter for estimation of relative position and relative attitude between a pair of uninhabited autonomous vehicles is developed. Line of sight measurements are taken using visual navigation beacons. Simulations are performed for a varying number of beacons. With initial condition errors only on the bias states, the number of beacons has no effect on estimation accuracy. The presence of initial condition errors on any other state requires a minimum of three beacons for convergence with smooth covariance bounds. A filter without any gravity related terms is shown to have the same results as the simulations in which state initial condition errors were added.
An optimal controller is also derived to develop a trajectory which minimizes the estimator covariance of the position states. Quasilinearization is used to solve the unconstrained problem. State constraints are also added using an exponential penalty function. A gradient approach is used to solve the constrained problem. Both techniques yield optimal trajectories that minimize the position covariance.
