Engineering Applications of Adaptive Kalman Filtering Based on Singular Value Decomposition (SVD)

Abstract

This paper presents the results of applying the new mechanization of the Kalman filter (KF) algorithm using singular value decomposition (SVD). The proposed algorithm is useful in applications where the influence of round-off errors reduces the accuracy of the numerical solution of the associated Riccati equation. When the Riccati equation does not remain symmetric and positive definite, the fidelity of the solution can degrade to the point where it corrupts the Kalman gain, and it can corrupt the estimate. In this research, we design an adaptive KF implementation based on SVD, provide its derivation, and discuss the stability issues numerically. The filter is derived by substituting the SVD of the covariance matrix into the conventional discrete KF equations after its initial propagation, and an adaptive estimation of the covariance measurement matrix $$R_k$$ is introduced. The results show that the algorithm is equivalent to current methods in terms of robustness, and it outperforms the estimation accuracy of the conventional Kalman filter, square root, and unit triangular matrix diagonal (UD) factorization methods under ill-conditioned and dynamic applications, and is applicable to most nonlinear systems. Four sample problems from different areas are presented for comparative study from an ill-conditioned sensitivity matrix, navigation with a dual-frequency Global Positioning System (GPS) receiver, host vehicle dynamic models, and distance measuring equipment (DME) using simultaneous slant range measurements, performed with a conventional KF and SVD-based (K-SVD) filter.

1. Introduction

By defining what robustness means in the context of this research, we can focus on the engineering applications of the proposed method. We refer to robustness as the “relative insensitivity of the solution to errors of some sort” [1]; also, we refer to numerical stability to denote robustness against round-off errors [2].

The design of the Kalman filter (KF) usually aims to “ensure that the covariance matrix of estimation uncertainty (the dependent variable of the matrix Riccati equation) remains symmetric and positive definite” [1,3]. The main problem to be solved in this research is the numerical robustness of KF. The proposed algorithm is useful in applications where the influence of round-off errors reduces the accuracy of the numerical solution of the associated Riccati equation. The fidelity of the solution may degrade to the point that it corrupts the Kalman gain when the Riccati equation does not stay symmetric and positive definite, and it can corrupt the estimate [1]. Verhaegen and Van Dooren have shown that the asymmetry of $P$ is one of the factors contributing to the numerical instability of the Riccati equation [4].

The problem was circumvented by Bierman [5] using unit triangular matrix diagonal (UD) factorization, which was one of several alternatives available to those in need of computationally efficient and stable algorithms. This decomposition of the covariance matrix is of the form:

(1) $P=UD{U}^T,$

where $D$ denotes the diagonal matrix and $U$ is the upper triangular matrix with ones along its main diagonal. One of the individual characteristics of UD factorization and the Cholesky method is that there is no need to solve simultaneous Equations [6]. Additionally, UD factorization does not use a square root numerical operation [6], but more recently square root filtering using standard matrix decomposition has found greater acceptance [7]. In this matrix,

(2) $S=P^{\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$2$}\right.}\mathrm{such}\mathrm{that}P=S{S}^T.$

S is obtained in a triangular matrix from the well-known Cholesky decomposition [8,9], a drawback of which is that it cannot be used if the covariance matrix is singular [6]. Both the square root and Bierman methods can be used only if one has single-dimensional measurements with uncorrelated measurement noise. Generally, one does not have this in practice [8,9]. To overcome the limitation, another method can be used—which is one of the topics of this research—that will facilitate the solution of this type of problem, as the computation can be performed in parallel because it is formulated in the form of matrix and matrix-vector operations without the need for additional transformations [10], where the solution also is able to deal with ill-conditioned matrices. This method is singular value decomposition (SVD).

The singular values (which are positive by definition) or principal standard deviation ${\sigma }_i$ are the square roots of the diagonal elements. For the discrete time linear problem, which is evaluated in this research, the first KF based on SVD was researched by Wang, Libert, and Manneback [8,9]. In this paper, the algorithm is further improved by making the filter adaptable to changes in sensor quality.

The Riccati equation of the Kalman filter is an important tool for designing navigation systems, particularly those that incorporate sensors with diverse error characteristics, such as the Global Navigation Satellite System (GNSS) and the inertial navigation system (INS). The Riccati equation permits us to predict the performance of a given system design as a function of the subsystem and component error characteristics [2]. Designers can then search for the mixture of components that will reach a predefined set of performance endpoints at the lowest framework cost. The design of the Kalman filter is aimed at making sure the estimation uncertainty of the covariance matrix (the dependent variable of the matrix Riccati equation) remains symmetric and positive definite [1]. In this research, we design an adaptive Kalman filter implementation based on SVD, provide its derivation, and discuss the stability issues numerically. The filter is derived by substituting the SVD of the covariance matrix into the conventional discrete Kalman filter equations after its initial propagation and adaptive estimation of the covariance measurement matrix $R_k$ .

One application of SVD in navigation is to apply it in calculating position errors to decompose the geometric matrix G, where error analysis with SVD is used to construct the pseudo inverse of G, making the solution numerically stable. While the linear least squares (LLS) is somewhat susceptible to round-up errors, where the normal equation is sometimes close to singular, SVD fixes the round-up problem and produces a solution that is the best approximation in the least-squares sense [11].

Different applications in the area of ionospheric tomography, as the problem to be solved, encounter the same ill-conditioned issue for the design matrix, which is composed of contributions of unknown parameters containing the ionospheric electron density (IED) in the total electron content (STEC) measurements, which is often not square, ill-posed, and ill-conditioned. Many methods have been employed to circumvent the problem of ill-posedness in the GNSS tomographic equation [12]. Those algorithms are iterative and include the Kalman filter, singular value decomposition, and algebraic reconstruction [7,9].

In the field of industrial electronic applications where numerical stability is necessary, an example is sensorless motor control, where the advantages of estimating the speed/position of the rotor include reduced hardware complexity, lower cost, reduced machine drive size, the elimination of the sensor cable, noise immunity, and increased reliability [13]; for example, a motor without a position sensor is suitable for harsh environments (such as submerged pumps). Other applications include diagnostics, the fault-tolerant control of AC drives, distributed generation and storage systems, robotics, vision and sensor fusion technologies, signal processing and instrumentation applications, and the real-time implementation of the Kalman filter for industrial control systems [13]. We briefly discuss the numerical stability of the Kalman filter related to industrial control systems and sensor fusion for cardiorespiratory signal extraction, but focus on numerical examples in aerospace applications like GNSS navigation.

The safe and efficient operation of control systems requires computer-based failure detection algorithms [13], where diagnosis may be used to provide fault-tolerant control. Several failures can occur in electrical systems, and redundant design is one of the solutions to assure the continuity and, therefore, the reliability of operations. Kalman filters can be used in fault-tolerant controls, for example, in aircraft control, home and municipal appliances (gas turbines, cooling systems, and heat pumps), and electric vehicles.

A challenge in the real-time implementation of Kalman filters is the high computational complexity, and among the main difficulties is numerical stability when computing the covariance matrices as well the computational load and execution time. There are three possible solutions to circumvent these issues, and one solution is to modify the algorithm, which is the scope of this paper. In addition, an efficient digital architecture to implement the estimator [13], and parallel computing can be used. The observability problem physically means that one or more state variables (or linear combinations) are hidden from the view of the observer (i.e., the measurements) [6]. This is a challenge in the sensorless control of AC drives, which can be accentuated when the stator frequency is close to zero and the rotor voltage carries small values. In addition, under continuous excitation conditions, when the stator frequency is close to zero and when the inductor rotor voltage carries small values, an estimation of the speed of an induction motor (IM) is not likely to be obtainable. In practice, compared with extended Luenberger observers and slipping modes, the Kalman filter appears to function well in these conditions with no divergence [13]. With this trade-off, the complexity of the Kalman filter is higher.

Other important applications of the KF are in the signal extraction and fusion of sensors, self-driving cars, GNSS/INS integration, and biomedical sensors; for example, when extracting cardiorespiratory signals from noninvasive and noncontacting sensor arrangements, such as magnetic induction sensors. In those applications, the signals are separated for monitoring purposes; by using the KF, it is possible to measure the heart and breathing rates in real time [14], improving the estimation of results, whereas other filtering techniques fail to extract relevant information due to the mixing of respiratory and cardiac signals and disturbance by measurement noise [14].

The second objective of this paper is to promote the use of continuous adaptation changes in the measurement error covariance matrix $R_k$ . The Kalman gain $K_k$ weights the innovation for the measurement error covariance matrix $R_k$ (the estimation error covariance matrix $P_{k+1}^{-}$ is influenced by $Q_k$ ), where higher values of $R_k$ make the measurement update step differ a little from the predicted value when the Kalman gain $K_k$ becomes very small [15]. On the contrary, high values of $Q_k$ engender more trust in the estimation of the state; Zhang et al. [16] showed that inappropriate tuning parameters may lead to low convergence by introducing adaptive technology consisting of adjustments to the noise covariance matrices to improve the estimation accuracy. Muñoz-Bañon et al. [17] showed that the Kalman filter can be used to integrate high-level localization subsystems, where the covariance information plays an essential role as new observations become available. This approach will be used in this research.

The proper selection of $R_k$ and $Q_k$ is vital for optimal filter performance, as the system model should describe as closely as possible the time evolution of the measured data, and the choices should reflect the system model error and measurement noise [15]. This research only explores changes in the measurement error covariance matrix $R_k$ . Zhang et al. [18] showed that when the system being considered has a stronger nonlinear action, the accuracy of the mode estimation will be poor. In order to address the shortcomings of the KF-based algorithm (K-SVD) and enhance the estimation precision, the adaptive filtering algorithm (K-SVD adaptive) was investigated, consisting of the adaptive estimation of $R_k$ , which is a function of the standard deviation.

2. Conventional Kalman Filter

The filter estimates, at each time, the a priori state of the system based on the a posteriori estimate for the previous moment and its dynamic equation. With this a priori estimate of the state, the corresponding predicted measurement $(H_k{\underset{\_}{x}}^{-})$ that corresponds to that state is obtained. Then, it is compared with the actual measurement $\underset{¯}{\left(z\right)}$ made at the same moment, then it corrects the a priori estimation as a function of its discrepancy and weight between the trust in the a priori estimates and the measurements represented by the gain $K$ of the filter. The process performed by the filter can be separated into two phases: prediction and correction [19].

Consider a discrete time model for a linear stochastic system:

(3) $x_{k+1}={\mathsf{\Phi }}_{k+1,k}{X}_k+G_k{w}_k,$

(4) $Z_k=H_k{x}_k+v_k,$

where $x_k\in {ℝ}^{nx1}$ is the state vector; $Z_k\in {ℝ}^{mx1}$ is the measurement vector; $w_k\in {ℝ}^{nx1}$ is the disturbance input vector; $v_k\in {ℝ}^{mx1}$ is the measurement noise vector; and ${\mathsf{\Phi }}_{k+1,k}\in {ℝ}^{nxn}$ , $G_k\in {ℝ}^{nxn}$ , and $H_k\in {ℝ}^{mxn}$ are the system matrices. The covariance of $w_k$ is $Q_k$ , known as the process covariance matrix, and that of $V_k$ is $R_k$ , known as the measurement covariance matrix, and they are presumed to be zero-mean Gaussian white noise with the symmetric positive definite covariance matrices $Q_k$ and $R_k$ , respectively. Then, $w~N\left(0,Q\right)\mathrm{and}v~N\left(0,R\right)$ at time $t_k$ , where a superscript $\left(-\right)$ indicates the a priori values of the variables and a superscript $\left(+\right)$ denotes values after the measurement update. The Kalman filter is described by following the recursive equations under the assumption that the system matrices are well known:

Time extrapolation:

(5) ${\widehat{x}}_{k+1}^{-}={\mathsf{\Phi }}_{k-1,k}{\widehat{x}}_{k-1}^{+},$

(6) $P_{k+1}^{-}={\mathsf{\Phi }}_{k-1}{P}_{k-1}^{+}{\mathsf{\Phi }}_{k-1,k}^T+{\Psi }_{k-1}{Q}_{k-1}{\Psi }_{k-1}^T.$

Measurement update:

(7) ${\widehat{x}}_k^{+}={\widehat{x}}_k^{-}+K_k\left(z_k-H_k{\widehat{x}}_k^{-}\right),$

(8) $K_k=P_k^{-}{H}_k^T{\left(H_k{P}_k^{-}{H}_k^T+R_k\right)}^{-1},$

(9) $P_k^{+}=\left(I-K_{k}H\right)P_k^{-},$

where $P_k$ is the covariance of estimation uncertainty, $K_k$ is the Kalman gain matrix, and $z_k-H_k{\widehat{x}}_k^{-}$ are the innovations of the discrete Kalman filter.

3. Singular Value Decomposition

Here, we consider singular value decomposition, which is a way of decomposing a matrix, $\mathrm{A}$ , in the product of three other matrices: $A=US{V}^T$ .

Remark 1. Let A be an $n$ $\times$ $d$ matrix with the right singular vectors $v_1$ , $v_2$ , …, $v_k$ ; left singular vectors $u_1$ , $u_2$ , …, $u_k$ ; and corresponding singular values ${\sigma }_1,{\sigma }_2,\dots ,{\sigma }_k$ . Then, A can be decomposed into a sum of rank-one matrices as follows:

(10) $A={\displaystyle \sum }_{i=1}^k{\sigma }_i{u}_i{v}_i^T\approx {\sigma }_1{u}_1{v}_1^T+{\sigma }_2{u}_2{v}_2^T+\dots +{\sigma }_k{u}_k{v}_k^T=U\Sigma V^T.$

Here, $U\in R^{mxm}$ and $V\in R^{nxn}$ , such that:

(11) $A=U\Sigma V^T,$

where:

(12) $\Sigma =\left(\begin{array}{cc}S& 0\\ 0& 0\end{array}\right).$

Here, $\Sigma \in R^{mxn}$ and $S=$ diag( ${\sigma }_2,\dots ,{\sigma }_{r)}$ , with:

${\sigma }_1\ge \cdots \ge {\sigma }_r\ge 0,$

${\sigma }_{r+1}=0\cdots {\sigma }_n=0.,$

The numbers ${\sigma }_1\dots {\sigma }_r$ along with ${\sigma }_{r+1}=0,\dots \sigma n=0$ are known as the singular values of A, and they are the positive square roots of the eigenvalues of $A^{T}A$ .

Columns U of A are the orthonormal eigenvectors of $A{A}^T$ , whereas columns $V$ of A are the orthonormal eigenvectors of $A^{T}A$ . It is understood that the singular values and singular vectors of a matrix are relatively insensitive to disturbance from the entries of the matrix and finite precision errors [8,20].

Given a non-square matrix $A=U\Sigma V^T$ , two matrices and their factorizations are of special interest:

(13) $A^{T}A=V{\Sigma }^2{V}^T,$

(14) $A{A}^T=U{\Sigma }^2{U}^T.$

The algorithm is developed by comparing both sides of the obtained matrix equations.

4. SVD of Kalman Filter

In practical terms, if $A^{T}A$ is positive definite, then Equation (13) can be compacted to:

(15) $A=U\left(\begin{array}{c}S\\ 0\end{array}\right)V^T,$

where $S$ is an $n$ × $n$ diagonal matrix. Specifically, if $A$ is itself positive definite, then it will have an asymmetric singular decomposition (which is also an eigendecomposition):

(16) $A=U\Sigma U^T=U{D}^2{U}^T,$

where the principal standard deviations ${\mathsf{\sigma }}_{\mathrm{i}}$ are the square roots of the diagonal elements of A. Assuming that the covariance of the Kalman filter has been propagated and updated for everything from the filter algorithm, the singular value decomposition of the equation of $P$ . is:

(17) $P_k^{+}=U_k^{+}{D}_k^{+2}{U}_k^{+T},$

Equation (6) can then be written as:

(18) $P_{k+1}^{-}={\mathsf{\Phi }}_{k+1}{U}_k^{+}{D}_k^{+2}{U}_k^{+T}{\mathsf{\Phi }}_{k+1,k}^T+{\Psi }_k{Q}_k{\Psi }_k^T.$

The idea is to find the factors $U_{k+1}$ and ${\mathrm{D}}_{k+1}$ from Equation (18), such that:

(19) $P_{k+1}=U_{k+1}{D}_{k+1}^2{U}_{k+1}^T.$

We define the following $\left(\mathrm{s}+n\right)\times n$ matrix:

(20) $\underset{Pre-array}{\underbrace{\left(\begin{array}{c}{D}^{+2}{U}^{+T}{\mathsf{\Phi }}_{k+1,k}^T\\ {\left(Q_1{}^{\frac{1}{2}}\right)}^T{\Psi }^T\end{array}\right)}}.$

Then, we compute its singular value decomposition:

(21) $\underset{Pre-array}{\underbrace{\left(\begin{array}{c}{D}^{+2}{U}^{+T}{\mathsf{\Phi }}_{k+1,k}^T\\ {\left(Q_1{}^{\frac{1}{2}}\right)}^T{\Psi }^T\end{array}\right)}}=\underset{Post-arraySVDfactos}{\underbrace{U_k^{\prime }\left(\begin{array}{c}{D}_k^{\prime }\\ 0\end{array}\right)V_k^{\prime }}}.$

Multiplying each term on the left by its transpose, we get:

(22) ${\mathsf{\Phi }}_{k+1,k}{U}_k^{+}{D}_k^{+T}{D}_k^{+}{U}_k^{+T}{\mathsf{\Phi }}_{k+1,k}{}^T+{\psi }_k{\left(Q_k\right)}^{\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$2$}\right.}{\left(Q_k^{\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$2$}\right.}\right)}^T{\psi }_k^T=V_k^{’}\left[D_k^{’T}|0\right]U^{T}U\left[\begin{array}{c}{D}_k^{’}\\ 0\end{array}\right]V_k^{’T}.$

We obtain:

(23) ${\mathsf{\Phi }}_{k+1,k}{U}_k^{+}{D}_k^{+2}{\mathsf{\Phi }}_{k+1,k}{}^T+{\psi }_k{Q}_k{\psi }_k^T=V_k^{’}{D}_k^{’2}{V}_k^{’T}.$

Comparing the result of Equation (23) with Equations (18) and (19), it is easy to associate $V_k^{’}$ and $D_k^{’}$ with $U_{k+1}$ and $D_{k+1}$ . In the orthodox Kalman measurement update, substituting Equation (8) into (9) yields:

(24) $P_k^{+}=P_k^{-}-P_k^{-}{H}_k^T{\left(H_k{P}_k^{-}{H}_k^T+R_k\right)}^{-1}{H}_k{P}_k^{-}.$

Lemma 1. Using the matrix inversion lemma in the case where the inverse of a positive definite matrix A adds a rank-one matrix ${\mathrm{bb}}^{\mathrm{T}}$ yields:

(25) ${\left(A+b{b}^T\right)}^{-1}=A^{-1}-A^{-1}b{\left(1+b^T{A}^{-1}\right)}^{-1}{b}^t{A}^{-1},$

where the superscript T denotes the complex conjugate transpose (or Hermitian) operation. It is well known in the literature that this formula is handy to develop a recursive least-squares algorithm for the recursive identification or design of an adaptive filter [21].

The lemma on Equations (24) and (25) yields:

(26) ${\left(P_k^{+}\right)}^{-1}=P_k^{-1}+H_k^T{R}_k^{-1}{H}_k.$

Implementing the singular value decomposition of a symmetric positive definite matrix into $P_{k+1}^{+}$ and $P_k$ yields:

(27) ${\left(U_k^{+}{D}_k^{+2}{U}_k^{+T}\right)}^{-1}={\left(U_k{D}_k^2{U}_k^T\right)}^{-1}+H_k^T{R}_k^{-1}{H}_k,$

(28) ${\left(U_k^{+}{D}_k^{+2}{U}_k^{+T}\right)}^{-1}={\left(U_k^T\right)}^{-1}\left(D_k^{+2}+U_k^T{H}_k^T{R}_k^{-1}{H}_k{U}_k\right)U_k^{-1}.$

To make the system adaptable to changes in sensor quality, the measurement noise is also estimated, which leads to a time-variant matrix. When the signal comes from different sensors, at each discrete time step $k$ the noise covariance matrix is updated by appraising the standard deviation (std) of an arithmetically differentiated (diff) small-signal segment [14]:

(29) $R_k=\left(\begin{array}{ccc}1& \dots & 0\\ ⋮& \ddots & 0\\ 0& 0& 1\end{array}\right)*{\sigma }_{noise,,k}^2,$

(30) ${\sigma }_{noise,k}^i=\frac{std\left(diff\left(S_i\right)\right)}{\sqrt{2}}.$

The adaptive computation of the KF and SVD-based (K-SVD) algorithm can be added independent of the SVD portion, as for some applications the measurement covariance matrix is known and can be pre-computed at the algorithm initiation.

After computing Equations (29) and (30), let

(31) $R_k^{-1}=L_k{L}_k^T$

be the Cholesky decomposition of the inverse of the covariance matrix. If covariance matrix $R$ is known, but not the inverse, then the reverse Cholesky decomposition $R^{\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$2$}\right.}{R}^{\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$2$}\right.T}=R_{0,}$ , $R^{\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$2$}\right.}$ [8] upper triangular can be identified. It then follows that $L_k={\left(R_k^{\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$2$}\right.T}\right)}^{-1}$ is the necessary Cholesky decomposition [20] in Equation (31).

Now, we consider the $\left(m+n\right)$ × n matrix:

(32) $\left[\begin{array}{c}{L}_k^T{H}_k{U}_k\\ D_k^{-1}\end{array}\right],$

and compute its singular value decomposition:

(33) $\underset{Pre=array}{\underbrace{\left(\begin{array}{c}{L}_k^T{H}_k{U}_k\\ D_k^{-1}\end{array}\right)}}=\underset{Post-arraySVDfactors}{\underbrace{U_k^{*}\left(\begin{array}{c}{D}_k^{*}\\ 0\end{array}\right)V_k^{*T}}}.$

Multiplying each term on the left by its transpose produces:

(34) $D_k^{-2}+U_k^T{H}_k^T{L}_k{L}_k^T{H}_k{U}_k=V_k^{*}{D}_k^{*2}{V}_k^{*T}.$

Then, Equation (28) can be written as follows:

(35) ${\left(U_k^T\right)}^{-1}{\left(D_k^{+}\right)}^{-2}{\left(U_k^{+}\right)}^{-1}={\left(U_k^T\right)}^{-1}{V}_k^{*}{D}_k^{*2}{V}_k^{*T}{U}_k^{-1},$

(36) ${\left(U_k^T\right)}^{-1}{\left(D_k^{+}\right)}^{-2}{\left(U_k^{+}\right)}^{-1}={\left[{\left(U_k{V}_k^{*}\right)}^T\right]}^{-1}{D}_k^{*2}{\left[U_k{V}_k^{*}\right]}^{-1}.$

Comparing both sides of Equation (36):

(37) $U_k^{+}=U_k{V}_k^{*},$

(38) $D_k^{+}={\left(D_k^{*}\right)}^{-1}.$

The update measurement formulation is obtained, which requires calculating the singular value decomposition of the nonsymmetrical matrix without specifically defining its left orthogonal component that has a larger dimension. The Kalman gain term can be derived as:

(39) $K_k=P_k{H}_k^T{\left(H_k{P}_k{H}_k^T+R_k\right)}^{-1}.$

Inserting $P_k^{+}{\left(P_k^{+}\right)}^{-1}$ and ${\left(R_k\right)}^{-1}{R}_k$ will not alter the gain. Thus, the Kalman filter gain can be written as:

(40) $K_k=P_k^{+}{\left(P_k^{+}\right)}^{-1}{P}_k{H}_k^T{R}_k^{-1}{R}_k{\left(H_k{P}_k{H}_k^T+R_k\right)}^{-1},$

(41) $K_k=P_k^{+}{\left(P_k^{+}\right)}^{-1}{P}_k{H}_k^T{R}_k^{-1}{\left(H_k{P}_k{H}_k^T{R}_k^{-1}+I\right)}^{-1},$

where $I$ denotes the identity matrix. Next, from Equation (26), for ${\left(P_k^{+}\right)}^{-1}$ we get:

(42) $K_k=P_k^{+}{H}_k^T{R}_k^{-1},$

(43) $K_k=U_k^{+}{D}_k^{+2}{U}_k^{+T}{H}_k^T{L}_k{L}_k^T.$

The Kalman gain $K_k$ is fully defined, and the state vector measurement is given by:

(44) ${\widehat{X}}_k^{+}={\widehat{X}}_k+K_k\left(Z_k-H_k{\widehat{X}}_k\right).$

Algorithm 1. Singular value decomposition-based Kalman filtering:

Input:

Initialization (k = 0). Perform SVD on ${\mathit{P}}_{\mathbf{0}}$ to determine the initial ${\mathit{U}}_{\mathbf{0}}$ and ${\mathit{D}}_{\mathbf{0}}$ . The initial ${\mathit{P}}_{\mathbf{0}} \in {ℝ}^{nxn}$ , where ${\mathit{U}}_{\mathbf{0}}$ and ${\mathit{D}}_{\mathbf{0}}$ are orthogonal and diagonal matrices, respectively. Matrix ${\mathit{U}}_{\mathbf{0}}$ contains the singular values of ${\mathit{P}}_{\mathbf{0}}$ .

Steps (k = 1 to n): Update ${\mathit{U}}_{\mathit{k}}^{+}$ and ${\mathit{D}}_{\mathit{k}}^{+}$ by constructing the $\left(\mathit{m}+\mathit{n}\right)\times \mathit{n}$ matrix using a pre-array (32). Use only the right SVD factor V. Compute the Kalman gain ${\mathit{K}}_{\mathit{k}}$ from Equation (43). Update the estimate ${\widehat{\mathit{x}}}_{\mathit{k}}^{+}$ from Equation (44). Notice that ${\mathit{L}}_{\mathit{k}}^{\mathit{T}}$ is triangular, and the computation of ${\mathit{H}}_{\mathit{k}}^{\mathit{T}}{\mathit{L}}_{\mathit{k}}={\left({\mathit{L}}_{\mathit{k}}^{\mathit{T}}{\mathit{H}}_{\mathit{k}}\right)}^{\mathit{T}}$ can be obtained from step 2. Compute the extrapolation ${\widehat{\mathit{x}}}_{\mathit{k}+\mathbf{1}}^{-}$ ; from Equation (3), ${\mathit{U}}_{\mathit{k}+\mathbf{1}}$ and $D_{k+1}$ can be obtained by the SVD from the Equation (21) pre-array matrix without forming the left orthogonal factor (the one associated with Equation (13) of the SVD). Compute the measured noise time-variant matrix using Equations (29) and (30).

Return: Compute ${\mathit{L}}_{\mathit{k}}$ using Equation (31) as ${\mathit{L}}_{\mathit{k}}={\left({\mathit{R}}_{\mathit{k}}^{\raisebox{1ex}{$\mathbf{1}$}\!\left/ \!\raisebox{-1ex}{$\mathbf{2}$}\right.\mathit{T}}\right)}^{-\mathbf{1}}$ .

The Algorithm 1 requires the Cholesky decomposition of noise covariance matrices $R$ ; for time-invariant matrices, the Cholesky decomposition is performed once during the initial step—i.e., before the KF recursion. Note that the algorithm also requires the m × m matrix inversion for the terms $R^{\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$2$}\right.}$ and $R^{\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$2$}\right.T}$ in Equation (31). Again, if the process noise covariance is constant over time, the matrix calculation is pre-computed while the filter is initialized.

5. Numerical Experiments: Applications in Navigation

The computer round-off for floating-point arithmetic is always demarcated by a single parameter ${ϵ}_{roundoff}$ , called the unit round-off error, which is demarcated as $1+{ϵ}_{roundoff\equiv }$ 1 in machine precision.

In MATLAB, the parameter “eps” satisfies the definition above [2].

Round-off errors are a side effect of computer arithmetic using fixed or floating-point data words with a fixed number of bits [2]. The next example demonstrates that round-off can still be a problem when implementing KF in MATLAB environments and how a problem that is well-conditioned can be made ill-conditioned by the filter implementation [2]. The problem is taken from Grewal and Andrews, and it is significant, as it shows the filter’s observational behavior. The example does not make use of the Algorithm 1 adaptive computation from Equations (29) and (30), and the resulting algorithm is called K-SVD.

Example 1. Let ${\mathrm{I}}_{\mathrm{n}}$ denote the n × n identity matrix. Considering the filtering problem with the measurement sensitivity matrix:

$H=\left[\begin{array}{ccc}1& 1& 1\\ 1& 1& 1+\mathsf{\delta }\end{array}\right],$

and covariance matrices.

$P_0=I_3$ and $R={\delta }^2{I}_2$ ; to simulate round-off, it is assumed that:

${\delta }^2<{ϵ}_{roundoff}$ but $\delta >{ϵ}_{roundoff}$ .

In this case, although $H$ has rank = 2 in machine precision, the product $H_k{P}_0^{-}{H}_k^T$ with round-off will equal:

$\left[\begin{array}{cc}3& 3+\mathsf{\delta }\\ 3+\mathsf{\delta }& 3+2\mathsf{\delta }\end{array}\right],$

which is singular, and the results remain unchanged when R is added to $H_k{P}_0^{-}{H}_k^T$ .

All the KF tests were carried out with the same precision (64-bit floating-point) in MATLAB running on a standard personal computer with an Intel(R) Core(TM) processor, i5-4200U CPU @ 1.60 GHz 2.30 GHz, 8.0 GB RAM, where the unit round-off error is $2^{-53}~{\mathrm{1.11.10}}^{-16}$ . The MATLAB eps function is twice the unit round-off error [2,22]. Hereafter, the set of numerical experiments is repeated for various values of the ill-conditioned parameter $\delta$ ; as such, delta tends toward the machine precision limit $\delta \to {ϵ}_{roundoff}$ . This example does not prove the robustness of the solution of the UD, square root Kalman (SQRK), and K-SVD methods. Full implementation would require temporal updates; however, observational updates have created difficulties with traditional implementation [2].

Figure 1 illustrates how the conventional KF and a few other choice implementation methods work on various ill-conditioned experiments as the conditioned parameter $\delta \to 0$. For this particular example, the square root-based (SQRK-based Carlson), Cholesky-based (UD-based Bierman), and K-SVD-based methods appear to degrade more gracefully than the others. The SQRK, UD decomposition, and K-SVD solutions also retain nine digits ( $\approx 30\mathrm{bits})$ of accuracy at $\delta \approx \sqrt{\in }$ , while the other methods have no bits of accuracy in the calculated solution [1]. The other methods worth mentioning in this research are conventional KF, Potter square root filter, and Joseph stabilization.

The problem analyzed corresponds to an ill-conditioned intermediate result where $R^{*}=HP{H}^T+R$ for inversion in the Kalman gain formula in Equation (8). In this case, although $H$ has rank = 2 in machine precision, the product $H{P}_0{H}^T+R$ is not invertible, and the observational filter updates fail. The results are provided graphically in Figure 1, showing that the K-SVD-based method performs as well as the UD-based and SQRK-based Kalman filter methods.

Examples 2 and 3 as outlined were run one time—i.e., the research team performed a simulation. In each run, the dynamic state real trajectory was $x_k^{true},k=1,\dots ,K$ ; the estimated dynamic state path was ${\widehat{x}}_{k/k},k=1,\dots ,K$ ; and the root mean square error (RMSE) of the state vector for each component is given as follows [22]:

(45) ${\mathrm{RMSE}}_{\mathrm{xi}}=\sqrt{\frac{1}{\mathrm{MK}}{{\displaystyle \sum }}_{\mathrm{i}=1}^{\mathrm{M}}{{\displaystyle \sum }}_{\mathrm{j}=1}^{\mathrm{k}}{\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{true}}-{\hat{\mathrm{x}}}_{\mathrm{k}/\mathrm{k}}\right)}^2}.$

Examples 2 and 3 use a GPS configuration loaded as Yuma ephemeris data from a GPS satellite from March 2014, downloaded from the US Coast Guard site YUMA Almanac [2].

Example 2. The following example shows a navigation simulation with a dual-frequency GPS receiver on a 100 km-long figure-8 track for 1 h with signal outages of 1 min each. The problem is taken from Grewal and Andrews, and it is significant as it shows the filter’s observational behavior [2]. The state vector corresponds to nine host vehicle model dynamic variables and two receiver clock error variables. The nine host vehicle variables are: east error (m), north error (m), altitude error (m), east velocity error (m/s), north velocity error (m/s), altitude rate error (m/s), east acceleration error (m), north acceleration error (m), and upward acceleration error (m). The MATLAB m-file solves the Riccati equation with a dynamic coefficient matrix structure [2], where the 9 × 9 matrix corresponds to the host vehicle dynamics and the 2 × 2 to the clock error.

(46) $\left[\begin{array}{ccc}9\times 9& \cdots & 0\\ ⋮& \ddots & ⋮\\ 0& \cdots & 2\times 2\end{array}\right]$

Table 1 summarizes the outcome for a simulation using real ephemeris GPS data and an ill-conditioned intermediate result where $R^{-1}=HP{H}^T+R$ for inversion in the Kalman gain formula in Equation (8). The example does not make use of the Algorithm 1 adaptive computation from Equations (29) and (30), and the resulting algorithm is called K-SVD.

The results in Table 1 show that the conventional KF is not suitable to gracefully handle round-off errors, and other filter methods can be used to better handle the round-off issue. A large $\mathsf{\delta }$ corresponds to a well-conditioned state. Once degradation happens, some of the filters decay faster than the K-SVD. The filter robustness can then be obtained by comparing the outcomes shown in Table 1 against those with the worst performance. The threshold where the filter can handle the error is reached when the algorithm produces “NaN,” or not a number, and this happens when the conventional KF is at $\mathsf{\delta }={10}^{-10}$ . A comparison of the K-SVD with SQR-KF and UD-KF shows that the K-SVD algorithm is superior in handling round-off errors. UD-KF is slightly better than SQR-KF, and the conventional KF performs as well as the other two. Regarding the execution time, K-SVD and conventional KF are slower than SQRT and UD, and this is because SVD is computationally more costly than Cholesky decomposition [22]. However, SVD is suitable for parallel computing and has a better numerical stability.

Example 3. To compare and evaluate the performance of the new algorithm with conventional KF implementations, we include example 3, which is taken from [2]. The example is a comparison of the GNSS navigation performance with four vehicle tracking models, using a simulated racetrack with a 1.5 km figure-8 track at 90, 140, 240, and 360 kph average speed and a nominal low-cost clock, with a relative stability of $1\times {10}^{-9}$ part/part over 1 s. The number of states in the KF model varies as satellites come into and fall from view. The vehicle states (position, velocity, and acceleration) are as follows: position north, position east, position up (vertical), velocity north, velocity east, velocity vertical, acceleration north, acceleration east, acceleration vertical. Models 3 and 5 do not include the three acceleration states (north, east, vertical).

Table 2 lists the number of state variables and the minimum and maximum number of visible satellites. Table 3 lists the parametric models used in this research to model a single component of the motion of a host vehicle. The model parameters mentioned in Table 3 include the standard deviation $\sigma$ and the correlation time constant $\tau$ of position, velocity, acceleration, and jerk (derivative of acceleration) [2]. The example 3 makes use of the Algorithm 1 adaptive computation from Equations (29) and (30), and the resulting algorithm is called K-SVD adaptive.

Regarding the host vehicle dynamic models in Table 3, the third model is a navigation model that is often used to track the GNSS receiver phase and frequency error, and to track the vehicle position and velocity. The fifth model is restricted root mean square (RMS) speed and acceleration, and is a further refinement of a vehicle with bounded velocity and acceleration. These also perform better when the signals are lost, because the model takes the true vehicle limitations into consideration. Model 6 is the bounded RMS location, and may be suitable for ships and land vehicles with restricted altitude excursions, including vehicles operating within tightly confined areas [2]. Each model was tuned to the simulated vehicle dynamics by putting its autonomous model parameters to fit the values described in [2].

The results from the experiment in Example 3 are shown in Table 4 and Figure 2, Figure 3, Figure 4, Figure 5, Figure 6, Figure 7, Figure 8, Figure 9, Figure 10, Figure 11, Figure 12 and Figure 13. The experiment was designed to have a side-by-side comparison between the conventional KF and its novel factored form, K-SVD and K-SVD adaptive.

Comparing the values for each model in Table 4 at different velocities, it is clear that when the velocity is increased (e.g., from 90 to 360 kph), the conventional KF always increases the RMS error absolute value well above the K-SVD by about 172%. At the model level, the best performer in all cases is model 5. From the figures above, it can be inferred that with all things being equal (the only difference in the MATLAB source code is the algorithm to compute the Kalman filter), K-SVD adaptive has a better prediction accuracy than the conventional KF and traditional factored KFs. This means that K-SVD adaptive is more robust than the current known factor-based algorithms, at the same time providing a better filtering capability when it performs the same operations. The algorithm captures the information that matters, which is the N-dimensional subspace of $R^N$ spanned by the first columns of V or the right orthogonal factor [8,9]. By performing a matrix multiplication, the algorithm realizes a low dimensionality reduction that captures much of the variability and corresponds with the selection of the eigenvectors with the largest eigenvalues (since SVD orders them from the largest to smallest). Additionally, by adapting to variations in the covariance measurement error, the algorithm has a higher accuracy.

Figure 14, Figure 15, Figure 16, Figure 17, Figure 18 and Figure 19 represent the results of Table 4. By observing Figure 14, it is easy to see that K-SVD adaptive model 6 performs better for the north position. K-SVD model 5 also performs relatively well compared with the conventional KF. Similar results can be inferred from Figure 15; K-SVD adaptive model 6 is the best performer, followed by K-SVD adaptive models 3 and 5. Conclusive results can also be seen for the downward position RMS error in Figure 16. Model 5 performs better, independent of the filter type. Model 6 is also a good performer for downward velocity, and the worst is model 3.

Figure 17 illustrates the results for the north velocity RMS. From this figure, K-SVD adaptive model 6 is the best performer, followed closely by K-SVD model 5. From Figure 18, the best performer is K-SVD adaptive model 5, and finally, regarding the downward speed velocity RMS from Figure 19, K-SVD models 3 and 6 perform relatively well.

Example 4. Distance measuring equipment (DME) operations will continue to expand as a different navigation source in space-based navigational systems such as GPS and Galileo [23]. In the 21st century, airborne navigation has become increasingly reliant on satellite guidance; nonetheless, satellite signals are extremely low, can be spoofed, and are not necessarily available. There are mandates from the US and European Union that require maintaining and upgrading ground-based navigation aids.

The following example simulates a simplified model of an electronic DME-to-DME position and velocity navigation system, where the noise measurement of a slant range (distance) is the underlying observable. The airborne distance measuring equipment transmits a pulse that is received and returned by the ground station, while the aircraft equipment triangulates the transit time in terms of distance based on the propagation delay. The aircraft flies along a fixed path at a cruising speed of 583 kph [6]. The duration of the flight is 3000 s, the initial coordinates at t = 0 are appropriately defined as normal random variables, ${\mathrm{x}}_0~\mathrm{N}\left(60,10{\mathrm{km}}^2\right)$ , ${\mathrm{y}}_0~\mathrm{N}\left(210,10{\mathrm{km}}^2\right)$ , sample time T = 2 s, $g=10$ m/s, and the maximum deviation in the acceleration for the aircraft is ${\sigma }_{amax}$ = 0.001 $\frac{m}{se{c}^2}$ .

Suppose that two DME stations are found on the x-axis, as shown in Figure 20. The positions of the DME stations are identified. The aircraft follows a circular path at the end of the trajectory, at a constant speed with the same velocity as the linear motion, turning for 270° with a gyro of 20 km. The trajectory is depicted in Figure 21 and assumed to be level, with constant velocity plus small perturbations caused by small random accelerations. This model is only practical for short intervals.

For navigation based on estimation, it is necessary to extrapolate the position or initial state of the airplane, which is supposedly known, to a posterior time step using the dynamic equation of the process and knowing the excitation that disturbs the system.

To obtain distance $\mathrm{z}1\mathrm{and}\mathrm{z}2$ , it is necessary to have the following geometric relations for both DMEs; the measurements in terms of $\mathrm{x}$ and $\mathrm{y}$ are then:

(47) $\mathrm{z}1=\sqrt{{\left(\mathrm{x}-{\mathrm{x}}_1\right)}^2+{\left(\mathrm{y}-{\mathrm{y}}_1\right)}^2},$

(48) $\mathrm{z}2=\sqrt{{\left(\mathrm{x}-{\mathrm{x}}_2\right)}^2+{\left(\mathrm{y}-{\mathrm{y}}_2\right)}^2},$

(49) $\mathrm{z}1=\mathrm{f}1\left(\mathrm{x},\mathrm{y}\right),\mathrm{z}2=\mathrm{f}2\left(\mathrm{x},\mathrm{y}\right).$

To estimate the aircraft position using the above mathematical expressions, it is necessary to clear x and y as a function of trigonometric functions, and the equations are solved using linearization by expansion:

(50) $\partial \mathrm{z}1=\frac{\partial \mathrm{f}1\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{x}}\mathrm{dx}+\frac{\partial \mathrm{f}1\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{y}}\mathrm{dy},$

(51) $\partial \mathrm{z}2=\frac{\partial \mathrm{f}2\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{x}}\mathrm{dx}+\frac{\partial \mathrm{f}2\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{y}}\mathrm{dy}.$

Writing Equations (50) and (51) in matrix form, we have:

(52) $\left(\begin{array}{c}\partial z1\\ \partial z2\end{array}\right)=\left[\begin{array}{cc}\frac{\partial \mathrm{f}1\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{x}}& \frac{\partial \mathrm{f}1\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{y}}\\ \frac{\partial \mathrm{f}2\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{x}}& \frac{\partial \mathrm{f}2\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{y}}\end{array}\right].\left(\begin{array}{c}\partial \mathrm{x}\\ \partial \mathrm{y}\end{array}\right),$

(53) $\left(\begin{array}{c}\partial \mathrm{z}1\\ \partial \mathrm{z}2\end{array}\right)=\left[\begin{array}{cc}\frac{\partial \mathrm{f}1\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{x}}& \frac{\partial \mathrm{f}1\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{y}}\\ \frac{\partial \mathrm{f}2\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{x}}& \frac{\partial \mathrm{f}2\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{y}}\end{array}\right].\left(\begin{array}{c}\partial \mathrm{x}\\ \partial \mathrm{y}\end{array}\right).$

Thus, the evaluation of the partial derivatives leads to:

(54) $\frac{\partial \mathrm{f}1\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{x}}=\frac{\mathrm{x}-{\mathrm{x}}_1}{\mathrm{z}1}=\sin \left(\mathsf{\theta }1\right),$

(55) $\frac{\partial \mathrm{f}1\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{y}}=\frac{\mathrm{y}-{\mathrm{y}}_1}{\mathrm{z}1}=\cos \left(\mathsf{\theta }1\right),$

(56) $\frac{\partial \mathrm{f}2\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{x}}=\frac{\mathrm{x}-{\mathrm{x}}_2}{\mathrm{z}2}=-\sin \left(\mathsf{\theta }2\right),$

(57) $\frac{\partial \mathrm{f}2\left(\mathrm{x},\mathrm{y}\right)}{\partial \mathrm{y}}=\frac{\mathrm{y}-{\mathrm{y}}_2}{\mathrm{z}2}=\cos \left(\mathsf{\theta }2\right).$

Substituting in (53), we have:

(58) $\left(\begin{array}{c}\partial \mathrm{z}1\\ \partial \mathrm{z}2\end{array}\right)=\left[\begin{array}{cc}\sin \left(\mathsf{\theta }1\right)& \cos \left(\mathsf{\theta }1\right)\\ -\sin \left(\mathsf{\theta }2\right)& \cos \left(\mathsf{\theta }2\right)\end{array}\right].\left(\begin{array}{c}\partial \mathrm{x}\\ \partial \mathrm{y}\end{array}\right).$

Then,

(59) $\left(\begin{array}{c}\Delta \mathrm{z}1\\ \Delta \mathrm{z}2\end{array}\right)=\left[\begin{array}{cc}\sin \left(\mathsf{\theta }1\right)& \cos \left(\mathsf{\theta }1\right)\\ -\sin \left(\mathsf{\theta }2\right)& \cos \left(\mathsf{\theta }2\right)\end{array}\right].\left(\begin{array}{c}\Delta \mathrm{x}\\ \Delta \mathrm{y}\end{array}\right).$

Finally, we notice that the equation above can be generalized further because the cosine and sine terms represent the line of sight into the two DME channels, along with direction cosine between the y- and x-axes [6]. Please note the connection between the observables, $\mathrm{z}2$ , and the estimated quantity, $\mathrm{x},\mathrm{y},$ is not linear. Therefore,

(60) $\mathrm{H}=\left[\begin{array}{cc}\sin \left(\mathsf{\theta }1\right)& \cos \left(\mathsf{\theta }1\right)\\ -\sin \left(\mathsf{\theta }2\right)& \cos \left(\mathsf{\theta }2\right)\end{array}\right].$

Matrix $\mathrm{H}$ is sometimes called the linearized matrix. Applying the inverse matrix method, we have:

(61) $\left(\begin{array}{c}\Delta \mathrm{x}\\ \Delta \mathrm{y}\end{array}\right)={\mathrm{H}}^{-1}.\left(\begin{array}{c}\Delta \mathrm{z}1\\ \Delta \mathrm{z}2\end{array}\right).$

Figure 21 depicts the results after running Equations (47)–(61) in MATLAB code:

As seen in Figure 21, the error does not go to infinity when the nominal trajectory goes through the origin, because the assumed aircraft dynamics (i.e., random walk) gives the filter some memory, and all the weight is not placed on the current measurement. From the figure, it can be seen that the aircraft reaches the origin in about 1063 s. What it is illustrated here is the classical error dilution of precision (DOP) when the line of sight is 180° apart relative to the aircraft. The detected peak is the greatest in the y-axis, and the error occurs due to the mathematical effect of navigating the DME geometry on the position measurement. After that, the position is calculated using conventional KF and K-SVD, and the general formulation is:

(62) ${\underset{\_}{\mathrm{x}}}_{\mathrm{k}+1}=\underset{¯}{\underset{¯}{\mathrm{A}.}}{\underset{\_}{\mathrm{x}}}_{\mathrm{k}}+\underset{¯}{\underset{¯}{\mathrm{B}.}}{\underset{\_}{\mathrm{u}}}_{\mathrm{k}}+{\underset{\_}{\mathrm{w}}}_{\mathrm{k}},$

(63) ${\underset{\_}{\mathrm{z}}}_{\mathrm{k}}=\underset{¯}{\underset{¯}{{\mathrm{C}}_{\mathrm{k}}}}.{\underset{\_}{\mathrm{x}}}_{\mathrm{k}}+{\underset{\_}{\mathrm{v}}}_{\mathrm{k}}.$

The covariance of $\underset{\_}{\mathrm{w}}\mathrm{k}$ is $\mathrm{Qk}$ and that of $\underset{\_}{\mathrm{v}}\mathrm{k}$ is $\mathrm{Rk}$ , then $\mathrm{W}~\mathrm{N}\left(0,\mathrm{Q}\right)\mathrm{and}\mathrm{V}~\mathrm{N}\left(0,\mathrm{R}\right)$ .

The state equations are then:

(64) $\left(\begin{array}{c}{\mathrm{x}}_{\mathrm{k}+1}\\ {\mathrm{y}}_{\mathrm{k}+1}\\ {\mathrm{v}}_{\mathrm{x}\mathrm{k}+1}\\ {\mathrm{v}}_{\mathrm{y}\mathrm{k}+1}\end{array}\right)=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].\left(\begin{array}{c}{\mathrm{x}}_{\mathrm{k}}\\ {\mathrm{y}}_{\mathrm{k}}\\ {\mathrm{v}}_{\mathrm{x}\mathrm{k}}\\ {\mathrm{v}}_{\mathrm{y}\mathrm{k}}\end{array}\right)+\left(\begin{array}{c}{\mathrm{w}1}_{\mathrm{k}}\\ {\mathrm{w}2}_{\mathrm{k}}\\ {\mathrm{d}\mathrm{w}1}_{\mathrm{k}}\\ {\mathrm{d}\mathrm{w}1}_{\mathrm{k}}\end{array}\right),$

(65) $\left(\begin{array}{c}{\mathrm{x}}_{\mathrm{k}+1}\\ {\mathrm{y}}_{\mathrm{k}+1}\\ {\mathrm{v}}_{\mathrm{x}\mathrm{k}+1}\\ {\mathrm{v}}_{\mathrm{y}\mathrm{k}+1}\end{array}\right)=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].\left(\begin{array}{c}{\mathrm{x}}_{\mathrm{k}}\\ {\mathrm{y}}_{\mathrm{k}}\\ {\mathrm{v}}_{\mathrm{x}\mathrm{k}}\\ {\mathrm{v}}_{\mathrm{y}\mathrm{k}}\end{array}\right)+\left(\begin{array}{cc}\frac{T^2}{2}& 0\\ 0& \frac{T^2}{2}\\ T& 0\\ 0& T\end{array}\right),$

where ${\mathrm{x}}_{\mathrm{k}+1}$ and ${\mathrm{y}}_{\mathrm{k}+1}$ denote the incremental position in the horizontal and vertical plane, respectively.

(66) ${\overline{\mathrm{z}}}_{\mathrm{k}}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right].\left(\begin{array}{c}{\mathrm{x}}_{\mathrm{k}}\\ {\mathrm{y}}_{\mathrm{k}}\\ {\mathrm{v}}_{\mathrm{x}\mathrm{k}}\\ {\mathrm{v}}_{\mathrm{y}\mathrm{k}}\end{array}\right)+\left(\begin{array}{c}{\mathrm{v}}_{\mathrm{x}\mathrm{k}}\\ {\mathrm{v}}_{\mathrm{y}\mathrm{k}}\end{array}\right).$

Let $\mathrm{Q}$ be the error in the process, and the matrix maintains the values of $P_k$ from Equation (6):

(67) $Q=E{\left(\left[\begin{array}{cc}\frac{T^2}{2}& 0\\ 0& \frac{T^2}{2}\\ T& 0\\ 0& T\end{array}\right]{\left[\begin{array}{cc}\frac{T^2}{2}& 0\\ 0& \frac{T^2}{2}\\ T& 0\\ 0& T\end{array}\right]}^T\left[w_k^a\right]\right)}^2,$

(68) $\mathrm{Q}=\left[\begin{array}{cccc}\frac{{\mathrm{T}}^4}{4}& 0& \frac{{\mathrm{T}}^3}{2}& 0\\ 0& \frac{{\mathrm{T}}^4}{4}& 0& \frac{{\mathrm{T}}^3}{2}\\ \frac{{\mathrm{T}}^3}{2}& 0& {\mathrm{T}}^2& 0\\ 0& \frac{{\mathrm{T}}^3}{2}& 0& {\mathrm{T}}^2\end{array}\right].{\left[{\sigma }_{amax}\right]}^2.$

${\mathrm{v}}_{\mathrm{k}}$ denotes the measurement error:

(69) ${\mathrm{v}}_{\mathrm{k}}=\left[\begin{array}{c}\begin{array}{cc}{\mathsf{\sigma }\mathrm{x}}^2& 0\end{array}\\ \begin{array}{cc}0& {\mathsf{\sigma }\mathrm{y}}^2\end{array}\end{array}\right].$

The results from calculating the position using conventional KF and Algorithm 1 without the adaptive computation (K-SVD) from Equations (29) and (30) are shown in Figure 22.

From Figure 22, is it easy to see that at about 333 m from the origin, the DME presents singularity in the calculation due to DOP, and at that critical point the KF and K-SVD perform fine; however, the KF does not perform as well as K-SVD at the cusp of the circular return. Additionally, the results show how far the RMS is above the KF compared with the K-SVD in this DME example (see Figure 23).

The results from Figure 23 indicate that the RMS is lower for the K-SVD filter compared with the conventional KF. When solving the observation equation to obtain the calculated position, it is likely that the error of the observables is manifested by producing erratic oscillations in the position. This makes the computed trajectory uneven, making the motion of the airplane very unstable. By using a prediction system that minimizes that noise, it is possible to produce estimates of unknown or known variables that tend to be more precise than those based on the measurement alone (see Equation (61)). It is demonstrated that advantages can be obtained when combining the position of the airplane based on observables and predictions. Given that the DME was left in place to circumvent the reliability problem of GPS and to increase reliability, the best filter to deal with the singularities at the origin is K-SVD.

6. Computational Complexity

Computational complexity is evaluated as time complexity as a function of input size, where m and n denote the input size and the complexity of the number of required operations (big $O$ ), the matrix inversion is $O(n^3$ ), and an economic SVD rank reduction step requires $O(m^{2}n$ ) operations. With the matrix multiplication $O(n^3$ ), to compute the Kalman gain $k_t$ requires $O(s{n}^2+n^3$ ). The Cholesky decomposition of the commonly used algorithm is $O(n^3$ ). Computing the matrix covariance is $O\left(N.n^2\right)$ , and in the worst case the algorithm complexity is $O(n^3$ ), and this is still within an acceptable range when compared with the conventional KF, where it has been established that the number of arithmetic operations to be executed at each sampling period is $O(n^3$ ) [24].

7. Conclusions and Future Scope

The computation of the novel Kalman-SVD algorithm is based on the computation of the singular value decomposition of an unsymmetrical matrix without explicitly forming its left orthogonal factor that has a high dimension [19]. K-SVD does a truncation that keeps only the top n right singular vectors by setting $V^T$ (where superscript T denotes the transpose of a matrix and n denotes the number of rows), equal to the n rows of $V^T$ (n × n matrix). That is equivalent to removing or zeroing out m number of singular values from the original matrix (m + n) by –n, where (m + n) denotes the rows, therefore reducing the dimension, which is far less than the original dimension, thus filtering out the noise.

The other conclusion of this research is related to the robustness of the adaptive algorithm. Compared with other KF-based decomposition algorithms, K-SVD adaptive can handle round-off errors, which makes the algorithm useful in critical applications, making it possible to deal with singular systems that are ill-conditioned.

From the analysis of example 3, we can conclude that the accuracy of position and speed can be improved by choosing the model that performs the best for the worst-case scenario, which in this case is the highly dynamic conditions of maneuvering a vehicle. The tracking capabilities can also be increased by adding to the K-SVD algorithm the adaptive computation of the measurement noise matrix as a time-variant matrix. From the analysis of example 4, we can conclude that K-SVD is suitable to deal with real dynamic conditions for ground navigation systems. From the analysis of the literature, it seems that one of the drawbacks of using K-SVD and KF in general is their high computational load. In that sense, K-SVD can be used in applications where the KF is currently used with the advantages of robustness and accuracy.

The algorithm developed in this research also has superior accuracy compared with regular decomposition-based KF algorithms by adapting the measurement matrix at each step and at the same time using the simplest form to update the error of the covariance matrix P, which is the most common equation to compute the algorithm. Additionally, the proposed Algorithm 1 does not require matrix inverting, and the system of interest does not need to have a square shape.

Our effort in this paper is guided by the increasing utilization of Kalman filtering in operations that demand high reliability, where the robustness of the algorithm is a necessary condition to perform critical operations. More work can be done in future K-SVD adaptive applications by including the Joseph stabilized form instead of the conventional form of the Riccati equation to avoid the propagation of antisymmetric problems and fine-tuning the process noise covariance matrix $Q_k$ . We believe that the key points and principles derived here will find their way to applications in high-speed signal processing by combining K-SVD, K-SVD adaptive and SVD compressive sensing [25]. For future research, we intend to explore particular technology uses.

Author Contributions

J.C.B.O. designed the mathematical model of the proposed algorithm and wrote the paper, R.M.A.V. performed the experiments, and V.F.G.C. analyzed the data. All authors have read and agreed to the published version of the manuscript.

Funding

This research received no external funding.

Conflicts of Interest

The authors declare no conflict of interest.

Figures and Tables

Figure 1. Degradation of observational updates with the problem conditioning of the Riccati equation.

Figure 2. Plan view of the estimated and true locations at 90 kph using the conventional Kalman filter algorithm.

Figure 3. Plan view of the estimated and true locations at 90 kph using the Kalman filter (KF) and singular value decomposition (SVD)-based (K-SVD) filter algorithm.

Figure 4. Plan view of the estimated and true locations at 90 kph using the conventional Kalman filter and singular value decomposition based (K-SVD) adaptive algorithm.

Figure 5. Plan view of the estimated and true locations at 140 kph using the conventional Kalman filter algorithm.

Figure 6. Plan view of the estimated and true locations at 140 kph using the K-SVD filter algorithm.

Figure 7. Plan view of the estimated and true locations at 140 kph using the K-SVD adaptive algorithm.

Figure 8. Plan view of the estimated and true locations at 240 kph using the conventional Kalman filter algorithm.

Figure 9. Plan view of the estimated and true locations at 240 kph using the K-SVD filter algorithm.

Figure 10. Plan view of the estimated and true locations at 240 kph using the K-SVD adaptive algorithm.

Figure 11. Plan view of the estimated and true locations at 360 kph using the conventional Kalman filter algorithm.

Figure 12. Plan view of the estimated and true locations at 360 kph using the K-SVD filter algorithm.

Figure 13. Plan view of the estimated and true locations at 360 kph using the K-SVD adaptive algorithm.

Figure 14. Vector norm of the absolute errors at the north position of the dynamic state at different speeds for models 3, 5, and 6.

Figure 15. Vector norm of the absolute errors at the east position of the dynamic state at different speeds for models 3, 5, and 6.

Figure 16. Vector norm of the absolute errors at the downward position of the dynamic state at different speeds for models 3, 5, and 6.

Figure 17. Vector norm of the absolute errors at the north velocity of the dynamic state at different speeds for models 3, 5, and 6.

Figure 18. Vector norm of the absolute errors at the east velocity of the dynamic state at different speeds for models 3, 5, and 6.

Figure 19. Vector norm of the absolute errors at the downward velocity of the dynamic state at different speeds for models 3, 5, and 6.

Figure 20. Distance measuring equipment (DME)-to-DME problem geometry.

Figure 21. Estimated parameters.

Figure 22. Position with the conventional Kalman filter and K-SVD vs. the estimated position.

Figure 23. RMS for K-SVD vs. the conventional Kalman filter.

Table 1

Vector norm of absolute errors for each component of the dynamic state—i.e., $\parallel RMS{E}_{xj}{\parallel }_2$ (example 2). RMSE, root mean square error; KF, Kalman filter; SVD, singular value decomposition; SQRK, square root; UD, unit triangular matrix diagonal; NaN, not a number.

Algorithm	$\parallel \mathit{R}\mathit{M}\mathit{S}{\mathit{E}}_{\mathit{x}\mathit{j}}{\parallel }_2,\mathit{i}=1,\dots ,19$ $\mathbf{while}\mathbf{Growing}\mathbf{Ill}-\mathbf{Conditioned}\mathit{\delta }\to \mathit{\epsilon }\mathbf{Round}-\mathbf{Off}$																Execution Time (ms)
	${10}^{-1}$	${10}^{-2}$	${10}^{-3}$	${10}^{-4}$	${10}^{-5}$	${10}^{-6}$	${10}^{-7}$	${10}^{-8}$	${10}^{-9}$	${10}^{-10}$	${10}^{-11}$	${10}^{-12}$	${10}^{-13}$	${10}^{-14}$	${10}^{-15}$	${10}^{-16}$
Kalman filter (KF)	0.318	0.32	3.2	3.185	3.186	3.186	3.186	3.186	3.846	NaN	NaN	NaN	NaN	NaN	NaN	NaN	1.75
K-SVD	0.318	0.32	3.2	3.186	3.186	3.186	3.186	3.186	3.186	3.186	3.186	3.186	3.186	3.186	3.186	1.2095	1.87
SQR-KF	0.317	0.32	3.2	3.185	3.186	3.186	3.186	3.198	3.380	NaN	NaN	NaN	NaN	NaN	NaN	NaN	0.123
UD-KF	0.318	0.32	3.2	3.185	3.186	3.186	3.186	3.201	2.607	NaN	NaN	NaN	NaN	NaN	NaN	NaN	0.06

Table 2

State variables of the host vehicle dynamic models and the min/max number of visible satellites.

Name	State Variables	Min Satellites	Max Satellites
MODL3	6	17	19
MODL5	6	17	19
MODL6	9	20	22

Table 3

Host vehicle dynamic models for example 3.

Model No.	Model Parameters (Each Axis)		Independent Parameters	Dependent Parameters
	F	Q
3	$\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]$	$\left[\begin{array}{cc}0& 0\\ 0& {\sigma }_{acc}^2\mathsf{\Delta }{t}^2\end{array}\right]$	${\sigma }_{acc}^2$	${\sigma }_{pos}^2\to \infty$
5	$\left[\begin{array}{ccc}0& 1& 0\\ 0& \frac{-1}{{\tau }_{vel}}& \\ & & \frac{}{{\tau }_{acc}}\end{array}\right]$	$\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& {\sigma }_{jerk}^2\mathsf{\Delta }{t}^2\end{array}\right]$	${\sigma }_{vel}^2$ ${\sigma }_{acc}^2$ ${\sigma }_{acc}^2$ ${\tau }_{acc}$	${\sigma }_{pos}^2\to \infty$ ${\tau }_{vel}$ ${\rho }_{vel,acc}$ ${\sigma }_{jerk}^2$ ${\tau }_{pos}$
6	$\left[\begin{array}{ccc}\frac{-1}{{\tau }_{pos}}& & \\ & \frac{-1}{{\tau }_{vel}}& \\ & & \frac{-1}{{\tau }_{acc}}\end{array}\right]$	$\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& {\sigma }_{jerk}^2\mathsf{\Delta }{t}^2\end{array}\right]$	${\sigma }_{pos}^2$ ${\sigma }_{pos}^2$ ${\sigma }_{acc}^2$ ${\tau }_{acc}$	${\tau }_{vel}$ ${\rho }_{pos,vel}$ ${\rho }_{pos,acc}$ ${\rho }_{vel,acc}$ ${\sigma }_{jerk}^2$

Table 4

Vector norm of absolute errors at each component of dynamic state—i.e., $\parallel RMS{E}_{xj}{\parallel }_2$ (example 3).

Velocity (kph)	Filter Algorithm	RMS N Pos. Err (m)			RMS E Pos. Err (m)			RMS D Err Pos. (m)			RMS N Vel. Err (m)			RMS E Vel. Err (m)			RMS D Vel. Err (m)
		Model			Model			Model			Model			Model			Model
		3	5	6	3	5	6	3	5	6	3	5	6	3	5	6	3	5	6
90	K-SVD	18.9795	13.1434	8.4985	15.2594	9.5004	7.696	4.9555	3.5034	3.1645	13.4455	18.1012	21.3442	12.1186	13.3198	15.7312	0.3772	0.37048	0.46792
	KF	49.7396	27.8908	8.8257	35.1804	17.7478	8.7422	5.2831	3.5222	3.3424	18.2056	20.1147	22.2134	16.1441	15.0062	16.8097	0.37895	0.37154	0.43506
	K-SVD Adaptive	10.2778	12.207	9.2506	9.5038	9.3347	8.8791	5.3669	3.4983	3.253	10.8538	17.3657	21.4174	9.2136	12.8466	15.7406	0.37345	0.36972	0.52434
140	K-SVD	34.381	16.4213	8.6789	24.1658	11.124	7.9977	5.7576	3.5104	3.5967	28.1594	24.968	23.545	23.4031	19.233	19.3927	0.58314	0.57733	0.68302
	KF	91.5971	45.4864	10.0976	49.6878	27.8549	11.2752	5.8059	3.5304	3.6835	36.5676	31.2012	27.0703	27.1445	24.0638	23.9997	0.58328	0.57843	0.65682
	K-SVD Adaptive	13.081	14.5208	10.1475	9.1574	9.8283	8.5083	6.3418	3.5069	3.6179	22.6147	22.186	22.6644	18.0133	17.2274	18.2008	0.57957	0.57607	0.6972
240	K-SVD	72.415	33.2644	11.4813	38.9716	19.4082	11.4123	6.2148	3.5281	3.8291	66.7895	50.9121	37.6559	47.4745	39.9117	42.427	1.0749	1.074	1.1929
	KF	147.4348	101.5482	33.01	58.1507	49.7875	32.2579	5.9236	3.5418	3.8778	74.0361	68.8543	57.5329	47.0228	49.0727	57.7828	1.0717	1.0736	1.1298
	K-SVD Adaptive	15.9439	18.8714	9.8239	10.7245	11.9533	8.9979	5.6269	3.5188	3.6314	49.4055	37.0557	28.8207	36.4643	29.9429	30.1337	0.99112	0.98908	1.0801
360	K-SVD	95.5756	50.6414	19.8398	43.9963	26.4262	18.2616	7.0764	3.5338	4.0798	96.503	79.6562	65.8811	64.3833	59.8545	72.5721	1.4835	1.4876	1.6069
	KF	163.5172	135.8009	69.2511	59.0267	57.0835	44.0271	6.0699	3.5439	3.9288	99.1573	101.0778	105.9726	62.6476	66.8818	81.1056	1.4832	1.4856	1.535
	K-SVD Adaptive	16.8871	24.0959	9.9347	10.5866	13.636	8.7987	6.0749	3.525	4.0164	81.9968	61.5704	47.4594	58.0575	49.3587	58.1295	1.4863	1.4855	1.5748