Joseph covariance formula adaptation to Square-Root Sigma-Point Kalman filters

The development of a reliable Sense And Avoid (SAA) system is one of the limiting aspects for the integration into civil airspace of unmanned aerial vehicles, for which the market demand is becoming viral in many fields. To overcome these limitations, it is required that the SAA system perform equally or even better than the human eye. This can be achieved integrating information from different sensors using data fusion algorithms, like Bayesian estimators or neural network techniques. SAA system degradation could arise from both single sensor shortcomings and bad numerical behaviours, injected by the specific fusion algorithm, such as real machine roundoff errors or divergences introduced by approximating strongly nonlinear functions. An alternative formulation of the Square-Root unscented Kalman filter (SRUKF) based on the Joseph form of the state covariance update step, is used in order to avoid numerical instabilities induced by ill-conditioned matrix problems. The novelty of this technique lies in the exploitation of the Sigma-Point Kalman filters, which ensure a higher-order accuracy in the nonlinear inference problem solving, and in the application of the Joseph update equation, which improves numerical robustness. Moreover, this approach prevents the algorithm from failing, avoiding the downdating process of the Cholesky factor when the SRUKF is used and to take advantage by the higher numerical stability assured by a lower matrix condition number.