Research on Projection Filtering Method Based on Projection Symmetric Interval and Its Application in Underwater Navigation

Abstract

For non-linear systems (NLSs), the state estimation problem is an essential and important problem. This paper deals with the nonlinear state estimation problems in nonlinear and non-Gaussian systems. Recently, the Bayesian filter designer based on the Bayesian principle has been widely applied to the state estimation problem in NLSs. However, we assume that the state estimation models are nonlinear and non-Gaussian, applying traditional, typical nonlinear filtering methods, and there is no precise result for the system state estimation problem. Therefore, the larger the estimation error, the lower the estimation accuracy. To perfect the imperfections, a projection filtering method (PFM) based on the Bayesian estimation approach is applied to estimate the state. First, this paper constructs its projection symmetric interval to select the basis function. Second, the prior probability density of NLSs can be projected into the basis function space, and the prior probability density solution can be solved by using the Fokker–Planck Equation (FPE). According to the Bayes formula, the proposed estimator utilizes the basis function in projected space to iteratively calculate the posterior probability density; thus, it avoids calculating the partial differential equation. By taking two illustrative examples, it is also compared with the traditional UKF and PF algorithm, and the numerical experiment results show the feasibility and effectiveness of the novel nonlinear state estimation filter algorithm.

1. Introduction

In recent years, the state estimation problems of the dynamic system have become very important in various applications of nonlinear systems (NLSs), such as underwater navigation positioning, mechanical equipment fault diagnosis, signal processing, space target orbit prediction, target tracking, etc. [1,2,3].

The earliest applied state estimation method is the Kalman filter method. The Kalman Filter (KF) in [4] is a state filter estimator based on the minimum mean square error (MMSE). The KF method is only suitable for the state estimation model in linear and Gaussian systems. However, the state estimation problems of many actual physical systems are nonlinear and non-Gaussian models. If KF is used to solve the state estimation problem in NLSs, the estimation error will be divergent, the estimation accuracy will be low, and the estimation result will not be convergent. In order to overcome these problems, many scholars have studied many sub-optimal estimators and evaluated the Extended Kalman Filter (EKF) in navigation in the literature [5]. The basic principle is to linearize the nonlinear system and use KF to solve the linear system. The final result is that when the linearization error becomes larger, the estimation accuracy is low. EKF is only effective for nonlinear systems that are approximately linear in the update interval, so it is difficult to apply to many physical systems. Subsequently, the Unscented Kalman Filter (UKF) in the literature [6] shows that the UKF method assumes that the system noise conforms to the Gaussian distribution and does not require the system to be approximately linear. For nonlinear systems, the performance of the UKF method is better than that of EKF. Although UKF has achieved good results in many navigation application systems [7,8,9], the UKF method should satisfy three conditions: (1) State estimation models of the nonlinear systems should be given in detail. (2) The deviation of the reference state trajectory must small. (3) The conditional Probability Density Function (PDF) should satisfy the Gaussian distribution. Otherwise, the performance of UKF will become unstable. Therefore, this is not suitable for general methods of state estimation problems.

There is another popular Bayesian estimation algorithm based on the Particle Filter method (PF) in NLSs [10], also known as a complex technique based on the sequential Monte Carlo (SMC) method, which was initiated by Gordon and Salmond et al. in 1993 [11]. This method is a non-parametric sequential Monte-Carlo analog recursive filtering algorithm based on the Bayesian estimate principle. The basic principle of PF is that it can approximate the posterior probability density of the state estimation system by applying a random sampled particle with different weights; according to the Bayesian principle, it can update the posterior probability density by introducing observation values. In the past two decades, many scholars have conducted a lot of studies to improve the performance of PF. They have successively proposed that auxiliary particle filters in [12], unscented particle filters in [13] and Gaussian particle filters in [14] can effectively improve particle importance resampling and particle degeneracy phenomena. In [15], particle filtering could effectively improve estimation accuracy and computational efficiency. PF can deal with a set of nonlinear, non-Gaussian state estimation problems. However, it still has some shortcomings, such as computational complexity and sample degradation.

In the past ten years, many scholars have widely proposed Bayesian estimators in the state estimation problems of a dynamic system [16,17,18,19]. An adaptive Kalman filter estimator in [20] was proposed to solve state estimation problems. Theoretically speaking, a state estimation model in NLSs can be denoted by Fokker–Planck Equations (FPEs) and the Bayesian estimation equation. While the former is a partial differential equation and can show how to use the PDF of the nonlinear system to update the state, the latter can use new measure data to update the measurement information [21].

In [22], a projection filter method has been proposed, which can only solve the problem of low-dimensional state estimation. To fill this gap, the Projection Filter Method (PFM) is given in this paper and is a classic method for solving the Fokker–Planck equation (FPE). PFM assumes that the numerical solution of FPE can be represented by a set of basis functions in a projection space. Inspired by [23,24], an accurate probability solution is obtained by a set of orthogonal basis functions. In fact, compared with PF or other classic filter methods (such as UKF and EKF), PFM has more advantages than others, such as computational speed and convergence.

This paper is organized as follows. In Section 2, a state estimation model in NLSs is given, and the probability solution of the state estimation problem is defined. In Section 3, the PFM program is divided into two parts to solve the state estimation problems: status update and measurement update. Section 3 also introduces how to select the basis function based on the projected symmetric interval. Then, illustrative examples and their simulation results are given in Section 4. In Section 5, the conclusions of this paper are analyzed in detail.

2. Background and Problem Formulation

2.1. State Estimation Model in NLSs

Actually, in most non-linear systems (NLSs), given the distribution of initial state $x_0$, the state equations of most physical systems will develop with continuous time $t$, and the measurement equations can only be performed periodically at discrete time $t_k$. Assume that the n-dimensional state estimation model can be denoted as

$$d{x}_t=f\left(x_t,t\right)dt+gd{\beta }_t$$

(1)

$$z_k=h\left(x_k,t_k\right)+v_k$$

(2)

where $x_t\in {ℝ}^n$ denotes the n-dimensional system state vector, $f\left(x_t,t\right):{ℝ}^n\times ℝ\to {ℝ}^n$ denotes the state transfer matrix, $z_k\in {ℝ}^m$ is the measurement vector, $h\left(x_k,t_k\right):{ℝ}^n\times ℝ\to {ℝ}^m$ is the measurement matrix and $g:{ℝ}^n\times ℝ\to {ℝ}^{n\times d}$ denotes the diffusion coefficient matrix. ${\beta }_t$ denotes a d-dimensional standard Brownian motion vector, and $v_k$ denotes a m-dimensional measured noise vector. Based on this, we have the following:

$$\left\{\begin{array}{c}\mathrm{{\rm E}}\left\{{\beta }_t\right\}=0,\begin{array}{c}\end{array}\mathrm{{\rm E}}\left\{{\beta }_t{\beta }_t{}^{\mathrm{T}}\right\}=\mathit{Q}\left(t\right)\hfill \\ \mathrm{E}\left\{{\mathit{v}}_k\right\}=0,\begin{array}{c}\end{array}\mathrm{E}\left\{{\mathit{v}}_k{\mathit{v}}_k{}^{\mathrm{T}}\right\}={\mathit{R}}_k\hfill \\ \mathrm{E}\left\{{\mathit{v}}_k{\beta }_t{}^{\mathrm{T}}\right\}=0\hfill \end{array}\right.$$

(3)

where ${\mathit{R}}_k$ and $\mathit{Q}\left(t\right)$ denote the covariance matrices of the Brownian motion process and measured noise vector.

2.2. Probability Solution of State Estimation Problem

According to the Bayesian filter estimation principle [17], the state estimation model needs to estimate the system state. Suppose that the posterior probability density distribution $p\left(x,t_{k+1}|z_{k+1}\right)$ of some dynamics system satisfies

$$p\left(x,t_{k+1}|z_{k+1}\right)=\frac{p\left(z_{k+1}|x\right)p\left(x,t_{k+1}^{-}|z_k\right)}{{\displaystyle {\int }_{\mathsf{\Omega }}p\left(z_{k+1}|x\right)p\left(x,t_{k+1}^{-}|z_k\right)dx}}$$

(4)

where the likelihood probability density $p\left(z_{k+1}|x\right)$ is defined as

$$p\left(z_{k+1}|x\right)=\frac{1}{\sqrt{{\left(2\pi \right)}^m\left|R_{k+1}\right|}}exp\left\{-\frac{1}{2}{\left(z_{k+1}-h\left(x_k,t_{k+1}\right)\right)}^{\mathrm{T}}{R}_{k+1}^{-1}\left(z_{k+1}-h\left(x_k,t_{k+1}\right)\right)\right\}$$

(5)

where $h\left(x_k,t_{k+1}\right)$ denotes the measurement matrix in (2) and represents the measurement vector in state space, and ${\mathit{R}}_k$ is expressed as the covariance matrix in Equation (3). $p\left(x,t_{k+1}^{-}|z_k\right)$ is prior probability density, and the prior probability density of the nonlinear system will satisfy the Fokker–Planck Equation (FPE) [21]:

$$L\left(p\right)=\dot{p}=\frac{1}{2}{\displaystyle \sum _{r,s=1}^n\frac{{\partial }^2\left(gQ\left(t\right)g^{\mathrm{T}}\cdot p\right)}{\partial x_r\partial x_s}}-{\displaystyle \sum _{r=1}^n\frac{\partial \left(f\left(x_{\mathrm{t}},t\right)\cdot p\right)}{\partial x_r}}$$

(6)

where probability density function (PDF) $p=p\left(x,t|z_t\right)$ is expressed as the probability solution of FPE. Assume that the initial probability distribution is $p\left(x_0,t_0|z_0\right)=p_0$.

3. Nonlinear Filter Estimator Design-Based Projection Filter Method

The Projection Filter Method (PFM) is a state estimation method that approximates the PDF of the state based on projection symmetric interval and basis function. To construct a set of finite-dimensional orthogonal basis functions in the projection space, the state can be projected into the basis function space, and the PFM uses the FPE to obtain the PDF of the state in the projection space. The problem of solving Partial Differential Equations (PDEs) (such as FPE) is transformed into a problem of solving linear ordinary differential equations. In this part, the numerical solution is simplified to avoid calculating PDEs. PFM is a useful numerical calculation method for solving a state estimation problem. The flowchart of a filter estimator design (PFM) is shown in Figure 1.

Research manuscripts reporting large datasets that are deposited in a publicly available database should specify where the data have been deposited and provide the relevant accession numbers. If the accession numbers have not yet been obtained at the time of submission, please state that they will be provided during the review. They must be provided prior to publication.

Interventionary studies involving animals or humans, and other studies that require ethical approval, must list the authority that provided approval and the corresponding ethical approval code.

3.1. Status Update

Suppose n-dimension state vector $x=\left(x_1,\cdots ,x_n\right)$, the PDF in NLSs can be denoted as $p\left(x,t\right)$. For a fixed time $t\ge 0$, assume that the PDF in NLSs can be expressed by an infinite sum of basis functions in the projection space

$$p\left(x,t\right)={\displaystyle \sum _{i=0}^{\infty }{m}_i\left(t\right){\varphi }_i\left(x\right)}$$

(7)

where ${\varphi }_i\left(x\right)$ denotes the orthogonal basis function in the projection subspace, $m_i\left(t\right)=〈p\left(x,t\right),{\varphi }_i\left(x\right)〉$ denotes the coefficients of $p\left(x,t\right)$ in (1), and, $〈\varphi 〉$ denotes the inner product, so $〈\eta ,\varphi 〉={\displaystyle {\int }_{\mathsf{\Omega }}\eta \left(x\right)\varphi \left(x\right)dx}$, $\mathsf{\Omega }\subset R^n$.

Equality $L_2$ is in the sense of the norm, and $V_N=span{\left\{{\varphi }_i\right\}}_0^{N-1}\subset V$ denotes a complete set of basis functions for $L_2$. $p\left(x,t\right)$ could be expressed as the following form:

$$\tilde{p}\left(x,t\right)={\displaystyle \sum _{i=0}^{N-1}{m}_i\left(t\right){\varphi }_i\left(x\right)}$$

(8)

where unknown coefficients $m_i\left(t\right)$ satisfy $m_i\left(t\right)=〈p\left(x,t\right),{\varphi }_i\left(x\right)〉$. These basis functions ${\varphi }_i\left(x\right)$ should satisfy the following orthogonality properties:

$${\left[M\right]}_{q+1,i+1}=〈{\varphi }_i,{\varphi }_q〉=\left\{\begin{array}{l}0\hspace{1em}\hspace{0.17em}i\ne q\\ 1\hspace{1em}\hspace{0.33em}i=q\end{array}\right.$$

(9)

where $\hspace{0.17em}i,q=0,1,2,\cdots ,N-1$.

So, $M$ can be given as $M=E_{N\times N}$.

Assume that $a\left(t\right)={\left[m_0\left(t\right)\hspace{1em}{m}_1\left(t\right)\hspace{1em}\cdots m_{N-1}\left(t\right)\right]}^T$ and $\mathsf{\Phi }\left(x\right)={\left[{\varphi }_0\left(x\right)\hspace{1em}{\varphi }_1\left(x\right)\cdots {\varphi }_{N-1}\left(x\right)\right]}^T$.

$p\left(x,t\right)$ in (7) will be denoted as $p\left(x,t\right)=m{\left(t\right)}^T\mathsf{\Phi }\left(x\right)$.

In function space $L_2\left(R^n\right)$, basic functions such as ${\varphi }_i\left(x\right)$ are known. To solve the coefficients matrix $m\left(t\right)$, the approximate PDF solution can be solved.

Having substituted Formula (8) into Formula (6), we can obtain

$$\frac{\partial {\displaystyle \sum _{i=0}^{N-1}{m}_i{\varphi }_i}}{\partial t}={\displaystyle \sum _{i=0}^{N-1}{m}_i\left\{\frac{1}{2}{\displaystyle \sum _{r,s=1}^n\frac{{\partial }^2\left(gQ\left(t\right)g^{\mathrm{T}}\cdot {\varphi }_i\right)}{\partial x_r\partial x_s}}\right\}}-{\displaystyle \sum _{i=0}^{N-1}{m}_i\left\{{\displaystyle \sum _{r=1}^n\frac{\partial \left(f\left(x,t\right)\cdot {\varphi }_i\right)}{\partial x_r}}\right\}}$$

(10)

Having projected Equation (10) onto the space $V_N=span{\left\{{\varphi }_q\right\}}_0^{N-1}$, we can obtain

$$\frac{\partial {\displaystyle \sum _{i=0}^{N-1}〈m_i{\varphi }_i,{\varphi }_q〉}}{\partial t}={\displaystyle \sum _{i=0}^{N-1}{m}_i\left\{\frac{1}{2}{\displaystyle \sum _{r,s=1}^n〈\frac{{\partial }^2\left(gQ\left(t\right)g^{\mathrm{T}}\cdot {\varphi }_i\right)}{\partial x_r\partial x_s},{\varphi }_q〉}\right\}}-{\displaystyle \sum _{i=0}^{N-1}{m}_i\left\{{\displaystyle \sum _{r=1}^n〈\frac{\partial \left(f\left(x,t\right)\cdot {\varphi }_i\right)}{\partial x_r},{\varphi }_q〉}\right\}}$$

(11)

If $Q$, $f$ and $g$ are independent of time $t$, Equation (10) can be rewritten as

$${\displaystyle \sum _{i=0}^{N-1}{\dot{m}}_i\cdot 〈{\varphi }_i,{\varphi }_q〉}={\displaystyle \sum _{i=0}^{N-1}{m}_i\left\{\frac{1}{2}{\displaystyle \sum _{r,s=1}^n〈\frac{{\partial }^2\left(gQ\left(t\right)g^{\mathrm{T}}\cdot {\varphi }_i\right)}{\partial x_r\partial x_s},{\varphi }_q〉}\right\}}-{\displaystyle \sum _{i=0}^{N-1}{m}_i\left\{{\displaystyle \sum _{r=1}^n〈\frac{\partial \left(f\left(x,t\right)\cdot {\varphi }_i\right)}{\partial x_r},{\varphi }_q〉}\right\}}$$

(12)

Equation (12) denotes a linear ordinary differential equation that may be written in matrix notation by defining

$$gQ{g}^{\mathrm{T}}=B$$

(13)

$${\left[M\right]}_{q,i}=〈{\varphi }_{i-1},{\varphi }_{q-1}〉=\left\{\begin{array}{l}1\hspace{1em}i=q\\ 0\hspace{1em}i\ne q\end{array}\right.$$

(14)

We have

$${\left[A\right]}_{q+1,i+1}=〈\left\{\frac{1}{2}{\displaystyle \sum _{r,s=1}^n\frac{B{\partial }^2\left({\varphi }_i\right)}{\partial x_r\partial x_s}}-{\displaystyle \sum _{r=1}^n\frac{\partial \left(f\left(x,t\right)\cdot {\varphi }_i\right)}{\partial x_r}}\right\},{\varphi }_q〉$$

(15)

where $i,q=0,1,\cdots N-1$, ${\left[A\right]}_{qi}$ is the element of matrix $A$ in Equation (15).

With these definitions $M=E_{N\times N}$, and Equation (12) can be rewritten as

$$M\dot{m}\left(t\right)=Am\left(t\right)$$

(16)

Due to $M=E_{N\times N}$, Equation (16) can be rewritten as

$$\dot{m}\left(t\right)=Am\left(t\right)$$

(17)

According to initial density function $p_0=p\left(x_0,t_0|z_0\right)$, at time $t=t_0$, the initial coefficients $a\left(t_0\right)$ can be obtained as

$$〈p_0,{\varphi }_j〉=〈{\displaystyle \sum _{i=0}^{N-1}{m}_i\left(t_0\right){\varphi }_i},{\varphi }_j〉={\displaystyle \sum _{i=0}^{N-1}{m}_i\left(t_0\right)\cdot 〈{\varphi }_i,{\varphi }_j〉}$$

(18)

$$s={\left[〈p_0,{\varphi }_0〉\hspace{1em}\cdots 〈p_0,{\varphi }_{N-1}〉\right]}^T$$

(19)

$$m\left(t_0\right)=M^{-1}s=s$$

(20)

Having substituted Equation (20) into Equation (17), we have

$$m\left(t\right)=exp\left\{{\displaystyle {\int }_{t_0}^{t}Ad\tau }\right\}s$$

(21)

If it is independent of time in $\left[t_k,t_{k+1}\right)$, Equation (11) has the following simple solution:

$$m\left(t\right)=exp\left(A\left(t-t_k\right)\right)m\left(t_k\right),t\in \left[t_k,t_{k+1}\right)$$

(22)

A priori coefficients can be obtained:

$$m^{-}\left(t_{k+1}\right)=exp\left(A\left(t_{k+1}-t_k\right)\right)m\left(t_k\right)$$

(23)

where $a\left(t_k\right)$ are the posterior coefficients at time $t_k$.

A priori probability density is given as

$$\tilde{p}\left(x,t_{k+1}^{-}|z_{k+1}\right)={\displaystyle \sum _{i=0}^{N-1}{m}_i{}^{-}\left(t_{k+1}\right){\varphi }_i}=m^{-}{\left(t_{k+1}\right)}^T\mathsf{\Phi }\left(x\right)$$

(24)

3.2. Measurement Update

Due to the Bayesian formula (Equation (6), the posterior PDF can be expressed as

$$p\left(x,t_{k+1}|z_{k+1}\right)={\displaystyle \sum _{i=0}^{\infty }{\mathrm{m}}_i\left(t_{k+1}\right){\varphi }_i}$$

(25)

We approximate $p\left(x,t_{k+1}|z_{k+1}\right)$ by truncating the sum

$$\tilde{p}\left(x,t_{k+1}|z_{k+1}\right)={\displaystyle \sum _{i=0}^{N-1}{m}_i\left(t_{k+1}\right){\varphi }_i}$$

(26)

Notice that the parameters $m_i\left(t_{k+1}\right)$ are unknown. The accurate propagation of the PDF requires the solution of new parameters $m_i\left(t_{k+1}\right)$.

Having projected the posterior PDF in Equation (26) onto the projection space $V_N=span{\left\{{\varphi }_q\right\}}_0^{N-1}$, we can obtain

$$\begin{gathered} 〈\tilde{p}\left(x,t_{k+1}|z_{k+1}\right),{\varphi }_q〉={\displaystyle \sum _{i=0}^{N-1}\left\{m_i\left(t_{k+1}\right)〈{\varphi }_i,{\varphi }_q〉\right\}}=\frac{〈p\left(z_{k+1}|x\right){\displaystyle \sum _{i=0}^{N-1}{m}_i\left(t_{k+1}^{-}\right){\varphi }_i},{\varphi }_q〉}{{\displaystyle {\int }_{\mathsf{\Omega }}p\left(z_{k+1}|x\right){\displaystyle \sum _{i=0}^{N-1}{m}_i\left(t_{k+1}^{-}\right){\varphi }_i}dx}} \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}=\frac{{\displaystyle \sum _{i=0}^{N-1}\left\{m_i\left(t_{k+1}^{-}\right)〈p\left(z_{k+1}|x\right){\varphi }_i,{\varphi }_q〉\right\}}}{{\displaystyle \sum _{i=0}^{N-1}\left\{m_i\left(t_{k+1}^{-}\right)〈p\left(z_{k+1}|x\right),{\varphi }_i^{\ast }〉\right\}}} \end{gathered}$$

(27)

where $q=0,1,\cdots N-1$.

The parameters of the posterior probability density are given by

$$\mathrm{m}\left(t_{k+1}\right)=\frac{F_{k+1}m\left(t_{k+1}^{-}\right)}{K_{k+1}^{\mathrm{T}}m\left(t_{k+1}^{-}\right)}$$

(28)

where

$$\left\{\begin{array}{l}{\left[F_{k+1}\right]}_{qi}=〈p\left(z_{k+1}|x\right){\varphi }_i,{\varphi }_q〉\\ {\left[K_{k+1}\right]}_i=〈p\left(z_{k+1}|x\right),{\varphi }_i^{\ast }〉\end{array}\right.$$

(29)

By substituting Equation (29) into Equation (28), the posterior PDF is given by

$$\tilde{p}\left(x,t_{k+1}|z_{k+1}\right)={\displaystyle \sum _{i=0}^{N-1}{m}_i\left(t_{k+1}\right){\varphi }_i}$$

(30)

According to the posterior PDF $\tilde{p}\left(x,t_{k+1}|z_{k+1}\right)$ in Equation (30), the conditional mean $\tilde{E}\left(x,t_{k+1}|z_{k+1}\right)$ = ${\tilde{E}}_{k+1}$ and the variance $\tilde{C}\left(x,t_{k+1}|z_{k+1}\right)$ = ${\tilde{C}}_{k+1}$ are given by the following equations:

$$\tilde{E}\left(x,t_{k+1}|z_{k+1}\right)={\displaystyle {\int }_{\mathsf{\Omega }}\eta \tilde{p}\left(\eta ,t_{k+1}|z_{k+1}\right)d\eta }\approx {\displaystyle \sum _{i=0}^{N-1}{\mathrm{m}}_i\left(t_{k+1}\right){\displaystyle {\int }_{\mathsf{\Omega }}{\varphi }_i\eta d\eta }}$$

(31)

$$\begin{array}{l}\tilde{C}\left(x,t_{k+1}|z_{k+1}\right)={\displaystyle {\int }_{\mathsf{\Omega }}{\left(\eta -\tilde{E}\left(x,t_{k+1}|z_{k+1}\right)\right)}^2\tilde{p}\left(\eta ,t_{k+1}|z_{k+1}\right)d\eta }\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\approx {\displaystyle \sum _{i=0}^{N-1}{m}_i\left(t_{k+1}\right){\displaystyle {\int }_{\mathsf{\Omega }}{\varphi }_i{\left(\eta -\tilde{E}\left(x,t_{k+1}|z_{k+1}\right)\right)}^{2}d\eta }}\end{array}$$

(32)

3.3. Selecting Basis Function Based on Projection Symmetric Interval

Steps of PFM-PSI (Projection Filter Method based on Projection Symmetric Interval) in the n-dimensional system can be obtained as:

(1) In $n$-dimensional state space, according to ${\widetilde{{\rm E}}}_{k+1}$ and ${\tilde{C}}_{k+1}$ in Equation (32), a symmetric interval $\left[\mathrm{b},\mathrm{c}\right]$ is given by

$$\left\{\begin{array}{l}\mathrm{b}=-\left(x\left|{}_{x=E_k}\right.\right)-10C_k\\ \mathrm{c}=\left(x\left|{}_{x=E_k}\right.\right)+10C_k\end{array}\right.$$

(33)

Assume that the coordinate series $x_i$ are given by

$$x_i=i\frac{c_i-b_i}{K}+b_i,i=0,1,\cdots ,K$$

(34)

where K is the interval number and is given by a positive integer.

Assume that the orthogonal complex exponential basis functions in [25,26] are given by

$$\left\{\begin{array}{l}{\varphi }_q\left(x\right)=\frac{1}{\sqrt{c_q-b_q}}exp\left\{j2\pi q\frac{x-b_q}{c_q-b_q}\right\}\\ {\varphi }_q{}^{\ast }\left(x\right)=\frac{1}{\sqrt{c_q-b_q}}exp\left\{-j2\pi q\frac{x-b_q}{c_q-b_q}\right\}\end{array}\right.$$

(35)

where $q=0,1,\cdots N-1$.

We have $M=E_{N\times N}$, and projection space can be expressed as $V_N=span{\left\{{\varphi }_q\right\}}_0^{N-1}\subset V$. The posterior coefficients $m\left(t_k\right)$ at time $t_k$ are given by

$$m\left(t_k\right)={\left[a_0\left(t_k\right)\hspace{0.33em}{a}_1\left(t_k\right)\hspace{0.33em}\cdots a_{N-1}\left(t_k\right)\right]}^{{\rm T}}$$

(36)

The optimum value of the number N of basis functions can be obtained by the initial state.

(2) Calculate prior coefficients $m\left(t_{k+1}^{-}\right)$ at time $t_{k+1}$.

By calculating the posterior probability density $p\left(x,t_k\right)$ at time $t_k$, the prior coefficients $m\left(t_{k+1}^{-}\right)$ at time $t_{k+1}$ can be obtained as

$$〈p\left(x,t_k\right),{\varphi }_q〉\approx {\displaystyle \sum _{i=0}^{N-1}{a}_i\left(t_k\right)〈{\varphi }_i,{\varphi }_q〉}$$

(37)

$$m\left(t_k\right)=s$$

(38)

Due to the property of the probability density ${\displaystyle {\int }_{\mathsf{\Omega }}\tilde{p}\left(x\right)dx}=1$, we have

$$\begin{array}{c}{\displaystyle {\int }_b^c\tilde{p}\left(x\right)dx}={\displaystyle {\int }_b^{c}real\left({\displaystyle \sum _{i=0}^{N-1}{a}_i{\varphi }_i}\right)dx}={\displaystyle \sum _{i=0}^{N-1}real\left({\displaystyle {\int }_b^c{a}_i{\varphi }_{i}dx}\right)}\\ =\sqrt{c-b}{\displaystyle \sum IFF{T}_1\left(real\left(m\right)\right)}=1\end{array}$$

(39)

We can obtain

$$m\left(t_k\right)=m\left(t_k\right)/\left[\sqrt{c-b}{\displaystyle \sum IFF{T}_1\left(real\left(m\left(t_k\right)\right)\right)}\right]$$

(40)

Projecting FPE onto the subspace $V_N$, we have

$$\begin{array}{l}{\left[A\right]}_{qi}=〈\left\{\frac{1}{2}\frac{{\partial }^2\left(gQ{g}^{\mathrm{T}}\cdot {\varphi }_i\right)}{\partial x^2}-\frac{\partial \left(f\cdot {\varphi }_i\right)}{\partial x}\right\},{\varphi }_q〉\\ =\frac{\sqrt{c-b}}{K^2}{\left[FF{T}_{}\left(\frac{1}{2}\frac{{\partial }^2\left(gQ{g}^{\mathrm{T}}\cdot {\varphi }_i\right)}{\partial x^2}-\frac{\partial \left(f\cdot {\varphi }_i\right)}{\partial x}\right)\right]}_q\end{array}$$

(41)

where $g$ and $K$ are the constant matrix, we have

$$\begin{array}{l}{\left[A\right]}_{qi}=\frac{\sqrt{c-b}}{K}{\left[FF{T}_{}\left({\left.\left(\frac{1}{2}gQ{g}^{\mathrm{T}}\frac{{\partial }^2{\varphi }_i}{\partial x^2}-\frac{\partial \left(f\cdot {\varphi }_i\right)}{\partial x}\right)\right|}_{x=xn}\right)\right]}_q\\ =\frac{\sqrt{c-b}}{K}{\left[FF{T}_{}\left({\left.-\frac{1}{2}gQ{g}^{\mathrm{T}}{\left(\frac{2\pi i}{c-b}\right)}^2{\varphi }_i-\frac{\partial f}{\partial x}{\varphi }_i+f\cdot j\frac{2\pi i}{c-b}{\varphi }_i\right|}_{x=xn}\right)\right]}_q\end{array}$$

(42)

the prior coefficients in Equation (23) can be obtained as

$$m\left(t_{k+1}^{-}\right)=\exp \left(A\left(t_{k+1}-t_k\right)\right)m\left(t_k\right)$$

(43)

By exploiting prior coefficients, normalization processing is carried out as

$$m\left(t_{k+1}^{-}\right)=m\left(t_{k+1}^{-}\right)/\left[\sqrt{c-b}{\displaystyle \sum _{i=0}^{K-1}IFF{T}_1\left(real\left(m\left(t_{k+1}^{-}\right)\right)\right)}\right]$$

(44)

(3) Calculate posterior coefficients $m\left(t_{k+1}\right)$.

$${\left[F_{k+1}\right]}_{qi}=〈p\left(z_{k+1}|x\right){\varphi }_i,{\varphi }_q〉=\frac{\sqrt{c-b}}{K}{\left[FF{T}_1\left({\left.p\left(z_{k+1}|x\right){\varphi }_i\right|}_{x=xn}\right)\right]}_q$$

(45)

$${\left[E_{k+1}\right]}_i=〈p\left(z_{k+1}|x\right),{\varphi }_i^{*}〉=\frac{\sqrt{c-b}}{K}{\left[IFF{T}_1\left({\left.p\left(z_{k+1}|x\right)\right|}_{x=xn}\right)\right]}_i$$

(46)

where $i,q=0,1,\cdots N-1$. In matrix notation,

$$m\left(t_{k+1}\right)=\frac{F_{k+1}m\left(t_{k+1}^{-}\right)}{E_{k+1}^{\mathrm{T}}m\left(t_{k+1}^{-}\right)}$$

(47)

Due to coefficients $m\left(t_{k+1}\right)$ in Equation (47), normalization processing is carried out as

$$m\left(t_{k+1}\right)=m\left(t_{k+1}\right)/\left[\sqrt{c-b}{\displaystyle \sum _{i=0}^{K-1}IFF{T}_1\left(real\left(m\left(t_{k+1}\right)\right)\right)}\right]$$

(48)

(4) Calculating conditional mean ${\tilde{E}}_{k+1}$ and variance ${\tilde{C}}_{k+1}$, posterior probability density function can be formulated as

$$\tilde{p}\left(x,t_{k+1}|z_{k+1}\right)={\displaystyle \sum _{i=0}^{N-1}{m}_i\left(t_{k+1}\right){\varphi }_i}$$

(49)

According to the posterior PDF, the state estimate mean ${\tilde{E}}_{k+1}$ can be evaluated as

$$\begin{array}{c}{\tilde{E}}_{k+1}={\displaystyle {\int }_b^c\eta \tilde{p}\left(\eta ,t_{k+1}|z_{k+1}\right)d\eta }={\displaystyle {\int }_b^c\eta real\left({\displaystyle \sum _{i=0}^{N-1}{a}_i\left(t_{k+1}\right){\varphi }_i}\right)d\eta }\\ =real\left({\displaystyle \sum _{i=0}^{N-1}{m}_i\left(t_{k+1}\right){\displaystyle {\int }_b^c\eta {\varphi }_{i}d\eta }}\right)=real\left({\Psi }^{\mathrm{T}}\cdot m\left(t_{k+1}\right)\right)\end{array}$$

(50)

where $\Psi =\frac{\sqrt{c-b}}{K}IFF{T}_1\left(xn\right)$.

The state estimate variance ${\tilde{C}}_{k+1}$ can be given as

$$\begin{array}{cc}\hfill {\tilde{C}}_{k+1}& ={\displaystyle {\int }_b^c{\left(\eta -\widehat{x}\right)}^2\tilde{p}\left(\eta ,t_{k+1}|z_{k+1}\right)d\eta }\hfill \\ & =real\left({\displaystyle \sum _{i=0}^{N-1}{m}_i\left(t_{k+1}\right)}{\displaystyle {\int }_b^c{\left(\eta -\widehat{E}\left(x|z_{k+1}\right)\right)}^2{\varphi }_{i}d\eta }\right)\hfill \\ & =real\left({\Gamma }^{\mathrm{T}}\cdot m\left(t_{k+1}\right)\right)\hfill \end{array}$$

(51)

where $\Gamma =\frac{\sqrt{c-b}}{K}IFF{T}_1\left(\left(xn-\widehat{x}\right)\cdot \left(xn-\widehat{x}\right)\right)$.

(5) Approximation of mean and variance are used to calculate a priori coefficients at time $t_{k+2}$.

4. Illustrative Examples and Simulations

4.1. Example 1

For the first example, the state estimation model will be given by the following dynamic system with the real solution in [27,28]:

$$u_t=u_{xx}-x^{2}u+\left(sin\left(t\right)+cos\left(t\right)+3x\right)e^{-\frac{x^2}{2}}$$

(52)

$$u\left(x,t\right)=\left(sin\left(t\right)+x\right)e^{-\frac{x^2}{2}}$$

(53)

It is easy to obtain that $u\left(x,t\right)=\left(\sin \left(t\right)+x\right)e^{-\frac{x^2}{2}}$ is the exact solution of Equation (52). The initial state is $u\left(0,0\right)=0$.

Here, using PFM-PSI (Projection Filter Method based on Projection Symmetric Interval), the interval of time is chosen as 1 s. The performance output of PFM-PSI is shown in Figure 2, Figure 3 and Figure 4. The posterior PDFs are plotted. The normalized density functions are plotted every other 0.1 s. The results of both real solution and PFM–PSI have been plotted in Figure 2. Having compared the estimated PDF and the true PDF of the state in Figure 3, we give the estimated error and root mean square (RMS) of PFM in Figure 3 and Figure 4. Obviously, for a nonlinear system, PFM–PSI has the effectiveness of the new state estimator algorithm.

4.2. Example 2: State Estimation Model in Underwater Navigation

In this section, a nonlinear system in underwater navigation is given by References [29,30], which is described as follows:

State vector: $X\left(t\right)={\left[x\left(t\right)\hspace{1em}y\left(t\right)\hspace{1em}{v}_x\left(t\right)\hspace{1em}{v}_y\left(t\right)\right]}^T$.

Control vector: $u\left(t\right)={\left[\begin{array}{cc}\psi \left(t\right)& a\left(t\right)\end{array}\right]}^{\mathrm{T}}$.

Measurement vector: $z\left(t\right)={\left[\begin{array}{ccc}{v}_x\left(t\right)& v_y\left(t\right)& h_r\left(t\right)+h\left(t\right)\end{array}\right]}^T$.

Underwater navigation state estimation model:

$$\dot{X}\left(t\right)=f\left(x\left(t\right),u\left(t\right),w\left(t\right)\right)=\left[\begin{array}{c}{v}_x\left(t\right)cos\left(\psi \left(t\right)\right)-v_y\left(t\right)sin\left(\psi \left(t\right)\right)\\ v_y\left(t\right)cos\left(\psi \left(t\right)\right)+v_x\left(t\right)sin\left(\psi \left(t\right)\right)\\ a\left(t\right)cos\left(\psi \left(t\right)\right)\\ a\left(t\right)sin\left(\psi \left(t\right)\right)\end{array}\right]+\left[\begin{array}{c}{w}_x\\ w_y\\ w_{v_x}\\ w_{v_y}\end{array}\right]$$

(54)

$$z_k=h\left(X\left(t_k\right)\right)+e_k=\left[\begin{array}{c}{v}_x\left(k\right)\\ v_y\left(k\right)\\ h_d\left(x\left(t_k\right),y\left(t_k\right)\right)\end{array}\right]+\left[\begin{array}{c}{e}_{v_x}\\ e_{v_y}\\ e_h\end{array}\right]$$

(55)

where $e={\left(e_{v_x},e_{v_y},e_h\right)}^T,w={\left(w_x,w_y,w_{v_x},w_{v_y}\right)}^T$ are, respectively, described as the measurement noise of the measurement equation and the process noise of the state equation. Zero mean white noise and the covariance matrix are, respectively, $Q$ and $R$. $h_d\left(x\left(t\right),y\left(t\right)\right)$ is expressed as a non-linear function.

Having set the initial state x0 = [2000 m; 18,000 m; 0.5 m/s; 0 m/s], the filter sampling time interval is $\mathsf{\Delta }T=1s$, the running time is 5000 s, the number of particles in the particle filter is 1000, and the number of orthogonal complex exponential basis functions is 10.

For the three non-linear filtering methods of UKF, PF and PFM-PSI (Projection Filter Method based on Projection Symmetric Interval), the terrain navigation in the experiment is displayed by using the interpolation method in Reference [25] to solve the terrain function. The state estimation results based on UKF, PF and PFM-PSI are shown in Figure 5, Figure 6, Figure 7, Figure 8, Figure 9, Figure 10, Figure 11, Figure 12, Figure 13, Figure 14, Figure 15, Figure 16 and Figure 17.

The real points in the experiment are generated by random numbers, and the results obtained from each simulation will be different. Therefore, the estimation accuracy of the verification algorithm is obtained by the statistical root mean square error (RMSE), which is defined as:

$$R=\frac{1}{ST}{\displaystyle \sum _{i=1}^{ST}\sqrt{\frac{{\displaystyle \sum _{k=1}^{times}{\left(S_k-Y_k\right)}^2}}{times}}}$$

(56)

where $R$ represents RMSE and $Y_k$ represents the estimated location information. the number of simulation experiments is $ST=100$, the simulation results are shown in Figure 18, Figure 19, Figure 20 and Figure 21.

In Figure 5, the true trajectory of a target can be viewed. The control vector with noise and the measured terrain profile are plotted in Figure 6 and Figure 7. Figure 8, Figure 9, Figure 10, Figure 11, Figure 12, Figure 13, Figure 14, Figure 15, Figure 16 and Figure 17 show the estimated trajectories, estimated mean and estimated standard deviation of state based on UKF, PF and PFM-PSI. We will give the performance output of PFM-PSI. Having compared the true state trajectory, estimated means and estimated standard deviation executed by UKF, PF and PFM-PSI, PFM-PSI is better than the other algorithms. These numerical experiment results show that the effectiveness of PFM-PSI is confirmed. Compare the true state trajectory and the conditional means of the trajectories executed by UKF, PF and PFM-PSI. Obviously, for a nonlinear system, the numerical experiment results show the feasibility and effectiveness of the novel nonlinear state estimation filter algorithm based on PFM.

The position RMSE and velocity RMSE of state can be plotted without terrain navigation in Figure 18 and Figure 19, and they can be plotted with interpolated terrain navigation in Figure 20 and Figure 21. Comparing position RMSE and velocity RMSE between terrain-free navigation and interpolation terrain navigation, the estimation accuracy of the three algorithms with interpolation terrain navigation is better. Compared with UKF and PF, PFM-PSI is significantly better than others, and the three algorithms are better with interpolated terrain navigation. In short, the estimation accuracy of PFM-PSI is obviously better than UKF and PF.

It can be seen from Figure 5, Figure 6, Figure 7, Figure 8, Figure 9, Figure 10, Figure 11, Figure 12, Figure 13, Figure 14, Figure 15, Figure 16, Figure 17, Figure 18, Figure 19, Figure 20 and Figure 21 that the PFM-PSI has better performance to obtain state estimation information in NLSs, so the PFM-PSI can estimate the PDF of state in the nonlinear non-gaussian systems. These all explain that PFM-PSI can improve the state estimation accuracy in Figure 5, Figure 6, Figure 7, Figure 8, Figure 9, Figure 10, Figure 11, Figure 12, Figure 13, Figure 14, Figure 15, Figure 16 and Figure 17. Although the PFM-PSI estimation accuracy is better, it does not quickly converge to the true state information in the NLSs, which also shows that the estimation accuracy of the PF is slightly lower than the PFM-PSI. In addition, Figure 6, Figure 7, Figure 8, Figure 9, Figure 10, Figure 11, Figure 12, Figure 13, Figure 14, Figure 15, Figure 16, Figure 17 and Figure 18 show that PFM-PSI needs to iteratively estimate state information with discrete time. This is consistent with the theoretical analysis in Section 3.

As to the efficiency, the CPU time for the PFM-PSI algorithm is 25.257 s for Times = 5000 s. Thus, for each time step, it only costs 0.005 s, and the calculation speed is much faster than the update time of 1 s. The CPU time for the PF algorithm is 31.100 s for Times = 5000 s. The CPU time for the UKF algorithm is 65.6224 s for Times = 5000 s. Obviously, the calculation speed of PFM-PSI is faster than that of PF and UKF.

Table 1. Mean RMSE 100 times in filtering simulations of state estimation model.

Title 1	Title 2	Title 3	Title 4	Title 4
UKF without terrain navigation UKF with interpolated terrain navigation	42.7971	48.6431	0.0482	0.0475
	24.3878	23.2341	0.0486	0.0484
PF without terrain navigation PF with interpolated terrain navigation	42.7971	48.6431	0.0482	0.0475
	13.9931	13.9590	0.0460	0.0454
PFM-PSI without terrain navigation PFM-PSI with interpolated terrain navigation	23.6374	23.0574	0.0483	0.0479
	13.8521	14.3815	0.0460	0.0451

shows the mean RMSE 100 times in filtering simulations of the state estimation model. The PFM has a lower RMSE than the UKF and PF in Table 1. Having compared interpolated terrain navigation and terrain-free navigation, PFM-PSI performance is better with terrain navigation. Compared with the UKF and PF, obviously, the performance of PFM-PSI is better than PF and UKF shown in Table 1.

5. Conclusions

The effectiveness of a PFM-based projection symmetric interval in the state estimation approach to NLSs has been demonstrated in this paper. The main principle of the filter estimator is to apply a set of basis functions to iteratively calculate parameters of the prior PDF and posterior PDF based on the Fokker–Planck equation method and the Bayesian equation. In addition, a detailed theoretical derivation process is given in this paper, which mainly includes two parts: a status update and a measurement update. By taking two simple examples, we find that the PFM filter estimation performance is better than the UKF and PF methods. It can be applied to deal with output data from sensors with complex nonlinear systems, especially to solve the problem of high-dimensional state estimation. Compared with UKF and PF, the performance of PFM is significantly improved, the calculation speed is slightly faster, and the calculation complexity is lower. Although PFM gives a highly accurate approximate solution at each time step, to choose a scaling factor is not easy for arbitrary functions. We expect to find more robust ways to obtain the approximate solution for offline data. These problems will be further resolved in our future work.