CN102252672B - Nonlinear filtering method for underwater navigation - Google Patents

Abstract

The invention discloses a nonlinear filtering method for underwater navigation, and is used to solve a weak solution from a random differential model of underwater terrain navigation. According to the invention, segmental approach to a weak solution of a navigation random differential model is performed by using a cubic spline interpolation function so as to obtain a prior probability density function of a state, wherein difficult problems for constructing a cubic spline interpolation point are solved by using a forward Kolmogorov equation, and a posterior probability density function of the state is obtained by a Bayes formula. The method of the invention makes full use of the cubic spline interpolation, has the characteristics of simple calculation, good stability, guaranteed convergence, easy realization on computers, and guaranteed smoothness of a whole curve, can track various possibility of the system state better, has extremely high estimation accuracy, convergence speed, and convergence smoothness, and can track the change of the system state well.

Description

Nonlinear filtering method for underwater navigation

Technical Field

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

Background

The precise navigation technology is one of the key technologies for ensuring the effective utilization and safe recovery of the underwater vehicle. Currently, most of underwater terrain aided navigation systems adopt a comprehensive navigation technology which mainly combines inertial navigation and a doppler log and is assisted by other correction information such as a GPS, gravity, terrain and the like. The filtering is an operation of filtering out specific band frequencies in signals, and is an important measure for suppressing and preventing interference, but the application of the traditional filtering method is greatly limited due to the fact that the underwater working environment is very complex and a plurality of uncertain factors exist.

The underwater topography-assisted navigation process is essentially an optimal estimation problem. Mr. dung divides the problem to be solved by the optimal estimation theory into three categories: 1. the estimation problem of model parameters; 2. estimation problems of time series, signals or states; 3. and (4) information fusion estimation problem. The terrain navigation problem belongs to the second category of problems. The development of the solution method is consistent with the development of the optimal estimation theory. The optimal estimation is subjected to three development stages of least square method, Kalman filtering and Bayesian estimation filtering:

(1) the most basic method for optimal estimation is a least square method, the estimation method only needs to establish a measurement model, the basic principle of the method is that the sum of squares of errors of an actual observation value and a model calculation value is minimum, the variance of a measurement error is only guaranteed to be minimum, batch processing is carried out on measurement data, and real-time processing is not facilitated.

(2) A Kalman (Kalman) filtering method solves the problem of state estimation based on a state space theory and a projective theory. The Kalman filtering adopts recursion calculation and is suitable for being realized by a computer. For a linear system 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, and the like. The first Filtering theory proposed by Kalman was only applicable to linear systems, and Bucy et al propose Extended Kalman Filtering (EKF), which is further applied to the nonlinear field (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 sub-optimal filter. Based on the idea that the approximation of the state-fitting gaussian distribution is much simpler than linearizing the non-linear function, Julier et al propose Unscented Kalman Filtering (UKF) (see s.j. Julier, j.k. uhlmann. non-transformed responses [ C ] Proceedings of the IEEE American control conference, Albuquerque, NM, USA, 1997, 4: 2369 @ 2373), considering that the probability density distribution of states can be described by a sigma point that captures the mean and variance of the density function, obtaining the sigma points and their weights by ut (Unscented transform) transformation, and then weighting and summing the sigma points and variances by the state equations transferred by the system. Specific references may be found in: tone L, Herman B, Joris D s. article Kalman Filters for nonliner systems published in 2004: a compliance of performance.

The existing Kalman filtering, EKF and UKF filtering are only suitable for a system with Gaussian distribution noise, and all aim to obtain a second moment of system state probability distribution, and the estimation precision is not very high. In a specific practical application, the actual distribution of the system state probability may not be normal distribution, and the filtering method is adopted to describe the probability density distribution of the state by obtaining the mean and the variance of the density function, however, the posterior probability density of the system state based on the measurement information can represent the actual distribution of the state most, so that the system state probability density function is directly solved to further clarify various possibilities of the system state.

(3) Based on a filtering method of Bayesian (Bayesian) estimation, the posterior probability density of the system state based on the measurement information is solved. The difficulty with bayesian estimation is solving the prior probability density function of the state and performing the integration operation. Only a few systems can obtain the optimal solution of bayesian estimation, for example, linear gaussian system can be solved by Kalman filtering, most systems are solved by approximation or approximation, for example, particle filtering (particle Filter) method is to approximate the probability density of a state by a set of particles (random samples) with weights through a state transfer equation, and convert the integral operation into the weighted sum of finite sample points.

Cubic spline interpolation algorithms also fall into the category of bayesian-based estimation. The literature: the invention discloses a Chinese patent application 'cubic spline function-based interpolation method' with publication number CN101226630 in 2008-7-23, which applies cubic spline interpolation algorithm to realize an image scaling method of edge adaptive processing and provides a cubic spline function-based adaptive interpolation method with smooth image and clear outline after interpolation. In recent years, the application of the particle filter algorithm in bayesian estimation is very wide, and the document is as follows: on a wish-sharing basis, Zhao faithfully published in 2006 6.6 "particle filter and application review thereof in navigation systems", which proposes that the traditional extended Kalman filtering method requires approximate linearization of a nonlinear system and may introduce a large model error, but the application of particle filter can solve the problem, and the particle filter algorithm can be directly applied to the nonlinear model of the original system without considering whether system noise and measurement noise are white Gaussian noise, so that a good filtering effect can be obtained. The literature introduces the theoretical basis of particle filtering-Bayes estimation and a specific implementation manner-Monte Carlo method; the degradation problem of particle filtering is pointed out, and importance sampling and resampling methods are introduced into an algorithm from the reduction of the degradation phenomenon; finally, some applications of particle filtering in navigation systems are described. However, aiming at the degradation problem of particle filtering, it is difficult to maintain higher convergence and smoothness, and how to better track the change of the system state and improve the distribution accuracy is a problem to be solved in the field of solving the random differential model of underwater terrain navigation, but a method and an introduction for solving the problem are not given in document 3.

Disclosure of Invention

The invention aims to solve the problem that the weak solution of the weak solution equation of a navigation random differential model established by taking an underwater terrain navigation system as a background is difficult to solve, and provides a nonlinear filtering method for underwater navigation.

The invention relates to a nonlinear filtering method for underwater navigation, which comprises the steps of firstly establishing a state differential and measurement discrete equation for the underwater navigation, obtaining a weak solution equation for representing the motion state of a submersible vehicle, and then estimating the motion state of the submersible vehicle by approximating the weak solution equation through the following steps:

step one, the conditional probability of the initial time is used for obtaining the probability densityDegree as normal distribution, and projection interval [ a, b ] is selected]And a segmentation interval a ═ x₀＜x₁＜x₂＜…＜x_nB, constructing 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

Wherein A is_i，B_iAs an integral constant with respect to time, 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；

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-1Constant A of the expression_i，B_iThereby obtaining the signal containing only the prior coefficient M_i，M_i-1A prior probability density function of;

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_iSubstituting formula (1) to obtain a prior probability density function;

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 current submersible vehicle;

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.

The invention has the advantages and positive effects that: (1) the filtering method increases the edge sharpness on the basis of ensuring the image smoothness, and simultaneously effectively eliminates the problem of virtual images at two sides of the edge caused by mutual interference of discontinuous state pixels at two sides of the image edge in the original cubic spline interpolation function interpolation method; (2) the filtering method of the invention fully utilizes the characteristics of cubic spline interpolation, constructs cubic spline interpolation conditions through a forward Kolmogorov equation, performs piecewise approximation on the prior probability density function of the state in a function space, obtains the posterior probability density of the system state through a Bayesian formula, has very high estimation precision, convergence speed and convergence smoothness, and can well track the change of the system state.

Drawings

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

in fig. 2, (a) is a schematic diagram of probability density of the moving state of the submersible vehicle at the 1 st second in the filtering method of the present invention; (b) the probability density diagram of the moving state of the submersible vehicle at the 2 nd second is shown by the filtering method; (c) the probability density diagram of the moving state of the submersible vehicle at the 31 st second by the filtering method is shown; (d) is a schematic diagram of the probability density of the moving state of the submersible vehicle at the 32 th second by the filtering method; (e) is a schematic diagram of the probability density of the moving state of the submersible vehicle at the 33 th second by the filtering method; (f) is a schematic diagram of the probability density of the moving state of the submersible vehicle at the 50 th second by the filtering method;

FIG. 3A is a schematic diagram showing a comparison of position averages obtained by simulation using the method of the present invention and a particle filtering method according to an embodiment of the present invention at a time interval of 0.1 second;

FIG. 3B is a schematic diagram showing a comparison of the standard deviation of positions obtained by simulation using the method of the present invention and the particle filtering method in the embodiment of the present invention at a time interval of 0.1 second;

FIG. 4A is a schematic diagram showing a comparison of position averages obtained by simulation using the method of the present invention and the particle filtering method in the embodiment of the present invention at a time interval of 0.01 second;

FIG. 4B is a schematic diagram showing a comparison of the standard deviation of positions obtained by simulation using the method of the present invention and the particle filtering method in the embodiment of the present invention at a time interval of 0.01 second;

FIG. 5A is a schematic diagram illustrating a comparison of root mean square error of the moving state of the submersible vehicle according to an embodiment of the present invention;

FIG. 5B is a graph illustrating a comparison of standard deviations of the states of motion of the submersible vehicle in an embodiment of the present invention.

Detailed Description

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

The invention discloses a nonlinear filtering method for underwater navigation, and aims to provide a method for solving a navigation random differential model weakly by utilizing the advantages of a filtering method based on Bayes estimation, so that the change of a system state can be better tracked.

In the context of an underwater terrain-aided navigation system, the motion state of a vehicle can be represented by a random process x (t), and it is assumed that for time t ∈ [0, ∞), all x (t) are defined in a fixed probability space (Ω, F, P), where Ω is a non-empty set called sample space, whose elements are called samples, F is a non-empty subset of the power set of the sample space, F must be a σ -algebra, and P is a probability measure, short for probability. Measurement z (t) is an algebraic domain in probability space, and z (t) is Pmeasurable. Simple notes

Unlike the discrete model in which both states and measurements are described by discrete equations, x is a random differential model for underwater navigation_t、z_tThe evolution process with time can be usedA differential equation of state and a measured dispersion 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)

wherein:

representing a real number set, n, c, d and m are all positive integers,

representing an n x 1-dimensional real number space,representing a real number space of dimension c x 1,representing a d x 1-dimensional real number space,

representing an m x 1 dimensional real number space.

Referred to as the drift function;

referred to as a control function;

referred to as the diffusion coefficient;

is a measurement function;

u_tis a c-dimensional control vector, beta_tIs a d-dimensional Brownian motion vector, E [ d β ]_t d β_t ^T]＝Q(t)dt，

e_kIs an m-dimensional white noise process, E [ E ]_ke_k ^T]＝R(k)，

β_tAnd e_kIndependently of each other, t_kRepresenting the sampling time. Q (t), R (k) are represented as covariance of process noise and measurement noise, respectively, and are abbreviated

The solution of the stochastic differential equation as a stochastic process can be abstractly applied with a functional x_t＝J[x₀，β_t，t]And (4) showing. The random differential equations have both strong and weak solutions, and only some types of random differential equations have closed solutions with strong solutions. The weak solution is a probability in a continuous function space, which is determined by a transfer function, and for the method of the invention, the weak solution refers to the prior probability obtained below. Bars appropriate for the coefficients μ (x, t), σ (x, t)Conditional probability density p (x, t | z) of state of motion of submersible_t) 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> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <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> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow> </math>

wherein:

m represents the dimension of the space of the moving state of the submersible vehicle, f (x, t) represents the relation between the change of the moving state of the submersible vehicle and the moving state of the submersible vehicle, g (x, t) represents the noise diffusion, and x (x, t) represents the noise diffusion_rAnd x_sRepresenting the coordinates of the x-vector in m-dimensional space.

Wherein the initial conditions are:

p′(x₀)＝0，p′(x_n)＝0 (4)

the forward kolmogorov equation (3) and its initial condition equation (4) constitute a differential equation side value problem. Omega is

The area in space with a smooth boundary,

is a solution to the edge value problem.

The weak solution equation composed of the formula (3) and the formula (4) can be obtained by establishing a state differential and measurement discrete equation, and the motion state of the submersible vehicle is estimated by approximating the weak solution. The nonlinear filtering method of the invention approximates the navigation random differential model weak solution by a cubic spline interpolation segment, thereby obtaining the motion state of the submersible vehicle.

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

Step 1, setting the conditional probability density of the initial moment as normal distribution, selecting a projection interval and a segmentation interval, and constructing 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 (5)

A_i，B_iAs an integral constant with respect to time, coefficient M_i＝p″(x_i) 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 at the initial time is set as normal distribution and can be used as the posterior probability density at the initial time.

Selecting a projection interval [ a, b ]]And a segmentation interval a ═ x₀＜x₁＜x₂＜…＜x_nB, defined by a cubic spline interpolation function p (x): at node x_iThe second derivative p' (x) of p (x)_i) Comprises the following steps: p' (x)_i)＝M_i(i ═ 0, 1,2, …, n), over the subinterval [ x [, ]_i-1，x_i]The upper cubic spline interpolation function p (x) p_i(x) It is a polynomial of not more than three times, so its second derivative must be a linear function or constant, represented by h_i＝x_i-x_i-1Then the second derivative p "(x) of the cubic spline interpolation function p (x)_i) Comprises the following steps:

<math> <mrow> <msup> <mi>p</mi> <mrow> <mo>&prime;</mo> <mo>&prime;</mo> </mrow> </msup> <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> <mrow> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>-</mo> <mi>x</mi> </mrow> <msub> <mi>h</mi> <mi>i</mi> </msub> </mfrac> <mo>+</mo> <msub> <mi>M</mi> <mi>i</mi> </msub> <mfrac> <msub> <mrow> <mi>x</mi> <mo>-</mo> <mi>x</mi> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>h</mi> <mi>i</mi> </msub> </mfrac> <mo>,</mo> </mrow> </math> x∈[x_i-1，x_i] (6)

the second derivative p' (x) obtained from equation (6)_i) Two successive integrations can yield equation (5).

Step 2, obtaining the value of the interpolation point through the posterior probability density function of the previous moment, constructing the interpolation point of the cubic spline interpolation function of the current moment by utilizing the forward Kolmogorov equation, substituting the interpolation point into the cubic spline interpolation function expression (5), and utilizing M_i，M_i-1Handle A_i，B_iShows that only M is obtained_i(i ═ 0, 1,2, …, n) the prior probability density function of the n +1 unknowns.

Due to p (x)_i)，p(x_i-1) Satisfies the forward kolmogorov equation (3):

simple notes

Because: <math> <mrow> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <msup> <mi>gQg</mi> <mi>T</mi> </msup> <mo>&CenterDot;</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>s</mi> </msub> </mfrac> <mo>=</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <msup> <mi>gQg</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>s</mi> </msub> </mfrac> <mo>&CenterDot;</mo> <mi>p</mi> <mo>+</mo> <msup> <mi>gQg</mi> <mi>T</mi> </msup> <mo>&CenterDot;</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>s</mi> </msub> </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> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> </mfrac> <mo>=</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>f</mi> <mo>)</mo> </mrow> </mrow> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> </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> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> </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> <msup> <mi>gQg</mi> <mi>T</mi> </msup> <mo>&CenterDot;</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>s</mi> </msub> </mrow> </mfrac> <mo>=</mo> <mfrac> <mrow> <msup> <mo>&PartialD;</mo> <mn>2</mn> </msup> <mrow> <mo>(</mo> <msup> <mi>gQg</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <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> <msup> <mi>gQg</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> </mfrac> <mo>&CenterDot;</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>s</mi> </msub> </mfrac> <mo>+</mo> <msup> <mi>gQg</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> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>s</mi> </msub> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow> </math>

further, it is possible to obtain:

<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> <msup> <mi>gQg</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <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> <msup> <mi>gQg</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> </mfrac> <mo>&CenterDot;</mo> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>s</mi> </msub> </mfrac> <mo>+</mo> <msup> <mi>gQg</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> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <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> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> </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> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> </mfrac> <mo>]</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</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> <msup> <mi>gQg</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <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> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> </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> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mfrac> <mrow> <mo>&PartialD;</mo> <mrow> <mo>(</mo> <msup> <mi>gQg</mi> <mi>T</mi> </msup> <mo>)</mo> </mrow> </mrow> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>r</mi> </msub> </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> <msup> <mi>gQg</mi> <mi>T</mi> </msup> <mover> <mo>=</mo> <mi>&Delta;</mi> </mover> <msub> <mi>&Delta;</mi> <mn>3</mn> </msub> <mo>,</mo> </mrow> </math>

if g, Q, f are independent of time, then Δ₁，Δ₂，Δ₃Independently of time t, then:

<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> <msup> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mn>2</mn> </msup> </mfrac> <mo>)</mo> </mrow> <mi>dt</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow> </math>

further 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> <msup> <mrow> <mo>&PartialD;</mo> <mi>p</mi> </mrow> <msub> <mi>t</mi> <mi>k</mi> </msub> </msup> <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> <msup> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mn>2</mn> </msup> </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>10</mn> <mo>)</mo> </mrow> </mrow> </math>

wherein,

respectively, at time t_k+1、t_kCubic spline interpolation function of (1). t is t_k+1Denotes the k +1 th time, which is the current time, t_kIndicating the k-th time, which is the previous time. Then t can be obtained from equation (10)_k+1At node x at time_iAnd node x_i-1Interpolation point of the cubic spline interpolation function of (1):

the integration constant over time is finally obtained:

handle A_i，B_iSubstituting formula (5) to obtain:

<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> <msub> <mrow> <mi>x</mi> <mo>-</mo> <mi>x</mi> </mrow> <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> <msup> <msub> <mi>h</mi> <mi>i</mi> </msub> <mn>2</mn> </msup> <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> <mrow> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>-</mo> <mi>x</mi> </mrow> <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> <msup> <msub> <mi>h</mi> <mi>i</mi> </msub> <mn>2</mn> </msup> <mn>6</mn> </mfrac> <msub> <mi>M</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mfrac> <msub> <mrow> <mi>x</mi> <mo>-</mo> <mi>x</mi> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>h</mi> <mi>i</mi> </msub> </mfrac> <mo>,</mo> </mrow> </math> x∈[x_i-1，x_i]

wherein i is 1,2, …, n. Then only M needs to be determined_iThe n +1 unknowns can be used to obtain the prior probability density function.

Step 3, listing an n +1 equation set by utilizing a first-order continuous derivative of a cubic spline interpolation function at each node and boundary conditions, and solving a prior coefficient M by using a pursuit method_iThe expression of the prior probability density function can be obtained by substituting equation (5).

Cubic spline interpolationFirst derivative of value function at subinterval node x_iAbove, the boundary conditions can be obtained:

p_i′(x_i-0)＝p_i+1′(x_i+0) (11)

the first derivative of the function is interpolated again by a cubic spline:

<math> <mrow> <msup> <msub> <mi>p</mi> <mi>i</mi> </msub> <mo>&prime;</mo> </msup> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <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>2</mn> </msup> <msub> <mi>h</mi> <mi>i</mi> </msub> </mfrac> <mo>+</mo> <msub> <mi>M</mi> <mi>i</mi> </msub> <mfrac> <msup> <mrow> <mo>(</mo> <msub> <mrow> <mi>x</mi> <mo>-</mo> <mi>x</mi> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <msub> <mi>h</mi> <mi>i</mi> </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> <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> </mrow> <msub> <mi>h</mi> <mi>i</mi> </msub> </mfrac> <mo>-</mo> <mfrac> <msub> <mi>h</mi> <mi>i</mi> </msub> <mn>6</mn> </mfrac> <mrow> <mo>(</mo> <msub> <mi>M</mi> <mi>i</mi> </msub> <mo>-</mo> <msub> <mi>M</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>,</mo> </mrow> </math> x∈[x_i-1，x_i] (12)

from the formulae (11) and (12), it is possible:

$\frac{h_i}{6}{M}_{i-1}+\frac{h_i+h_{i+1}}{3}{M}_i+\frac{h_{i+1}}{6}{M}_{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}---\left(13\right)$

multiplication of two sides of formula (13) simultaneously

The equation is obtained:

$\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)

is provided with <math> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>u</mi> <mi>i</mi> </msub> <mo>=</mo> <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> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&lambda;</mi> <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> <mo>=</mo> <mn>1</mn> <mo>-</mo> <msub> <mi>u</mi> <mi>i</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>g</mi> <mi>i</mi> </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> <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> </mrow> <msub> <mi>h</mi> <mi>i</mi> </msub> </mfrac> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> </math>

(i＝1，2，…，n-1)

From the boundary condition p' (x)₀)＝0，p′(x_n) When the ratio is 0, the following:

<math> <mrow> <msup> <mi>p</mi> <mo>&prime;</mo> </msup> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>+</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <msub> <mi>p</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>p</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> </mrow> <msub> <mi>h</mi> <mn>1</mn> </msub> </mfrac> <mo>-</mo> <mfrac> <msub> <mi>h</mi> <mn>1</mn> </msub> <mn>6</mn> </mfrac> <msub> <mi>M</mi> <mn>1</mn> </msub> <mo>-</mo> <mfrac> <msub> <mi>h</mi> <mn>1</mn> </msub> <mn>3</mn> </mfrac> <msub> <mi>M</mi> <mn>0</mn> </msub> <mo>=</mo> <mn>0</mn> </mrow> </math>

<math> <mrow> <msup> <mi>p</mi> <mo>&prime;</mo> </msup> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>n</mi> </msub> <mo>-</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <msub> <mi>p</mi> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>p</mi> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> <msub> <mi>h</mi> <mi>n</mi> </msub> </mfrac> <mo>+</mo> <mfrac> <msub> <mi>h</mi> <mi>n</mi> </msub> <mn>6</mn> </mfrac> <msub> <mi>M</mi> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mfrac> <msub> <mi>h</mi> <mi>n</mi> </msub> <mn>3</mn> </mfrac> <msub> <mi>M</mi> <mi>n</mi> </msub> <mo>=</mo> <mn>0</mn> </mrow> </math>

two equations are obtained:

${2\mathrm{<figure-callout\; id="1"\; label="M"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">M</figure-callout>}}_1+{\mathrm{<figure-callout\; id="0"\; label="M"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">M</figure-callout>}}_0=6\frac{p_0\left(x_1\right)-p_0\left(x_0\right)}{{{\mathrm{<figure-callout\; id="1"\; label="h"\; filenames="HDA0000058244850000021.png,HDA0000058244850000022.png"\; state="\{\{state\}\}">h</figure-callout>}}_1}^2}---\left(15\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(16\right)$

n +1 formulae consisting of formulae (14), (15) and (16) can be obtained:

due to the fact thatAnd if the system is not singular, a unique solution exists in the equation system.

M can be obtained by catch-up method_iHandle M_iSubstitution of formula (5) gives the following formula:

<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> <msub> <mrow> <mi>x</mi> <mo>-</mo> <mi>x</mi> </mrow> <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> <msup> <msub> <mi>h</mi> <mi>i</mi> </msub> <mn>2</mn> </msup> <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> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>-</mo> <mi>x</mi> <mo>)</mo> </mrow> <msub> <mi>h</mi> <mi>i</mi> </msub> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow> </math>

$+(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, obtaining an expression of prior probability density p (x):

$p\left(x\right)=\left\{\begin{array}{cc}& \\ p_i\left(x\right)& i=\mathrm{1,2},...,n\\ & \end{array}\right.---\left(17\right)$

and 4, calculating the likelihood probability density according to the measurement information of the current moment, substituting the prior probability and the likelihood probability density into a Bayes formula, and solving the posterior probability density of the motion state of the submersible vehicle at the current k +1 moment.

From bayesian formula: <math> <mrow> <mi>p</mi> <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> <mi>p</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <msub> <msup> <mi>t</mi> <mo>-</mo> </msup> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>/</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <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> <mi>p</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <msub> <msup> <mi>t</mi> <mo>-</mo> </msup> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>/</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mi>dx</mi> </mrow> </mfrac> </mrow> </math>

wherein, p (z)_k+1/x) represents likelihood probability density, z_kIndicating measurement information at the k-th time, z_k+1Represents the measurement information at the k +1 th time, p (x, t)^- _k+1/z_k) Denotes the t-th_k+1A priori probability density of time instants, p (x, t)_k+1/z_k+1) The posterior probability density of the state of motion of the submersible vehicle at the time k +1 is represented.

Wherein the likelihood probability density 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>18</mn> <mo>)</mo> </mrow> </mrow> </math>

m represents the dimension of the state space of the vehicle motion, R_kRepresents the covariance of the metrology noise, h (x, t)_k) Representing the measurement function.

Substituting the prior probability densities shown in the formulas (16) and (17) into a Bayesian formula to obtain the posterior probability density p (x, t) of the motion state of the current submersible vehicle_k+1/z_k+1) The piecewise function of (c) is as follows:

<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> </mrow> </math> x∈[x_i-1，x_i](i＝1，2，…，n) (19)

and obtaining the mean value Ex and the variance of the motion state of the submersible vehicle at the current moment

<math> <mrow> <mi>Ex</mi> <mo>=</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> </munderover> <msubsup> <mo>&Integral;</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <msub> <mi>x</mi> <mrow> <mi>i</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </msubsup> <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>&CenterDot;</mo> <mi>xdx</mi> </mrow> </math>

<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> </mrow> </math>

And 5, if the simulation time is not finished, returning to the step 2 to execute, and obtaining the prior probability density of the system state at the next moment. And if the simulation time is reached, giving a simulation result of the state of the tracking system, wherein the simulation result specifically refers to the mean value and the variance of the posterior probability density obtained in the step 4, and then ending the simulation.

In the following embodiment of the invention, a simulation model is constructed through an underwater terrain auxiliary navigation model, and the method is realized by adopting computer matlab program simulation.

(1) And (3) carrying out a simulation experiment by utilizing matlab, and evaluating the performance of the method in a one-dimensional state space. The underwater terrain aided navigation system model used in the experiment is as follows:

<math> <mrow> <mover> <mi>x</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>sin</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>/</mo> <mn>2</mn> <mo>)</mo> </mrow> <mo>+</mo> <mi>w</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </math>

z(k)＝h(x_k，t_k)+e(k)

wherein: e [ ww^T]＝0.001²，E[ee^T]＝0.5²，h(x_k，t_k)＝x_k。

Selecting a projection interval [ -4, 4 [ -4]And a segmentation interval x_i-4+ i/5, i-0, 1, 2., 40, with the initial state being a normal distribution, initial state x₀＝0.5，P₀The number of particles is 500, the simulation time is 50 times, and the time interval Δ t is 0.1 second.

In the embodiment of the invention, the method is adopted to solve the underwater terrain navigation random differential model, and part of experimental processes are shown in figure 2 and table 1:

table 1 integral value of partial probability density in experiment

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 the one-dimensional space. In fig. 2, spline represents the method of the present invention, the comparison method is a particle filtering method (PF), and both spline and PF are methods for solving recursive bayesian estimation, one is to perform piecewise approximation on a probability density function of a motion state of a submersible vehicle in a function space, and the other is to perform approximation on state distribution in the motion state space of the submersible vehicle. In fig. 2 (d) it can be seen that small variations occur near position 1, for reasons that may be: the selection of cubic spline interpolation points in the method has deviation; (II) may be related to the cubic spline requirement to satisfy the property of second order continuous conductance; and thirdly, the projection interval is not properly selected, so that the curve is concave-convex. From table 1, it is basically feasible to approximate the probability density of the system using the method of the present invention. The probability density of the system is based on the posterior probability density, and the reference of comparison is the integral property of meeting the probability density.

As shown in fig. 3A and 3B, the time interval Δ t is 0.1 second, and the simulation time ST is 50 times, and the comparison between the position average and the position standard deviation obtained by the simulation using the particle filter method and the method of the present invention is shown. As shown in fig. 4A and 4B, the time interval Δ t is 0.01 seconds, the simulation time ST is 50 times, and the comparison between the position average and the position standard deviation obtained by the simulation using the particle filter method and the method of the present invention is shown. Fig. 3A and fig. 4A show that the overall probability distribution of the two methods is relatively close to the real situation, and when different time intervals are selected, the effect is not changed greatly, so that the method of the present invention can track the evolution of the motion state of the submersible vehicle. It can be seen from fig. 3B and 4B that the position standard deviation described by the method of the present invention converges, and the convergence speed and the convergence smoothness are better compared with the particle filtering method.

Fig. 5A and 5B are overall comparison diagrams of two methods adopted in the embodiment of the present invention. Fig. 5A is a schematic diagram showing a comparison between root mean square errors of the moving states of the submersible vehicles in the two methods, wherein the degree of dispersion of the observation points obtained by the method of the present invention is not much different from the degree of dispersion obtained by the particle filtering method. FIG. 5B is a schematic diagram showing the comparison of the standard deviation of the moving state of the submersible vehicle in the two methods, and 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 filtering method.

The above embodiment illustrates that the solution of the method of the present invention, which adopts cubic spline interpolation to approximate the navigation random differential model, has high estimation accuracy, convergence rate and convergence smoothness, and the filtering method of the present invention can better track the change of the motion state of the submersible vehicle.

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