Sigma-Point Filters in Robotic Applications

Sigma-Point Kalman Filters (SPKFs) are popular estimation techniques for high nonlinear system applications. The benefits of using SPKFs include (but not limited to) the following: the easiness of linearizing the nonlinear matrices statistically without the need to use the Jacobian matrices, the ability to handle more uncertainties than the Extended Kalman Filter (EKF), the ability to handle different types of noise, having less computational time than the Particle Filter (PF) and most of the adaptive techniques which makes it suitable for online applications, and having acceptable performance compared to other nonlinear estimation techniques. Therefore, SPKFs are a strong candidate for nonlinear industrial applications, i.e. robotic arm. Controlling a robotic arm is hard and challenging due to the system nature, which includes sinusoidal functions, and the dependency on the sensors’ number, quality, accuracy and functionality. SPKFs provide with a mechanism that reduces the latter issue in terms of numbers of required sensors and their sensitivity. Moreover, they could handle the nonlinearity for a certain degree. This could be used to improve the controller quality while reducing the cost. In this paper, some SPKF algorithms are applied to 4DOF robotic arm that consists of one prismatic joint and three revolute joints (PRRR). Those include the Unscented Kalman Filter (UKF), the Cubature Kalman Filter (CKF), and the Central Differences Kalman Filter (CDKF). This study gives a study of those filters and their responses, stability, robustness, computational time, complexity and convergences in order to obtain the suitable filter for an experimental setup.


Introduction
Robotic applications, especially robotic arm, become widely used in industries due to their simplicity and the ability to do multi-task/multi-function with few numbers of settings and/or arrangements.The problem with such applications is the necessary to apply nonlinear control signals to achieve the desired trajectories.The latter is not easy to be implemented and has several limitations [1]- [3].For example, Sliding Mode Control (SMC) [1] is one of the robust control approaches.However, it suffers from chattering.Although several researches have proposed to eliminate the chattering, the problem is still not fully solved.The limitation of such controllers increases as uncertainties present, i.e. modeling uncertainties and noise.This becomes worse when the number of measurement is less than the number of states.
Filters, especially model based filters [2]- [7], have been used to remove some of those constrains.It is a cheap method that could be used to obtain the unmeasured-hidden-states, and/or it could be used to reduce the noise effect.The optimal solution for such applications in their linear case is the Kalman Filter (KF) [7]- [12].When the system is nonlinear, the KF is modified to be applicable for such applications.Several researches have been developed to overcome this limitation.Those include linearizing the system by Taylor Series Approximation (TSA) up to the first order such as the Perturbation Kalman Filter [9] [13] [14], the Extended Kalman filter (EKF) [8] [15]- [17], and the Iterated Extended Kalman filter (IEKF) [7] [15] [18]- [20], or up to higher order such as the Higher Order Extended Kalman Filter (HOEKF) [15] [21]- [23].The later shows that in order to increase the accuracy of high nonlinear application, TSA is not a suitable approach as it takes long computation time with complicated structure [24].Therefore, different approaches were developed including the combination of KF with intelligent techniques such as [25]- [29], or finding different approaches to approximate the nonlinearity such as the Sigma-Point Kalman Filter (SPKF) [2] [4] [5] and the Particle Filter (PF) [30].The rest of the paper will be divided as the following: Section two includes an introduction to the SPKF including the algorithms used in this paper, UKF, CKF and CDKF.The mathematical model of the PRRR robotic arm application is showed in Section three.Results, discussion and conclusion are listed and discussed in Sections four and five.

The Sigma-Point Kalman Filter
The SPKFs linearize the nonlinear models statistically using weighted linear regression method.This is done by obtaining a certain number of points, referred to as sigma points, from the state neighborhood using the probability distribution function as shown in Figure 1.Those points are projected through the system model, and then combined together using appropriate weights as shown in Figure 2.This provides with a mechanism that covers  the actual mean and covariance without the need to linearize the model by TSA and calculate the Jacobian matrices.Moreover, it accommodates noise disturbances that are not Gaussian [4] [5] [15] [31]- [34].
Several algorithms have been created using the above principle.Although, different approaches were used to derive those algorithms, the general outline remain the same as will be proven in the next subsections.The major differences between those methods could be summarized to the number of the sigma points, how to choose them, and what are the appropriate weights for the combining step.Moreover, they may differ on calculating the covariance matrices [35].Some SPKFs algorithm will be described on the next subsections.

The Unscented Kalman Filter
The Unscented Kalman Filter is a SPKF that has been developed using the unscented transformations.The latter has several form including general unscented [15], simplex unscented [35] [36], and spherical unscented [36] [37], transformations.The structures of the resulting filters are similar and could be summarized by the pseudo code of Table 1, where 1  and 2  are parameters used to select the sigma points for the a priori and a post- eriori estimates, respectively.Those differ from a filter to another and it result on obtaining different sigma points.Consequently, different number of sigma points and different associated weights are obtained.Those are illustrated by Table 2 Go back to Start ////Comments //// q is the number of the sigma point //// draw the sigma points and their weights using Table 2 //// propagate the points through the filter //// combining the sigma points to obtain the a priori estimate //// calculating the a priori covariance matrix //// Redefine the sigma point and their weight from Table 2 to obtain their a priori measurements //// combining the sigma points' measurements to obtain the a priori measurement //// Calculating the output's error covariance matrix //// The correction gain //// Updating the estimate and its covariance matrix //// Repeat Stages Table 2.The differences between the UKF methods [15].
Method ( ) , 1, 2; 1, 2, , As n i ρ is obtained recursively as follows: (number of the states) The statistical regression used in unscented filters provides with better approximation that the Jacobian matrices.It has been proven that UKFs approximates up to a third order TSA for Gaussian distributions [15], and second order TSA for non-Gaussian distributions [31].Both, the simplex and the spherical unscented KFs are used to reduce the computational time; as they use less sigma points.However, their stability is limited for few order of TSA [15] [37].The general UKF provide with better estimation compared to the previous two.However, it has a larger computational time.

The Cubature Kalman Filter
The Cubature Kalman filter (CKF) is derived by using the third-degree cubature rule to numerically approximate the Gaussian-weighted integrals defined as [38] [39]: where W is the weight function and it is Gaussian with the form ( ) ; ; x x σ  , and x σ are the Gaussian's mean and standard deviation.Assuming that the states are Gaussian as well, a scheme similar to the UKF could be obtained.However, due to the Gaussian Nature, the covariance matrices will differ from those obtained from UKF.Those are illustrated by Table 3.

The Central Difference Kalman Filter
The Central Difference Kalman Filter (CDKF), described in [40]- [42], was derived in two major stages.The first stage was to linearize the system model using TSA.In the second stage, the derivatives were replaced with their numerical Stirling's polynomial interpolation forms (NSPI) [43], that is defined as the follow [44]: Table 3. Thepseudocode of the cubature kalman filter [38] [39].
The previous stages result on a scheme that is similar to the weighted regression of the UKF as shown in Table 4.However, it differs from the UKF on how to obtain the sigma points, how to calculate the weights, and how to calculate the covariance matrices.The CDKF has been found to have a superior performance among the other SPKFs [15] [30] [45].Moreover, the CDKF uses one control parameter, cd T , which derived in [45] to have a value of 3 for Gaussian distributions.

PRRR-Mathematical Model
The algorithms in section two are applied to a four DOF robotic arm that consists of one prismatic joint and three revolute joints (PRRR) that is presented by Figure 3 and Figure 4.The model has been derived in [1] and [2], and is summarized as follow.
( ) ( ) ( ) ) ) [ ] ) The system is discretized using the following definition ( ) where s T is the sampling time and it is equal to 0.001 sec.If the states defined as the following.
[ ] And knowing that Then the overall state space could be defined as

Conclusion
This work discussed the benefits of using Sigma-Point Kalman Filters in nonlinear application, i.e.PRRR robotic arm.Three types of SPKFs were used, namely Unscented, Cubature, and Central difference Kalman Filters.Four cases were used: the first and the second cases involved with system with no modeling errors; the third and the fourth cases involved with system injected with uncertainties.The first and the third cases assumed all the states were measured which was not the case in the other cases.The results showed that the filters gave good performance when all the states were measured.Reducing the number of measurements affected the results a little bit.The errors became larger than 10 times of those obtained in case 1 when modeling errors were presented and not all the states were measured.However, the CDKF showed stable performance in all cases.The latter gave an indication to use the CDKF in such applications.

Figure 5 .Table 5 .
Figure 5.The performance of the filters for the third angler velocity, cases 1 and 2.Table 5. Parameters' Value for the robotic arm.Parameter Value Parameter Value Parameter Value 1 m 21.5 kg

3Figure 6 .
Figure 6.The performance of the filters for the fourth angler velocity, cases 1 and 2.

Figure 7 .
Figure 7.The performance of the filters for the fourth angler velocity, case 4.

4 (Figure 8 .
Figure 8.The error in estimating the fourth angular velocity using UKF for all cases.

Figure 9 .
Figure 9.The error in estimating the fourth angular velocity using CKF for all cases.

Figure 10 .
Figure 10.The error in estimating the fourth angular velocity using CDKF for all cases.

Table 6 .
The root mean square error for the filters UKF, CKF and CDKF for case 1.

Table 7 .
The root mean square error for the filters UKF, CKF and CDKF for case 2.

Table 8 .
The root mean square error for the filters UKF, CKF and CDKF for case 3.

Table 9 .
The root mean square error for the filters UKF, CKF and CDKF for case 4.