Kalman Filter Adaptation to Disturbances of the Observer’s Parameters

Abstract

Currently, one of the most effective algorithms for state estimation of stochastic systems is a Kalman filter. This filter provides an optimal root-mean-square error in state vector estimation only when the parameters of the dynamic system and its observer are precisely known. In real conditions, the observer’s parameters are often inaccurately known; moreover, they change randomly over time. This in turn leads to the divergence of the Kalman estimation process. The problem is currently being solved in a variety of ways. They include the use of interval observers, the use of an extended Kalman filter, the introduction of an additional evaluating observer by nonlinear programming methods, robust scaling of the observer’s transmission coefficient, etc. At the same time, it should be borne in mind that, firstly, all of the above ways are focused on application in specific technical systems and complexes, and secondly, they fundamentally do not allow estimating errors in determining the parameters of the observer themselves in order to compensate them for further improving the accuracy and stability of the filtration process of the state vector. To solve this problem, this paper proposes the use of accurate observations that are irregularly received in a complex measuring system (for example, navigation) for adaptive evaluation of the observer’s true parameters of the stochastic system state vector. The development of the proposed algorithm is based on the analytical dependence of the Kalman estimate variation on the observer’s parameters disturbances obtained using the mathematical apparatus for the study of perturbed multidimensional dynamical systems. The developed algorithm for observer’s parameters adaptive estimation makes it possible to significantly increase the accuracy and stability of the stochastic estimation process as a whole in the time intervals between accurate observations, which is illustrated by the corresponding numerical example.

1. Introduction

To assess the stochastic systems state, a significant number of different algorithms and techniques [1] have been developed, among which one of the most effective is the Kalman filter [2]. However, often, a problem arises with the practical application of the Kalman filter. It is due to the fact that this filter calculates an optimal root-mean-square error (RMS-error) of state vector estimation only when both the parameters—system and state vector observer—are known exactly [3]. When operating real systems—control, communications, navigation, etc.—the parameters of both the system itself and its state vector meters, as a rule, change randomly over time. This is especially evident in imperfect dynamical systems, the interest in the study of which has only been increasing lately [4,5,6,7,8]. Such instability of the system parameters when trying to assess its current state causes the divergence of the Kalman estimation process. This disadvantage of the Kalman filter requires the development of appropriate methods to ensure, with random variations of the observer’s parameters, a given accuracy in system state estimation [9]. To date, the following approaches are known to solve this issue:

• Synthesis of observers with the property of invariance to disturbances of their own parameters [10].
• Extending the state vector estimated by the Kalman filter due to undefined parameters [10,11].
• Two-stage estimation of the system state parameters using the extended Kalman filter, which reduces its computational complexity when expanding the state vector [12].
• Complexing of filtering algorithms with fuzzy logic algorithms [13] or control algorithms [14] to compensate for random disturbances.
• Development of so-called “interval observers” for systems with nonstationary fluctuating parameters whose variations are within the specified intervals [15,16].
• Extension of the observation vector based on an additional evaluating observer formed using nonlinear programming [17].
• Robust scaling of the observer’s transmission coefficient, which increases the stability of the evaluation process [18].
• Ensuring the invariance of the Kalman filtering process to parametric disturbances of the observer through the use of integrated neural networks [19], etc.

Each of the above approaches is focused, as a rule, on use in a specific technical system, which is due to the significantly nonlinear type and the complexity of latter dynamics. The analysis of the practical use of adaptive assessment methods allows us to conclude about their effectiveness only under the condition of their problem-oriented development for a specific class of information and measurement systems, among which the most widely used in practice is the class of complexing measurement systems. For these systems, it is characteristic to process measurements both according to the primary indications of “rough” sensors (sensors of low accuracy) and according to the indications of high-precision sensors, according to which the correction of primary measurements is carried out. Such a correction is made, as a rule, at certain (often random) time intervals that exceed the clock cycle of “rough” measurements. As such systems, the following can be cited:

• Integrated inertial-satellite navigation systems, in which the measurements of the inertial navigation systems, whose errors grow over time, are corrected according to the indications of the satellite navigation systems which in this case is the reference [20,21,22,23].
• Orientation and navigation systems of mobile robots, in which the navigation parameters of the robot are adjusted based on taking into account the zero speed of the lower point of the wheel (or the robot’s foot) at the moment of contact with the earth’s surface [24].
• Transport information and measurement systems of various types—railway, automobile, marine, unmanned aerial vehicle (UAV), etc., in which the orientation and navigation parameters of an object are corrected at the time of passing reference points with precisely known coordinates (for example, traffic lights, eurobalises, radio frequency tags, buoys, etc.) [25,26,27,28,29,30].
• Complexing orientation and navigation systems based on inertial sensing elements, allowing to solve the navigation problem inside confined rooms [31] and so on.

Currently, the estimated state parameters are adjusted, as a rule, by directly replacing their current estimates with their corresponding accurate measurements without any change in the estimation algorithm or its parameters. With such an approach, firstly, estimation errors do not decrease in the time interval following the moment of the current accurate measurement (which is observed in the measurement and navigation systems listed above) [20,21,22,23,24,25], and, secondly, it is impossible to estimate the errors in determining the observer’s parameters itself.

Due to the need to solve this problem, the possibility of using accurate observations received at some (irregular) points in time in the measurement system is considered below to develop an algorithm for adaptive evaluation of the true observer’s parameters of the object state vector, the use of which in the structure of the filtering algorithm will significantly increase its accuracy and stability.

Previously, the idea of using accurate observations entering the measuring system at irregular (including random) points of time was considered during adaptive estimation of the a posteriori covariance matrix [32,33], as well as measurement noise covariance matrix in the Kalman filter [34], which, compared with the traditional scheme, significantly reduced estimation errors. Unfortunately, the approach described in Sokolov et al. (2018, 2021), Sokolov and Novikov (2021) [32,33,34], cannot be applied to solving the problem of estimating the matrix of itself observer’s parameters, since the right part of the filter equation depends on the matrix significantly nonlinearly. In this regard, a different approach to solving this problem is considered below, based on a mathematical apparatus that is fundamentally different from the one used in [32,33,34]—on the mathematical apparatus for studying disturbed multidimensional linear systems [35]. As shown below, the use of this mathematical apparatus can significantly improve the accuracy and stability of the Kalman filtering process under the conditions of the observer’s parametric disturbances. However, at the same time, the major difficulty of using it is the impossibility of approximating the variations of the observer’s parameters with white Gaussian noise, as shown in Reference [35] and allowed us to obtain simple recurrent dependences of the estimation error on the variations of the filter parameters there. In the vast majority of practical applications, a random change in the observer’s parameters over time occurs rather slowly [10,11,12,13,14,15,16], which makes it impossible to use the previously obtained ratios and requires the derivation of fundamentally new dependencies of estimation errors on variations in the observer’s parameters. In this regard, the problem solution was further considered on the basis of joint integration of the two approaches mentioned above, which are fundamentally different from each other—the use of irregular accurate observations of the state vector of the object and the mathematical apparatus for the study of disturbed multidimensional linear systems.

2. Theoretical Assumptions

2.1. Task Definition

Since accurate observations are made at discrete points in time, the adaptation of the state vector estimation process is further considered for a discrete stochastic system

$${\xi }_{i+1}={\Phi }_{i+1/i}\cdot {\xi }_i+W_{i+1}, \xi \left(0\right)={\xi }_0,$$

(1)

where ξ_i is the N-dimensional state vector at a discrete time i, ${\Phi }_{i+1/i}$ is the transition state matrix of N × N dimension, and W_i+1 is the N-dimensional vector of white Gaussian noise of an object with zero mean and a known intensity matrix G_i+1·δ_i+1,j+1 (δ_i,j is the Kronecker Delta function). For a discrete stochastic meter, the vector of the output signals of the meter is described by the equation

$$z_{i+1}=H_{i+1}{\xi }_{i+1}+V_{i+1},$$

(2)

where z_i+1 is the M-dimensional measurement vector, H_i+1 is the measurement matrix of M × N dimensions, and V_i+1 is the M-dimensional vector of white Gaussian noise of an object with zero mean and a known intensity matrix R_i+1·δ_i+1,j+1.

For the “object–observer” system (Equations (1) and (2)), the estimation of the state vector is performed by an optimal discrete Kalman filter [3,32,34]:

$$\begin{gathered} {\widehat{\xi }}_{i+1}={\Phi }_{\frac{i+1}{i}}{\widehat{\xi }}_i+K_{i+1}\left(z_{i+1}-H_{i+1}{\Phi }_{\frac{i+1}{i}}{\widehat{\xi }}_i\right), \\ {K}_{i+1}=P_{\frac{i+1}{i}}\cdot H_{i+1}^T{\left(H_{i+1}\cdot P_{\frac{i+1}{i}}{H}_{i+1}^T+R_{i+1}\right)}^{-1}, \\ {P}_{\frac{i+1}{i}}={\Phi }_{\frac{i+1}{i}}{P}_i{\Phi }_{\frac{i+1}{i}}^T+G_{i+1}, \\ {P}_{i+1}=\left(E-K_{i+1}{H}_{i+1}\right)P_{\frac{i+1}{i}},{\widehat{\xi }}_0=M\left({\xi }_0\right), \\ {P}_0=M\left\{\left({\xi }_0-{\widehat{\xi }}_0\right){\left({\xi }_0-{\widehat{\xi }}_0\right)}^T\right\}. \end{gathered}$$

(3)

where E is the unit matrix of the relevant dimension, and M is the dimension of the measurement vector.

As follows from the analysis of Equation (3), the error in determining the a posteriori covariance matrix, and therefore both the gain error and the discrepancy error in the estimation equation, depend significantly nonlinearly on the error δH in determining the measurement matrix H_i. In the direct formulation, calculating the true values of the H_i matrix from accurate observations of the state vector is a solution to the inverse dynamics problem, computationally unrealizable in real time with existing iterative procedures for solving systems of nonlinear equations due to the essentially nonlinear dependence of the evaluation vector on the measurement matrix.

In order to develop an effective computational algorithm that provides a real-time adaptive assessment of the system state under the uncertainty of the measurement matrix, two assumptions are made. The first is on the interval between accurate measurements, the error δH in determining the measurement matrix is constant, and the second is that its variations of the smallness second-order δ⁽²⁾H can be neglected. These assumptions allow us to use for the development of the desired algorithm the method of studying disturbed multidimensional linear systems described by Chernov and Yastrebov [35]. For its application, it is predefined for an arbitrary matrix A of m × n dimension a column vector A^(ν) formed from its elements as follows [35]:

$$A^{\left(\nu \right)}={\left|a_{11}{a}_{21}\dots a_{m1}{a}_{12}{a}_{22}\dots a_{m2}\dots a_{1n}{a}_{2n}\dots a_{mn}\right|}^T$$

The above conversion will be used later for the vector recording of the disturbed system estimation error.

2.2. Task Solution

Earlier, when task setting, Equation (3) of the filter error caused by the disturbance δH was written down. Since in general, the equation of the disturbed Kalman filter was obtained earlier in papers [35], then for the case under study it was written, taking into account only the disturbance δH:

$$\delta {\widehat{\xi }}_{i+1}=A_{\left(1\right)i+1}\delta {\widehat{\xi }}_i+A_{\left(2\right)i+1}\delta H_{}^{\left(\nu \right)}+A_{\left(3\right)i+1}\delta P_i^{\left(\nu \right)},$$

(4)

where

$$\begin{gathered} A_{(1)i+1}=\left(E-K_{i+1}{H}_{i+1}\right){\Phi }_{i+1/i,} \\ {A}_{(2)i+1}=P_i\otimes r^T{S}^{-1}-{\widehat{\xi }}_{i+1}^T\otimes K_{i+1}=P_i\otimes r^T{S}^{-1}-{\left({\Phi }_{i+1/i}{\widehat{\xi }}_i+K_{i+1}r\right)}^T\otimes K_{i+1}, \\ {A}_{(3)i+1}=r^T{S}^{-1}{H}_{i+1}{\Phi }_{i+1/i}\otimes \left(E-K_{i+1}{H}_{i+1}\right){\Phi }_{i+1/i}, \\ S=H_{i+1}{P}_{i+1/i}{H}^{\mathrm{T}}{}_{i+1}+R_{i+1}, \\ r=z_{i+1}-H_{i+1}{\Phi }_{i+1/i}{\widehat{\xi }}_i, \end{gathered}$$

⊗ is the block matrix multiplication symbol, and E is the unit matrix.

The random vector $\delta P_i^{\left(\nu \right)}$ included in the right part of Equation (4), also depends on fluctuations of the measurement matrix. Therefore, the equation of the vector $\delta P_i^{\left(\nu \right)}$, taking into account only the disturbance δH, was written in the following form:

$$\delta P_i^{\left(\nu \right)}=B_{\left(1\right)i}\delta P_{i-1}^{\left(\nu \right)}+B_{\left(2\right)i}\delta H_{}^{\left(\nu \right)},$$

(5)

where $\delta P_0^{\left(\nu \right)}$ is the error vector of elements of the a priori covariance matrix (equal to 0),

$$\begin{gathered} B_{(1)i}=[\left(E-K_{i}Hi\right){\Phi }_{i/i-1}{E}_{V1}\otimes \left(E-K_i{H}_i\right){\Phi }_{i/i-1}]\widehat{\otimes }{E}^{(\nu )}; \\ {B}_{\left(2\right)i}=-\left(K_i{E}_{V2}\otimes P_i\right)\widehat{\otimes }{E}^{\left(\nu \right)}-\left(P_i{E}_{V1}\otimes K_i\right)\widehat{\otimes }{E}^{\left(\nu \right)}; \\ {E}_{V1}=\left|E_n^{\left(1\right)}\otimes E_{m\left(1\right)}⋮E_n^{\left(2\right)}\otimes E_{m\left(1\right)}⋮ \dots ⋮ E_n^{\left(n\right)}\otimes E_{m\left(m\right)}\right|. \end{gathered}$$

$E_K^{\left(j\right)}$ is the j-th row of the unit matrix E of k × k dimension; E_K(j) is the j-th column,

$$E_{V2}=\left|E_{n\left(1\right)}\otimes E_m^{\left(1\right)}⋮E_{n\left(2\right)}\otimes E_m^{\left(1\right)}⋮ \dots ⋮ E_{n\left(n\right)}\otimes E_m^{\left(1\right)}⋮ \dots ⋮ E_{n\left(1\right)}\otimes E_m^{\left(2\right)}⋮ \dots ⋮E_{n\left(n\right)}\otimes E_m^{\left(m\right)}\right|.$$

Based on the recurrent relations arising from Equation (5), the vector $\delta P_i^{\left(v\right)}$ equations for each point of time i, starting from the initial one, were written in the following form:

$$\begin{gathered} \delta P_1^{\left(\nu \right)}=B_{\left(1\right)1}\delta P_0^{\left(\nu \right)}+B_{\left(2\right)1}\delta H_{}^{\left(\nu \right)}=B_{\left(2\right)1}\delta H_{}^{\left(\nu \right)}, \\ \delta P_2^{\left(\nu \right)}=B_{\left(1\right)2}\delta P_1^{\left(\nu \right)}+B_{\left(2\right)2}\delta H_{}^{\left(\nu \right)}=B_{\left(1\right)2}{B}_{\left(2\right)1}\delta H_{}^{\left(\nu \right)}+B_{\left(2\right)2}\delta H_{}^{\left(\nu \right)}, \\ \delta P_3^{\left(\nu \right)}=B_{\left(1\right)3}\delta P_2^{\left(\nu \right)}+B_{\left(2\right)3}\delta H_{}^{\left(\nu \right)}= \\ =B_{\left(1\right)3}{B}_{\left(1\right)2}{B}_{\left(2\right)1}\delta H_{}^{\left(\nu \right)}+B_{\left(1\right)3}{B}_{\left(2\right)2}\delta H_{}^{\left(\nu \right)}+B_{\left(2\right)3}\delta H_{}^{\left(\nu \right)}, \\ ⋮ \\ \delta P_i^{\left(\nu \right)}={\displaystyle \sum }_{j=1}^i({\displaystyle \prod }_{k=j+1}^i{B}_{\left(1\right)K})B_{\left(2\right)j}\cdot \delta H_{}^{\left(\nu \right)}=B_{iH}\cdot \delta H_{}^{\left(\nu \right)}, \end{gathered}$$

(6)

where $B_{iH}={\displaystyle \sum }_{j=1}^i({\displaystyle \prod }_{k=j+1}^i{B}_{\left(1\right)K})B_{\left(2\right)j}.$ In turn, the expression of the current vector $\delta {\widehat{\xi }}_{i+1}$, taking into account the Equation (6) obtained above, was previously presented as

$$\begin{gathered} \delta {\widehat{\xi }}_{i+1}=A_{\left(1\right)i+1}\delta {\widehat{\xi }}_i+A_{\left(2\right)i+1}\delta H_{}^{\left(\nu \right)}+A_{\left(3\right)i+1}\delta P_i^{\left(\nu \right)}= \\ =A_{\left(1\right)i+1}\delta {\widehat{\xi }}_i+A_{\left(2\right)i+1}\delta H_{}^{\left(\nu \right)}+A_{\left(3\right)i+1}{B}_{iH}\delta H_{}^{\left(\nu \right)}= \\ =A_{\left(1\right)i+1}\delta {\widehat{\xi }}_i+\left(A_{\left(2\right)i+1}+A_{\left(3\right)i+1}{B}_{iH}\right)\delta H_{}^{\left(\nu \right)}. \end{gathered}$$

(7)

Equation (7) allows, using the recurrent relations derived from it,

$$\begin{gathered} \delta {\widehat{\xi }}_1=A_{\left(1\right)1}\delta {\widehat{\xi }}_0+\left(A_{\left(2\right)1}+A_{\left(3\right)1}{B}_{0H}\right)\delta H_{}^{\left(\nu \right)}=A_{\left(2\right)1}\cdot \delta H_{}^{\left(\nu \right)}; \\ \delta {\widehat{\xi }}_2=A_{\left(1\right)2}\delta {\widehat{\xi }}_1+\left(A_{\left(2\right)2}+A_{\left(3\right)2}{B}_{1H}\right)\delta H_{}^{\left(\nu \right)}= \\ =A_{\left(1\right)2}{A}_{\left(2\right)1}\cdot \delta H_{}^{\left(\nu \right)}+\left(A_{\left(2\right)2}+A_{\left(3\right)2}{B}_{1H}\right)\delta H_{}^{\left(\nu \right)}= \\ =\left(A_{\left(1\right)2}{A}_{\left(2\right)1}+A_{\left(2\right)2}+A_{\left(3\right)2}{B}_{1H}\right)\delta H_{}^{\left(\nu \right)}; \\ \delta {\widehat{\xi }}_3=A_{\left(1\right)3}\delta {\widehat{\xi }}_2+\left(A_{\left(2\right)3}+A_{\left(3\right)3}{B}_{2H}\right)\delta H_{}^{\left(\nu \right)}= \\ =A_{\left(1\right)3}\left(A_{\left(1\right)2}{A}_{\left(2\right)1}+A_{\left(2\right)2}+A_{\left(3\right)2}{B}_{1H}\right)\delta H_{}^{\left(\nu \right)}+ \\ +\left(A_{\left(2\right)3}+A_{\left(3\right)3}{B}_{2H}\right)\delta H_{}^{\left(\nu \right)}= \\ =(A_{\left(1\right)3}\left(A_{\left(1\right)2}{A}_{\left(2\right)1}+A_{\left(2\right)2}+A_{\left(3\right)2}{B}_{1H}\right)+ \\ +A_{\left(2\right)3}+A_{\left(3\right)3}{B}_{2H})\delta H_{}^{\left(\nu \right)}; \\ \cdots \\ \delta {\widehat{\xi }}_i={\displaystyle \sum }_{j=1}^i\left({\displaystyle \prod }_{k=j+1}^i{A}_{\left(1\right)K}\right)A_{2j}\cdot \delta H_{}^{\left(\nu \right)}+ \\ +{\displaystyle \sum }_{j=1}^{i-1}\left({\displaystyle \prod }_{k=j+2}^i{A}_{\left(1\right)K}\right)A_{3\left(j+1\right)}\cdot B_{jH}\delta H_{}^{\left(\nu \right)}= \\ =[{\displaystyle \sum }_{j=1}^i\left({\displaystyle \prod }_{k=j+1}^i{A}_{\left(1\right)K}\right)A_{2j}+ \\ +{\displaystyle \sum }_{j=1}^{i-1}\left({\displaystyle \prod }_{k=j+2}^i{A}_{\left(1\right)K}\right)A_{3\left(j+1\right)}\cdot B_{jH}]\delta H_{}^{\left(\nu \right)} \end{gathered}$$

to express by induction the current value of the estimation error vector $\delta {\widehat{\xi }}_{i+1}$

$$\delta {\widehat{\xi }}_{i+1}=[{\displaystyle \sum }_{j=1}^{i+1}\left({\displaystyle \prod }_{k=j+1}^{i+1}{A}_{\left(1\right)K}\right)A_{2j}+{\displaystyle \sum }_{j=1}^i\left({\displaystyle \prod }_{k=j+2}^{i+1}{A}_{\left(1\right)K}\right)A_{3\left(j+1\right)}\cdot B_{jH}]\delta H_{}^{\left(\nu \right)}=A_{\left(i+1\right)H}\delta H_{}^{\left(\nu \right)},$$

(8)

where $A_{\left(i+1\right)H}={{\displaystyle \sum }}_{j=1}^{i+1}\left({{\displaystyle \prod }}_{k=j+1}^{i+1}{A}_{\left(1\right)K}\right)A_{2j}+{{\displaystyle \sum }}_{j=1}^i\left({{\displaystyle \prod }}_{k=j+2}^{i+1}{A}_{\left(1\right)K}\right)A_{3\left(j+1\right)}\cdot B_{jH}$ Since the found dependence of the disturbance of the estimate on the variations of the measurement matrix that caused it

$$\delta {\widehat{\xi }}_{i+1}=A_{\left(i+1\right)H}\delta H_{}^{\left(\nu \right)}$$

(9)

is linear, then after obtaining an accurate measurement of the state vector ${\xi }_{i+1}$ using the standard matrix inversion procedure and taking into account equality $\delta {\widehat{\xi }}_{i+1}={\xi }_{i+1}-{\widehat{\xi }}_{i+1}$, the error vector of the measurement matrix δH^(ν) components is easily determined. At the same time, in order to be able to correctly apply such a procedure, the dimensions of the vectors $\delta {\widehat{\xi }}_{i+1},\delta H_{}^{\left(\nu \right)}$ must match. In the presence of a single accurate measurement of the state vector, this is possible only in the case of a scalar observer (see Equation (2))—when the measurement matrix has 1 × N dimension. Then, the vector δH^(ν) is defined directly from Equation system (9):

$$\delta H_{}^{\left(\nu \right)}=A^{-1}{}_{(i+1)H}({\xi }_{i+1}-{\widehat{\xi }}_{i+1})$$

If the measurement matrix has M × N dimension, then the number of accurate measurements should be increased to M—in general, at random times: I + 1 + s₁, i + 1 + s₂, i + 1 + s₃, …, i + 1 + s_M, where s_i are random time intervals. Then, the Equation system (9) is converted to the following form:

$$\left|\begin{array}{c}\delta {\widehat{\xi }}_{i+1+s_1}\\ \delta {\widehat{\xi }}_{i+1+s_2}\\ \delta {\widehat{\xi }}_{i+1+s_3}\\ ⋮\\ \delta {\widehat{\xi }}_{i+1+s_M}\end{array}\right|=\left|\begin{array}{c}{A}_{\left(i+1+s_1\right)H}\\ A_{\left(i+1+s_2\right)H}\\ A_{\left(i+1+s_3\right)H}\\ ⋮\\ A_{\left(i+1+s_M\right)H}\end{array}\right|\delta H_{}^{\left(\nu \right)},$$

Which allows to correctly apply the matrix reversal procedure already for a general form measurement matrix:

$$\delta H_{}^{\left(\nu \right)}={\left|\begin{array}{c}{A}_{\left(i+1+s_1\right)H}\\ A_{\left(i+1+s_2\right)H}\\ A_{\left(i+1+s_3\right)H}\\ ⋮\\ A_{\left(i+1+s_M\right)H}\end{array}\right|}^{-1}\left|\begin{array}{c}\delta {\widehat{\xi }}_{i+1+s_1}\\ \delta {\widehat{\xi }}_{i+1+s_2}\\ \delta {\widehat{\xi }}_{i+1+s_3}\\ ⋮\\ \delta {\widehat{\xi }}_{i+1+s_M}\end{array}\right|,$$

having finally solved the task in general case.

The found value of the vector δH^(ν) makes it possible to correct the measurement matrix, thereby increasing the accuracy and stability of the Kalman filtering process as a whole.

Analyzing the aspects of the computational implementation of the proposed algorithm, it should be borne in mind that with the sequential execution of the recurrence relations given in Equations (6) and (8)—at each step of measurement, the calculation of matrices A_(i+1)H and B_iH require only three matrix multiplications and additions, which does not present any difficulties for modern estimator. To illustrate the effectiveness of the proposed approach, the following example was considered.

3. Results and Discussions

Numerical Solving the Adaptive Assessment of Navigation Parameters of an Unmanned Vehicle

Currently, one of the problems hindering the widespread use of an unmanned vehicle (UV) is the problem of ensuring their high-precision positioning by noisy measurements of the satellite navigation system in conditions of intense urban interference. At the same time, in urban conditions, it is easy to organize active information points with precisely known coordinates (beacons, optical reference points, etc.), which can provide the navigation system of unmanned vehicles with additional accurate positional measurements. In this regard, we will consider in this aspect the possibility of improving the accuracy of solving the navigation problem of an unmanned vehicle based on the proposed approach.

When solving the navigation problem, the UV-Equations of its navigation parameters (latitude and longitude) in the geographical coordinate system (GCS) had the following form:

$$\begin{gathered} \dot{\phi }=\frac{V_y}{\left(r+h\right)}, \\ \dot{\lambda }=\frac{V_x}{\cos \varphi \left(r+h\right)}, \end{gathered}$$

(10)

where φ and λ are geographical UV-latitude and UV-longitude, r is the radius of the Earth, h is the UV-altitude above sea level, and V_y, V_x are the linear UV-velocity projections on the GCS-axes.

The following data were determined as the initial data for modeling:

• Starting point coordinates of the UV-movement φ₀ = 0.76 rad and λ₀ = 0.32 rad;
• Time interval [0; 1000] s;
• UV-movement constant velocity V = 18 m s⁻¹;
• UV-trajectory along the Earth’s surface is loxodromic with an azimuthal angle A = 0.23 rad;
• Random height h changes generated by the trajectory relief are distributed according to the Gaussian law with zero mean and dispersion G_h = (0.14 m)²;
• Type of UV navigation system is satellite-based.

Based on the initial conditions, the UV-velocity projections on the GSK-axis are determined as follows:

$$\begin{gathered} V_x=V\cdot \sin A, \\ {V}_y=V\cdot \cos A. \end{gathered}$$

According to satellite messages received by the UV navigation system with a τ = 0.1 s clock cycle, navigation measurements are formed along longitude λ and latitude φ channels with a covariance matrix of measurement noise $R=\left|\begin{array}{cc}4\cdot {10}^{-10}& 0\\ 0& 9\cdot {10}^{-10}\end{array}\right|.$

Since a given UV-movement velocity causes a slight change in its coordinates at a given time interval, it turns out to be advisable to use instead of the general navigation Equation (10) their linearized version:

$$\begin{gathered} \dot{\phi }\left(t\right)=\frac{V_y\cdot \left(r+2h_0\right)}{{(r+h_0)}^2}-\frac{V_y}{{(r+h_0)}^2}\cdot h, \\ \begin{array}{c}\dot{\lambda }(t)=\frac{V_x}{\cos {\phi }_0\cdot (r+h_0)}\cdot \left[\frac{r+2h_0}{r+h_0}-\frac{sin{\phi }_0\cdot {\phi }_0}{\cos {\phi }_0}\right]-\frac{V_x}{\cos {\phi }_0\cdot {(r+h_0)}^2}\cdot h+\\ \\ +\frac{V_x\cdot sin{\phi }_0}{{\cos }^2{\phi }_0\cdot (r+h_0)}\cdot \phi ,\end{array} \end{gathered}$$

(11)

where h₀ = 0.

Equation (11) of true UV-movement was integrated by the Runge–Kutta method of the fourth order with a step of 0.001 s. Each 100th value was used to form satellite measurements (additive overlay of the corresponding noise of satellite navigation measurements) and for subsequent comparison with the estimates obtained.

The discrete Kalman filter, constructed according to the linearized equation (11) of UV-movement, has the following form:

$$\begin{gathered} {\widehat{\xi }}_{i+1}={\Phi }_{i+1/i}{\widehat{\xi }}_i+{\Omega }_{i+1}+K_{i+1}\left(z_{i+1}-H_{i+1}{\Phi }_{i+1/i}{\widehat{\xi }}_i\right), \\ {\widehat{\xi }}_i=\left|\begin{array}{c}{\widehat{\phi }}_i\\ {\widehat{\lambda }}_i\end{array}\right|, \\ {\Phi }_{i+\frac{1}{i}}=\left|\begin{array}{c}10\\ \tau \frac{V_{x}tg{\phi }_0}{r\cos {\phi }_0}1\end{array}\right|, \\ {\Omega }_{i+1}=\left|\begin{array}{c}\tau \frac{V_y}{r}\\ \tau \frac{V_x}{r\cos {\phi }_0}\left(1-{\phi }_{0}tg{\phi }_0\right)\end{array}\right|, \\ {G}_{i+1}=\left|\begin{array}{c}{(\frac{V_y}{r^2})}^2{\tau }^2{G}_h\frac{V_y}{r^2}\cdot \frac{V_x}{r^2\cos {\phi }_0}\cdot {\tau }^2{G}_h\\ \frac{V_y}{r^2}\cdot \frac{V_x}{r^2\cos {\phi }_0}\cdot {\tau }^2{G}_h{(\frac{V_x}{r^2\cos {\phi }_0})}^2{\tau }^2{G}_h\end{array}\right|, \\ {H}_{i+1}=\left|\begin{array}{c}1 0\\ 0 1\end{array}\right|. \end{gathered}$$

(12)

When moving along the trajectory, the UV crossed points with exactly known coordinates λ and φ (traffic lights—radio beacons) at 150, 158, 250, and 255 s. Up to 158 s, the error δH in determining the measurement matrix was set as an additive matrix $\delta H=\left|\begin{array}{c}0.31 0.04\\ 0.05 0.28\end{array}\right|$, from the 200th to the 255th s, respectively, as $\delta H=\left|\begin{array}{c}0.11 0.015\\ 0.03 0.12\end{array}\right|$. According to the proposed approach, the exact values of λ and φ at the 158th and 255th s were used to determine the values of the components of the δH matrices, which were then used to correct the measurement matrix in the filter (12).

Modeling of the UV-movement and evaluation of its navigation parameters was carried out for 100 variants of trajectory modeling and evaluation of its parameters for both correctable and noncorrectable measurement matrices.

The graphs of errors in estimating navigation parameters obtained by implementing the filter (12) for a typical case of estimation with an uncorrectable measurement matrix are shown in Figure 1. In this case, the errors in the estimates of the current coordinates of the BA are very significant: at the end of the modeling interval, the error in estimating latitude reached 33.8 m, with a tendency for a further sharp increase (longitude—9 m), also having a divergent character. Moreover, the statistical analysis of estimation errors carried out for all modeling variants showed that the average variance of the deviation relative to the true values of the navigation parameters reaches 650 m².

When organizing an adaptive filter in accordance with the above algorithm, the estimation errors for the same modeling variant have the form shown in Figure 2.

In this case, it can be seen that the use of the proposed algorithm led to a sharp decrease in estimation errors compared to the traditional Kalman filter—up to 1.2 m in latitude and 1.12 m in longitude. At the same time, such a sharp decrease in errors begins already from about the 500th s while having a pronounced stable character until the end of the modeling interval. Statistical analysis of estimation errors for the proposed algorithm showed that in this case, the average variance of the deviation relative to the true values of the navigation parameters does not exceed 3 m², which is much more accurate than the traditional filtering scheme.

4. Conclusions

The sharp difference between the statistical estimates of the average variances of deviations relative to the true values of the navigation parameters obtained using the classical Kalman and the proposed filtering algorithms allows us to conclude about the advantages of the latter, despite such a disadvantage as a slight increase in computational costs. In general, the simplicity and accuracy of the proposed algorithm provide the possibility of its effective application for the widest class of information and measurement systems. Moreover, based on this approach, developed by complexing two fundamentally different methods, the use of irregular accurate observations of the object’s state vector and the mathematical apparatus for the study of disturbed multidimensional linear systems, it is possible to further generalize the identification procedure to the remaining parameters of the Kalman filter, in particular the covariance matrices of object noise and measurements, which will further improve the accuracy and stability of the filtration process as a whole.