The cubic root unscented kalman filter to estimate the position and orientation of mobile robot trajectory

Received Nov 30, 2019 Revised Apr 26, 2020 Accepted May 6, 2020 In this paper we introduce a cubic root unscented kalman filter (CRUKF) compared to the unscented kalman filter (UKF) for calculating the covariance cubic matrix and covariance matrix within a sensor fusion algorithm to estimate the measurements of an omnidirectional mobile robot trajectory. We study the fusion of the data obtained by the position and orientation with a good precision to localize the robot in an external medium; we apply the techniques of kalman filter (KF) to the estimation of the trajectory. We suppose a movement of mobile robot on a plan in two dimensions. The sensor approach is based on the CRUKF and too on the standard UKF which are modified to handle measurements from the position and orientation. A real-time implementation is done on a three-wheeled omnidirectional mobile robot, using a dynamic model with trajectories. The algorithm is analyzed and validated with simulations.


INTRODUCTION
The mobile robots are equipped with sensors to measure the distance of the robot from the space of the objects where they move. Measurements which are always linked by noises are then fused together in a filter to obtain an estimate of the position and orientation of the mobile robot trajectory in the space and plan [1,2]. The most important problems related to robots are situated in sensors powere by their battery which reduces the autonomy. Also there are situations where sensors cannot function simultaneously, when they use the same frequency band [3,4]. The Kalman filter is an optimal linear estimator when the process noise and the measurement noise can be modeled by white Gaussian noise. Non-linear problems can be solved with the unscented Kalman filter (UKF) and we propose a new algorithm that is called the cubic root unscented Kalman filter (CRUKF) to solve also nonlinear problems. In our work, we use this technique to estimate the position and the angle of the robot in its displacement then a comparison between the performances of the two filters is presented to give an evaluation of more precised measurement and will appreciate better statistical properties [2,5].
This paper is organized as follow; we present the modeling of the mobile robot with the equations of trajectory and angles of the robot. Next, we pass to the presentation of the UKF algorithm and we propose a CRUKF algorithm to improve the accuracy of the state estimate. Finally we present and discuss the simulation results and outline some possible extensions for future investigations.

MODELING OF MOBILE ROBOT
The robot takes the position trajectory T between two moments from sampling with the variation in the position and the direction then we supposes in our paper the omnidirectional mobile robot like shown in  Figure 1. Our robot have three wheels whoos effect on the robot speed because of their weight and friction on the ground, we suppose 1 , 2 and 3 the robot mass applied on the wheels. The omnidirectional mobile Robot has two independently speed-controlled wheels equipped with sensors, and a castor wheel [2,[6][7][8][9][10][11][12]. For such a robot, an approximated, discrete-time model is: where ( , ) is the position of the robot at time , with = ( is the radius of the wheels and L is the length of the axes. The omnidirectional mobile robot is equipped with sensors to measure its position and its orientation like shown in Figure 2. The figure also defines the environment of the robot, assumed perfectly known [7,10,13]. In our work we propose the trajectory T on which we apply the technical of Kalman filters CRUKF and UKF to estimate this trajectory. ( 1 + 2 + 3 )/3 L; then = 0,0083 g.
We have our model: and we make square: For the trajectory T we take: 1 = 0, 2 = 0 and 3 = 0 Then the final equation of T is: + We have

The angle of robot
) is the angle of robot with axis, or can we find this angle by (3): is the angle of robot with wheels axis

UNSCENTED KALMAN FILTER AND HER ALGORITHM
The UKF has been developed in recent years to overcome two main problems of the EKF (extended Kalman filter), the poor approximation properties of the first order approximation and the requirement for the noises to be Gaussian. The basic idea behind the UKF is that of finding a transformation that allows approximating the mean and the covariance of a random vector of length when it is transformed by a nonlinear map. This is done by computing a set of 2 + 1 points, called sigma points, on the basis of the mean and variance of the original vector, transforming these points by the nonlinear map and then approximating the mean and variance of the transformed vector from the transformed sigma points. We refer to [2][3][4][5][14][15][16][17] for the theoretical aspects.
As for approximating properties of the filter, it has been shown that, while the EKF estimate of the state is accurate to the first order, the UKF estimate is accurate to the third order in the case of Gaussian noises. The covariance estimate also is accurate to the first order for the EKF, and to the second order for the UKF and her algorithm is:

THE CUBIC ROOT UKF (CRUKF) PROPOSITION
In our paper we propose the cubic root CRUKF algorithm which is based on square root UKF algorithm [18,19]. So we use in our algorithm a proposition of new function ( ) and UKF algorithm [4]. We study the coefficients of covariance matrix to introduce the cubic root. In this technique, we based on the same principals of the UKF algorithm.

The function qq(x) proposition
The new function ( ) is the quadrature quadrature or orthogonal orthogonal function for find and calculates two matrixes quadrature or orthogonal and its equation is:  is the dimension of the augmented state-vector 0 < < 1 is the primary scaling factor determining the extent of the spread of the sigma-points around the prior mean. Typical range for is 1 − 3 < < 1. is a secondary scaling factor used to emphasize the weighting on the zeros sigma-point for the posterior covariance calculation. can be used to minimize certain higher-order error terms based on known moments of the prior . For Gaussian priors, = 2 is optimal. is a tertiary scaling factor and is usually set equal to 0. In general, the optimal values of these scaling parameters will be a specific problem [2,18,19,[20][21][22][23][24][25]. . / : Efficient least-squares pseudo inverse implemented using orthogonal decomposition with pivoting finally this parameters for two filters CRUKF and UKF.

SIMULATION RESULTS
We compare the performance of CRUKF and UKF in our paper, we starte from the point of trajectory T : ( 0 , 0 ) = (1, 1) with 0 = 0. In the chosen sample time = 1s and each trajectory has been completed in = 400 , imposing a profile to the angular velocities of the wheels. The parameters and weights used in the CRUKF and UKF are: = 10 −4 ; = 5 ; = (8 -)/2 ; = 2 ( + ) -. = 3 minimizes the error of the a posteriori covariance when the random vector is Gaussian. The comparison between CRUKF and UKF at our case as follows: we start with time, we take three values for k to each algorithm. Table 1 show the comparison and the simulation results show in different figures: Figures 3-6. Figures 3 and 4 show the simulation results using the UKF and CRUKF applied to the trajectory estimation and errors. Figure 3 shows the robot trajectory with its estimate using UKF (red) compared to the CRUKF (bleu), the poursuit was well done. Figure 4 presents the estimation error between the original trajectory and both of the two filters also the MSE (mean square error) of the algorithems, it can be seen the CRUKF was better then the UKF.  Figure 5 shows the angle estimate of the robot using the two filters. Figure 6 exhibits estimation errors of these filters where the UKF was here better than the CRUKF.

CONCLUSION
The analysis of the results in Table 1 is that the two filters perform comparably the position and orientation of the robot, we see that the estimation error of UKF is weak than those the estimation error of CRUKF in angle estimation contrary in the position where the CRUKF was better because the approximating properties of each filter. These two filters, always giving a value of the error covariance π of index which is better than those given by any sensor, and completely close to that obtained when all the sensors are used. Results shows the estimate reference trajectory T and estimate error by CRUKF and UKF too MSE of this case and the estimate of the robot angle and its estimate error by CRUKF and UKF too MSE of this case, we see that the estimations were optimal. In the near future this technique of work can be done and develop on other types of robots and other applications with less sensor but with a great measuring accuracy in realtime but its better to make one filter complete the other.