Square‐rooting approaches to accurate mixed‐type continuous‐discrete extended and fifth‐degree cubature Kalman filters

IET Radar, Sonar & NavigationVolume 14, Issue 11 p. 1671-1680 Research ArticleFree Access Square-rooting approaches to accurate mixed-type continuous-discrete extended and fifth-degree cubature Kalman filters Maria V. Kulikova, Corresponding Author Maria V. Kulikova maria.kulikova@ist.utl.pt orcid.org/0000-0001-8470-9831 CEMAT (Center for Computational and Stochastic Mathematics), Instituto Superior Técnico, Universidade de Lisboa, Av. Rovisco Pais, 1049-001 Lisboa, PortugalSearch for more papers by this authorGennady Yu. Kulikov, Gennady Yu. Kulikov orcid.org/0000-0002-1663-6009 CEMAT (Center for Computational and Stochastic Mathematics), Instituto Superior Técnico, Universidade de Lisboa, Av. Rovisco Pais, 1049-001 Lisboa, PortugalSearch for more papers by this author Maria V. Kulikova, Corresponding Author Maria V. Kulikova maria.kulikova@ist.utl.pt orcid.org/0000-0001-8470-9831 CEMAT (Center for Computational and Stochastic Mathematics), Instituto Superior Técnico, Universidade de Lisboa, Av. Rovisco Pais, 1049-001 Lisboa, PortugalSearch for more papers by this authorGennady Yu. Kulikov, Gennady Yu. Kulikov orcid.org/0000-0002-1663-6009 CEMAT (Center for Computational and Stochastic Mathematics), Instituto Superior Técnico, Universidade de Lisboa, Av. Rovisco Pais, 1049-001 Lisboa, PortugalSearch for more papers by this author First published: 01 November 2020 https://doi.org/10.1049/iet-rsn.2020.0161Citations: 1AboutSectionsPDF ToolsRequest permissionExport citationAdd to favoritesTrack citation ShareShare Give accessShare full text accessShare full-text accessPlease review our Terms and Conditions of Use and check box below to share full-text version of article.I have read and accept the Wiley Online Library Terms and Conditions of UseShareable LinkUse the link below to share a full-text version of this article with your friends and colleagues. Learn more.Copy URL Share a linkShare onFacebookTwitterLinked InRedditWechat Abstract The fifth-degree cubature Kalman filter (5D-CKF) has been recently developed for both the discrete- and continuous-time non-linear stochastic systems. In the published works, it has been mentioned that numerically stable square-root 5D-CKF implementations are not feasible to derive, although they are easily obtained for the third-degree CKF. The key problem of the underlying mathematical derivation is negative weight coefficients appeared in the fifth-degree cubature rule, which prevents the required square-root factorisation of the filters' equations. The authors resolve this essential problem existed for the 5D-CKF methodology by utilising hyperbolic transformations instead of the usual ones traditionally used for square-rooting in the engineering literature. The authors' solution is given within both the Cholesky and singular value decomposition (SVD) and is based on matrix calculus with the hyperbolic QR and SVD transformations involved. The theoretical results are illustrated in a case of the continuous-discrete mixed-type estimator ACD-EKF-5DCKF and can be applied to any other 5D-CKF strategy. Numerical experiments are also provided. 1 Introduction In this paper, we deal with the modern fifth-degree cubature Kalman filter (5D-CKF) recently proposed for both the discrete- and continuous-time non-linear stochastic systems in [1, 2]. It is worth noting here that the 5D-CKF estimator belongs to the class of non-linear Bayesian filtering methods developed under the local approach discussed in [3]. This means that the non-linear filters are designed by fixing the posterior density to take a priori form (it is usually assumed to be Gaussian). The resulted estimators are fast to execute, although they yield the sub-optimal solution to the non-linear Bayesian filtering problem. The attractive simplicity of local filters, their good estimation quality and practical feasibility make these estimators be preferable methods for solving practical applications; e.g. see the extended Kalman filter (EKF) [4, 5], the unscented Kalman filter (UKF) [6, 7, 8], the quadrature Kalman filter (QKF) [9, 10] and the cubature Kalman filter (CKF) [3, 11]. The cited third-degree cubature rule has been recently extended on the fifth-degree case to obtain the 5D-CKF estimation method. Here, we focus on the continuous-discrete 5D-CKF filter proposed in [2]. For the discussed 5D-CKF methodology, the lack of numerically stable square-root implementations is indicated as one of the essential problems that are still unresolved [2, p. 1230]. Recall, the square-root filtering methods are widely used in the engineering community for improving the numerical robustness of the Kalman filter (KF)-like estimators in a finite precision arithmetics underlying any computational device. The machine precision yields round-off errors that may destroy the theoretical properties (symmetric form and positive definiteness) of the filter error covariance matrices involved. These doubts the results of computations and correctness of the obtained estimates; see the discussion of a square-root filtering approach in textbooks [12, 13, 14] and many other studies. To enhance the reliability and numerical robustness to round-off errors, the Cholesky-based implementations have been designed for the third-degree cubature filters within both the covariance form in [3, 11] and information form in [15, 16]. The comprehensive overview of the existing third-degree CKF implementations can be also found in [17, Chapter 6]. Meanwhile, the square-root solution is still not found for the fifth-degree cubature rule because the traditionally used square-rooting methodology is not feasible in this case. The problem source is in negative weight coefficients utilised in the fifth-degree cubature rule approximation that is not a case for the third-degree one. The same problem exists for the UKF estimation strategy and we briefly discuss it in our work as well. The occurred negative coefficients prevent a mathematical derivation within the classical square-rooting methodology existing in the engineering literature. More precisely, one cannot factorise and express the formulas for computing the filters' covariance matrices in the required square-root form. Additionally, the traditionally used transformations utilised for updating the square-root matrix factors are not valid. In this paper, we suggest a novel methodology for the square-rooting problem that is of special interest when the traditional square-rooting strategy is not feasible. In particular, our solution is based on matrix calculus with the hyperbolic and singular value decomposition (SVD) that should be utilised instead of the usual ones. The novel square-rooting approach suggested in this work has a general form and might be applied to any 5D-CKF estimator within both linearised discretisation and discretised linearisation approximation procedures; see more details in [2]. In this paper, we restrict our self by discretised linearisation approach for designing continuous-discrete filtering methods. This implies a derivation of the corresponding moment differential equations and utilisation of numerical methods derived for solving ordinary differential equations (ODEs), e.g. see the continuous-discrete UKF and the third-degree CKF in [18, 19] as well as the continuous-discrete EKF in [20]. For the illustrative purpose of the novel square-root 5D-CKF rule solution, we design a mixed-type estimator where the time update step is performed according to the accurate continuous-discrete EKF (ACD-EKF) strategy meanwhile the measurement update step follows the novel 5D-CKF methodology. The main benefit of mixed-type algorithms is that they collect the best features of two estimation approaches involved, e.g. see the discussion in [21, 22]. In particular, the accurate implementation of the time update step by using the ACD-EKF approach implies automatic mechanisation for bounding the discretisation error occurred at this stage in line with a user-supplied accuracy tolerance value provided; e.g. see [23]. Such self-adaptive continuous-discrete filtering methods are of special interest for accurate state estimation with irregular sampling interval scenarios. Meanwhile, the use of the 5D-CKF rule at the measurement update step reduces the moment approximation error. Thus, the novel square-root ACD-EKF-5DCKF filtering methods proposed in this paper are both accurate (the discretisation error at the time update filtering steps is bounded) and numerically robust (the reliability in a finite precision arithmetics is improved by solving the 5D-CKF square-rooting problem). The performance of the novel filtering methods is examined in both well- and ill-conditioned state estimation scenarios. The remaining part of this paper is organised as follows. Section 2 presents the continuous-discrete stochastic system to be investigated together with the mixed-type ACD-EKF-5DCKF estimation strategy. The square-rooting problem is solved in Section 3. Finally, Section 4 provides the numerical results and Section 5 concludes the paper. 2 Mixed-type ACD-EKF-5DCKF method Consider a continuous-discrete stochastic system of the form (1) (2) where is the unknown state vector to be estimated and the vector-function is the time-variant drift function. The process uncertainty is modelled by the additive noise term, where is the time-invariant diffusion matrix and is the q -dimensional Brownian motion whose increment is independent of and has the covariance . Finally, measurement equation (2) implies a non-linear relationship between the unknown dynamic state and available data measured at some discrete-time points . This measured information comes with the sampling rate (sampling period) . The measurement noise term is assumed to be white Gaussian noise with zero mean and known covariance , i.e. and is the Kronecker delta function. The initial state , , and noise processes are assumed to be statistically independent. In this paper, we study a mixed-type estimator where the time update step is performed according to the ACD-EKF strategy; meanwhile, the measurement update step follows the novel 5D-CKF methodology. A similar mixed-type filtering strategy with the use of well-known third-degree spherical-radial cubature rule has been investigated in [22]. The main benefit of the mixed-type algorithms is that they collect the best features of the continuous-discrete ACD-EKF approach and the CKF estimation strategy. In particular, the accurate implementation of the time update step by using the ACD-EKF approach implies automatic mechanisation for bounding the discretisation error occurred at this stage in line with a user-supplied accuracy tolerance value, say . In our research, we utilise the variable-step-size nested implicit Runge–Kutta formulas with the built-in automatic global error control suggested in [24]. The self-adaptive continuous-discrete filtering methods are of special interest for an accurate state estimation with irregular sampling interval scenarios; e.g. when missing measurements are encountered in practice. Apart from the discretisation error arisen in the propagation filtering steps, the non-linear KF-like estimators additionally introduce the moment approximation error. The modern CKF and UKF sampled-data filters provide a higher-order approximation for a posteriori mean than the EKF approach, i.e. they reduce the discussed error. Hence, we perform the measurement update step by using the recently suggested accurate 5D-CKF strategy [1, 2]. In summary, we design a mixed-type ACD-EKF-5DCKF estimator that consists of the following two steps. First, according to the continuous-discrete EKF implementation framework in [5], the moment differential equations should be solved in each with the initial values and as (3) (4) in order to determine the one-step-ahead predicted estimate and the related error covariance matrix . The matrix is the Jacobian matrix. It is worth noting here that one may utilise different numerical integration schemes for solving (3) and (4) because they are not coupled [20]. This simplifies the calculations. Special attention should be paid to an accurate numerical solution of (3) because of its possible complicated dynamics. We give all details of the ACD-EKF time update step in the Appendix; also may see [22, 25, 26]. The second part of the novel mixed-type continuous-discrete ACD-EKF-5DCKF filter is the measurement update stage performed for calculating the filtered (a posteriori) estimate and the corresponding filter covariance matrix at time instance . For that, the fifth-degree spherical-radial cubature rule is utilised for approximating the Gaussian-weighted integrals. For a better exposition of the arisen square-rooting problem discussed in Section 3, we start with the third-degree spherical-radial cubature rule. In [3, 11], it is utilised for computing the n -dimensional Gaussian-weighted integrals as follows: (5) where n is the dimension of the state vector to be estimated, the covariance matrix is factorised as , where is a square-root factor of and the cubature vectors are defined by (6) where denotes the i th unit coordinate vector in . Thus, the total number of third-degree CKF points is . The fifth-degree spherical-radial cubature rule is given by [2] (7) where (8) (9) Thus, the total number of fifth-degree cubature points is . The 5D-CKF cubature vectors and cubature points , are generated by the following general rule: (10) with the following notation utilised for the 5D-CKF rule: (11) (12) (13) (14) where with and . The vector is the zero column of the size n. In [1, 2], the 5D-CKF measurement update equations derived for processing the data at time instance are (15) (16) where the residual and cross-covariance matrices are calculated by (17) (18) (19) where the 5D-CKF cubature nodes are defined by (10). More precisely, at the time instance , we have and the square-root covariance matrix is , where the filter covariance matrix is factorised . In [18, p. 1638], if one defines the weight column vector from the 5D-CKF coefficients in (10) as well as the matrix by (20) (21) and set a matrix from the 5D-CKF cubature vectors by columns , i.e. it is of the size , then formulas (17)–(19) can be simply written as (22) (23) (24) where is the unitary column of the size N and is the identity matrix of that size, the symbol is the Kronecker tensor product. Thus, we combine the ACD-EKF time update step with the 5D-CKF rule at the measurement update step to get an accurate mixed-type ACD-EKF-5DCKF estimator. The new method is given in the form of a pseudo-code presented in (see Fig. 1). In this paper, we solve the square-rooting problem existing for the 5D-CKF filters, as pointed out in [2] and, hence, we design some stable square-root ACD-EKF-5DCKF implementations. Fig. 1Open in figure viewerPowerPoint Algorithm 1: ACD-EKF-5DCKF (conventional implementation) 3 Square-root factored-form filtering The square-root factored-form implementation methods are widely used in the engineering community for improving the numerical robustness of the KF-like filtering strategies in a finite precision arithmetics underlying computational devices in use. The key problem is in a machine precision and arisen round-off errors that may destroy the theoretical properties (symmetric form and positive definiteness) of the filter error covariance matrices and at any time instance and, hence, it doubts the results of computations and correctness of the obtained estimates; see a discussion of the KF failure reasons in textbooks [12-14]. The first ill-conditioned KF estimation scenario has been investigated in Examples 7.1 and 7.2 in [27]. The general way to resolve the numerical instability problem is to derive the square-root filtering methods. The key idea is to factorise the filter covariance matrix once (at the initial filtering step) and to re-derive the underlying filtering equations in order to process the square-root factors, but not the entire covariance matrices. This approach ensures the symmetric form and positive (semi-) definiteness of the filter error covariance matrix, although the round-off errors influence its square-root calculation [14]. Unfortunately, the derivation of mathematically equivalent filtering formulas in terms of the square-root matrix factors is not always possible to perform. For instance, the square-rooting problem still exists for the UKF. Although some square-root UKF methods based on the Cholesky decomposition are known (e.g. see [3, 28] and many other studies), it is correctly noted in [3] that all these methods are, in fact, the pseudo-square-root algorithms. All of them are based on the one-rank Cholesky update procedure performed at each filtering step for processing the negative UKF sigma-point coefficients encountered. Recall, the Cholesky decomposition exists and is unique when the symmetric matrix to be decomposed is positive definite [29]. For the one-rank Cholesky update procedure, the downdated matrix should also be positive definite. If this property is violated due to the round-off, then the failure of the one-rank Cholesky update procedure yields the UKF estimator failure because of unexpected interruption of the computations. The same problem exists for the novel 5D-CKF estimator. More precisely, the square-rooting problem for the discussed 5D-CKF estimator is stated in [2]. Similarly to the UKF estimation strategy, the problem is in the negative 5D-CKF weights , when the number of unknown states to be estimated is . Indeed, let us consider (23) for the residual covariance computation. It should be factorised and represented in a symmetric square-root form. For that, one should take the square root of the scalars , defined in (10) and involved in formula (21) for computing the weight matrix . It is not feasible for negative real numbers and this fact prevents the derivation of square-root methods for the fifth-degree cubature rule. In contrast, the third-degree cubature rule implies a simple derivation of square-root algorithms by using the traditional approach. Indeed, following the approximation in (5), the weight coefficients utilised are always positive since for all . In summary, to resolve the 5D-CKF square-rooting problem under discussion, one could follow the square-root approach suggested for the UKF estimators in [28] and utilise the one-rank Cholesky update procedure for processing negative coefficients. However, this approach yields the pseudo-square-root algorithms, as discussed in [3]. This means that the exact square-root solution for the advanced 5D-CKF estimation methodology still does not exist. Below, we propose a novel square-rooting methodology for resolving the discussed problem with the use of hyperbolic transformations for updating the filter square-root covariance factors. Additionally, we derive two square-rooting strategies, which are within the Cholesky factorisation and the SVD. 3.1 Cholesky factorisation-based approach The Cholesky-based square-root filtering approach implies a factorisation of any covariance matrix involved, say , in the form , where is a lower triangular matrix. Next, one should express the original filtering equations in terms of updating the Cholesky factors and avoid processing. Having done that, one obtains mathematically equivalent way for implementing the examined estimator, but it is more numerically stable in a finite precision arithmetics [30]. In [2], the derivation of square-root methods for the novel 5D-CKF estimator is still a challenge that should be addressed. In this paper, we propose a general solution and the following elegant strategy. First, we define the square-root matrix , where the required square-root operation is taken for absolute values . In addition, we determine the corresponding signature matrix in order to save the information about the sign of these entries [For zero entries we set .]. In summary, (25) (26) It is well known that stable orthogonal rotations can be utilised for computing the Cholesky factorisation of a positive definite matrix ; e.g. see [31]. Let us consider (23) for computing the residual covariance matrix . Due to the appearance of the signature matrix, it has a form of , in general. Thus, the traditional way and usual orthogonal transformations are not applicable for deriving square-root 5D-CKF. The appearance of a signature matrix in our square-root solution demands the utilisation of a hyperbolic factorisation instead of traditionally used standard factorisation. In [31], the J -orthogonal matrix is defined as one, which satisfies , where . Clearly, if the matrix equals to the identity matrix, i.e. , then the J -orthogonal matrix reduces to usual orthogonal one. In [31], the J -orthogonal transformations are used for computing the Cholesky factorisation of a positive definite matrix , where and . If we can find a J -orthogonal matrix such that (27) with , upper triangular, then (28) so R is the Cholesky factor, which we are looking for. The factorisation in (27) is called hyperbolic factorisation and its effective implementation methods are discussed in [32]. In the case of the examined 5D-CKF estimator, we have negative weights for all estimation problems of the size , i.e. we get and, hence, the signature matrix is . In other words, (23) for computing the residual covariance matrix can be represented in the form required for computing the Cholesky factor of . Now, we are ready to formulate the first square-root implementation methods for the 5D-CKF estimation strategy. Similarly to [33], let us consider the pre-array and block triangularise it by applying any J -orthogonal rotation (29) where is any J -orthogonal matrix with , and are defined in (25) and (26), respectively. One needs to define the blocks , and in (29). Following the definition of the hyperbolic factorisation and the fact that , one may determine the required blocks from the algebraic equivalence between transformation (29) and original equations (16), (23) and (24). Indeed, let us consider the first equality (30) i.e. is the Cholesky factor, which we are looking for. Similarly, we define and as follows: (31) i.e. and, hence . Thus, the gain matrix in (16) is calculated by using the available post-array blocks and as follows: (32) Finally, we have an equality (33) i.e. we get and, hence, from the last formula in (16) we conclude that . This completes the derivation of the first square-root rule for the 5D-CKF estimation strategy. Having combined it with the square-root ACD-EKF time update step in [25], we obtain the first Cholesky factorisation-based mixed-type ACD-EKF-5DCKF estimation method summarised in Algorithm 1a. One more square-root method can be designed. For that, one should derive a kind of the symmetric Joseph-type stabilised equation existing for the classical KF, e.g. see [13, 14]. Consider (34) Taking into account that and , we derive (35) The obtained symmetric formula allows for the hyperbolic factorisation as follows: (36) where is any J -orthogonal transformation with that block triangularises the pre-array. Algorithm 1a.ACD-EKF-5DCKF-SR (Cholesky-based method)1 Initialisation : () Set , ;2 Define by (25) and signature S by (26);3 Cholesky dec.: , ;4 Time Update : () Integrate (3), (4) with 5 Repeat Time Update step of Algorithm 1 (see Fig. 1), exceptline 15;6 Instead of line 15 in Algorithm 1 (Fig. 1), apply 7 , is any orthogonal rotation that lower triangulates ;8 and ;9 Measurement Update : ()10 Define the 5D-CKF cubature nodes and weights by (10);11 Generate , ();12 Collect ;13 Propagate ;14 Compute the predicted residual ;15 Build pre-array and block lower triangulates it (37)with is any -orthogonal rotation;16 Extract and save the blocks , and ;17 Calculate the filter gain ;18 Update . An alternative ACD-EKF-5DCKF implementation based on formula (35) can be summarised as follows: Algorithm 1b.ACD-EKF-5DCKF-SR (Cholesky-based method)1 Initialisation : () Repeat from Algorithm 1a;2 Time Update: () Repeat from Algorithm 1a;3 Measurement Update : ()4 Repeat lines 10–14 of Algorithm 1a;5 Build pre-array and block lower triangulates it (38)with is any -orthogonal rotation;6 Extract and find ;7 Calculate the filter gain ;8 Build pre-array and block lower triangulates it (39)with is any -orthogonal rotation;9 Extract and . 3.2 SVD factorisation-based approach The square rooting problem can be resolved within the SVD as well [34]. This approach yields a full rectangular square-root matrix, in contrast to a triangular square-root factor within the Cholesky decomposition-based methodology. Besides, the SVD approach is the most accurate method for factorising a matrix, especially when it is close to a singular one. Finally, the SVD-based filtering algorithms provide a user with extra information about the matrix structure involved; e.g. the information about eigenvalues is available and might be effectively used for automatic model analysis and/or reduction. We start with the rank-r SVD definition. For a matrix with , it has the following form [35, Theorem 1.1.6]: , where , the matrices and are orthogonal ones, and are the singular values of . Within the SVD-based filtering approach, the filter error covariance matrix is factorised as follows: , where is an orthogonal matrix and is a diagonal factor, which contains the singular values of . The square-root covariance matrix required for the 5D-CKF cubature nodes generation by (10) is defined by , where is SVD factorised. Finally, for propagating the involved factors, say and , the SVD factorisation of the pre-arrays is utilised instead of the factorisation implied within Cholesky-based implementation methods. As a result, the general square-rooting solution for the 5D-CKF cubature rule requires the hyperbolic SVD (HSVD) transformation instead of the usual SVD presented above. In [36], for any matrix and signature , the HSVD is the decomposition (40) where is an orthogonal matrix, while is a J -orthogonal matrix, and is a (block) diagonal one. The non-zero diagonal entries of are called the hyperbolic singular values of . Clearly, a special case when is a J -orthogonal matrix with yields a usual SVD. Finally, efficient HSVD implementation methods are discussed, e.g. in [37, 38]. We show how the SVD-based filtering equations can be derived. Following the definition of the HSVD, we get equality (41) where is a pre-array, to which the HSVD with the signature matrix is applied, i.e. . Let us consider the pre-array utilised for the square-root factor calculation according to (23) and apply the HSVD to the pre-array instead of the hyperbolic factorisation used in the Cholesky-based algorithms. We also assume that the measurement process covariance is SVD factorised, i.e. . Following (41), we obtain (42) i.e. is the orthogonal SVD factor of and is the diagonal SVD factor with the singular values of . Having computed the SVD factors of the residual covariance matrix , the gain in (16) is computed as follows: (43) Similarly, we apply the HSVD with to the pre-array obtained in (35) for the filter error covariance matrix (44) and get the post-array as follows: . Next, we justify that and by using equality (41). This completes the derivation of the SVD-based square-root solution for the 5D-CKF estimation strategy. Having combined it with the existing SVD-based ACD-EKF time update step, we obtain the first SVD-based ACD-EKF-5DCKF summarised in Algorithm 1c. Algorithm 1c.ACD-EKF-5DCKF-SVD (SVD-based method)1 Initialisation : () Set , ;2 Define by (25) and signature by (26);3 SVD dec.: , ;4 Time Update : () Integrate (3), (4) with 5 Repeat Time Update step of Algorithm 1 (Fig. 1), exceptline 15;6 Instead of line 15 in Algorithm 1 (Fig. 1), set pre-array7 ;8 , extract , ;9 , , ;10 Measurement Update : ()11 Define the 5D-CKF cubature nodes and weights by (10);12 Set the square-root factor ;13 Generate , ();14 Collect ;15 Propagate ;16 Compute the predicted residual ;17 Form pre-array and apply hyperbolic SVD as follows:18 , ;19 , extract , ;20 Compute ;21 Form pre-array and apply HSVD with :22 ;23 , extract , ;24 Update . It is worth noting here that we can construct one more SVD-based method for the examined ACD-EKF-5DCKF estimator as follows: Algorithm 1d.ACD-EKF-5DCKF-SVD (SVD-based method)1 Initialisation : () Repeat from Algorithm 1c;2 Time Update : () Repeat from Algorithm 1c;3 Measurement Update : ()4 Repeat lines 11–20 of Algorithm 1c;5 Form and apply HSVD with :6 ;7 ;8 Update . To justify Algorithm 1d, let us consider formula (16) for computing and factorise it in the following way: (45) where the SVD factors and are computed from (42), meanwhile and are known from the time update step. We apply the HSVD with to the pre-array and get the post-array as follows: . From equality (41), one determines , . This completes the derivation. 4 Numerical experiments Consider the test problem used for illustrating the performance of the third- and fifth-degree CKF methods in [2, 11]. To get some numerical insights about the numerical stability to round-off, we extend it on an ill-conditioned scenario as well. Example