CN102252672B - A Nonlinear Filtering Method for Underwater Navigation - Google Patents

Abstract

The invention discloses a nonlinear filtering method for underwater navigation, which is used for obtaining a weak solution from a stochastic differential model of underwater terrain navigation. The present invention uses the cubic spline interpolation function to segmentally approximate the weak solution of the navigation stochastic differential model to obtain the prior probability density function of the state, wherein the forward Kolmogorov equation is used to solve the problem of constructing the cubic spline interpolation points The difficult problem of the state, and then get the posterior probability density function of the state by the Bayesian formula. The method of the invention makes full use of the cubic spline interpolation, which has the advantages of simple calculation, good stability, guaranteed convergence, easy realization on the computer, and can ensure the smoothness of the overall curve, and can better track various possibilities of the system state, and has the advantages of High estimation accuracy, convergence speed and convergence smoothness, can track the changes of the system state very well.

Description

A Nonlinear Filtering Method for Underwater Navigation

technical field

The invention relates to the research field of underwater terrain aided navigation, in particular to a nonlinear filtering method for underwater navigation that uses a piecewise function to approximate the weak solution of a navigation stochastic differential model under a nonlinear system with non-Gaussian noise.

Background technique

Precise navigation technology is one of the key technologies to ensure the effective utilization and safe recovery of underwater vehicles. At present, most underwater terrain-assisted navigation systems use the combination of inertial navigation and Doppler log, supplemented by integrated navigation technology such as GPS, gravity, terrain and other correction information. Filtering is the operation of filtering out the frequency of a specific band in the signal. It is an important measure to suppress and prevent interference. However, due to the complex underwater working environment and many uncertain factors, the application of traditional filtering methods is greatly limited. .

The underwater terrain aided navigation process is essentially an optimal estimation problem. Mr. Deng Zili divided the problems to be solved by the optimal estimation theory into three categories: 1. Estimation of model parameters; 2. Estimation of time series, signals or states; 3. Estimation of information fusion. Terrain navigation problems belong to the second category of problems. The development of solution methods coincides with the development of optimal estimation theory. Optimal estimation has gone through three stages of development: least squares method, Kalman filtering and Bayesian estimation filtering:

(1) The most basic method of optimal estimation is the least squares method. This type of estimation method only needs to establish a measurement model. Its basic principle is that the sum of squares of the errors between the actual observation value and the model calculation value is the smallest, and it only guarantees the measurement error. The variance is the smallest, and the measurement data is processed in batches, which is not conducive to real-time processing.

(2) Kalman filtering method, based on the state space theory and projective theory to solve the state estimation problem. Kalman filtering adopts recursive calculation, which is suitable for computer implementation. For linear systems with Gaussian distributed noise, a recursive minimum mean square error estimate of the system state can be obtained. Kalman filtering has been widely used in various fields, such as navigation guidance, target tracking, signal processing, control, etc. The filtering theory originally proposed by Kalman is only applicable to linear systems. Bucy et al. proposed Extended Kalman Filtering (EKF for short), which further applies Kalman filtering theory to the nonlinear field (reference: R S Bucy , K D Senne. Digital synthesis of nonlinear filters [J]. Automatica, 1971, 7: 287-298). The basic idea of EKF is to linearize the nonlinear system and then perform Kalman filtering, so EKF is a suboptimal filtering. Compared with the linearization of nonlinear functions, the approximation of the state conforming to the Gaussian distribution is much simpler. Based on this idea, Julier et al. proposed the Unscented Kalman Filter (UKF) (reference: S.J.Julier , J.K.Uhlmann.non-divergent estimation algorithm in the presence of unknown correlations[C].Proceedings of the IEEE American Control Conference, Albuquerque, NM, USA, 1997, 4:2369-2373), that the probability density distribution of the state can be captured by energy The mean value and variance of the density function are described by the sigma points, and the sigma points and their weights are obtained through UT (Unscented Transction) transformation, and then the weighted sum of the sigma points passed through the state equation of the system is obtained to obtain the posterior mean and covariance . For details, please refer to the article: Kalman Filters for nonlinear systems: a comparison of performance published by Tine L, Herman B, Joris D S. in 2004.

Existing Kalman filtering, EKF and UKF filtering are only suitable for systems with Gaussian distribution noise, and they all aim to obtain the second-order moment of the system state probability distribution, and the estimation accuracy is not very high. And in specific practical applications, the actual distribution of the system state probability may not be a normal distribution. This kind of filtering method is used to describe the probability density distribution of the state by obtaining the mean and variance of the density function. However, the system state is based on the quantity The posterior probability density of the measured information can best represent the actual distribution of the state, so directly solving the probability density function of the system state can clarify the various possibilities of the system state.

(3) The filtering method based on Bayesian (Bayesian) estimation solves the posterior probability density of the system state based on the measurement information. The difficulty of using Bayesian estimation is to solve the prior probability density function of the state and perform integral operations. Only a small number of systems can obtain the optimal solution of Bayesian estimation. For example, the linear Gaussian system can be solved by Kalman filter, and most systems are solved by approximation or approximation. For example, the Partical Filter method is A group of weighted particles (random samples) is used to approximate the probability density of the state through the state transfer equation, and the integral operation is transformed into a weighted sum of finite sample points.

The cubic spline interpolation algorithm also belongs to the category based on Bayesian estimation. Document: Publication No. CN101226630 published on 2008-7-23 Chinese Invention Patent Application "Interpolation Method Based on Cubic Spline Function". , provides an adaptive interpolation method based on a cubic spline function with smooth interpolated images and clear outlines. The technical solution adopted is to first detect the position state of the interpolation point, and then reassess the interpolation point in a different position state. Interpolation operations are performed using different processing methods. In recent years, the particle filter algorithm has been widely used in Bayesian estimation. Literature: Zhang Gongyuan, Zhao Zhong published "A Review of Particle Filter and Its Application in Navigation Systems" in June 2006, which proposed The traditional extended Kalman filter method requires approximate linearization of the nonlinear system, which may introduce a large model error, but the application of particle filter can solve this problem, and the particle filter algorithm can be directly applied to the nonlinear model of the original system Among them, and regardless of whether the system noise and measurement noise are Gaussian white noise, a good filtering effect can be obtained. This document introduces the theoretical basis of particle filter-Bayesian estimation and the specific implementation method-Monte Carlo method; points out the degradation problem of particle filter, and introduces the importance sampling and re-sampling methods to reduce the degradation phenomenon. into the algorithm; Finally, some applications of particle filter in navigation system are expounded. However, in view of the degradation problem of the particle filter, it is difficult to maintain high convergence and smoothness. How to better track the changes of the system state and improve its distribution accuracy is an urgent problem in the field of solving the stochastic differential model for underwater terrain navigation. problem, but there is no solution and enlightenment given in Document 3.

Contents of the invention

The purpose of the present invention is to solve the problem that the weak solution of the weak solution equation of the navigation stochastic differential model established in the background of the underwater terrain navigation system is difficult to obtain, and proposes a nonlinear filtering method for underwater navigation , using a cubic spline interpolation function to approximate its solution in pieces, the prior probability density function of the state can be obtained, and then the posterior probability density function of the state is obtained by the Bayesian formula, and the construction of the cubic spline interpolation condition is also solved It can better track the changes of the system state, and has high estimation accuracy, convergence speed and convergence smoothness.

The present invention is a nonlinear filtering method for underwater navigation. First, the state differential and measurement discrete equations are established for underwater navigation, and a weak solution equation used to represent the motion state of the submersible is obtained, and then the weak solution is obtained through the following steps. The solution equation is approximated to estimate the motion state of the submersible:

Step 1. Take the conditional probability density at the initial moment as a normal distribution, select the projection interval [a, b] and the segment interval a=x ₀ <x ₁ <x ₂ <...<x _n =b, and construct a cubic spline Interpolation function p _i (x):

$p_i\left(x\right)=m_{i-1}\frac{{(x_i-x)}^3}{{6h}_i}+m_i\frac{{\left({x-x}_{i-1}\right)}^3}{{6h}_i}+A_i\left({x-x}_{i-1}\right)+B_i,$ x∈[x _i-1 , x _i ], i=1, 2, ..., n

Among them, A _i , B _i are integral constants with respect to time, coefficient M _i =p″(xi ₎ , p″(xi ₎ is the second order derivative of the cubic spline interpolation function p(x) at node _xi , n represents the number of segmentation intervals, which is an integer greater than 0, h _i represents the length of the i-th segmentation interval, h _i =xi _{- xi-1} ;

Step 2. Obtain the value of the interpolation point through the posterior probability density function at the previous moment, and then use the forward Kolmogorov equation to construct the interpolation point of the cubic spline interpolation function at the current moment, and substitute the interpolation point into the formula (1 ), get the constants A _i , B _i represented by M _i , M _i-1 , so as to obtain the prior probability density function containing only the prior coefficients M _i , M _i-1 ;

Step 3: Use the first-order continuous derivative of the cubic spline interpolation function at each node and the boundary conditions to list n+1 equations, use the catch-up method to find the prior coefficient M _i , and substitute the prior coefficient M _i into the formula (1) Obtain the prior probability density function;

Step 4. Determine the likelihood probability density according to the measurement information at the current moment, and bring the prior probability density and the likelihood probability density into the Bayesian formula to obtain the posterior probability density of the current submersible motion state;

Step 5. Determine whether the simulation time is up. If the simulation time is not up, go to step 2 to continue execution; if the simulation time is up, output the simulation result and end the simulation.

The advantages and positive effects of the present invention are: (1) the filtering method of the present invention increases the edge sharpness on the basis of ensuring the smoothness of the image, and effectively eliminates the problem caused by both sides of the image edge in the original cubic spline interpolation function interpolation method at the same time. The problem of virtual images on both sides of the edge caused by the mutual interference of discontinuous state pixels; (2) the filtering method of the present invention makes full use of the characteristics of cubic spline interpolation, and constructs the cubic spline interpolation condition through the forward Kolmogorov equation. In the function space, the prior probability density function of the state is approximated in sections, and then the posterior probability density of the system state is obtained through the Bayesian formula, which has high estimation accuracy, convergence speed and smoothness of convergence, and can track well A change in the state of the system.

Description of drawings

Fig. 1 is a schematic flow chart of the filtering method of the present invention;

Among Fig. 2, (a) is the probability density schematic diagram of the submersible motion state of the filtering method of the present invention in the 1st second; (b) is the probability density schematic diagram of the submersible motion state of the filtering method of the present invention in the 2nd second; (c) is a schematic diagram of the probability density of the submersible motion state of the filtering method of the present invention at the 31st second; (d) is a schematic diagram of the probability density of the submersible motion state of the filtering method of the present invention at the 32nd second; (e) is A schematic diagram of the probability density of the motion state of the submersible in the filtering method of the present invention at the 33rd second; (f) is a schematic diagram of the probability density of the motion state of the submersible in the 50th second of the filtering method of the present invention;

Fig. 3A is a schematic diagram of the comparison of the position mean values obtained by simulation using the method of the present invention and the particle filter method in the embodiment of the present invention when the time interval is 0.1 second;

Fig. 3B is a schematic diagram of the comparison of position standard deviations obtained by using the method of the present invention and using the particle filter method for simulation in the embodiment of the present invention when the time interval is 0.1 second;

Fig. 4A is a schematic diagram of the comparison of the position mean values obtained by simulation using the method of the present invention and the particle filter method in the embodiment of the present invention when the time interval is 0.01 second;

Fig. 4B is a schematic diagram of the comparison of the position standard deviation obtained by using the method of the present invention and using the particle filter method for simulation in the embodiment of the present invention when the time interval is 0.01 second;

Fig. 5A is a comparative schematic diagram of the root mean square error of the motion state of the submersible in the embodiment of the present invention;

Fig. 5B is a comparative schematic diagram of the standard deviation of the motion state of the submersible in the embodiment of the present invention.

Detailed ways

The present invention will be further described in detail with reference to the accompanying drawings and embodiments.

The present invention is a kind of non-linear filter method for underwater navigation, purpose is to utilize the advantage of the filter method based on Bayesian estimation, provide a kind of cubic spline interpolation method of approaching the weak solution of navigation stochastic differential model in sections, thereby effectively It is ensured that it can better track the change of the system state, and the nonlinear filtering method of the present invention has high estimation accuracy, convergence speed and convergence smoothness.

Taking the underwater terrain-assisted navigation system as the background, the motion state of the submersible can be represented by a random process x(t), assuming that for time t∈[0,∞), all x(t) are defined in a fixed probability space ( Ω, F, P), where Ω is a non-empty set called a sample space whose elements are called samples, F is a non-empty subset of the power set of the sample space, the set F must be a σ-algebra, and P is the probability measure , referred to as probability. The measurement z(t) is an algebraic field in the probability space, and z(t) is P-measurable. shorthand

Unlike the discrete model in which the state and measurement are both described by discrete equations, in the stochastic differential model of underwater navigation, the evolution process of x _t and z _t with time can be described by a state differential equation and a measurement discrete equation:

dx _t = f(x _t , t)dt+b(u _t , t)dt+g(x _t , t)dβ _t (1)

z _k =h(x _k ,t _k )+e _k (2)

表示实数集，n、c、d、m都为正整数，

表示n×1维实数空间，表示c×1维实数空间，表示d×1维实数空间，

表示m×1维实数空间。in:

Represents a set of real numbers, n, c, d, m are all positive integers,

Represents an n×1-dimensional real number space, Represents a c×1-dimensional real number space, Represents a d×1-dimensional real number space,

Represents the m×1-dimensional real number space.

称作漂移函数；

称作控制函数；

is called the drift function;

is called the control function;

是量测函数； is called the diffusion coefficient;

is the measurement function;

e_k是m维白噪声过程，E[e_ke_k ^T]＝R(k)，

β_t与e_k相互独立，t_k表示采样时间。Q(t)、R(k)分别表示为过程噪声和量测噪声的协方差，简记

u _t is c-dimensional control vector, β _t is d-dimensional Brownian motion vector, E[d β _t d β _t ^T ]=Q(t)dt,

e _k is an m-dimensional white noise process, E[e _k e _k ^T ]=R(k),

β _t and e _k are independent of each other, and t _k represents the sampling time. Q(t) and R(k) are respectively expressed as the covariance of process noise and measurement noise, and the abbreviation

As a stochastic process, the solution of the stochastic differential equation can be expressed abstractly by a functional x _t =J[x ₀ ,β _t ,t]. There are two kinds of stochastic differential equations, strong solutions and weak solutions, and only some types of stochastic differential equations have closed solutions of strong solutions. The weak solution is a probability in the continuous function space, which is determined by the transfer function. For the method of the present invention, the weak solution refers to the prior probability obtained below. Under the condition that the coefficients μ(x, t) and σ(x, t) are appropriate, the conditional probability density p(x, t|z _t ) of the motion state of the submersible satisfies the forward Kolmogorov equation:

$\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\frac{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; p</span>}}{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}\underset{\mathrm{<span\; class="notranslate">\; r</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; m</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}\frac{{<span\; class="notranslate">\; \&PartialD;</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; g</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; Q</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; g</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}}<span\; class="notranslate">\; -</span>\underset{\mathrm{<span\; class="notranslate">\; r</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; m</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}\frac{<span\; class="notranslate">\; \&PartialD;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

m表示潜器运动状态空间的维数，f(x，t)表示潜器运动状态变化与潜器运动状态之间的关系，g(x，t)表示噪声扩散，x_r与x_s表示在m维空间中x向量的坐标。in:

m represents the dimension of the motion state space of the submersible, f(x, t) represents the relationship between the motion state change of the submersible and the motion state of the submersible, g(x, t) represents the noise diffusion, x _r and x _s represent the Coordinates of the x-vector in m-dimensional space.

where the initial conditions are:

p'(x ₀ )=0, p'(x _n )=0 (4)

空间中具光滑边界的区域，

是该边值问题的解。The forward Kolmogorov equation (3) and its initial condition (4) constitute a differential equation boundary value problem. Ω is

A region with a smooth boundary in space,

is the solution to the boundary value problem.

The weak solution equation composed of formula (3) and formula (4) can be obtained by establishing state differential and measurement discrete equations, and the motion state of the submersible is estimated by approximating the weak solution. The non-linear filtering method of the present invention approximates the weak solution of the navigation stochastic differential model in sections through cubic spline interpolation, thereby obtaining the motion state of the submersible.

The nonlinear filtering method of the present invention is specifically shown in FIG. 1 , and includes the following steps.

Step 1. Set the conditional probability density at the initial moment as a normal distribution, select the projection interval and segment interval, and construct the cubic spline interpolation function p _i (x):

$p_i\left(x\right)=m_{i-1}\frac{{(x_i-x)}^3}{{6h}_i}+m_i\frac{{\left({x-x}_{i-1}\right)}^3}{{6h}_i}+A_i\left({x-x}_{i-1}\right)+B_i,$ x∈[x _i-1 , x _i ], i=1, 2, ..., n (5)

A _i , B _i are integral constants about time, coefficient M _i =p″(xi ₎ , n represents the number of segment intervals, which is an integer greater than 0, h _i represents the length of the i-th segment interval, h _i = _xi −xi ₋₁ .

The conditional probability density at the initial moment is assumed to be a normal distribution, which can be used as the posterior probability density at the initial moment.

The selected projection interval [a, b] and segment interval a=x ₀ <x ₁ <x ₂ <...<x _n =b are defined by the cubic spline interpolation function p(x): at node x _i The second order derivative p″( _xi ) of p(x) is: p″(xi ₎ =M _i (i=0,1,2,…,n), on the subinterval [xi _-1 , x _i ] Cubic spline interpolation function p(x)=p _i (x), which is a polynomial not higher than cubic, so its second order derivative must be a linear function or constant, by h _i =xi _{- xi- 1} , so the second order derivative p″( _xi ) of the cubic spline interpolation function p(x) is:

$p^{\&prime;\&prime;}\left(x\right)=m_{i-1}\frac{x_i-x}{h_i}+m_i\frac{{x-x}_{i-1}}{h_i},$ x∈[x _i-1 ，x _i ] (6)

Formula (5) can be obtained by continuously integrating the second derivative p″( _xi ) obtained from formula (6) twice.

Step 2. Obtain the value of the interpolation point through the posterior probability density function at the previous moment, and then use the forward Kolmogorov equation to construct the interpolation point of the cubic spline interpolation function at the current moment, and substitute the interpolation point into the cubic spline The interpolation function expression (5), then can use M _i , M _i-1 to express A _i , B _i , get only containing M _i (i=0,1,2,...,n) this n+1 The prior probability density function of unknowns.

简记

Since p( _xi ), p(xi _-1 ) satisfies the forward Kolmogorov equation (3):

shorthand

because: $\frac{\&PartialD;({\mathrm{wxya}}^T\&Center\; Dot;p)}{{\&PartialD;x}_{\mathrm{the\; s}}}=\frac{\&PartialD;\left({\mathrm{wxya}}^T\right)}{{\&PartialD;x}_{\mathrm{the\; s}}}\&Center\; Dot;p+{\mathrm{wxya}}^T\&Center\; Dot;\frac{\&PartialD;\left(p\right)}{{\&PartialD;x}_{\mathrm{the\; s}}},$ $\frac{\&PartialD;(f\&CenterDot;p)}{{\&PartialD;x}_r}=\frac{\&PartialD;\left(f\right)}{{\&PartialD;x}_r}\&CenterDot;p+f\&CenterDot;\frac{\&PartialD;\left(p\right)}{{\&PartialD;x}_r},$ then:

$\frac{{<span\; class="notranslate">\; \&PartialD;</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; wxya</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}}<span\; class="notranslate">\; =</span>\frac{{<span\; class="notranslate">\; \&PartialD;</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; wxya</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&CenterDot;</span>\frac{<span\; class="notranslate">\; \&PartialD;</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; wxya</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}}<span\; class="notranslate">\; \&Center\; Dot;</span>\frac{<span\; class="notranslate">\; \&PartialD;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; wxya</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>\frac{{<span\; class="notranslate">\; \&PartialD;</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$

Further can get:

$\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\frac{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; p</span>}}{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}\underset{\mathrm{<span\; class="notranslate">\; r</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; no</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}<span\; class="notranslate">\; [</span>\frac{{<span\; class="notranslate">\; \&PartialD;</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; wxya</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>\frac{<span\; class="notranslate">\; \&PartialD;</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; wxya</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}}<span\; class="notranslate">\; \&Center\; Dot;</span>\frac{<span\; class="notranslate">\; \&PartialD;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; wxya</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>\frac{{<span\; class="notranslate">\; \&PartialD;</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}}<span\; class="notranslate">\; ]</span><span\; class="notranslate">\; -</span>\underset{\mathrm{<span\; class="notranslate">\; r</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; no</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}<span\; class="notranslate">\; [</span>\frac{<span\; class="notranslate">\; \&PartialD;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>\frac{<span\; class="notranslate">\; \&PartialD;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}}<span\; class="notranslate">\; ]</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 8</span>}<span\; class="notranslate">\; )</span>$

Short note: $\frac{1}{2}\underset{r,\mathrm{the\; s}=1}{\overset{\mathrm{no}}{\mathrm{\&Sigma;}}}\frac{{\&PartialD;}^2\left({\mathrm{wxya}}^T\right)}{{\&PartialD;x}_r{\&PartialD;x}_{\mathrm{the\; s}}}-\underset{r=1}{\overset{\mathrm{no}}{\mathrm{\&Sigma;}}}\frac{\&PartialD;\left(f\right)}{{\&PartialD;x}_r}\stackrel{\mathrm{\&Delta;}}{=}{\mathrm{\&Delta;}}_1,$ $\underset{r=1}{\overset{\mathrm{no}}{\mathrm{\&Sigma;}}}\frac{\&PartialD;\left({\mathrm{wxya}}^T\right)}{{\&PartialD;x}_r}-f\stackrel{\mathrm{\&Delta;}}{=}{\mathrm{\&Delta;}}_2,$ ${\mathrm{wxya}}^T\stackrel{\mathrm{\&Delta;}}{=}{\mathrm{\&Delta;}}_3,$

If g, Q, f are independent of time, then Δ ₁ , Δ ₂ , Δ ₃ are independent of time t, then:

${<span\; class="notranslate">\; \&Integral;</span>}_{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}}^{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{<span\; class="notranslate">\; \&Integral;</span>}_{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}}^{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}\frac{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; p</span>}}{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; t</span>}}\mathrm{<span\; class="notranslate">\; dt</span>}<span\; class="notranslate">\; =</span>{<span\; class="notranslate">\; \&Integral;</span>}_{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}}^{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}\frac{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; p</span>}}{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}\frac{{<span\; class="notranslate">\; \&PartialD;</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}\mathrm{<span\; class="notranslate">\; p</span>}}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; dt</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 9</span>}<span\; class="notranslate">\; )</span>$

Further, by the mean value theorem of integrals, we get:

${\mathrm{<span\; class="notranslate">\; p</span>}}^{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; p</span>}}^{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}^{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}\frac{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; p</span>}}^{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}}}{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}\frac{{<span\; class="notranslate">\; \&PartialD;</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}^{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}}}{{<span\; class="notranslate">\; \&PartialD;</span>\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 10</span>}<span\; class="notranslate">\; )</span>$

分别表示在时刻t_k+1、t_k的三次样条插值函数。t_k+1表示第k+1时刻，是当前时刻，t_k表示第k时刻，是上一时刻。则根据式(10)可得到t_k+1时刻在节点x_i处以及节点x_i-1处的三次样条插值函数的插值点：

in,

represent the cubic spline interpolation functions at time t _k+1 and t _k respectively. t _k+1 represents the k+1th moment, which is the current moment, and t _k represents the kth moment, which is the previous moment. Then, according to formula (10), the interpolation points of the cubic spline interpolation function at node x _i and node x _i-1 at time t _k+1 can be obtained:

Finally the integral constant with respect to time is obtained:

Substitute A _i and B _i into formula (5) to get:

$p_i\left(x\right)=m_{i-1}\frac{{(x_i-x)}^3}{{6h}_i}+m_i\frac{{\left({x-x}_{i-1}\right)}^3}{{6h}_i}+(p_i\left(x_{i-1}\right)-\frac{{{\mathrm{<figure-callout\; id="2"\; label="h\; i"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">h</figure-callout>}}_i}^2}{6}{m}_{i-1})\&CenterDot;\frac{x_i-x}{h_i}+(p_i\left(x_i\right)-\frac{{{\mathrm{<figure-callout\; id="2"\; label="h\; i"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">h</figure-callout>}}_i}^2}{6}{m}_i)\frac{{x-x}_{i-1}}{h_i},$ x∈[x _i-1 , x _i ]

Wherein, i=1, 2, . . . , n. Then it is only necessary to determine the n+1 unknowns of M _i to obtain the prior probability density function.

Step 3. Use the first-order continuous derivative of the cubic spline interpolation function at each node and the boundary conditions to list n+1 equations, use the catch-up method to find the prior coefficient M _i , and substitute it into formula (5) to get the first The expression of the test probability density function.

Because the first-order derivative of the cubic spline interpolation function is continuous on the subinterval node x _i , the boundary conditions can be obtained:

p _i '(x _i -0)=p _i+1 '(x _i +0) (11)

Then by the first derivative of the cubic spline interpolation function:

${p_i}^{\&prime;}\left(x\right)=-m_{i-1}\frac{{(x_i-x)}^2}{h_i}+m_i\frac{{\left({x-x}_{i-1}\right)}^2}{h_i}+\frac{p_i\left(x_i\right)-p_i\left(x_{i-1}\right)}{h_i}-\frac{h_i}{6}(m_i-m_{i-1}),$ x∈[xi _-1 ， _xi ] (12)

From formulas (11) and (12), we can get:

$\frac{{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}{\mathrm{<span\; class="notranslate">\; 6</span>}}{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; 6</span>}}{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>\frac{{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span>}{{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; -</span>\frac{{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}{{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 13</span>}<span\; class="notranslate">\; )</span>$

即得方程：Both sides of equation (13) are multiplied by

That is the equation:

$\frac{h_i}{h_i+h_{i+1}}{m}_{i-1}+{2m}_i+\frac{h_{i+1}}{h_i+h_{i+1}}{m}_{i+1}=\frac{6}{h_i+h_{i+1}}(\frac{p_i\left(x_{i+1}\right)-p_i\left(x_i\right)}{h_{i+1}}-\frac{p_i\left(x_i\right)-p_i\left(x_{i-1}\right)}{h_i})$ (i=1, 2, . . . , n-1) (14)

set up $\left\{\begin{array}{c}{u}_i=\frac{h_i}{h_i+h_{i+1}}\\ {\mathrm{\&lambda;}}_i=\frac{h_{i+1}}{h_i+h_{i+1}}=1-u_i\\ g_i=\frac{6}{h_i+h_{i+1}}(\frac{p_i\left(x_{i+1}\right)-p_i\left(x_i\right)}{h_{i+1}}-\frac{p_i\left(x_i\right)-p_i\left(x_{i-1}\right)}{h_i})\end{array}\right.$

(i=1, 2, . . . , n-1)

From the boundary conditions p′(x ₀ )=0, p′(x _n )=0, we get:

${\mathrm{<span\; class="notranslate">\; p</span>}}^{<span\; class="notranslate">\; \&prime;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\frac{{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}{{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; -</span>\frac{{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="h"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">h</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; 6</span>}}{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>\frac{{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="h"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">h</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="0"\; label="m"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">m</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}$

${\mathrm{<span\; class="notranslate">\; p</span>}}^{<span\; class="notranslate">\; \&prime;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\frac{{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}{{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}{\mathrm{<span\; class="notranslate">\; 6</span>}}{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}{\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}$

That is, two formulas are obtained:

${\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="m"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">m</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="0"\; label="m"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">m</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 6</span>}\frac{{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}{{{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="h"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">h</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 15</span>}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 6</span>}\frac{{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}{{{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 16</span>}<span\; class="notranslate">\; )</span>$

The n+1 formulas composed of formulas (14), (15) and (16) can be obtained:

because non-singular, then there is a unique solution to the system of equations.

_Mi can be obtained by using the chasing method, and _Mi can be substituted into formula (5) to obtain the following formula:

${\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\frac{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 3</span>}}}{{\mathrm{<span\; class="notranslate">\; 6</span>}\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\frac{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 3</span>}}}{{\mathrm{<span\; class="notranslate">\; 6</span>}\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\frac{{{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="h\; i"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">h</figure-callout></span><figure-callout\; id="2"\; label="h\; i"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">\; </figure-callout>}}_{\mathrm{<span\; class="notranslate">\; </span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}{\mathrm{<span\; class="notranslate">\; 6</span>}}{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&CenterDot;</span>\frac{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span>}{{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 16</span>}<span\; class="notranslate">\; )</span>$

$+(p_i\left(x_i\right)-\frac{{{\mathrm{<figure-callout\; id="2"\; label="h\; i"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">h</figure-callout>}}_i}^2}{6}{m}_i)\frac{\left({x-x}_{i-1}\right)}{h_i},$ x∈[x _i-1 , x _i ]

Finally, the prior probability density p(x) expression is obtained:

$\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}& \\ {\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1,2</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}\\ & \end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 17</span>}<span\; class="notranslate">\; )</span>$

Step 4. Calculate the likelihood probability density according to the measurement information at the current moment, bring the prior probability and the likelihood probability density into the Bayesian formula, and obtain the posterior probability density of the motion state of the submersible at the current k+1 time.

By Bayes formula: $p(x,t_{k+1}/z_{k+1})=\frac{p(z_{k+1}/x)p(x,{t^{-}}_{\mathrm{<figure-callout\; id="1"\; label="k\; +"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">k</figure-callout>}+1}/z_k)}{{\&Integral;}_{\mathrm{\&Omega;}}p(z_{k+1}/x)p(x,{t^{-}}_{\mathrm{<figure-callout\; id="1"\; label="k\; +"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">k</figure-callout>}+1}/z_k)\mathrm{dx}}$

Among them, p(z _k+1 /x) represents the likelihood probability density, z _k represents the measurement information at the kth moment, z _k+1 represents the measurement information at the k+1th moment, p(x, t ^- _{k +1} /z _k ) represents the prior probability density at the t _k+1th moment, and p(x, t _k+1 /z _k+1 ) represents the posterior probability density of the submersible’s motion state at the k+1th moment.

where the likelihood density is:

$\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; exp</span>}<span\; class="notranslate">\; \{</span><span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; T</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; \}</span>}{\sqrt{{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; \&pi;</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; m</span>}}\mathrm{<span\; class="notranslate">\; det</span>}{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 18</span>}<span\; class="notranslate">\; )</span>$

m represents the dimension of the submersible motion state space, R _k represents the covariance of measurement noise, and h(x, t _k ) represents the measurement function.

Substituting the prior probability density shown in formula (16) and formula (17) into the Bayesian formula to obtain the subsection of the posterior probability density p(x, t _k+1 /z _k+1 ) of the current submarine motion state The function is as follows:

$p_i(x,t_{k+1}/z_{k+1})=\frac{p(z_{k+1}/x)p_i\left(x\right)}{{\&Integral;}_{\mathrm{\&Omega;}}p(z_{k+1}/x)p_i\left(x\right)\mathrm{dx}},$ x∈[x _i-1 , x _i ] (i=1, 2,..., n) (19)

And get the mean value Ex and variance of the motion state of the submersible at the current moment

$\mathrm{<span\; class="notranslate">\; Ex</span>}<span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}}{\overset{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{<span\; class="notranslate">\; \&Integral;</span>}_{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}^{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; /</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; xdx</span>}$

$\mathrm{<span\; class="notranslate">\; cov</span>}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; E.</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; Ex</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; Ex</span>}<span\; class="notranslate">\; )</span>$

Step 5. If the simulation time is not over, return to step 2 to obtain the prior probability density of the system state at the next moment. If the simulation time has arrived, give the simulation result of the state of the tracking system, the simulation result specifically refers to the mean value and variance of the posterior probability density obtained in step 4, and then end the simulation.

In the following embodiments of the present invention, the simulation model is constructed through the underwater terrain-aided navigation model, and the method of the present invention is realized by computer matlab program simulation.

(1) Utilize matlab to carry out simulation experiment, evaluate the performance of the inventive method in one-dimensional state space. The underwater terrain aided navigation system model used in the experiment is:

$\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; sin</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; w</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>$

z(k)=h(x _k ,t _k )+e(k)

Where: E[ww ^T ]=0.001 ² , E[ee ^T ]=0.5 ² , h(x _k , t _k )=x _k .

Select the projection interval [-4, 4] and segment interval x _i =-4+i/5, i=0, 1, 2, ..., 40, the initial state is a normal distribution, the initial state x ₀ = 0.5, P ₀ =1, the number of particles is 500, the simulation time times=50 times, and the time interval Δt=0.1 second.

In the embodiment of the present invention, the method of the present invention is used to solve the stochastic differential model of underwater terrain navigation, and part of the experimental process is shown in Figure 2 and Table 1:

Integral values of some probability densities in the experiment in Table 1

The simulation results in Fig. 2 and Table 1 show that the filtering method of the present invention can approximate the prior probability density of the motion state of the system submersible in one-dimensional space. Among Fig. 2, spline represents the method of the present invention, and contrasting method is particle filter method (PF), and spline and PF all are the method for solving recursive Bayesian estimation, and one is to carry out in function space to submersible motion state probability density function Segmental approximation, one is to approximate the state distribution in the motion state space of the submersible. In the embodiment of the present invention, the method of the present invention and the particle filter method are respectively simulated. Due to the consideration of the simulation time and the experimental results of the comparative analysis of the two methods, With the underwater vehicle topographic aided navigation system as the background, the position correction information is measured by single wave velocity to obtain the position error and standard deviation curve. In (d) in Fig. 2, it can be seen that a small change occurs near position 1, and the reasons may have: (one) the selection of the cubic spline interpolation point in the method of the present invention has deviation; (two) may be different from the requirements of the cubic spline It is related to satisfying the property of second-order continuous derivability; (3) The selection of the projection interval is inappropriate, resulting in a concave-convex curve. From Table 1, it is basically feasible to adopt the method of the present invention to approximate the probability density of the system. The probability density of the system is based on the posterior probability density, and the benchmark for comparison is that it satisfies the integral property of the probability density.

As shown in FIG. 3A and FIG. 3B , the time interval is Δt=0.1 second, the number of simulations ST=50 times, and the comparison schematic diagram of the position mean value and position standard deviation obtained by using the method of the present invention and the particle filter method for simulation. As shown in Figure 4A and Figure 4B, the time interval is Δt = 0.01 seconds, the number of simulations ST = 50 times, the schematic diagram of the comparison of the position mean value and position standard deviation obtained by using the method of the present invention and the particle filter method for simulation. It can be seen from Fig. 3A and Fig. 4A that the overall probability distribution of the two methods is closer to the real situation. When different time intervals are selected, the effect changes little. It can be seen that the method of the present invention can track the motion state of the submersible. evolve. It can be seen from Fig. 3B and Fig. 4B that the position standard deviation described by the method of the present invention converges, and the convergence speed and convergence smoothness are better than those of the particle filter method.

As shown in FIG. 5A and FIG. 5B , it is an overall comparison diagram of the two methods adopted in the embodiment of the present invention. As shown in Figure 5A, it is a schematic diagram of the comparison of the root mean square error of the submersible motion state of the two methods. The degree of dispersion of the observation points obtained by the method of the present invention shown in the figure is similar to that obtained by the particle filter method. Figure 5B is a schematic diagram of the comparison of the standard deviation of the motion state of the submersible by the two methods. As shown in the figure, it can be seen that the degree of deviation of the estimated value of the method of the present invention from the true value is better than that of the particle filter method.

The above-mentioned embodiment has illustrated that the method of the present invention adopts cubic spline interpolation to approximate the solution of the navigation stochastic differential model with high estimation accuracy, convergence speed and smoothness of convergence, and the filtering method of the present invention can better track the motion state of the submersible Variety.

Claims (5)

1. A nonlinear filtering method for underwater navigation is characterized in that a state differential and measurement discrete equation is established for an underwater terrain aided navigation system, a weak solution equation used for representing the motion state of a submersible vehicle is obtained, and then the motion state of the submersible vehicle is estimated by approximating the weak solution equation through the following steps:

step one, taking the conditional probability density of the initial time as normal distribution, and selecting a projection interval [ a, b ]]And a segmentation interval a ═ x₀＜x₁＜x₂＜...＜x_nB, constructing triplicate samplesStripe interpolation function p_i(x)：

<math> <mrow> <msub> <mi>p</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>M</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mfrac> <msup> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>-</mo> <mi>x</mi> <mo>)</mo> </mrow> <mn>3</mn> </msup> <msub> <mrow> <mn>6</mn> <mi>h</mi> </mrow> <mi>i</mi> </msub> </mfrac> <mo>+</mo> <msub> <mi>M</mi> <mi>i</mi> </msub> <mfrac> <msup> <mrow> <mo>(</mo> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>3</mn> </msup> <msub> <mrow> <mn>6</mn> <mi>h</mi> </mrow> <mi>i</mi> </msub> </mfrac> <mo>+</mo> <msub> <mi>A</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>B</mi> <mi>i</mi> </msub> <mo>,</mo> <mi>x</mi> <mo>&Element;</mo> <mo>[</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>]</mo> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1,2</mn> <mo>,</mo> <mo>&CenterDot;</mo> <mo>&CenterDot;</mo> <mo>&CenterDot;</mo> <mo>,</mo> <mi>n</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </math>

Wherein A is_i,B_iAs an integral constant with respect to time, a priori coefficient M_i＝p″(x_i)，p″(x_i) Interpolating the function p (x) at node x for a cubic spline_iThe second derivative of (a), n represents the number of segment intervals and is an integer greater than 0, h_iDenotes the length of the i-th segmentation interval, h_i＝x_i-x_i-1；

The conditional probability density satisfies the forward kolmogorov equation:

<math> <mrow> <mi>L</mi> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mi>p</mi> </mrow> <mrow> <mo>&PartialD;</mo> <mi>t</mi> </mrow> </mfrac> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>r</mi> <mo>,</mo> <mi>s</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>m</mi> </munderover> <mfrac> <mrow> <msup> <mo>&PartialD;</mo> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>g</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>Q</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msup> <mi>g</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>&CenterDot;</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>s</mi> </msub> </mrow> </mfrac> <mo>-</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>r</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>m</mi> </munderover> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>&CenterDot;</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> </math>

wherein p represents conditional probability density, t represents time, f (x, t) represents the relation between the change of the motion state of the submersible vehicle and the motion state of the submersible vehicle, g (x, t) represents noise diffusion, Q (t) is covariance of process noise, m represents the dimension of the motion state space of the submersible vehicle, and x represents the dimension of the motion state space of the submersible vehicle_rAnd x_sCoordinates representing an x-vector in an m-dimensional space; l (p) represents the forward Kolmogoroff operator;

step two, obtaining the value of an interpolation point through a posterior probability density function of the previous moment, constructing the interpolation point of a cubic spline interpolation function of the current moment by utilizing a forward Kolmogorov equation, and substituting the interpolation point into a formula (1) to obtain a formula M_i,M_i-1Integral constant A of expression_i,B_iThereby obtaining a product containing onlyA prior probability density function of the coefficients is examined;

step three, listing n +1 equation sets by utilizing the first-order continuous derivative and boundary conditions of the cubic spline interpolation function at each node, and solving the prior coefficient M by using a pursuit method_iA priori coefficient M_iAfter the formula (1) is substituted, obtaining the prior probability density of the current moment;

determining likelihood probability density according to the measurement information of the current moment, and substituting the prior probability density and the likelihood probability density into a Bayes formula to obtain the posterior probability density of the motion state of the submersible vehicle at the current moment;

step five, judging whether the simulation time is up, if not, turning to the step two to continue execution; if the simulation time is up, the simulation result is output, and the simulation is finished.

2. The nonlinear filtering method for underwater navigation according to claim 1, wherein the second step is specifically:

first, at node x_i、x_i-1A cubic spline interpolation function p (x)_i)、p(x_i-1) Satisfies the forward kolmogorov equation: <math> <mrow> <mi>L</mi> <mrow> <mo>(</mo> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mi>p</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <mi>t</mi> </mrow> </mfrac> <msub> <mo>|</mo> <mrow> <mi>x</mi> <mo>=</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> </msub> <mo>,</mo> <mi>L</mi> <mrow> <mo>(</mo> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mi>p</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <mi>t</mi> </mrow> </mfrac> <msub> <mo>|</mo> <mrow> <mi>x</mi> <mo>=</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </msub> <mo>,</mo> </mrow> </math> simple notes <math> <mrow> <mi>g</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <mo>=</mo> <mi>&Delta;</mi> </mover> <mi>g</mi> <mo>,</mo> <mi>Q</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <mo>=</mo> <mi>&Delta;</mi> </mover> <mi>Q</mi> <mo>,</mo> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <mo>=</mo> <mi>&Delta;</mi> </mover> <mi>f</mi> <mo>,</mo> </mrow> </math> Comprises the following steps:

<math> <mrow> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>gQ</mi> <msup> <mi>g</mi> <mi>T</mi> </msup> <mo>&CenterDot;</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>s</mi> </msub> </mrow> </mfrac> <mo>=</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>gQ</mi> <msup> <mi>g</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>s</mi> </msub> </mrow> </mfrac> <mo>&CenterDot;</mo> <mi>p</mi> <mo>+</mo> <mi>gQ</mi> <msup> <mi>g</mi> <mi>T</mi> </msup> <mo>&CenterDot;</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>s</mi> </msub> </mrow> </mfrac> <mo>,</mo> </mrow> </math> <math> <mrow> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>f</mi> <mo>&CenterDot;</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> </mrow> </mfrac> <mo>=</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>f</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> </mrow> </mfrac> <mo>&CenterDot;</mo> <mi>p</mi> <mo>+</mo> <mi>f</mi> <mo>&CenterDot;</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> </mrow> </mfrac> <mo>,</mo> </mrow> </math>

then, the following steps are obtained:

<math> <mrow> <mfrac> <mrow> <msup> <mo>&PartialD;</mo> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>gQ</mi> <msup> <mi>g</mi> <mi>T</mi> </msup> <mo>&CenterDot;</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>s</mi> </msub> </mrow> </mfrac> <mo>=</mo> <mfrac> <mrow> <msup> <mo>&PartialD;</mo> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>gQ</mi> <msup> <mi>g</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>s</mi> </msub> </mrow> </mfrac> <mo>&CenterDot;</mo> <mi>p</mi> <mo>+</mo> <mn>2</mn> <mo>&CenterDot;</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>gQ</mi> <msup> <mi>g</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> </mrow> </mfrac> <mo>&CenterDot;</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>s</mi> </msub> </mrow> </mfrac> <mo>+</mo> <mi>gQ</mi> <msup> <mi>g</mi> <mi>T</mi> </msup> <mo>&CenterDot;</mo> <mfrac> <mrow> <msup> <mo>&PartialD;</mo> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>s</mi> </msub> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow> </math>

further obtained by formula (3):

<math> <mrow> <mi>L</mi> <mrow> <mo>(</mo> <mi>p</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mi>p</mi> </mrow> <mrow> <mo>&PartialD;</mo> <mi>t</mi> </mrow> </mfrac> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>r</mi> <mo>,</mo> <mi>s</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mo>[</mo> <mfrac> <mrow> <msup> <mo>&PartialD;</mo> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>gQ</mi> <msup> <mi>g</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>s</mi> </msub> </mrow> </mfrac> <mo>&CenterDot;</mo> <mi>p</mi> <mo>+</mo> <mn>2</mn> <mo>&CenterDot;</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>gQ</mi> <msup> <mi>g</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> </mrow> </mfrac> <mo>&CenterDot;</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>P</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>s</mi> </msub> </mrow> </mfrac> <mo>+</mo> <mi>gQ</mi> <msup> <mi>g</mi> <mi>T</mi> </msup> <mo>&CenterDot;</mo> <mfrac> <mrow> <msup> <mo>&PartialD;</mo> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>s</mi> </msub> </mrow> </mfrac> <mo>]</mo> <mo>-</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>r</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mo>[</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>f</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> </mrow> </mfrac> <mo>&CenterDot;</mo> <mi>p</mi> <mo>+</mo> <mi>f</mi> <mo>&CenterDot;</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> </mrow> </mfrac> <mo>]</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow> </math>

for brevity, the following notes: <math> <mrow> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <munderover> <mi>&Sigma;</mi> <mrow> <mi>r</mi> <mo>,</mo> <mi>s</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mfrac> <mrow> <msup> <mo>&PartialD;</mo> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>gQ</mi> <msup> <mi>g</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>s</mi> </msub> </mrow> </mfrac> <mo>-</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>r</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>f</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> </mrow> </mfrac> <mover> <mo>=</mo> <mi>&Delta;</mi> </mover> <msub> <mi>&Delta;</mi> <mn>1</mn> </msub> <mo>,</mo> </mrow> </math> <math> <mrow> <munderover> <mi>&Sigma;</mi> <mrow> <mi>r</mi> <mo>,</mo> <mi>s</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>gQ</mi> <msup> <mi>g</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&PartialD;</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> </mrow> </mfrac> <mo>-</mo> <mi>f</mi> <mover> <mo>=</mo> <mi>&Delta;</mi> </mover> <msub> <mi>&Delta;</mi> <mn>2</mn> </msub> <mo>,</mo> </mrow> </math> <math> <mrow> <mi>gQ</mi> <msup> <mi>g</mi> <mi>T</mi> </msup> <mover> <mo>=</mo> <mi>&Delta;</mi> </mover> <msub> <mi>&Delta;</mi> <mn>3</mn> </msub> <mo>,</mo> </mrow> </math>

then, when g, Q, f are independent of time t, then Δ₁,Δ₂,Δ₃Also independent of time t, formula (5) is obtained:

<math> <mrow> <msubsup> <mo>&Integral;</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msubsup> <mi>L</mi> <mrow> <mo>(</mo> <mi>p</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mo>&Integral;</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msubsup> <mfrac> <mrow> <mo>&PartialD;</mo> <mi>p</mi> </mrow> <mrow> <mo>&PartialD;</mo> <mi>t</mi> </mrow> </mfrac> <mi>dt</mi> <mo>=</mo> <msubsup> <mo>&Integral;</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>&Delta;</mi> <mn>1</mn> </msub> <mi>p</mi> <mo>+</mo> <msub> <mi>&Delta;</mi> <mn>2</mn> </msub> <mfrac> <mrow> <mo>&PartialD;</mo> <mi>p</mi> </mrow> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> </mfrac> <mo>+</mo> <msub> <mi>&Delta;</mi> <mn>3</mn> </msub> <mfrac> <mrow> <msup> <mo>&PartialD;</mo> <mn>2</mn> </msup> <mi>p</mi> </mrow> <mrow> <mo>&PartialD;</mo> <msup> <mi>x</mi> <mn>2</mn> </msup> </mrow> </mfrac> <mo>)</mo> </mrow> <mi>dt</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow> </math>

further, formula (6) is obtained from the median theorem of integration:

<math> <mrow> <msup> <mi>p</mi> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msup> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mi>p</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>&Delta;</mi> <mn>1</mn> </msub> <msup> <mi>p</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msup> <mo>+</mo> <msub> <mi>&Delta;</mi> <mn>2</mn> </msub> <mfrac> <mrow> <mo>&PartialD;</mo> <msup> <mi>p</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msup> </mrow> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> </mfrac> <mo>+</mo> <msub> <mi>&Delta;</mi> <mn>3</mn> </msub> <mfrac> <mrow> <msup> <mo>&PartialD;</mo> <mn>2</mn> </msup> <msup> <mi>p</mi> <msub> <mi>t</mi> <mi>k</mi> </msub> </msup> </mrow> <mrow> <mo>&PartialD;</mo> <msup> <mi>x</mi> <mn>2</mn> </msup> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>&CenterDot;</mo> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow> </math>

wherein,

respectively, at time t_k+1、t_kThe cubic spline interpolation function of, said t_k+1Represents the k +1 th time, which is the current time, t_kIndicating that the kth time is the last time; then t is obtained according to equation (6)_k+1At node x at time_iAnd node x_i-1Interpolation point of the cubic spline interpolation function of (1):

finally, the interpolation points are substituted in equation (1) to obtain the integration constant with respect to time:

$A_i=\frac{p_i\left(x_i\right)-p_i\left(x_{i-1}\right)}{h_i}-\frac{h_i}{6}(M_i-M_{i-1}),$ $B_i=p_i\left(x_{i-1}\right)-\frac{h_i^2}{6}{M}_{i-1};$

integrating constant A_i,B_iSubstituting formula (1) to obtain the product containing only prior coefficient M_i,M_i-1Prior probability density function of (a):

<math> <mrow> <msub> <mi>p</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>M</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mfrac> <msup> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>-</mo> <mi>x</mi> <mo>)</mo> </mrow> <mn>3</mn> </msup> <msub> <mrow> <mn>6</mn> <mi>h</mi> </mrow> <mi>i</mi> </msub> </mfrac> <mo>+</mo> <msub> <mi>M</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mfrac> <msup> <mrow> <mo>(</mo> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>3</mn> </msup> <msub> <mrow> <mn>6</mn> <mi>h</mi> </mrow> <mi>i</mi> </msub> </mfrac> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>p</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mfrac> <msubsup> <mi>h</mi> <mi>i</mi> <mn>2</mn> </msubsup> <mn>6</mn> </mfrac> <msub> <mi>M</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>&CenterDot;</mo> <mfrac> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mi>x</mi> </mrow> </msub> <msub> <mi>h</mi> <mi>i</mi> </msub> </mfrac> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>p</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mfrac> <msubsup> <mi>h</mi> <mi>i</mi> <mn>2</mn> </msubsup> <mn>6</mn> </mfrac> <msub> <mi>M</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mfrac> <mrow> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> <msub> <mi>h</mi> <mi>i</mi> </msub> </mfrac> <mo>,</mo> <mi>x</mi> <mo>&Element;</mo> <mo>[</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>]</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow> </math>

wherein i is 1,2, …, n.

3. The nonlinear filtering method for underwater navigation according to claim 1, wherein the n +1 equation sets in step three are specifically:

<math> <mrow> <mfrac> <msub> <mi>h</mi> <mi>i</mi> </msub> <mrow> <msub> <mi>h</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mi>h</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mrow> </mfrac> <msub> <mi>M</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mrow> <mn>2</mn> <mi>M</mi> </mrow> <mi>i</mi> </msub> <mo>+</mo> <mfrac> <msub> <mi>h</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mrow> <msub> <mi>h</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mi>h</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mrow> </mfrac> <msub> <mi>M</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfrac> <mn>6</mn> <mrow> <msub> <mi>h</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mi>h</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mrow> </mfrac> <mrow> <mo>(</mo> <mfrac> <mrow> <msub> <mi>p</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>p</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> </mrow> <msub> <mi>h</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mfrac> <mo>-</mo> <mfrac> <mrow> <msub> <mi>p</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <msub> <mrow> <mo>-</mo> <mi>p</mi> </mrow> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> <msub> <mi>h</mi> <mi>i</mi> </msub> </mfrac> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>=</mo> <mn>1,2</mn> <mo>,</mo> <mo>&CenterDot;</mo> <mo>&CenterDot;</mo> <mo>&CenterDot;</mo> <mo>,</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow> </math>

${2M}_1+M_0=6\frac{p_0\left(x_1\right)-p_0\left(x_0\right)}{{h_1}^2}---\left(9\right)$

$M_{n-1}+{2M}_n=-6\frac{p_{n-1}\left(x_n\right)-p_{n-1}\left(x_{n-1}\right)}{{h_n}^2}---\left(10\right)$

wherein p is_i(x_i+1)、p_i(x_i) Respectively represent t_k+1At node x at time_i+1And node x_iA cubic spline interpolation function of (1); obtaining prior coefficient M by using n +1 equation sets through a catch-up method_iThen, the further obtained prior probability density function is:

<math> <mrow> <msub> <mi>p</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>M</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mfrac> <msup> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>-</mo> <mi>x</mi> <mo>)</mo> </mrow> <mn>3</mn> </msup> <msub> <mrow> <mn>6</mn> <mi>h</mi> </mrow> <mi>i</mi> </msub> </mfrac> <mo>+</mo> <msub> <mi>M</mi> <mi>i</mi> </msub> <mfrac> <msup> <mrow> <mo>(</mo> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>3</mn> </msup> <msub> <mrow> <mn>6</mn> <mi>h</mi> </mrow> <mi>i</mi> </msub> </mfrac> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>p</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mfrac> <msubsup> <mi>h</mi> <mi>i</mi> <mn>2</mn> </msubsup> <mn>6</mn> </mfrac> <msub> <mi>M</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>&CenterDot;</mo> <mfrac> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mi>x</mi> </mrow> </msub> <msub> <mi>h</mi> <mi>i</mi> </msub> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow> </math>

<math> <mrow> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>p</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mfrac> <msubsup> <mi>h</mi> <mi>i</mi> <mn>2</mn> </msubsup> <mn>6</mn> </mfrac> <msub> <mi>M</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mfrac> <mrow> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> <msub> <mi>h</mi> <mi>i</mi> </msub> </mfrac> <mo>,</mo> <mi>x</mi> <mo>&Element;</mo> <mo>[</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>]</mo> </mrow> </math>

equation (11) is the piecewise function of the prior probability density of the motion state of the submersible vehicle at the current moment.

4. The nonlinear filtering method for underwater navigation according to claim 1, wherein the fourth step is specifically:

first, a likelihood probability density p (z) of the current time is determined_k+1The/x) is:

<math> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>/</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <mi>exp</mi> <mo>{</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msup> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>-</mo> <mi>h</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msup> <msub> <mi>R</mi> <mi>k</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>-</mo> <mi>h</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>)</mo> </mrow> <mo>}</mo> </mrow> <msqrt> <msup> <mrow> <mo>(</mo> <mn>2</mn> <mi>&pi;</mi> <mo>)</mo> </mrow> <mi>m</mi> </msup> <mi>det</mi> <msub> <mi>R</mi> <mi>k</mi> </msub> </msqrt> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow> </math>

wherein m represents the dimension of the motion state space of the submersible vehicle, R_kRepresents the covariance of the metrology noise, h (x, t)_k) Representing a measurement function, t_k+1Represents the k +1 th time and is the current time;

then the obtained prior probability density and likelihood probability densitySubstituting into Bayes formula to obtain the posterior probability density of the current time, which is the segment function p_i(x,t_k+1/z_k+1)：

<math> <mrow> <msub> <mi>p</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <msub> <mi>t</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>/</mo> <msub> <mi>z</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>/</mo> <mi>x</mi> <mo>)</mo> </mrow> <msub> <mi>p</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mo>&Integral;</mo> <mi>&Omega;</mi> </msub> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>/</mo> <mi>x</mi> <mo>)</mo> </mrow> <msub> <mi>p</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mi>dx</mi> </mrow> </mfrac> <mo>,</mo> <mi>x</mi> <mo>&Element;</mo> <mo>[</mo> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>]</mo> <mrow> <mo>(</mo> <mi>i</mi> <mo>=</mo> <mn>1,2</mn> <mo>,</mo> <mo>&CenterDot;</mo> <mo>&CenterDot;</mo> <mo>&CenterDot;</mo> <mo>,</mo> <mi>n</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow> </math>

Wherein z is_kIndicating measurement information at the k-th time, z_k+1Measurement information indicating the k +1 th time; Ω is the sample space.

5. The nonlinear filtering method for underwater navigation according to claim 1, wherein the simulation result in the fifth step is the mean and variance of the posterior probability density of the motion state of the submersible vehicle obtained in the fourth step:

the mean value Ex of the posterior probability density is as follows:

wherein n represents the number of segment intervals, p_i(x,t_k+1/z_k+1) Is a posterior probability density piecewise function;

the variance of the posterior probability density

Comprises the following steps: <math> <mrow> <mi>cov</mi> <mrow> <mo>(</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <mo>=</mo> <mi>E</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>&CenterDot;</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>-</mo> <mrow> <mo>(</mo> <mi>Ex</mi> <mo>)</mo> </mrow> <mo>&CenterDot;</mo> <mrow> <mo>(</mo> <mi>Ex</mi> <mo>)</mo> </mrow> <mo>.</mo> </mrow> </math>

Images