CN104022757B - A kind of linear expansion method of the multilamellar Unscented kalman filtering device of High Order Moment coupling - Google Patents

Abstract

The invention discloses the linear expansion method of the Unscented kalman filtering device of a kind of High Order Moment coupling, belong to nonlinear filtering technique field.This method comprises the following steps: 1) sets up the state equation of nonlinear system and measures equation；2) initial state value of system is determined；3) state estimation based on previous step and state equation, use linear expansion Unscented transform calculates the distribution characteristics of the stochastic variable of a step status predication；4) linear expansion Unscented transform is used to calculate the stochastic variable of status predication distribution characteristics after measuring equation transform；5) prediction of Kalman gain Fusion Strain and actual measurement data is used to calculate the distribution characteristics of optimum state；6) judge whether iteration terminates.The present invention fills the thought of use ratio correction and the structure of Symmetric Orthogonal sample and multiple sampling is combined, and by mating more High Order Moment so that close approximation precision significantly improves, reduces computation complexity, greatly improves computational efficiency.

Description

Linear extension method of high-order moment matched multilayer unscented Kalman filter

Technical Field

The invention relates to the technical field of information fusion such as nonlinear filtering, digital signal processing, target positioning and tracking and the like, in particular to a Linear-extension method (LUKF) of a high-order moment matching multilayer unscented Kalman filter.

Background

Almost all real-world systems are non-linear, particularly in the fields of aircraft navigation, target tracking, and industrial control. For example: in the process of positioning and tracking the target, when the radar is used for observing the aerial target, the radar can obtain the azimuth angle of the aerial target relative to the radar, but the observation contains noise, the azimuth observed quantity of the radar in the observation equation is a nonlinear function of a target position parameter to be estimated, the motion state of the target cannot be directly obtained by using a linear filtering method, the problem of nonlinear filtering is essentially solved, and the method is a common problem in the research fields of target tracking, digital signal processing and the like.

For the problem of nonlinear filtering, two types of filtering methods are often adopted: one is to perform linear approximation on a nonlinear function and adopt neglect or approximation measures on high-order terms, wherein the most widely used is an Extended Kalman Filter (EKF), and the basic idea is to perform first-order linear truncation on a Taylor expansion of the nonlinear function, so as to convert the nonlinear problem into linearity; the other is to approximate the nonlinear distribution by sampling method, commonly used Particle Filter (PF) and Unscented Kalman Filter (UKF), whose basic principle is to approximate the distribution of random variables of the nonlinear function using sample points in combination with their weights.

In contrast to unscented kalman filters, EKFs have the following three disadvantages: (1) when the high-order term of the nonlinear function Taylor expansion cannot be ignored, the linearization can cause a system to generate a large error, even the filter is difficult to stabilize; (2) the Jacobian matrix of the nonlinear function is difficult to obtain in many practical problems, even does not exist; (3) because the EKF requires derivation, the specific form of the nonlinear function must be clearly understood, and black box encapsulation cannot be achieved, so that the EKF is difficult to modularize and apply. Currently, although there are many improved methods for EKF, such as high-order truncated EKF, iterative EKF, etc., these drawbacks are still difficult to overcome. Research shows that the estimation result given by UKF is more accurate than EKF, higher-order calculation precision can be achieved, and the calculation amount is the same order as EFK. In contrast to the unscented kalman filter, the particle filter uses a very large number of random sample points, and the number of sample points increases geometrically with the dimension of the problem, which is very expensive to calculate. The UKF method adopts deterministic typical sample points closely related to the distribution thereof, the number of the deterministic typical sample points is relatively greatly reduced, and the random distribution with general dimension n only needs 2n +1 sample points, even only needs n +1 sample points to achieve the precision of matching second moment.

Common mainstream sampling strategies of the unscented kalman filter are mainly classified into four types: (1) symmetric sampling, which is characterized in that sample points are symmetrically distributed about an average point; (2) simplex sampling, which is characterized in that the distribution of sampling sigma points is not symmetrical about a mean value point, sample points are few, and the number of the sample points is a little more than that of the dimension; (3) scaling correction, which is characterized in that scaling is carried out on sampled sample points; (4) high order sampling, which is characterized by adopting various strategies to match the distributed high order moments.

At present, the UKF applied in the target tracking field of people is a second-order UKF method, and for a Gaussian nonlinear system, the estimation precision of the second-order UKF can only reach the cubic Taylor expansion of a nonlinear function, so that the precision is limited, and in practical application, a sampling strategy considering both the precision and the calculation efficiency is urgently needed.

Disclosure of Invention

In view of the above, an object of the present invention is to provide a linear expansion method for a high-order moment-matched multilayer unscented kalman filter, which is used to solve the problems of accuracy and computational efficiency of a nonlinear filter in an actual application process, and in combination with an existing sampling strategy, the accuracy is improved by using a high-order moment and multiple single-line samples, and meanwhile, the computational complexity is reduced by using symmetric sampling and a simplified proportion correction method, so as to achieve synchronous improvement of the accuracy and the computational efficiency.

In order to achieve the purpose, the invention provides the following technical scheme:

a linear extension method of a multilayer unscented Kalman filter matched with a high-order moment specifically comprises the following steps:

1) establishing a state equation and a measurement equation of a nonlinear system according to actual engineering application;

2) determining initial state values of the system, namely random distribution characteristics of the initial state, including the mean value, covariance and high-order moment of the random distribution characteristics, the distribution characteristics of noise and initial measurement values;

3) and (3) one-step state prediction: calculating the distribution characteristics of random variables of one-step state prediction by using linear extended traceless transformation based on the state estimation and the state equation of the previous step;

4) and (3) one-step measurement prediction: based on the state prediction and measurement equation in the step 3), calculating the distribution characteristics of the random variables of the state prediction after being transformed by the measurement equation by using linear expansion unscented transformation;

5) calculating the distribution characteristics of the optimal state by using Kalman gain fusion state prediction and actual measurement data to complete a one-step estimation task of the nonlinear system;

6) and judging whether the iteration is finished, if not, substituting the characteristics of the random variables of the current step into the step 3) to be used as the state estimation of the previous step, and carrying out the calculation of the next step.

Further, the state equation and the measurement equation in step 1) are:

wherein: x is the number of_k+1Is the n-dimensional state vector of step k +1, z_k+1For the m-dimensional measurement vector of step k +1, f (-) and h (-) are non-linear functions, w_kIs n-dimensional random system noise, and the system noise obeys zero mean value and Q covariance_k(ii) a gaussian distribution of; v. of_k+1Random measurement noise in m dimensions, where the measurement noise obeys a mean of zero and a covariance of R_kAnd system noise and measurement noise are not correlated with each otherCorrelation; function f (x)_k) Is a mathematical model of the system state transformation, function h (x)_k+1) Is a mathematical model of the system state measurement.

Further, the step 3) of calculating the distribution characteristics of the random variables of the one-step state prediction by using the linear extended unscented transformation based on the state estimation and the state equation of the previous step specifically includes the following four steps:

3-1) predicting the union (x) of the random variables and the system noise random variables according to the state of the previous step_k,w_k) Determining the high-order moment to be matched, further determining the number l of the layering layers, and selecting a positive number sequence 0 < r according to the specific problem characteristics₁＜r₂＜…＜r_lDetermining the layering of the sample points, and determining the sample points based on the layering, wherein the sigma points are selected according to the following formula:

$\left\{\begin{array}{ccc}{\mathrm{\&chi;}}_{0,k}=\left[\begin{array}{c}{\left(x_{0,k}\right)}_{n\&times;1}\\ {\left(W_{0,k}\right)}_{n\&times;1}\end{array}\right]=\left[\begin{array}{c}{\left({\tilde{x}}_{k|k}\right)}_{n\&times;1}\\ 0_{n\&times;1}\end{array}\right]& & \\ {\mathrm{\&chi;}}_{i,k}^j=\left[\begin{array}{c}{\left(x_{i,k}^j\right)}_{n\&times;1}\\ {\left(W_{i,k}^j\right)}_{n\&times;1}\end{array}\right]=\left[\begin{array}{c}{({\tilde{x}}_{k|k}+{\left(\sqrt{r_j{P}_{x,k|k}}\right)}_i)}_{n\&times;1}\\ 0_{n\&times;1}\end{array}\right]& 1\&le;i\&le;n,1\&le;j\&le;l& \\ {\mathrm{\&chi;}}_{i,k}^j=\left[\begin{array}{c}{\left(x_{i,k}^j\right)}_{n\&times;1}\\ {\left(W_{i,k}^j\right)}_{n\&times;1}\end{array}\right]\text{=}\left[\begin{array}{c}{({\tilde{x}}_{k|k}-{\left(\sqrt{r_j{P}_{x,k|k}}\right)}_{i-n})}_{n\&times;1}\\ 0_{n\&times;1}\end{array}\right]& n<i\&le;2n,1\&le;j\&le;l& \\ {\mathrm{\&chi;}}_{i,k}^j=\left[\begin{array}{c}{\left(x_{i,k}^j\right)}_{n\&times;1}\\ {\left(W_{i,k}^j\right)}_{n\&times;1}\end{array}\right]=\left[\begin{array}{c}{\left({\tilde{x}}_{k|k}\right)}_{n\&times;1}\\ {\left({\left(\sqrt{r_j{Q}_k}\right)}_{i-2n}\right)}_{n\&times;1}\end{array}\right]& 2n<i\&le;3n,1\&le;j\&le;l& \\ {\mathrm{\&chi;}}_{i,k}^j=\left[\begin{array}{c}{\left(x_{i,k}^j\right)}_{n\&times;1}\\ {\left(W_{i,k}^j\right)}_{n\&times;1}\end{array}\right]=\left[\begin{array}{c}{\left({\tilde{x}}_{k|k}\right)}_{n\&times;1}\\ {(-{\left(\sqrt{r_j{Q}_k}\right)}_{i-3n})}_{n\&times;1}\end{array}\right]& 3n<i\&le;4n,1\&le;j\&le;l& \end{array}\right.---\left(2\right)$

wherein:corresponding to the random variable (x) in the k step_k,w_k) The ith sigma point vector of the jth layer of (1), from the state sample point vectorAnd system noise sample point vectorComposition is carried out; chi shape_0,kIs the sigma point vector of the innermost layer,represents the optimal estimated mean vector, P, of the k-th step_x,k|kEstimating covariance for the k-th step;

3-2) weighting the sigma points obtained in the step 3-1), and the rule is as follows: χ in formula (2)_0,kCorresponding weight is W_0,k，Corresponding weight isThe weight value of the sample point of each layer is required to be as large, i.e.J is more than or equal to 1 and less than or equal to l, 1 is more than or equal to η and less than or equal to 2n, and lambda is more than or equal to 1 and less than or equal to 2 n;

3-3) matching high order moments: solving the linear equation system according to the formula (3) to obtain the weight:

$\left\{\begin{array}{c}\left[\begin{array}{c}{\tilde{x}}_{k|k}\\ 0_{n\&times;1}\end{array}\right]=W_{0,k}{\mathrm{\&chi;}}_0+\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k}^j{\mathrm{\&chi;}}_{i,k}^j\\ P_{\mathrm{xw},k|k}=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k}^j({\mathrm{\&chi;}}_{i,k}^j-{\mathrm{\&chi;}}_{0,k}){({\mathrm{\&chi;}}_{i,k}^j-{\mathrm{\&chi;}}_{0,k})}^T\\ m_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\left(x_{k|k}\right)=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k}^j{({\mathrm{\&chi;}}_{i,k}^j-{\mathrm{\&chi;}}_{0,k})}_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\\ \underset{j=1}{\overset{l}{\mathrm{\&Sigma;}}}\underset{i=1}{\overset{4n}{\mathrm{\&Sigma;}}}{W}_{i,k}^j=1\end{array}\right.---\left(3\right)$

wherein, $P_{\mathrm{xw},k|k}=\left[\begin{array}{cc}{P}_{x,k|k}& 0_{n\&times;n}\\ 0_{n\&times;n}& Q_k\end{array}\right],$ is a random vector x_k|kα order moments of the β th random variable.

3-4) calculating the distribution characteristics of random variable state equation transformation:

according to the transformation function, calculating the transformation sigma point of the sigma point after the transformation of the state equationThe calculation method comprises the following steps:

Y_0,k＝f(x_0,k)+W_0,k, $Y_{i,k}^j=f\left(x_{i,k}^j\right)+W_{i,k}^j,i=1,...,4n,1\&le;j\&le;l---\left(4\right)$

calculating a transformed random variable x according to equation (5)_k+1|k＝f(x_k)+w_kMean vector of (2):

${\tilde{x}}_{k+1|k}=W_{0,k}{Y}_{0,k}+\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k}^j{Y}_{i,k}^j---\left(5\right)$

calculating a transformed random variable x according to equation (6)_k+1|kThe covariance of (a):

$P_{x,k+1|k}=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k}^j(Y_{i,k}^j-{\tilde{x}}_{k+1|k}){(Y_{i,k}^j-{\tilde{x}}_{k+1|k})}^T---\left(6\right)$

calculating a transformed random variable x according to equation (7)_k+1|kHigher order moments of (a):

$m_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\left(x_{k+1|k}\right)=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k}^j{(Y_{i,k}^j-{\tilde{x}}_{k+1|k})}_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}---\left(7\right)$

further, the step 4) of calculating the distribution characteristics of the random variables of the state prediction after the transformation of the measurement equation by using the linear extended unscented transformation based on the state prediction and the measurement equation of the step 3) specifically comprises the following four steps:

4-1) Joint (x) of predicting random variables from states and measuring noise random variables_k+1|k,v_k+1) Determining the high-order moment to be matched, further determining the number l of the layering layers, and selecting a positive number sequence 0 < r according to the specific problem characteristics₁＜r₂＜…＜r_lDetermining the layering of the sample points, and determining the sample points based on the layering, wherein the sigma points are selected according to the following formula:

$\left\{\begin{array}{ccc}{X}_{0,k+1}=\left[\begin{array}{c}{\left(X_{0,k+1}\right)}_{n\&times;1}\\ {\left(V_{0,k+1}\right)}_{m\&times;1}\end{array}\right]=\left[\begin{array}{c}{\left({\tilde{x}}_{k+1|k}\right)}_{n\&times;1}\\ 0_{m\&times;1}\end{array}\right]& & \\ X_{i,k+1}^j=\left[\begin{array}{c}{\left(X_{i,k+1}^j\right)}_{n\&times;1}\\ {\left(V_{i,k+1}^j\right)}_{m\&times;1}\end{array}\right]=\left[\begin{array}{c}{({\tilde{x}}_{k+1|k}+{\left(\sqrt{r_j{P}_{x,k+1|k}}\right)}_i)}_{n\&times;1}\\ 0_{m\&times;1}\end{array}\right]& 1\&le;i\&le;n,1\&le;j\&le;l& \\ X_{i,k+1}^j=\left[\begin{array}{c}{\left(X_{i,k+1}^j\right)}_{n\&times;1}\\ {\left(V_{i,k+1}^j\right)}_{m\&times;1}\end{array}\right]\text{=}\left[\begin{array}{c}{({\tilde{x}}_{k+1|k}-{\left(\sqrt{r_j{P}_{x,k+1|k}}\right)}_{i-n})}_{n\&times;1}\\ 0_{m\&times;1}\end{array}\right]& n<i\&le;2n,1\&le;j\&le;l& \\ X_{i,k+1}^j=\left[\begin{array}{c}{\left(X_{i,k+1}^j\right)}_{n\&times;1}\\ {\left(V_{i,k+1}^j\right)}_{m\&times;1}\end{array}\right]=\left[\begin{array}{c}{\left({\tilde{x}}_{k+1|k}\right)}_{n\&times;1}\\ {\left({\left(\sqrt{r_j{R}_k}\right)}_{i-2n}\right)}_{m\&times;1}\end{array}\right]& 2n<i\&le;2n+m,1\&le;j\&le;l& \\ X_{i,k+1}^j=\left[\begin{array}{c}{\left(X_{i,k+1}^j\right)}_{n\&times;1}\\ {\left(V_{i,k+1}^j\right)}_{m\&times;1}\end{array}\right]=\left[\begin{array}{c}{\left({\tilde{x}}_{k+1|k}\right)}_{n\&times;1}\\ {(-{\left(\sqrt{r_j{R}_k}\right)}_{i-3n})}_{m\&times;1}\end{array}\right]& 2n+m<i\&le;2(n+m),1\&le;j\&le;l& \end{array}\right.---\left(8\right)$

wherein,denotes (x) in the k-th step_k+1|k,v_k+1) Corresponding ith sigma point vector of j layer, measuring sample point vectorAnd measuring the noise sample point vectorComposition X_0,k+1Is the sigma point of the corresponding innermost layer,indicating the state of the kth stepMeasuring the estimated mean vector, P_x,k+1|kCovariance is estimated for the state prediction at step k.

4-2) weighting the sigma points obtained in the step 3-1), and the rule is as follows: in the formula (8), X_0,k+1Corresponding weight is W_0,k+1，Corresponding weight isThe weight value of the sample point of each layer is required to be as large, i.e.J is more than or equal to 1 and less than or equal to l, 1 is more than or equal to η and less than or equal to 2n, and lambda is more than or equal to 1 and less than or equal to 2 n;

4-3) matching high order moments: solving the linear equation system according to the formula (9) to obtain the weight:

$\left\{\begin{array}{c}\left[\begin{array}{c}{\tilde{x}}_{k+1|k}\\ 0_{m\&times;1}\end{array}\right]=W_{0,k+1}{X}_{0,k+1}+\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k+1}^j{X}_{i,k+1}^j\\ P_{\mathrm{xv},k+1|k}=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k+1}^j(X_{i,k+1}^j-X_{0,k+1}){(X_{i,k+1}^j-X_{0,k+1})}^T\\ m_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\left(x_{k+1|k}\right)=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k+1}^j{(X_{i,k+1}^j-X_{0,k+1})}_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\\ \underset{j=1}{\overset{l}{\mathrm{\&Sigma;}}}\underset{i=1}{\overset{2(m+n)}{\mathrm{\&Sigma;}}}{W}_{i,k+1}^j=1\end{array}\right.---\left(9\right)$

wherein, $P_{\mathrm{xv},k+1|k}=\left[\begin{array}{cc}{P}_{x,k+1|k}& 0_{n\&times;m}\\ 0_{m\&times;n}& R_k\end{array}\right],$ is a random vector x_k+1|kα order moments of the β th random variable;

4-4) calculating the distribution characteristics of the random variables after transformation by a measurement equation:

calculating the transformed sigma point of the sigma point after the transformation of the measurement equationThe calculation method comprises the following steps:

Z_0,k+1＝h(X_0,k+1)+V_0,k+1, $Z_{i,k+1}^j=h\left(X_{i,k+1}^j\right)+V_{i,k+1}^j,i=1,...,2(m+n),1\&le;j\&le;l---\left(10\right)$

calculating a transformed random variable z according to equation (11)_k+1＝h(x_k+1)+v_k+1Mean vector of (2):

${\tilde{z}}_{k+1|k}=W_{0,k+1}{Z}_{0,k+1}+\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k+1}^j{Z}_{i,k+1}^j---\left(11\right)$

calculating a transformed random variable z according to equation (12)_k+1|kThe covariance of (a):

$P_{z,k+1|k}=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k+1}^j(Z_{i,k+1}^j-{\tilde{z}}_{k+1|k}){(Z_{i,k+1}^j-{\tilde{z}}_{k+1|k})}^T---\left(12\right)$

calculating the higher order moments of the transformed random variables according to equation (13):

$m_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\left(z_{k+1|k}\right)=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k+1}^j{(Z_{i,k+1}^j-{\tilde{z}}_{k+1|k})}_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}---\left(13\right)$

based on sigma pointAndintercept about x_kAnd x_k+1|kOf (2) samplingAndcalculating x according to equation (14)_k+1|kAnd z_k+1|kCross covariance:

$P_{\mathrm{xz},k+1|k}=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k+1}^j(X_{i,k+1}^j-{\tilde{x}}_{k+1|k}){(Z_{i,k+1}^j-{\tilde{z}}_{k+1|k})}^T---\left(14\right)$

further, the step 5) of calculating the distribution characteristics of the optimal state by using kalman gain fusion state prediction and measurement data includes the specific process of completing the one-step estimation task of the nonlinear system:

calculating x according to equation (15)_k+1Average value of (d):

${\tilde{x}}_{k+1}={\tilde{x}}_{k+1|k}+K_{k+1}(z_{k+1}-{\tilde{z}}_{k+1|k})---\left(15\right)$

in the formula $K_{k+1}=P_{\mathrm{xz},k+1|k}{P}_{z,k+1|k}^{-1},$ Is the Kalman gain;

calculating x according to equations (16) and (17)_k+1Of the best estimate of (2) covariance P_x,k+1|k+1And high order moment

$P_{x,k+1|k+1}=P_{x,k+1|k}-K_{k+1}{P}_{z,k+1|k}{K}_{k+1}^T---\left(16\right)$

$m_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\left(x_{k+1}\right)=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k+1}^j{(X_{i,k+1}^j+K_{k+1}(y_c-(h\left(X_{i,k+1}^j\right)+V_{i,k+1}^j))-{\tilde{z}}_{k+1|k})}_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}---\left(17\right)$

Wherein: y is_cIs the actual measured data value.

The invention has the advantages that: the invention fully utilizes the geometrical characteristics of random distribution, combines the orthogonal symmetrical sample and the multi-sampling structure by using the idea of proportional correction, obviously improves the approximate approximation precision by matching more high-order moments, and simultaneously reduces the calculation complexity by using the symmetrical sampling and simplified proportional correction method, thereby greatly improving the calculation efficiency.

Additional advantages, objects, and features of the invention will be set forth in part in the description which follows and in part will become apparent to those having ordinary skill in the art upon examination of the following or may be learned from practice of the invention.

Drawings

In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention will be further described in detail with reference to the accompanying drawings, in which:

FIG. 1 is a detailed process flow diagram of the present invention;

FIG. 2 is a graph of absolute error in the application of a one-dimensional non-stationary growth model based on the LUKF method and the UKF method provided by the present invention, in which the dotted line is a curve of the LUKF and the solid line is a curve of the UKF method;

fig. 3 is a mean square error curve diagram in the application of the one-dimensional non-stationary growth model based on the LUKF method and the UKF method provided by the present invention, wherein a dash-dot line is a curve of the LUKF, and a solid line is a curve of the UKF method.

Detailed Description

The preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings; it should be understood that the preferred embodiments are illustrative of the invention only and are not limiting upon the scope of the invention.

In the process of target tracking, the observation station can obtain target position information containing noise, the relationship between the position information and target position information to be estimated is nonlinear, and usually an EKF, second-order UKF, second-order CKF or higher-order CKF nonlinear filtering method is applied to obtain the motion state of the target. However, the filtering method cannot meet the requirements on occasions with higher target positioning requirements. Compared with the existing method, the method provided by the invention has higher estimation precision and can improve the precision of target tracking.

When the k-th step is performed, the following embodiment is used to specifically describe the following steps:

step 1): according to an economically one-dimensional non-stationary growth model (UNGM), a state equation (18) and an observation equation (19) describing the target tracking system are established as follows:

$x_k=\frac{x_{k-1}}{2}+\frac{25\&CenterDot;x_{k-1}}{1+x_{k-1}^2}+8\&CenterDot;\cos \left(\frac{6(k-1)}{5}\right)+w_k---\left(18\right)$

$z_k=\frac{x_{k-1}^2}{20}+v_k---\left(19\right)$

wherein k is 1,2, …, N is the number of simulation steps, and in this example, N takes 1000 steps; random system noise vector w_kObedience mean zero, variance Q_k-1Is a gaussian distribution of the identity matrix; random measurement noise vector v_kObedience mean 0, variance R_kIs a gaussian distribution of the identity matrix.

Step 2): determining the initial state value of the system, namely the random distribution characteristics of the initial state, including the mean value, covariance and high-order moment:

initial state value: x is the number of₀0.1; initial covariance matrix P₀1, high pitch: $m_1^8\left(x_0\right)=105.$

step 3): and calculating the distribution characteristics of the random variables of the one-step state prediction by using linear extended non-trace transformation based on the state estimation and the state equation of the previous step.

The step 3 specifically comprises the following four steps:

3-1) estimating a random variable and a combination (x) of the noise random variables according to the state of the step (k-1)_k-1,w_k-1) Characteristic of the distribution, i.e. mean valueCovariance P_x,k-1|k-1And variance Q of noise_k-1Specifically, when k is 1,P_x,0|0＝P₀(ii) a According to the random distribution characteristics, because 8-order high-order moment exists, the number of layered layers is further determined to be 4, and a positive number sequence 0 & lt r is selected according to the specific problem characteristics₁＝1＜r₂＝2＜r₃＝3＜r₄Layering is carried out as 4;

the sigma point is selected by the formula (20):

$\left\{\begin{array}{ccc}{\mathrm{\&chi;}}_{0,k-1}=\left[\begin{array}{c}{\left(x_{0,k-1}\right)}_{1\&times;1}\\ {\left(W_{0,k-1}\right)}_{1\&times;1}\end{array}\right]=\left[\begin{array}{c}{\left({\tilde{x}}_{k-1|k-1}\right)}_{1\&times;1}\\ 0_{1\&times;1}\end{array}\right]& & \\ {\mathrm{\&chi;}}_{i,k-1}^j=\left[\begin{array}{c}{\left(x_{i,k-1}^j\right)}_{1\&times;1}\\ {\left(W_{i,k-1}^j\right)}_{1\&times;1}\end{array}\right]=\left[\begin{array}{c}{({\tilde{x}}_{k-1|k-1}+{\left(\sqrt{r_j{P}_{x,k-1|k-1}}\right)}_i)}_{1\&times;1}\\ 0_{1\&times;1}\end{array}\right]& i=1,1\&le;j\&le;4& \\ {\mathrm{\&chi;}}_{i,k-1}^j=\left[\begin{array}{c}{\left(x_{i,k-1}^j\right)}_{1\&times;1}\\ {\left(W_{i,k-1}^j\right)}_{1\&times;1}\end{array}\right]\text{=}\left[\begin{array}{c}{({\tilde{x}}_{k-1|k-1}-{\left(\sqrt{r_j{P}_{x,k-1|k-1}}\right)}_{i-1})}_{1\&times;1}\\ 0_{1\&times;1}\end{array}\right]& i=2,1\&le;j\&le;4& \\ {\mathrm{\&chi;}}_{i,k-1}^j=\left[\begin{array}{c}{\left(x_{i,k-1}^j\right)}_{1\&times;1}\\ {\left(W_{i,k-1}^j\right)}_{1\&times;1}\end{array}\right]=\left[\begin{array}{c}{\left({\tilde{x}}_{k-1|k-1}\right)}_{1\&times;1}\\ {\left({\left(\sqrt{r_j}\right)}_{i-2}\right)}_{1\&times;1}\end{array}\right]& i=3,1\&le;j\&le;4& \\ {\mathrm{\&chi;}}_{i,k-1}^j=\left[\begin{array}{c}{\left(x_{i,k-1}^j\right)}_{1\&times;1}\\ {\left(W_{i,k-1}^j\right)}_{1\&times;1}\end{array}\right]=\left[\begin{array}{c}{\left({\tilde{x}}_{k-1|k-1}\right)}_{1\&times;1}\\ {(-{\left(\sqrt{r_j}\right)}_{i-3})}_{1\&times;1}\end{array}\right]& i=4,1\&le;j\&le;4& \end{array}\right.---\left(20\right)$

wherein,represents the corresponding random variable (x) in step k-1_k-1,w_k-1) The ith sigma point vector of the jth layer of (1), from the state sample point vectorAnd system noise sample point vectorComposition is carried out; chi shape_0,k-1Is the sigma point vector of the corresponding innermost layer,represents the optimal estimated vector, P, of step k-1_x,k-1|k-1The covariance matrix is optimally estimated in the step k-1;

3-2) weighting the sigma points obtained in the step 3-1) according to the following rules: χ in equation (20)_0,k-1Corresponding weight is W_0,k-1，Corresponding weight isWhere i 1, …,4, j 1, …,4, we generally require that the weight values for the sample points of each layer be as large, i.e., 1 j l, 1 η n, 1 λ 2n,for a total of 5 unknown weights.

3-3) matching high order moments: solving a linear equation set according to a formula (3) to obtain a weight; due to the symmetric sample points, equation (3) is rewritten as equation (21), for a total of 5 linear equations, there is a unique solution:

$\left\{\begin{array}{c}{P}_{\mathrm{xw},k-1|k-1}=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k-1}^j({\mathrm{\&chi;}}_{i,k-1}^j-{\mathrm{\&chi;}}_{0,k-1}){({\mathrm{\&chi;}}_{i,k-1}^j-{\mathrm{\&chi;}}_{0,k-1})}^T\\ m_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\left(x_{k-1|k-1}\right)=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k-1}^j{({\mathrm{\&chi;}}_{i,k-1}^j-{\mathrm{\&chi;}}_{0,k-1})}_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\\ \underset{j=1}{\overset{4}{\mathrm{\&Sigma;}}}\underset{i=1}{\overset{4}{\mathrm{\&Sigma;}}}{W}_{i,k-1}^j=1\end{array}\right.---\left(21\right)$

wherein, $P_{\mathrm{xw},k-1|k-1}=\left[\begin{array}{cc}{P}_{x,k-1|k-1}& 0\\ 0& 1\end{array}\right],$ is a random vector x_k-1|k-1α order moments of the β th random variable, in this example α is 4,6, 8.

3-4) calculating the distribution characteristics of random variable state equation transformation:

according to the transformation function, calculating the transformation sigma point of the sigma point transformed by the state equationAccording to the formula (22), the calculation method is as follows:

Y_0,k-1＝f(x_0,k-1)+W_0,k-1, $Y_{i,k-1}^j=f\left(x_{i,k-1}^j\right)+W_{i,k-1}^j,i=1,...,4,1\&le;j\&le;4---\left(22\right)$

then, a transformed random variable x is calculated according to equation (23)_k|k-1＝f(x_k-1)+w_k-1Mean vector of (2):

${\tilde{x}}_{k|k-1}=W_{0,k-1}{Y}_{0,k-1}+\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k-1}^j{Y}_{i,k-1}^j---\left(23\right)$

calculating a transformed random variable x according to equation (24)_k|k-1The covariance of (a):

$P_{x,k|k-1}=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k-1}^j(Y_{i,k-1}^j-{\tilde{x}}_{k|k-1}){(Y_{i,k-1}^j-{\tilde{x}}_{k|k-1})}^T---\left(24\right)$

calculating x of the transformed random variable according to equation (25)_k|k-1High order moment:

$m_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\left(x_{k|k-1}\right)=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k-1}^j{(Y_{i,k-1}^j-{\tilde{x}}_{k|k-1})}_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}---\left(25\right)$

step 4): based on the state prediction and measurement equation in the step 3), calculating the distribution characteristics of the random variables of the state prediction after being transformed by the measurement equation by using linear expansion unscented transformation;

the step 4) specifically comprises the following four steps:

4-1) predicting the union (x) of the random variables and the noisy random variables according to the state of step 3)_k|k-1,v_k) Characteristic of the distribution, i.e. mean valueCovariance P_x,k|k-1And measuring the variance R of the noise_k. According to the random distribution characteristics, because 8-order high-order moment exists, the number of layered layers is further determined to be 4, and a positive number sequence 0 & lt r is selected according to the specific problem characteristics₁＝1＜r₂＝2＜r₃＝3＜r₄Layering is carried out as 4; based on the hierarchy, the sigma point is chosen by equation (26).

$\left\{\begin{array}{ccc}{X}_{0,k}=\left[\begin{array}{c}{\left(X_{0,k}\right)}_{1\&times;1}\\ {\left(V_{0,k}\right)}_{1\&times;1}\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_{k|k-1}\\ 0\end{array}\right]& & \\ X_{i,k}^j=\left[\begin{array}{c}{\left(X_{i,k}^j\right)}_{1\&times;1}\\ {\left(V_{i,k}^j\right)}_{1\&times;1}\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_{k|k-1}+{\left(\sqrt{r_j{P}_{x,k|k-1}}\right)}_i\\ 0\end{array}\right]& i=1,1\&le;j\&le;4& \\ X_{i,k}^j=\left[\begin{array}{c}{\left(X_{i,k}^j\right)}_{1\&times;1}\\ {\left(V_{i,k}^j\right)}_{1\&times;1}\end{array}\right]\text{=}\left[\begin{array}{c}{\tilde{x}}_{k|k-1}-{\left(\sqrt{r_j{P}_{x,k|k-1}}\right)}_{i-1}\\ 0\end{array}\right]& i=2,1\&le;j\&le;4& \\ X_{i,k}^j=\left[\begin{array}{c}{\left(X_{i,k}^j\right)}_{1\&times;1}\\ {\left(V_{i,k}^j\right)}_{1\&times;1}\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_{k|k-1}\\ {\left(\sqrt{r_j}\right)}_{i-2}\end{array}\right]& i=3,1\&le;j\&le;4& \\ X_{i,k}^j=\left[\begin{array}{c}{\left(X_{i,k}^j\right)}_{1\&times;1}\\ {\left(V_{i,k}^j\right)}_{1\&times;1}\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_{k|k-1}\\ -{\left(\sqrt{r_j}\right)}_{i-3}\end{array}\right]& i\&le;4,1\&le;j\&le;4& \end{array}\right.---\left(26\right)$

Wherein,represents the random variable (x) in step k-1_k|k-1,v_k) Corresponding ith sigma point vector of j layer, measuring sample point vectorAnd measuring the noise sample point vectorComposition is carried out; x_0,kIs the sigma point of the corresponding innermost layer.

4-2) giving step one place according to the following rulesWeighting sigma points: in the formula (26), X_0,kCorresponding weight is W_0,k，Corresponding weight isWhere i 1, …,4, j 1, …,4, we generally require that the weight values for the sample points of each layer be as large, i.e., 1 j l, 1 η n, 1 λ 2n,for a total of 5 unknown weights.

4-3) matching high order moments: and solving the linear equation system according to the formula (9) to obtain the weight. Due to the symmetric sample points, equation (9) is rewritten as equation (27), for a total of 5 linear equations, there is a unique solution:

$\left\{\begin{array}{c}{P}_{\mathrm{xv},k|k-1}=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k}^j(X_{i,k}^j-X_{0,k}){(X_{i,k}^j-X_{0,k})}^T\\ m_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\left(x_{k|k-1}\right)=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k}^j{(X_{i,k}^j-X_{0,k})}_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\\ \underset{j=1}{\overset{4}{\mathrm{\&Sigma;}}}\underset{i=1}{\overset{4}{\mathrm{\&Sigma;}}}{W}_{i,k}^j=1\end{array}\right.---\left(27\right)$

wherein, $P_{\mathrm{xv},k|k-1}=\left[\begin{array}{cc}{P}_{x,k|k-1}& 0\\ 0& 1\end{array}\right],$ is a random vector x_k|k-1α order moments of the β th random variable.

4-4) calculating the distribution characteristics of random variable measurement quantity equation transformation:

according to the transformation function, calculating the transformation sigma point of the sigma point after transformation of the measurement equationAccording to the formula (28), the calculation method is as follows:

Z_0,k＝h(X_0,k)+V_0,k, $Z_{i,k}^j=h\left(X_{i,k}^j\right)+V_{i,k}^j,i=1,...,4,1\&le;j\&le;4---\left(28\right)$

then, the transformed random variable z is calculated according to equation (29)_k＝h(x_k)+v_kMean vector of (2):

${\tilde{z}}_{k|k-1}=W_{0,k}{Z}_{0,k}+\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k}^j{Z}_{i,k}^j---\left(29\right)$

calculating a transformed random variable z according to equation (30)_k|k-1The covariance of (a):

$P_{z,k|k-1}=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k}^j(Z_{i,k}^j-{\tilde{z}}_{k|k-1}){(Z_{i,k}^j-{\tilde{z}}_{k|k-1})}^T---\left(30\right)$

calculating a transformed random variable z according to equation (31)_k|k-1Higher order moments of (a):

$m_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\left(z_{k|k-1}\right)=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k}^j{(Z_{i,k}^j-{\tilde{z}}_{k|k-1})}_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}---\left(31\right)$

based on sigma pointAndintercept about x_k-1And x_k|k-1Of (2) samplingAndwe calculate x according to equation (32)_k|k-1And z_k|k-1Cross covariance:

$P_{\mathrm{xz},k|k-1}=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k}^j(X_{i,k}^j-{\tilde{x}}_{k|k-1}){(Z_{i,k}^j-{\tilde{z}}_{k|k-1})}^T---\left(32\right)$

step 5): and (3) calculating the distribution characteristics of the optimal state by using Kalman gain fusion state prediction and actual measurement data, completing one-step estimation task of the nonlinear system, iterating and returning to the step 3), and performing the estimation task at the next moment.

The method specifically comprises the following steps: according to formula (33)x_kThe average value of (a) of (b),

${\tilde{x}}_k={\tilde{x}}_{k|k-1}+K_k(z_k-{\tilde{z}}_{k|k-1})---\left(33\right)$

in the formula, $K_k=P_{\mathrm{xz},k|k-1}{P}_{z,k|k-1}^{-1}$ is the Kalman gain;

calculating x according to equation (16)_kOf the best estimate of (2) covariance P_x,k|k：

$P_{x,k|k}=P_{x,k|k-1}-K_k{P}_{z,k|k-1}{K}_k^T---\left(34\right)$

Calculating x according to equation (16)_kOf the best estimated higher order moment

$m_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}\left(x_k\right)=\underset{j}{\mathrm{\&Sigma;}}\underset{i}{\mathrm{\&Sigma;}}{W}_{i,k}^j{(X_{i,k}^j+K_k(y_c-(h\left(X_{i,k}^j\right)+V_{i,k}^j))-{\tilde{z}}_{k|k-1})}_{\mathrm{\&beta;}}^{\mathrm{\&alpha;}}---\left(35\right)$

Then judging whether the iteration is ended or not, if not, then judging the random variable x_kThe characteristic of (2) is substituted into the step three to be used as the state estimation of the previous step, and the calculation of the (k + 1) th step is carried out.

The implementation process comprises the following steps: in the simulation process, the absolute error performance index defined as follows is adopted, see formula (36), the mean square error performance index, see formula (37), and various filtering methods are compared:

$\mathrm{AE}\left(k\right)=|{\tilde{x}}_k-x_k|---\left(36\right)$

$\mathrm{MSE}\left(k\right)=\frac{1}{k}\sqrt{\underset{n=1}{\overset{k}{\mathrm{\&Sigma;}}}{({\tilde{x}}_k-x_k)}^2}---\left(37\right)$

the smaller the mean square error of the target position estimation is, the higher the target tracking precision is, and the better the effect is. Wherein, the simulation time is 1000 steps. Under the same simulation conditions, the UKF method and the LUKF method are respectively adopted for testing, and 500 Monte Carlo simulations are carried out in each method for comparison.

The implementation effect is as follows: fig. 2 and fig. 3 respectively show absolute error curves in the use of the UNGM based on the LUKF method provided by the present invention and the use of the UNGM based on the UKF method, and mean square error curves in the use of the UNGM based on the LUKF method provided by the present invention and the UKF method. Obviously, the maximum fluctuation value of the absolute error curve of the LUKF method is much smaller than that of the UKF method, and the fluctuation is more stable; the mean square error of the LUKF method is smaller than that of the UKF method, and the mean square error approaches to a stable error value.

Finally, it is noted that the above-mentioned preferred embodiments illustrate rather than limit the invention, and that, although the invention has been described in detail with reference to the above-mentioned preferred embodiments, it will be understood by those skilled in the art that various changes in form and detail may be made therein without departing from the scope of the invention as defined by the appended claims.

Claims (3)

1. A linear expansion method of a multilayer unscented Kalman filter with high-order moment matching is characterized by comprising the following steps:

1) establishing a state equation and a measurement equation of a nonlinear system according to actual engineering application;

2) determining initial state values of the system, namely random distribution characteristics of the initial state, including the mean value, covariance and high-order moment of the random distribution characteristics, the distribution characteristics of noise and initial measurement values;

3) and (3) one-step state prediction: calculating the distribution characteristics of random variables of one-step state prediction by using linear extended traceless transformation based on the state estimation and the state equation of the previous step;

4) and (3) one-step measurement prediction: based on the state prediction and measurement equation in the step 3), calculating the distribution characteristics of the random variables of the state prediction after being transformed by the measurement equation by using linear expansion unscented transformation;

5) calculating the distribution characteristics of the optimal state by using Kalman gain fusion state prediction and actual measurement data to complete a one-step estimation task of the nonlinear system;

6) judging whether iteration is finished, if not, substituting the characteristics of the random variables of the current step into the step 3) to be used as state estimation of the previous step, and calculating the next step;

the state equation and the measurement equation in the step 1) are as follows:

wherein: x is the number of_k+1Is the n-dimensional state vector of step k +1, z_k+1For the m-dimensional measurement vector of step k +1, f (-) and h (-) are non-linear functions, w_kIs n-dimensional random system noise, and the system noise obeys zero mean value and Q covariance_k(ii) a gaussian distribution of; v. of_k+1Random measurement noise in m dimensions, where the measurement noise obeys a mean of zero and a covariance of R_kAnd the system noise and the measurement noise are uncorrelated with each other; function f (x)_k) Is a mathematical model of the system state transformation, function h (x)_k+1) Is a mathematical model of the system state measurement;

step 3) calculating the distribution characteristics of the random variables of the one-step state prediction by using linear extended unscented transformation based on the state estimation and the state equation of the previous step, which specifically comprises the following four steps:

3-1) predicting the union (x) of the random variables and the system noise random variables according to the state of the previous step_k,w_k) Determining the high-order moment to be matched, further determining the number l of the layering layers, and selecting a positive number sequence 0 < r according to the specific problem characteristics₁＜r₂＜…＜r_lDetermining the layering of the sample points, and determining the sample points based on the layering, wherein the sigma points are selected according to the following formula:

wherein:corresponding to the random variable (x) in the k step_k,w_k) The ith sigma point vector of the jth layer of (1), from the state sample point vectorAnd system noise sample point vectorComposition is carried out; chi shape_0,kIs the sigma point vector of the innermost layer,represents the optimal estimated mean vector, P, of the k-th step_x,k|kEstimating covariance for the k-th step;

3-2) weighting the sigma points obtained in the step 3-1), and the rule is as follows: χ in formula (2)_0,kCorresponding weight is W_0,k，Corresponding weight isThe weight value of the sample point of each layer is required to be as large, i.e.J is more than or equal to 1 and less than or equal to l, 1 is more than or equal to η and less than or equal to 2n, and lambda is more than or equal to 1 and less than or equal to 2 n;

3-3) matching high order moments: solving the linear equation system according to the formula (3) to obtain the weight:

wherein, is a random vector x_k|kα order moments of the β th random variable;

3-4) calculating the distribution characteristics of random variable state equation transformation:

according to the transformation function, calculating the transformation sigma point of the sigma point after the transformation of the state equationThe calculation method comprises the following steps:

Y_0,k＝f(x_0,k)+W_0,k,

calculating a transformed random variable x according to equation (5)_k+1|k＝f(x_k)+w_kMean vector of (2):

calculating a transformed random variable x according to equation (6)_k+1|kThe covariance of (a):

calculating a transformed random variable x according to equation (7)_k+1|kHigher order moments of (a):

2. the linear expansion method of the multilayer unscented kalman filter of the higher order moment matching according to claim 1, characterized in that, the step 4) uses the linear expansion unscented transform to calculate the distribution characteristics of the random variables of the state prediction after being transformed by the measurement equation based on the state prediction and measurement equation of the step 3), and specifically includes the following four steps:

4-1) Joint (x) of predicting random variables from states and measuring noise random variables_k+1|k,v_k+1) Determining the high-order moment to be matched, further determining the number l of the layering layers, and selecting a positive number sequence 0 < r according to the specific problem characteristics₁＜r₂＜…＜r_lDetermining the layering of the sample points, and determining the sample points based on the layering, wherein the sigma points are selected according to the following formula:

wherein,denotes (x) in the k-th step_k+1|k,v_k+1) Corresponding ith sigma point vector of j layer, measuring sample point vectorAnd measuring the noise sample point vectorComposition X_0,k+1Is the sigma point of the corresponding innermost layer,mean vector of state prediction estimates, P, representing the k-th step_x,k+1|kEstimating covariance for the state prediction of step k;

4-2) weighting the sigma points obtained in step 3-1)And the rule is as follows: in the formula (8), X_0,k+1Corresponding weight is W_0,k+1，Corresponding weight isThe weight value of the sample point of each layer is required to be as large, i.e.J is more than or equal to 1 and less than or equal to l, 1 is more than or equal to η and less than or equal to 2n, and lambda is more than or equal to 1 and less than or equal to 2 n;

4-3) matching high order moments: solving the linear equation system according to the formula (9) to obtain the weight:

wherein, is a random vector x_k+1|kα order moments of the β th random variable;

4-4) calculating the distribution characteristics of the random variables after transformation by a measurement equation:

calculating the transformed sigma point of the sigma point after the transformation of the measurement equationThe calculation method comprises the following steps:

Z_0，k+1＝h(X_0,k+1)+V_0,k+1,

calculating a transformed random variable z according to equation (11)_k+1＝h(x_k+1)+v_k+1All areVector of values:

calculating a transformed random variable z according to equation (12)_k+1|kThe covariance of (a):

calculating the higher order moments of the transformed random variables according to equation (13):

based on sigma pointAndintercept about x_kAnd x_k+1|kOf (2) samplingAndcalculating x according to equation (14)_k+1|kAnd z_k+1|kCross covariance:

。

3. the linear expansion method of the multilayer unscented kalman filter of the higher order moment matching according to claim 1, characterized in that, the distribution feature of the optimal state is calculated by using kalman gain fusion state prediction and measurement data in step 5), and the specific process of completing the one-step estimation task of the nonlinear system is as follows:

calculating x according to equation (15)_k+1Average value of (d):

in the formulaIs the Kalman gain;

calculating x according to equations (16) and (17)_k+1Of the best estimate of (2) covariance P_x,k+1|k+1And high order moment

Wherein: y is_cIs the actual measured data value.