A general weighted total Kalman filter algorithm with numerical evaluation

An applicable algorithm for Total Kalman Filter (TKF) approach is proposed. Meanwhile, we extend it to the case in which we can consider arbitrary weight matrixes for the observation vector, the random design matrix and possible correlation between them. Also the updated dispersion matrix of the predicted unknown is given. This approach makes use of condition equations and straightforward variance propagation rules. It is applicable to data fusion within a dynamic errors-in-variables (DEIV) model, which usually appears in the determination of the position and attitude of mobile sensors. Then, we apply for the first time the TKF algorithm and its extended version named WTKF to a DEIV model and compare the results. The results show the efficiency of the proposed WTKF algorithm. In particular in the case of large weights, WTKF shows approximately 25% improvement in contrast to TKF approach.