Huber-based unscented filtering and its application to vision-based relative navigation

A new algorithm called Huber-based unscented filtering (UF) is derived and applied to estimate the precise relative position, velocity and attitude of two unmanned aerial vehicles in the formation flight. The relative states are estimated using line-of-sight measurements between the vehicles along with acceleration and angular rate measurements of the follower. By making use of the Huber technique to modify the measurement update equations of standard UF, the new filtering could exhibit robustness with respect to deviations from the commonly assumed Gaussian error probability, for which the standard unscented filtering would exhibit severe degradation in estimation accuracy. Furthermore, contrast to standard extended Kalman filtering, more accurate estimation and faster convergence could be achieved from inaccurate initial conditions. During filter design, the global attitude parameterisation is given by a quaternion, whereas a generalised three-dimensional attitude representation is used to define the local attitude error. A multiplicative quaternion-error approach is used to guarantee that quaternion normalisation is maintained in the filter. Simulation results are shown to compare the performance of the new filter with standard UF and standard extended Kalman filtering for non-Gaussian case.