CN113608442A

CN113608442A - State estimation method of nonlinear state model system based on characteristic function

Info

Publication number: CN113608442A
Application number: CN202110899253.7A
Authority: CN
Inventors: 王金诚; 袁洢苒; 文成林; 王秋鹏; 张俊锋
Original assignee: Hangzhou Dianzi University
Current assignee: Hangzhou Dianzi University
Priority date: 2021-08-05
Filing date: 2021-08-05
Publication date: 2021-11-05

Abstract

The invention discloses a state estimation method of a nonlinear state model system based on a characteristic function. The nonlinear state model is linearized based on Taylor expansion, continuous guidable points are searched for a nonlinear part of the state model, then Taylor expansion is carried out on the nonlinear part at the guidable points, the state model is linearized by reserving a first-order term after Taylor expansion and eliminating a high-order term, and a measurement model is not changed, so that the system model meets the condition that the state is linear and the measurement is nonlinear, and meets the use condition of characteristic function filtering to carry out state estimation by using the characteristic function filtering. The linearization method of the nonlinear state model expands the application range of the characteristic function filtering in the nonlinear system, can improve the estimation precision to a great extent, and can save the measurement and maintenance cost in practical application, particularly in the fields of target tracking, communication navigation and the like.

Description

State estimation method of nonlinear state model system based on characteristic function

Technical Field

The invention belongs to the field of space target tracking of a nonlinear dynamic system, and particularly relates to the field of space target tracking of which a state model is nonlinear and a measurement model is also a nonlinear system, which can be used for optimizing the real-time position and speed of a target in the space target tracking process.

Background

The filtering method is an important method in state estimation, and the state estimation has very wide application in the fields of fault diagnosis, target tracking, signal processing, computer vision, communication, navigation and the like. The traditional Kalman filtering is only suitable for a system in which a state model and a measurement model are linear and noise is white Gaussian noise. When the noise of the system is no longer white gaussian noise or the system is no longer a linear system, the conventional kalman filtering method is no longer applicable.

In practical application systems, most system models are nonlinear or non-gaussian, and therefore, for a nonlinear system or a system with non-gaussian noise, various filters are extended on the basis of a kalman filter in order to realize state estimation of the nonlinear system or the system. For example, an Extended Kalman Filter (EKF) is used, but the EKF can only achieve a second-order approximation at most, and discarded information of high-order terms brings certain errors to a filtering result; unscented Kalman Filter (UKF) and volume kalman filter (CKF) are all approximate through getting the point, to nonlinear gauss system, although EKF, UKF and CKF's application is all comparatively extensive, its nonlinear approximation all can cause certain error to can't be with these three kinds of wave filters application to nonlinear gauss system, the limitation is great. Particle Filters (PF) developed later, for non-linear non-gaussian systems, although they are better solved in theory, their implementation depends on a large number of particle samples, so that the computational complexity is very high, and the degradation phenomenon of particles during resampling can reduce the speed and accuracy of filtering, affecting practical applications.

For a large number of existing nonlinear systems, the existing filtering method can only improve the estimation accuracy as much as possible, but cannot completely eliminate errors, which is still a difficult problem to be solved. The recently developed Characteristic Function Filter (CFF) only aims at a system with a linear state model, has no requirement on a measurement model of the system, has no requirement on Gaussian or non-Gaussian noise, and replaces a probability density function with the characteristic function, so that the problem of model change caused by the EKF or UKF of the model is avoided, and the CFF is expected to solve the problem of state estimation accuracy of the nonlinear system.

Although eigenfunction filtering is theoretically superior to any other nonlinear filtering method, the eigenfunction is only proposed for linear state models. For the nonlinear state model, in order to be used in combination with the feature function filtering, the nonlinear state model needs to be linearized by a certain method and then the feature function filtering is performed.

The good performance of the characteristic function filtering can only improve the estimation precision to the greatest extent in the practical use of the target tracking field, but the ideal situation of no error at all can not be achieved. The main causes of estimation errors come from the following four components, first, inaccurate data collection. For a target moving in space, the position and the speed of the target are changed in real time, and information is transmitted through wireless, so that the situation of time delay is inevitable, and the collected data is inaccurate. Second, random noise settings are inaccurate. When modeling a system, random setting of noise is too ideal, and in an actual space dynamic system, a target is greatly influenced by an external environment and some random factors, so that a large deviation exists between an actual error and a random error set by the system. Third, the complexity of the system model is high. For a target moving in space, the state comprises six state variables of position and speed in three directions, and compared with a common model, the complexity is too high. In the actual filtering process, each state generates a certain error when being updated, and the error may be larger by comprehensively considering the six state models. Fourth, the performance of the measurement device itself changes. Due to aging of the components inside the measuring device or the change of parameters caused by the service life or the damage caused by the moisture, the inaccuracy of the measuring result can be caused.

Disclosure of Invention

In order to overcome the above-mentioned drawbacks of the prior art, it is an object of the present invention to provide a method that enables applying a characteristic function filter to a nonlinear state model.

The invention carries out Taylor expansion on the nonlinear part of the state model, retains the first order term and omits the high order term to carry out linearization, so that the system model meets the condition that the state is linear and the measurement is nonlinear, thereby meeting the use condition of characteristic function filtering and carrying out state estimation by using the characteristic function filtering. Because the high-order terms of the Taylor expansion are continuously guided at a certain point, the value of the high-order terms tends to be infinitesimal after the partial derivatives are obtained for multiple times, and the linearization is carried out by a method of eliminating the high-order terms. The linearization method of the nonlinear state model expands the application range of the characteristic function filtering in the nonlinear system, can improve the estimation precision to a great extent, and can save the measurement and maintenance cost in practical application.

In order to achieve the purpose, the invention adopts the technical scheme that:

the invention comprises the following steps:

(1) designing a state space target tracking model, wherein the model of the target doing irregular motion in the space is as follows:

wherein x (k) is a system state vector, and y (k) is a measurement vector; w (k) and v (k +1) are the process noise and the measurement noise of the system, respectively; f (-) and h (-) are all nonlinear functions.

(2) Linearize the nonlinear part of the equation of state:

the specific implementation process of the step is as follows:

(2a) selecting Taylor expansion point x_c(k)；

(2b) Combining (2a) and the definition of a Taylor expansion formula, carrying out Taylor expansion on the nonlinear part in the state equation (1), and calculating f (x (k)) after expansion;

(2c) calculating f (x (k)) after linearization according to (2 b);

(2d) and (4) obtaining a linearized nonlinear state model according to the step (2 c).

(3) Under the CFF framework, the optimal estimation value of the state at the k +1 moment is calculated

The specific implementation process of the step is as follows:

(3a) calculating initial state estimation error

(3b) According to the target tracking model, combining (3a) to calculate the predicted value of the target state at the moment from k to k +1

(3c) According to (3b), calculating the measured value of the target from k to k +1

(3d) Calculating residual information from measurement equations (2) and (3c)

(3e) Establishing a filter state model at the moment of k + 1;

(3f) calculating a state error equation e (k);

(3g) calculating a k +1 moment error recurrence equation e (k +1) according to the formulas (1), (3e) and (3 f);

(3h) simultaneously solving characteristic functions on two sides of the equation (3 g);

(3i) establishing a known target feature function

(3j) Establishing a weight function matrix of the filter;

(3k) establishing a filter parameter index J according to (3h), (3i) and (3J)₀(k+1)；

(3l) establishing a filter performance index function J (k +1) according to the (3 k);

(3m) simplify (3 k);

(3n) obtaining a simplified filter performance index J' (k +1) according to (3k), (3l) and (3 m);

(3o) establishing a filter gain matrix K (K +1) to be estimated;

(3p) solving a first order partial derivative of K (K +1) according to (3 n);

(3q) solving a second-order partial derivative of K (K +1) according to (3 n);

(3r) obtaining a gain matrix K (K +1) from (3n), (3p) and (3 q);

(3s) calculating an estimated value of the state at the time k +1 from (3b), (3d), and (3r)

The estimated value obtained at this time

It is the best estimate of the state at time k + 1.

So far, the nonlinear state model linearization filtering method design based on the characteristic function is completely finished.

Compared with the prior art, the invention has the following advantages:

(1) the invention creates a method for applying characteristic function filtering to a nonlinear state model, and linearizes the state model by a method of preserving low-order terms through Taylor expansion, so that a theoretically optimal nonlinear state estimation method can be applied to the nonlinear state model, and estimation errors can be greatly reduced.

(2) The state estimation method is based on the characteristic function filtering, the probability density function is replaced by the characteristic function, the problem of algorithm complexity is avoided, meanwhile, the characteristic function is directly used, and modeling errors caused by the fact that measurement models are changed like EKF and UKF are avoided.

(3) The method can greatly improve the tracking precision of the space moving target.

Drawings

FIG. 1 is a graph of comparative estimation errors for multiple filtering methods for a target location in the x-axis direction;

FIG. 2 is a graph of comparative estimation errors for various filtering methods for a target location in the y-axis direction;

FIG. 3 is a graph of comparative estimation errors for various filtering methods for a target location in the z-axis direction;

FIG. 4 is a graph of comparative estimation errors for multiple filtering methods for target velocity in the x-axis direction;

FIG. 5 is a graph of comparative estimation errors for various filtering methods for a target velocity in the y-axis direction;

FIG. 6 is a graph of comparative estimation errors for various filtering methods for a target velocity in the z-axis direction.

Detailed Description

Embodiments of the present invention are described in detail below with reference to the accompanying fig. 1-6 and examples.

The method comprises the steps of firstly applying a space target motion tracking model to a characteristic function filtering method, obtaining continuous guidable points through a nonlinear state function, then carrying out Taylor expansion on the continuous guidable points, linearizing the nonlinear state model in a mode of reserving a first-order term and eliminating a high-order term, then obtaining a model condition meeting the characteristic function filtering, and updating the position and the speed of a target in real time through continuously updating the radial distance and the direction angle between the target and a measuring center.

The invention relates to a nonlinear state model state estimation method based on a characteristic function, which is applied to a space target tracking system and comprises the following steps:

step 1, a system model is set, and a model of an irregular motion of a target in a space is as follows:

the system state equation:

x(k+1)＝f(x(k))+w(k) (1)

the measurement equation:

y(k+1)＝h(x(k+1))+v(k+1) (2)

wherein x (k) is a system state vector, and y (k) is a measurement vector; w (k) and v (k +1) are the process noise and the measurement noise of the system, respectively; f, (g), h (g) are all non-linear functions.

Step 2 linearizes the nonlinear part of the state equation

The specific implementation process of the step is as follows:

(2a) selecting Taylor expansion point x_c(k)

X (k) is the derivable point of the nonlinear function f (x (k)) such that x (k) equals 0 in equation (3), where x is noted_c(k)＝x(k)。

(2b) And (2) combining the definition of the formula (2a) and Taylor expansion, performing Taylor expansion on the nonlinear part in the equation of state (1), and calculating f (x (k)) after expansion

In the above formula, R_n(x) Is the remainder of the taylor expansion, an infinitesimal quantity.

(2c) From (2b), f (x (k)) after linearization was calculated

f(x(k))＝f(x_c(k))+f'(x_c(k))(x(k)-x_c(k)) (5)

In the above equation, linearization is performed by keeping the first order terms of the taylor expansion, while rounding the higher order terms.

(2d) Obtaining the linearized nonlinear state model according to (2c)

x(k+1)＝f(x_c(k))+f'(x_c(k))(x(k)-x_c(k))+w(k)

Step 3, under the CFF framework, calculating the optimal estimation value of the k +1 time state

The specific implementation process of the step is as follows:

(3a) calculating initial state estimation error

In the above formula

Is given at the initial moment.

In the above formula, the first and second carbon atoms are,

is the state estimate of the target at time k.

(3d) Calculating residual information from measurement equations (2) and (3c)

(3e) Establishing a filter state model at the moment of k +1

(3f) Calculating the State error equation e (k)

(3g) Calculating a k +1 time error recurrence equation e (k +1) according to the formulas (1), (3e) and (3f)

(3h) Simultaneous determination of characteristic functions for both sides of the (3g) equation

(3i) Establishing a known target feature function

(3j) Establishing a weight function matrix for a filter

In the above formula, i is an imaginary unit, m_iAnd M_iAre parameters that can be given according to the actual application environment.

(3k) Establishing a filter parameter index J according to (3h), (3i) and (3J)₀(k+1)

(3l) establishing a filter performance index function J (k +1) based on (3k)

J(k+1)＝J₀(k+1)+(K(k+1))^TR(k+1)K(k+1) (17)

(3m) simplify (3k), let

(3n) obtaining a simplified filter performance index J' (k +1) from (3k), (3l) and (3m)

(3o) establishing a filter gain matrix K (K +1) to be estimated

(3p) solving the first order partial derivative of K (K +1) according to (3n)

(3q) solving the second order partial derivative of K (K +1) according to (3n)

(3r) obtaining a gain matrix K (K +1) from (3n), (3p) and (3q)

The estimated value obtained at this time is the optimal estimated value of the state at the time k + 1.

So far, the nonlinear state model linearization filtering method design based on the characteristic function is completely finished. The effect of the invention can be further illustrated by the following simulation results and field tests:

the target tracking model of the space moving target is as follows:

the system state equation is:

the observation equation is:

x₁、x₂、x₃、x₄、x₅、x₆representing position and velocity in the x, y, z axes, respectively, y₁、y₂、y₃Respectively representing the radial distance of the target from the fusion center and the two orientation angles. Characteristic functions of process noise and measurement noise are

And I is a unit array.

Weighting function of filter

Wherein mu is [0.0001,0.0002, 0.0003 ]]^T，M₁＝0.0001I，M₂＝0.0002I，M₃0.0003I; the weight matrix r (k) diag ([1 ] is a function of10^-5,2×10^-5,3×10^-5,3×10^-5,2×10^-5,1×10^-5]) The initial condition is that x (0) ═ 1,1,1,1,1,1]^T。

Analysis of Experimental results

FIGS. 1 to 6 show the estimated error maps of the target position and velocity in the x, y and z directions, respectively, under various nonlinear filtering methods;

the data were recorded as follows:

status value	CFF-Talyor	UKF	EKF
				x1	0.0528	0.0547	0.0566
x2	0.0597	0.0621	0.0628
				x3	0.0689	0.0693	0.0724
x4	0.0761	0.0771	0.0798
				x5	0.0783	0.0788	0.0817
x6	0.0821	0.0843	0.0859

Through experimental data and simulation results, it can be seen that, for a nonlinear state model, the estimation accuracy obtained by linearizing the state model through taylor expansion and then filtering using a characteristic function is higher than that of the most commonly used Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) in a nonlinear system. The characteristic function filtering is to directly solve the characteristic function of the model so as to carry out filtering, the system model is not changed like other nonlinear filtering methods, and meanwhile, the characteristic function depends on probability theory, the calculation complexity is low, and very accurate filtering results can be obtained. Thus, the above experiments demonstrate the effectiveness of the method of the present invention.

Claims

1. A state estimation method of a nonlinear state model system based on feature function filtering is applied to a space target tracking system and comprises the following steps:

the state equation is as follows:

x(k+1)＝f(x(k))+w(k)

the measurement equation:

y(k+1)＝h(x(k+1))+v(k+1)

wherein x (k) is a system state vector, and y (k) is a measurement vector; w (k) and v (k +1) are the process noise and the measurement noise of the system, respectively; f (·), h (g) are all non-linear functions;

(2) linearizing the nonlinear part of the state equation, specifically:

(2a) selecting Taylor expansion point x_c(k)；

(2b) Combining (2a) and the definition of a Taylor expansion formula, carrying out Taylor expansion on a nonlinear part in the state equation, and calculating f (x (k)) after expansion;

(2c) calculating f (x (k)) after linearization according to (2 b);

(2d) obtaining a linearized nonlinear state model according to the step (2 c);

The method comprises the following steps:

(3a) calculating initial state estimation error

(3d) Calculating residual information from the measurement equations and (3c)

(3e) Establishing a filter state model at the moment of k + 1;

(3f) calculating a state error equation e (k);

(3g) calculating an error recurrence equation e (k +1) at the moment k +1 according to the system state method, (3e) and (3 f);

(3i) establishing a known target feature function

(3j) Establishing a weight function matrix of the filter;

(3m) simplified Filter parameter index J₀(k+1)；

(3o) establishing a filter gain matrix K (K +1) to be estimated;

(3p) solving a first order partial derivative of K (K +1) according to (3 n);

(3q) solving a second-order partial derivative of K (K +1) according to (3 n);

(3r) obtaining a gain matrix K (K +1) from (3n), (3p) and (3 q);

The estimated value obtained at this time

It is the best estimate of the state at time k + 1.

2. The method of estimating states of a nonlinear state model system based on eigen-function filtering as defined in claim 1, wherein: the linearized f (x (k)) in step (2c) is obtained by linearizing by rounding the higher order terms while retaining the first order terms of the taylor expansion.