High-Order Extended Kalman Filter for State Estimation of Nonlinear Systems

Abstract

In general, the extended Kalman filter (EKF) has a wide range of applications, aiming to minimize symmetric loss function (mean square error) and improve the accuracy and efficiency of state estimation. As the nonlinear model complexity increases, rounding errors gradually amplify, leading to performance degradation. After multiple iterations, divergence may occur. The traditional extended Kalman filter cannot accurately estimate the nonlinear model, and these errors still have an impact on the accuracy. To improve the filtering performance of the extended Kalman filter (EKF), this paper proposes a new extended Kalman filter (REKF) method that utilizes the statistical properties of the rounding error to enhance the estimation accuracy. After establishing the state model and measurement model, the residual term is used to replace the higher-order term in the Taylor expansion, and the least squares method is applied to identify the residual term step by step. Then, the iterative process of updating the extended Kalman filter is carried out. Within the Kalman filter framework, a higher-order rounding error-based extended Kalman filter (REKF) is designed for the joint estimation of rounding error and random variables, and the solution method for the rounding error is considered for the multilevel approximation of the original function. Through numerical simulations on a general nonlinear model, the higher-order rounding error-based extended Kalman filter (REKF) achieves better estimation results than the extended Kalman filter (EKF) and improves the filtering accuracy by utilizing the higher-order rounding error information, which also proves the effectiveness of the proposed method.

1. Introduction

The Kalman filter (KF) is a popular state estimation algorithm used in a variety of applications, including localization, navigation, sensor networks, and battery management [1]. The Kalman filter estimates the state for a linear system and obtains an optimal estimate in the presence of stochastic disturbances; therefore, the KF method is also known as the optimal linear estimation method [2]. The KF is based on the minimum mean square error for state estimation, and the state estimation process can be regarded as a filtering process because the state and the observed data are usually affected by system noise [3]. The KF is a recursive estimation algorithm that iteratively obtains updated state values and predicted values. It provides a powerful tool to deal with the state estimation problem of linear systems [4]. However, many engineering systems cannot be described by simple linear systems. For example, the autopilot system of a car and the target tracking system of a drone are complex nonlinear systems. If we only use the Kalman filter (KF) for state estimation, it cannot reach the accuracy we need. When the drone performs tracking tasks or the car turns on the autopilot system, it may cause serious accidents. In 2023, in San Francisco, United States, more than 700 cases of car accidents occurred involving automated vehicles that failed to slow down, resulting in hundreds of deaths. Most of these autopilot incidents were the result of insufficient accuracy. For this reason, improving the accuracy of the algorithm is a very important task. In order to solve the problem of nonlinear systems, they need to be converted into linear systems to be solved. Usually, the extended Kalman filter (EKF) is used to deal with nonlinear systems. The EKF algorithm is an algorithm adapted to a variety of complex systems [5]. Currently, the extended Kalman filter (EKF) has successfully become the most widely used state observer for nonlinear systems [6]. The EKF is used in systems with zero-mean Gaussian process uncertainty and measurement noise [7]. The EKF is an extension of the KF for nonlinear dynamical systems in that it provides a more accurate estimate of the state when the modeling of the dynamical system exactly matches the dynamics it describes. The system is linearized around the predicted state estimates [8]. The state estimation process can also be viewed as a filtering process since the state and the observed data are usually affected by system noise. The traditional extended Kalman filter (EKF) has been successfully applied to many systems. The EKF uses a Taylor expansion of the nonlinear part of the model at the estimates, and linearization is achieved by a first-order approximation, which transforms the nonlinear problem into a linear KF problem [9]. However, when the linearization error is large and the model is uncertain, the performance of the EKF will be greatly reduced, and even divergence phenomena can be observed [10,11]. This paper proposes a method to utilize the rounding error to solve the EKF problem. In this paper, we propose a new extended Kalman filter (EKF) method designed to enhance estimation accuracy by leveraging the statistical properties of the rounding error [12,13]. This approach involves approximating the rounding error using a multilevel approximation of the original function and substituting the rounded higher-order term with the residual term in the traditional extended Kalman filter. This modification aims to address nonlinear equations more accurately and stably, making it particularly suitable for engineering systems [14].

The first section introduces KF and EKF’s importance in applied systems, shows that EKF falls short in certain complex systems’ requirements, and proposes a new REKF method to improve precision. The second part mainly explains the principles and calculations of EKF and REKF, compares their performances, and analyzes their theoretical advantages and disadvantages in accuracy and stability. In the third part, MATLAB simulations are conducted to verify the stability and accuracy of the REKF method. Lastly, the article summarizes REKF’s advantages and looks toward the future.

2. Algorithms of EKF and REKF

2.1. Principle of EKF Algorithm

The mathematical structure of the extended Kalman filter (EKF) is very simple and uses a recursive approach, where the a posteriori estimate obtained from the last computation is used as the a priori estimate for the next computation [15], and each time, the current state estimate is computed recursively from only the measured variables from the previous computation, which is able to give optimal estimates for the system variables, and the second-order and higher terms are omitted from the Taylor expansion to obtain an approximately linear model [16].

A class of systems exists where the equations of state are nonlinear and the observation equations are linear: in this paper, the rounding error rounded off after Taylor expansion is considered, where $\xi (k)$ represents the rounding error of the Taylor expansion of a higher order.

Definition 1.

Consider the following time-invariant stochastic differential equation.

$$x(k+1)=f(x(k))+w(k)$$

(1)

$$y(k+1)=h(x(k+1))+v(k+1)$$

(2)

where $f(\cdot )$ and $h(\cdot )$ represent the state equation and measurement equation, respectively, $k\in T:=\left\{k\right|k=1,2,3\cdots ,T-1\}$. The time scale is $T$, $x(k+1)\in R^n$ is the $n$ dimensional state vector, $y(k+1)\in R^m$ is the $m$ dimensional measurement vector, and $w(k)$ and $v(k+1)$ are the state modeling error noise vector and measurement error noise vector, respectively. $f(x(k))$ is a nonlinear functional model of the state, with $x(k),w(k),k$ as the common actions. For the state, there are $x(k)$ with order derivatives continuous up to $r$, and order derivatives exist at $r+1$. $h(x(k+1))$ is a nonlinear functional model of the state with $x(k+1),v(k+1),k+1$. The vectors $w~N(0,Q)$ and $v~N(0,R)$ are independent and obey Gaussian distributions. The Gaussian random variables denote the process noise and the measurement noise, respectively, whose corresponding covariance matrices are $Q$ and $R$, and they also satisfy the following statistical properties:

$$\{\begin{array}{c}E\{w(k)\}=0,E\{w(k)w^T(k)\}=Q(k);\\ E\{v(k+1)\}=0,E\{v(k+1)v^T(k+1)\}=R(k+1);\\ E\{w(k)v^T(k+1)\}=0,E\{w(k)v^T(k+1)\}=0; \\ E\{w(k)x^T(k)\}=0, E\{v(k+1)x^T(k)\}=0;\end{array}$$

(3)

The filtered estimate at time $k$ is regarded as $\widehat{x}(k|k)$. The nonlinear function $f(x(k))$ of the state equation in Equation (1) is Taylor-expanded at $\widehat{x}(k|k)$.

$$\begin{array}{c}f(x(k))=f(\widehat{x}(k|k))+{\displaystyle \sum _{l=1}^r{\frac{1}{l!}\frac{{\partial }^{r}f(\widehat{x}(k|k))}{\partial x^r(k)}|}_{x(k)=\widehat{x}(k|k)}}{[x(k)-\widehat{x}(k|k)]}^r\\ +{\frac{1}{(r+1)!}\frac{{\partial }^{r+1}f(\widehat{x}(k|k))}{\partial x^{r+1}(k)}|}_{x(k)=\xi (k)}{[x(k)-\widehat{x}(k|k)]}^{r+1}\end{array}$$

(4)

By omitting the second-order and its higher-order term, the following is obtained:

$$f(x(k))\approx f(\widehat{x}(k|k))+{\frac{\partial f(\widehat{x}(k|k))}{\partial x(k)}|}_{x(k)=\widehat{x}(k|k)}[x(k)-\widehat{x}(k|k)]$$

(5)

In Equation (5), let:

$$A(k+1|k)={\frac{\partial f(\widehat{x}(k|k))}{\partial x(k)}|}_{x(k)=\widehat{x}(k|k)}$$

(6)

$$△f(\widehat{x}(k|k))=f(\widehat{x}(k|k))-A(k+1|k)\widehat{x}(k|k)$$

(7)

The equation of the state with noise $x(k+1)$ is as follows:

$$x(k+1)\approx A(k+1|k)x(k)+w(k)+△f(\widehat{x}(k|k))$$

(8)

The nonlinear part $h(x(k+1))$ in Formula (2) is expanded by the Taylor formula in the estimate $\widehat{x}(k+1|k)$.

$$\begin{array}{c}h(x(k+1))=h(\widehat{x}(k+1|k))+{\displaystyle \sum _{l=1}^r{\frac{1}{l!}\frac{{\partial }^{r}h(\widehat{x}(k+1|k))}{\partial x^r(k+1)}|}_{x(k+1)=\widehat{x}(k+1|k)}}{[x(k+1)-\widehat{x}(k+1|k)]}^r\\ {+\frac{1}{(r+1)!}\frac{{\partial }^{r+1}h(\widehat{x}(k+1|k))}{\partial x^{r+1}(k+1)}|}_{x(k+1)=\widehat{x}(k+1|k)}{[x(k+1)-\widehat{x}(k+1|k)]}^{r+1}\end{array}$$

(9)

By omitting the second-order and its higher-order term, the following is obtained:

$$h(x(k+1))\approx h(\widehat{x}(k+1|k))+{\frac{\partial h(\widehat{x}(k+1|k))}{\partial x(k+1)}|}_{x(k+1)=\widehat{x}(k+1|k)}[x(k+1)-\widehat{x}(k+1|k)]$$

(10)

In Equation (10), let:

$${H(k+1)=\frac{\partial h(\widehat{x}(k+1|k))}{\partial x(k+1)}|}_{x(k+1)=\widehat{x}(k+1|k)}$$

(11)

$$△h(\widehat{x}(k+1|k))=h(\widehat{x}(k+1|k))-H(k+1)\widehat{x}(k+1|k)$$

(12)

The measurement equation can be approximately represented as:

$$y(k+1)=H(k+1)x(k+1)+v(k+1)+△h(\widehat{x}(k+1|k))$$

(13)

where $\widehat{x}(k+1|k)$ is the state prediction value of the Kalman filter, $y(k+1)$ is the measurement observation of the Kalman filter, $A(k+1|k)$ and $H(k+1)$ are the first-order Jacobi matrices of the nonlinear model of the system where the state equation and the observation equation are derived at $\widehat{x}(k|k)$ and $\widehat{x}(k+1|k)$, and $△f(\widehat{x}(k|k))$ and $△h(\widehat{x}(k+1|k))$ are known terms of the Taylor expansion of the equations of state and observation and can be regarded as a constant term.

Extending the prediction and update process of the Kalman filter:

The prediction estimates and prediction error values for the state equations in the model can be obtained according to Equation (8):

Projected estimates:

$$\widehat{x}(k+1|k)\approx A(k+1|k)\widehat{x}(k|k)+△f(\widehat{x}(k|k))$$

(14)

Predicted error value:

$$\tilde{x}(k+1|k)\approx A(k+1|k)\tilde{x}(k|k)+w(k)$$

(15)

The measurement estimates and measurement estimation errors of the measurement equations in the model can be obtained according to Equation (13):

Measurement estimate:

$$\widehat{y}(k+1|k)\approx H(k+1)\widehat{x}(k+1|k)+△h(\widehat{x}(k+1|k))$$

(16)

Measurement estimation error:

$$\tilde{y}(k+1|k)\approx H(k+1)\tilde{x}(k+1|k)+v(k+1)$$

(17)

Predicted error covariance:

$$P_{xx}(k+1|k)\approx A(k+1|k)P(k|k)A{(k+1|k)}^T+Q(k)$$

(18)

The filter values for the Kalman observer with time-varying gain are as follows:

$$x(k+1|k+1)\approx \widehat{x}(k+1|k)+K(k+1)(\tilde{y}(k+1|k))$$

(19)

According to the orthogonality principle $E\{\tilde{x}(k+1|k+1)y^T(k+1)\}=0$, the Kalman gain $K(k+1)$ is solved for:

$$K(k+1)\approx P_{xx}(k+1|k)H{(k+1)}^T{(H(k+1)P_{xx}(k+1|k)H{(k+1)}^T+R(k+1))}^{-1}$$

(20)

The state $x(k+1)$ estimation error covariance was calculated:

$$P_{xx}(k+1|k+1)\approx (I-K(k+1)H(k+1))P_{xx}(k+1|k)$$

(21)

According to the standard extended Kalman filter, the statistical properties of the initial state $x(0)$ are known: $E\{x(0)\}={\widehat{x}}_0,$ $E\{(x(0)-{\widehat{x}}_0){(x(0)-{\widehat{x}}_0)}^T\}=P_0$ [17], where $\tilde{x}(k+1|k)$ is the estimation error, $\widehat{y}(k+1|k)$ is the prediction observation, $\tilde{y}(k+1|k)$ is the error, $P_{xx}(k+1|k)$ and $P_{xx}(k+1|k+1)$ represent the prediction error covariance and estimation error covariance, $K(k+1)$ represents the Kalman gain, and $x(k+1|k+1)$ represents the filtered value of the extended Kalman.

2.2. Extended Kalman Filter with Residuals (REKF)

Based on the extended Kalman filter (EKF), the consideration of higher-order terms in the Taylor expansion of the state equation can further improve the model’s accuracy by introducing residuals to replace the higher-order terms. By applying the least squares method step by step to identify the higher-order terms replaced by residuals, the residual extended Kalman filter (REKF) enhances the utilization of the effects of higher-order terms compared to the EKF. This can reflect the real dynamics of the system more accurately so as to improve the filtering accuracy. This method is particularly suitable for systems with strong nonlinearity and can effectively enhance the accuracy of state estimation.

2.2.1. Extended Kalman Filtering Using Residual Terms Instead of Higher-Order Terms

Assumption 1.

In Equation (1), the equation of state $x(k+1)$ has continuity up to the order derivative of r, and the order derivative of $r+1$ exists.

The nonlinear function $f(x(k))$ in the model when Assumption 1 holds is given by the $\widehat{x}(k|k)$ Taylor expansion:

$$\begin{array}{c}x(k+1)=f(\widehat{x}(k|k))+{\displaystyle \sum _{l=1}^r{\frac{1}{l!}\frac{{\partial }^{r}f(\widehat{x}(k|k))}{\partial x^r(k)}|}_{x(k)=\widehat{x}(k|k)}}{[x(k)-\widehat{x}(k|k)]}^r+\\ {\frac{1}{(r+1)!}\frac{{\partial }^{r+1}f(\widehat{x}(k|k))}{\partial x^{r+1}(k)}|}_{x(k)=\xi (k)}{[x(k)-\widehat{x}(k|k)]}^{r+1}+w(k)\end{array}$$

(22)

Replace the second-order term and its higher-order term with the residual term of the equation of state:

$$\begin{array}{ll}x(k+1)& =f(x(k))+w(k)\\ & =△f(\widehat{x}(k|k))+A(k+1|k)x(k)+\xi (k)+w(k)\end{array}$$

(23)

Among these, $\xi (k)={\xi }_1(k)$, $\xi (k)$ represents the second order of the Taylor expansion and its higher-order terms.

The residual term in the equation of state is first identified by the least squares method. The identification process is as follows:

Replace the second-order term of the Taylor expansion with a residual term in the equation for which the Taylor expansion is performed at $\widehat{x}(k|k)$:

$$\begin{array}{ll}x(k+1)& =f(x(k))+w(k)\\ & =△f(\widehat{x}(k|k))+A(k+1|k)x(k)+{\xi }_1(k)+w(k)\end{array}$$

(24)

Bring Equation (24) into the observation equation:

$$\begin{array}{ll}y(k+1)& =H(k+1)x(k+1)+v(k+1)+△h(\widehat{x}(k+1|k)\\ & =H(k+1)(△f(\widehat{x}(k|k)+A(k+1|k)x(k)\\ & +{\xi }_1(k)+w(k))+v(k+1)+△h(\widehat{x}(k+1|k)\end{array}$$

(25)

Reduce (25) to (26):

$$y^{(1)}(k+1)=H(k+1){\xi }_1(k)+v^{(1)}(k+1)$$

(26)

Among these, $y^{(1)}(k+1)$ and $v^{(1)}(k+1)$. The parameters are as follows:

$$\{\begin{array}{l}{y}^{(1)}(k+1)=y(k+1)-H(k+1)△f(\widehat{x}(k|k)-H(k+1)A(k+1|k)\widehat{x}(k|k)-△h(\widehat{x}(k+1|k)\\ v^{(1)}(k+1)=H(k+1)w(k)+v(k+1)+H(k+1)A(k+1|k)\tilde{x}(k|k)\end{array}$$

(27)

Based on the least squares method, ${\xi }_1(k)$, ${\xi }_1(k)$ in (26) is identified as a Gaussian distribution with ${\widehat{\xi }}_1(k|k)$ as the variance and ${P_{\xi }}^{(1)}(k|k)$ as the variance, ${\xi }_1(k)~[{\widehat{\xi }}_1(k|k),{P_{\xi }}^{(1)}(k|k)]$. The identification process is as follows:

$$\{\begin{array}{l}{\xi }_1(k)={\widehat{\xi }}_1(k|k)+{\tilde{\xi }}_1(k|k)\\ {\widehat{\xi }}_1(k|k)=H^T(k+1){[H(k+1)H^T(k+1)]}^{-1}(y^{(1)}(k+1))\end{array}$$

(28)

Judge whether to meet the system accuracy requirements: By setting the threshold $\lambda$, determine whether the paradigm $\parallel {\widehat{\xi }}_1(k|k)\parallel <\lambda$ of the estimate of the residual term is valid. If it is valid, then it means that the Taylor expansion only needs to be expanded to the second order to reach the accuracy needed. If it is not valid, then the Taylor expansion needs to continue (the specific parameters are in Appendix A).

It can be recursively proved up to order 2 and r − 1: Appendix B.

Definition 2.

According to the mathematical induction method, it can be deduced to the order of $r+1$. Assuming that the system accuracy requirement is satisfied when the Taylor expansion is carried out to the order of $r+1$, the residual term ${\xi }_r(k|k)$ is used to replace the order term of $r+1$ after the Taylor expansion, ${\widehat{\xi }}_r(k|k)$ is the valid information extracted from ${\tilde{\xi }}_{r-1}(k|k)$, and ${\tilde{\xi }}_{r-1}(k|k)$ is attributed to ${\tilde{\xi }}_r(k|k)$ after the feature extraction. Moreover, $\xi (k)={\xi }_1(k)+\cdots +{\xi }_r(k)$.

$$\begin{array}{ll}x(k+1)& =f(x(k))+w(k)\\ & =△f(\widehat{x}(k|k))+A(k+1|k)x(k)+{\xi }_1(k)+\cdots +{\xi }_r(k)+w(k)\end{array}$$

(29)

Bring Equation (29) into the observation equation:

$$\begin{array}{ll}y(k+1)& =H(k+1)x(k+1)+v(k+1)+△h(\widehat{x}(k+1|k)\\ & =H(k+1)(△f(\widehat{x}(k|k))+A(k+1|k)x(k)\\ & +{\xi }_1(k)+\cdots +{\xi }_r(k)+w(k))+v(k+1)+△h(\widehat{x}(k+1|k)\end{array}$$

(30)

Organize (30) as:

$$y^{(r)}(k+1)=H(k+1){\xi }_r(k)+v^{(r)}(k+1)$$

(31)

The parameter is $y^{(r)}(k+1)$. The equation is:

$$\begin{array}{ll}{y}^{(r)}(k+1)& =y(k+1)-H(k+1)△f(\widehat{x}(k|k)-H(k+1)A(k+1|k)\widehat{x}(k|k)\\ & -H(k+1){\widehat{\xi }}_1(k|k)-\cdots -H(k+1){\widehat{\xi }}_{r-1}(k|k)-△h(\widehat{x}(k+1|k)\end{array}$$

(32)

The parameter is $v^{(r)}(k+1)$. The equation is:

$$v^{(r)}(k+1)=H(k+1)A(k+1|k)\tilde{x}(k|k)+H(k+1)w(k)+v(k+1)$$

(33)

Using the least squares method, the identification parameters ${\xi }_r(k)$, ${\xi }_r(k)$ conform to a Gaussian distribution, with ${\widehat{\xi }}_r(k|k)$ as the mean and ${P_{\xi }}^{(r)}(k|k)$ as the variance:

$${\widehat{\xi }}_r(k|k)=H{(k+1)}^T{[H(k+1)H{(k+1)}^T]}^{-1}(y^{(r)}(k+1))$$

(34)

In nonlinear models, when the Taylor expansion is carried out up to the nth order, the approximation is valid, satisfying the required system accuracy, and the parameter identification for all the higher-order terms is complete, with the residual term replacing the higher-order terms (the specific parameters are in Appendix C).

REKF algorithm flow chart (Figure 1):

2.2.2. REKF Implementation Projections and Updates

The nonlinear observation equations and nonlinear state equations have been linearized, allowing the prediction and update steps to be performed in accordance with the Kalman filtering process. This approach streamlines and optimizes the overall filtering procedure. The process is shown in Figure 1, and the steps are as follows:

The state and observation equations for a class of nonlinear system models are:

$$\begin{array}{ll}x(k+1)& =f(x(k))+w(k)\\ & =△f(\widehat{x}(k|k))+A(k+1|k)x(k)+{\xi }_1(k)+\cdots +{\xi }_r(k)+w(k)\end{array}$$

(35)

(1)

REKF prediction steps:

The predicted estimate of the state equation:

$$\widehat{x}(k+1|k)=△f(\widehat{x}(k|k))+A(k+1|k)\widehat{x}(k|k)+{\widehat{\xi }}_1(k)+\cdots +{\widehat{\xi }}_r(k)$$

(36)

The prediction error of the state equation:

$$\tilde{x}(k+1|k)=A(k+1|k)\tilde{x}(k|k)+{\tilde{\xi }}_r(k)+w(k)$$

(37)

In this context, $\tilde{\xi }(k)$ represents the prediction error of the residual term in the state equation, while $\widehat{\xi }(k)$ denotes the predicted estimate of the residual term in the state equation.

According to the solution formula of Kalman filtering, it can be seen that the estimation error covariance $P(k+1|k)$ is as follows:

$$\begin{array}{ll}P(k+1|k)=& A(k+1|k)P(k|k)A{(k+1|k)}^T\\ & +{P_{\xi }}^{(r)}(k+1|k)+Q(k)\end{array}$$

(38)

In this context, $P(k|k)=\{\tilde{x}(k|k)\tilde{x}{(k|k)}^T\}$ represents the estimated error covariance at time k.

The predicted estimate of the measurement equation:

$$\widehat{y}(k+1|k)=H(k+1)\widehat{x}(k+1|k)+△h(\widehat{x}(k+1|k))$$

(39)

Predictive estimation errors in the measurement equations:

$$\tilde{y}(k+1|k)=H(k+1)\tilde{x}(k+1|k)+v(k+1)$$

(40)

(2)

Update steps:

Kalman gain $Kk(k+1)$:

$$Kk(k+1)=P(k+1|k)H{(k+1)}^T{(H(k+1)P(k+1|k)H{(k+1)}^T+R(k+1))}^{-1}$$

(41)

Expanded-Kalman-filtered value of the state equation at $k+1$ time:

$$\widehat{x}(k+1|k+1)=\widehat{x}(k+1|k)+Kk(k+1)\tilde{y}(k+1|k)$$

(42)

Find the estimation error covariance at the time of $k+1$:

$$\begin{array}{ll}P(k+1|k+1)& =E\{\tilde{x}(k+1|k+1){(\tilde{x}(k+1|k+1))}^T\}\\ & =(I-Kk(k+1)H(k+1))P(k+1|k)\end{array}$$

(43)

The extended Kalman prediction and update solution for the remaining substitutions is complete.

2.3. REKF Performance Analysis

(1)

Predictive stage performance analysis

During the prediction stage, the REKF state equation performs better than the EKF in feature extraction. It utilizes a multi-level approximation method for rounding error calculation to improve the estimation accuracy. And the error exists only in the highest-order residual term.

In terms of performance, the smaller the error covariance, the closer the filtered value in the system is to the true value. During the prediction stage, a (r + 1)th order $x_{r+1}(k+1)$ in the Taylor expansion is considered the true value, and the error exists only in the highest-order residual term. Refer to Appendix D for details.

The following content focuses solely on presenting the results.

Taylor expansion to an $r+1$ order of error covariance:

$${p^{(r+1)}}_{xx}(k+1|k)=A(k+1|k)p(k|k)A{(k+1|k)}^T+{p^{(r)}}_{\zeta }(k|k)+Q(k)$$

(44)

Taylor expansion to an  order of error covariance:

$${p^{(r)}}_{xx}(k+1|k)={p^{(r+1)}}_{xx}(k+1|k)+{\widehat{\xi }}^{(r)}(k|k){\widehat{\xi }}^{(r)}{(k|k)}^T$$

(45)

Estimated error covariance in the Taylor expansion to the first order:

$${p^{(1)}}_{xx}(k+1|k)={p^{(2)}}_{xx}(k+1|k)+{\widehat{\xi }}^{(1)}(k|k){\widehat{\xi }}^{(1)}{(k|k)}^T$$

(46)

${\widehat{\xi }}^{(1)}(k|k){\widehat{\xi }}^{(1)}{(k|k)}^T\cdots {\widehat{\xi }}^{(r)}(k|k){\widehat{\xi }}^{(r)}{(k|k)}^T$ are all greater than or equal to 0, and ${\widehat{\xi }}^{(1)}(k){\widehat{\xi }}^{(1)}{(k)}^T$ is equal to 0 only if the matrix is orthogonal to ${\widehat{\xi }}^{(1)}(k){\widehat{\xi }}^{(1)}{(k)}^T$. Similarly, ${\widehat{\xi }}^{(r)}(k|k){\widehat{\xi }}^{(r)}{(k|k)}^T$ is equal to 0 only if the matrix is orthogonal to ${\widehat{\xi }}^{(r)}(k|k){\widehat{\xi }}^{(r)}{(k|k)}^T$.

From this, it follows that:

$${p^{(1)}}_{xx}(k+1|k)\ge {p^{(2)}}_{xx}(k+1|k)\ge \cdots \ge {p^{(r+1)}}_{xx}(k+1|k)$$

(47)

Expand to the $r+1$ order error covariance minimum:

$$\begin{array}{c}{p^{(r)}}_{xx}(k+1|k)={p^{(r+1)}}_{xx}(k+1|k)+{\widehat{\xi }}^{(r)}(k|k){\widehat{\xi }}^{(r)}{(k|k)}^T\\ {p^{(r-1)}}_{xx}(k+1|k)={p^{(r)}}_{xx}(k+1|k)+{\widehat{\xi }}^{(r-1)}(k|k){\widehat{\xi }}^{(r-1)}{(k|k)}^T\\ ⋮\\ ⋮\\ {p^{(2)}}_{xx}(k+1|k)={p^{(3)}}_{xx}(k+1|k)+{\widehat{\xi }}^{(2)}(k|k){\widehat{\xi }}^{(2)}{(k|k)}^T\\ {p^{(1)}}_{xx}(k+1|k)={p^{(2)}}_{xx}(k+1|k)+{\widehat{\xi }}^{(1)}(k|k){\widehat{\xi }}^{(1)}{(k|k)}^T\end{array}$$

(48)

Upon comparison, it becomes evident that the (r + 1)th order Taylor expansion exhibits the smallest error covariance and the highest accuracy. In the prediction stage, REKF extracts more information, leading to a reduced prediction error covariance matrix. Consequently, the prediction accuracy of REKF is superior to that of the traditional EKF.

(2)

Analysis of performance indicators in the update phase

Our REKF accuracy is greater than EKF in the prediction phase and in the update phase:

$$\begin{array}{c}{p^{(1)}}_{xx}{(k+1|k+1)}^{-1}={p^{(1)}}_{xx}{(k+1|k)}^{-1}+(H^T(k+1)R^{-1}(k)H(k+1));\\ {p^{(2)}}_{xx}{(k+1|k+1)}^{-1}={p^{(2)}}_{xx}{(k+1|k)}^{-1}+(H^T(k+1)R^{-1}(k)H(k+1));\\ ⋮\\ ⋮\\ {p^{(r)}}_{xx}{(k+1|k+1)}^{-1}={p^{(r)}}_{xx}{(k+1|k)}^{-1}+(H^T(k+1)R^{-1}(k)H(k+1));\\ {p^{(r+1)}}_{xx}{(k+1|k+1)}^{-1}={p^{(r+1)}}_{xx}{(k+1|k)}^{-1}+(H^T(k+1)R^{-1}(k)H(k+1));\end{array}$$

(49)

Available by comparison:

$${p^{(1)}}_{xx}{(k+1|k+1)}^{-1}\le {p^{(2)}}_{xx}{(k+1|k+1)}^{-1}\le \cdots \le {p^{(r+1)}}_{xx}{(k+1|k+1)}^{-1}$$

(50)

By analyzing the performance of EKF and REKF, it becomes evident that the new extended Kalman filter leverages the statistical properties of the rounding error to enhance the estimation accuracy. A higher-order extended Kalman filter (REKF) was established for the joint estimation of rounding errors and random variables. The method of solving rounding errors, considering the multilevel approximation of the original function, offers higher accuracy compared to the traditional EKF approach.

3. Simulation Experiments

Simulation experiments can be conducted to verify the enhanced performance of the REKF compared to the EKF. These experiments can be carried out using MATLABR 2021b software.

Simulation experiment I: Consider a system where the measurement equation is linear and the equation of state is nonlinear.

$$\begin{array}{l}x(k+1)=\left[\begin{array}{c}{x}_1(k+1)\\ x_2(k+1)\end{array}\right]=\left[\begin{array}{c}5\sin x_1(k+1)+5\cos x_2(k+1)\\ \sin x_1(k+1)+\cos x_2(k+1)\end{array}\right]+w_k\\ \left[y(k+1)\right]=\left[2x_1(k+1)\right]+\left[2x_2(k+1)\right]+v_{k+1}\end{array}$$

where $x(k+1)={[x_1,x_2]}^T$, $y(k+1)={[y_1,y_2]}^T$, $w_k={[w_1,w_2]}^T$, $v_{k+1}={[v_1,v_2]}^T$$w_k$, and $v_{k+1}$ are independent white noises obeying normal distribution in the system, $w_k~N(0,Q)$ and $v_{k+1}~N(0,R)$, and satisfy the conditions in Equation (3). $w_k$ and $v_{k+1}$ are mutually uncorrelated white noise, $Q=diag\{0.7,1.3\}$ and $R=1$, assuming that the initial true value of the original state model is the initial value of the original model state, ${\widehat{x}}_0={[1,1]}^{\mathrm{T}}$, and the initial estimation error covariance matrix is $P_0=I\in R^{2\times 2}$. The estimation of the target states $x_1$ and $x_2$ are compared to the target states using the extended Kalman filter (EKF) with the Kalman filter with high-precision residual term substitution (REKF).

Figure 2 shows that the method proposed in this paper is more accurate.

Table 1. X1 performance comparison.

X1 Performance Enhancement	Value	EKF	First-Order EKF
EKF root mean square error	1.2922	/	/
EKF first-order root mean square error	1.2757	1.28%	/
EKF higher-order root mean square error	1.2028	6.92%	5.62%

shows that: In X1, the precision of the high-order REKF with residual substitution was improved by about 6.92% compared to the traditional EKF, and the precision of the high-order REKF with residual substitution was improved by about 5.62% compared to the first-order REKF with residual substitution.

In the scenario where the state equation exhibited weak nonlinearity and the observation equation was linear, the REKF demonstrated superior performance compared to the EKF in terms of error reduction and stability improvement.

Table 2. X2 performance comparison.

X2 Performance Enhancement	Value	EKF	First-Order EKF
EKF root mean square error	1.2723	/	/
EKF first-order root mean square error	1.2543	1.41%	/
EKF higher-order root mean square error	1.1797	7.28%	5.95%

shows that: In X2, the precision of the high-order REKF with residual substitution was improved by about 7.28% compared to the traditional EKF, and the precision of the high-order REKF with residual substitution was improved by about 5.95% compared to the first-order REKF with residual substitution.

A simulation experiment was conducted using MATLAB, with the data in the table representing the root mean square error obtained by averaging the results from 50 iterations of the Kalman filter experiment. In scenarios where the state equation exhibited weak nonlinearity and the measurement was linear, with relatively large random noise in the function model, the figure clearly shows that the REKF outperformed the EKF in terms of accuracy. Furthermore, in the case of an under-measured nonlinear model, the enhancement of the first-order REKF with residual term replacement was smaller, while the enhancement of the higher-order REKF with residual term replacement was larger. This observation is also validated in the table, where it can be seen that the accuracy of the higher-order REKF with residual term replacement was about 3% higher than that of the first order REKF with residual term replacement. Additionally, the accuracy of the higher-order REKF with residual term replacement was about 6% higher than that of the traditional EKF. The REKF with residual term replacement performed better than the traditional EKF method in practical applications.

Simulation Experiment II: Consider systems where both the measurement equation and the equation of state are weakly nonlinear.

$$\begin{array}{l}\left[\begin{array}{c}{x}_1(k+1)\\ x_2(k+1)\end{array}\right]=\left[\begin{array}{c}{x}_1(k)-x_2(k)-\frac{1}{6}{x}_1^3(k)-\frac{1}{6}{x}_2^3(k)+\frac{1}{120}{x}_1^5(k)+\frac{1}{120}{x}_2^5(k)\\ 1-\frac{1}{2}{x}_1^2(k)-\frac{1}{2}{x}_2^2(k)+\frac{1}{24}{x}_1^4(k)+\frac{1}{24}{x}_2^4(k)\end{array}\right]+\left[\begin{array}{c}{w}_1(k)\\ w_2(k)\end{array}\right]\\ \left[y(k+1)\right]=\left[x_1(k+1)+x_2(k+1)+\frac{1}{2}{x}_1^2(k+1)x_2(k+1)+\frac{1}{2}{x}_1(k+1)x_2^2(k+1)+\sin x_1(k+1)\right]+v(k+1)\end{array}$$

where $x(k+1)={[x_1,x_2]}^T$, $y(k+1)={[y_1,y_2]}^T$, $w_k={[w_1,w_2]}^T$, and $v_{k+1}={[v_1,v_2]}^T$. $w_k$ and $v_{k+1}$ are mutually uncorrelated white noise. $Q=diag\{0.01,0.01\}$, $R=0.5$, $w_k$, and $v_{k+1}$ are independent white noises in the system obeying normal distribution, $w_k~N(0,Q)$ and $v_{k+1}~N(0,R)$, and satisfy the condition in Equation (3). The initial true value of the model is assumed to be the initial value of the original model state, ${\widehat{x}}_0={[0.1,0.1]}^{\mathrm{T}}$, and the initial estimation error covariance matrix is $P_0=I\in R^{2\times 2}$. Figure 3 shows that the method proposed in this paper is more accurate.

Table 3. Comparison of the performance of the EKF with the first-order EKF remainder substitution and the higher-order EKF remainder substitution for X1.

X1 Performance	Value	EKF	First-Order EKF
EKF root mean square error	0.2528	/	/
EKF first-order root mean square error	0.2476	2.06%	/
EKF higher-order root mean square error	0.2400	5.06%	3.07%

shows that: In X1, the precision of the high-order REKF with residual substitution was improved by about 5.06% compared to the traditional EKF, and the precision of the high-order REKF with residual substitution was improved by about 3.07% compared to the first-order REKF with residual substitution.

Table 4. Comparison of the root mean square error of the first-order EKF residual replacement and the higher-order EKF residual replacement for X2.

X2 Performance	Value	EKF	First-Order EKF
EKF root mean square error	0.2384	/	/
EKF first-order root mean square error	0.2338	1.93%	/
EKF higher-order root mean square error	0.2224	6.71%	4.88%

shows that: In X2, the precision of the high-order REKF with residual substitution was improved by about 6.71% compared to the traditional EKF, and the precision of the high-order REKF with residual substitution was improved by about 4.88% compared to the first order REKF with residual substitution.

MATLAB was used to carry out the simulation experiment. Since the measurement equation and the state equation are nonlinear functions of the model, a comparison was made between the traditional EKF and the REKF methods mentioned in this paper. The data in the figures are the average values after running the code 50 times. The figure showing the comparison curves of the root mean square errors shows that the residual term increased the accuracy of the traditional EKF method by about 2% compared to the first-order REKF method. Additionally, the higher-order residual term improved the accuracy by about 6% compared to the first-order REKF method and was more accurate than the traditional EKF method. In order to improve the accuracy, the higher-order residual term should be used instead of the extended Kalman filter (REKF). REKF uses Taylor expansion to round off higher-order terms in the model and extract higher-order items of information, which results in better filtering performance compared to the EKF.

4. Conclusions

In this paper, we propose and design an advanced version of the extended Kalman filter (REKF) that specifically accounts for the effects of higher-order terms. Through a series of detailed experiments, we demonstrated that in dealing with nonlinear Gaussian systems, the estimation accuracy of the REKF significantly surpassed that of the traditional extended Kalman filter (EKF). We discovered that the REKF, which includes higher-order terms, not only has greater accuracy but also exhibits higher stability under various conditions. However, it should be noted that our research mainly focuses on scenarios where the state equation is nonlinear. When the measurement equation is also nonlinear, rounding errors are introduced. These rounding errors may have a correlation between the equation of state and the measurement equation. In the error covariance, the product term of the rounding errors of the equation of state and the equation of observation will appear. We have not found an effective solution in our current research. In future work, we plan to delve deeper into and resolve this issue to further optimize and improve the performance of the REKF.