State estimation method for observation data fusion of multi-sensor nonlinear system

Abstract

The invention discloses a multi-sensor nonlinear system observation data fusion state estimation method, and relates to the technical field of multi-sensor nonlinear system information fusion state estimation. The method comprises the following steps: firstly, acquiring original observation data of an observation target acquired by each sensor in a multi-sensor nonlinear system at the current moment, then carrying out second-order Taylor expansion approximation on the state of the observation target according to a nonlinear observation equation of each sensor, and determining an approximate observation equation of each sensor; according to the sequence of obtaining the original observation data of the sensors, based on the approximate observation equations of the sensors, the original observation data of the sensors are compressed in pairs in sequence through a weighted least square method, compressed approximate observation equations are obtained, filtering is conducted through a second-order extended Kalman filtering algorithm, and the original observation data of the sensors are obtained. And state estimation is carried out on the observation target. According to the method, the data waiting time is saved, the dimensionality of the observation equation is reduced, and the state estimation efficiency of the multi-sensor nonlinear system is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-sensor nonlinear system information fusion state estimation, and in particular to a multi-sensor nonlinear system observation data fusion state estimation method. Background Art

[0002] At present, filtering algorithms play an important role in high-tech fields such as target tracking, positioning and robotics. In practical applications, for the state estimation of multi-sensor nonlinear systems, the observation data of multiple sensors usually need to arrive at the fusion center at the same time for centralized fusion processing, and then the state estimation is performed through filtering algorithms. Centralized fusion processing leads to a lot of waiting time. In addition, the dimension of the observation equation after the observation data of multiple sensors is augmented is usually large, which increases the computational burden. This often becomes a key problem that needs to be solved in the fields of autonomous driving, indoor positioning, integrated navigation, etc.

[0003] In order to solve the problem of high computational burden caused by high-dimensional observations of nonlinear systems and maintain global optimality of estimation, there is currently a batch weighted observation fusion state estimation method. However, this method still requires all observation data to reach the fusion center before fusion and subsequent filtering state estimation, which wastes a lot of waiting time and leads to low state estimation efficiency of multi-sensor nonlinear systems. Summary of the invention

[0004] Based on this, it is necessary to provide a state estimation method for multi-sensor nonlinear system observation data fusion to address the above technical problems.

[0005] The present invention adopts the following technical solutions:

[0006] The present invention provides a state estimation method for multi-sensor nonlinear system observation data fusion, comprising:

[0007] Obtain the original observation data collected by each sensor in the multi-sensor nonlinear system on the observation target at the current moment;

[0008] According to the nonlinear observation equation of each sensor, the state of the observed target is approximated by a second-order Taylor expansion to linearize the nonlinear observation equation of each sensor and determine the approximate observation equation of each sensor;

[0009] According to the order of obtaining the original observation data of each sensor, the original observation data of each sensor are compressed in pairs by weighted least square method based on the approximate observation equation of each sensor to obtain the compressed approximate observation equation;

[0010] According to the state equation of the observed target, the predicted state of the observed target at the next moment is determined; and according to the compressed approximate observation equation, the predicted state is corrected through the second-order extended Kalman filter algorithm to estimate the state of the observed target at the next moment.

[0011] Optionally, performing a second-order Taylor expansion approximation on the state of the observed target according to the nonlinear observation equation of each sensor to linearize the nonlinear observation equation of each sensor and determine the approximate observation equation of each sensor specifically includes:

[0012] The state of the observed target is approximated by a second-order Taylor expansion according to the nonlinear observation equation of each sensor through the following formula to determine the approximate observation equation of each sensor:

[0013]

[0014] Among them, z j (k)∈ is the original observation data of the jth sensor at time k, , is the state forecast estimate at time k, h j () is the nonlinear observation function of the jth sensor, H j (k) = is the measurement matrix of the jth sensor, U j (k) is the second-order expansion term, P(k|k-1) is the state prediction error variance matrix of the multi-sensor nonlinear system, x(k) is the state vector of the multi-sensor nonlinear system at time k, v j (k) is the observation noise of the jth sensor, tr[] is the trace of the matrix, h j The i-th component of (x(k),k) is , the coefficient matrix of the second-order expansion term, h j The i-th component of (x(k),k), is an m j ×1 column vector, whose i-th element is 1 and the rest are 0, m j is the dimension of the jth sensor observation data.

[0015] Optionally, the compressing the observation equations of the sensors in sequence by weighted least squares method to obtain compressed observation equations specifically includes:

[0016] Let the virtual observation data be Get the virtual observation equation y j (k) = H j (k)x(k)+v j (k);

[0017] For each compression, the virtual observation data of the last compression and the virtual observation data of the sensor acquired most recently are expanded to obtain an augmented virtual observation equation;

[0018] Perform full rank decomposition on the augmented observation matrix in the augmented virtual observation equation to obtain the corresponding column full rank matrix and row full rank matrix;

[0019] The weighted least squares method is applied to perform weighted observation fusion compression and dimension reduction on the virtual observation data expanded before the j-th compression by the following formula:

[0020]

[0021] Through multiple rounds of weighted observation fusion compression and dimensionality reduction, the final compressed virtual observation equation is obtained, and the inverse transformation is performed to obtain the compressed approximate observation equation;

[0022] In the formula, is the virtual observation data after the jth compression, is the original observation data after the jth compression, is the nonlinear observation function of the sensor after the jth compression, The second-order expansion term after the j-th compression is: = is the observed noise after the jth compression, is the observation noise variance matrix after the jth compression, is the observation matrix after the jth compression, is the original observation data augmented by dimensional expansion before the jth compression, The column full rank matrix of the augmented observation matrix obtained by expanding the dimension before the jth compression, R j+1 (k) is the observation noise variance matrix of the j+1th sensor, is the nonlinear observation function of the sensor augmented by the dimension expansion before the jth compression, is the second-order expansion term obtained by expanding the dimension before the j-th compression, is the augmented observation noise obtained by dimensional expansion before the jth compression, It is the augmented virtual observation data obtained by dimensional expansion before the j-th compression. The superscript “T” indicates that the matrix is ​​transposed.

[0023] Optionally, the predicted state is corrected by a second-order extended Kalman filter algorithm according to the compressed approximate observation equation to estimate the state of the observed target at the next moment, specifically including:

[0024] According to the compressed approximate observation equation finally obtained by compression, the multi-sensor nonlinear system is reconstructed by the following formula:

[0025]

[0026] The Kalman filter one-step predictor performs state prediction based on the following formula to obtain the predicted state:

[0027] P(k+1|k)=F(k)P(k|k)F T (k)+Q(k);

[0028] The predicted state is corrected by the compressed original observation data through the Kalman filter to estimate the state of the observed target:

[0029]

[0030] Where N is the number of sensors, f i (x(k),k) is the i-th component of f(x(k),k), is an n×1 column vector, where the i-th element is 1 and the rest are all 0. is the state filter estimate, P(k|k) is the filter error The variance matrix of is the state estimation result of the observed target, is the predicted state of the observed target, K(k+1) is the Kalman filter gain matrix for correcting the predicted state, and P(k+1|k+1)= is the error covariance matrix of the Kalman filter.

[0031] The present invention provides a state estimation device for multi-sensor nonlinear system observation data fusion, comprising:

[0032] An acquisition module is used to acquire the original observation data collected by each sensor in the multi-sensor nonlinear system on the observation target at the current moment;

[0033] An approximate observation equation determination module is used to perform a second-order Taylor expansion approximation on the state of the observed target according to the nonlinear observation equation of each sensor, so as to linearize the nonlinear observation equation of each sensor and determine the approximate observation equation of each sensor;

[0034] A compression module is used to compress the original observation data of each sensor in pairs in turn by weighted least square method based on the approximate observation equation of each sensor according to the order of obtaining the original observation data of each sensor, so as to obtain the compressed approximate observation equation;

[0035] The state estimation module is used to determine the predicted state of the observed target at the next moment based on the state equation of the observed target; and to correct the predicted state through the second-order extended Kalman filter algorithm based on the compressed approximate observation equation to estimate the state of the observed target at the next moment.

[0036] The present invention provides a computer-readable storage medium, wherein the storage medium stores a computer program, and when the computer program is executed by a processor, the state estimation method for fusion of observation data of a multi-sensor nonlinear system is implemented.

[0037] The present invention provides a computer device, comprising a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor implements the state estimation method of multi-sensor nonlinear system observation data fusion when executing the program.

[0038] At least one of the above technical solutions adopted by the present invention can achieve the following beneficial effects:

[0039] The present invention first obtains the original observation data collected by each sensor of the multi-sensor nonlinear system on the observation target at the current moment, and then performs a second-order Taylor expansion approximation on the state of the observation target according to the nonlinear observation equation of each sensor, so as to linearize the nonlinear observation equation of each sensor and determine the approximate observation equation of each sensor. Then, according to the order of obtaining the original observation data of each sensor, the original observation data of each sensor are compressed in pairs in turn based on the approximate observation equation of each sensor by weighted least square method to obtain the compressed approximate observation equation. Finally, according to the state equation of the observation target, the predicted state of the observation target at the next moment is determined, and the predicted state is corrected by the second-order extended Kalman filter algorithm to estimate the state of the observation target at the next moment.

[0040] The present invention fuses the original observation data of each sensor in the multi-sensor nonlinear system through the sequential weighted least square method, so that in the transmission process of the original observation data of each sensor, sequential weighted pairwise fusion is performed according to the order of the original observation data that has been transmitted to the fusion center in real time, so there is no need to wait for all the original observation data to arrive before fusion, which saves waiting time and improves fusion efficiency. At the same time, the dimension of the observation equation is effectively reduced, the amount of calculation is significantly reduced, and the calculation complexity of the second-order extended Kalman filter of the subsequent nonlinear system is further reduced, thereby improving the state estimation efficiency of the multi-sensor nonlinear system. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] The drawings described herein are used to provide a further understanding of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0042] Figure 1 A schematic flow chart of a state estimation method for multi-sensor nonlinear system observation data fusion provided by the present invention;

[0043] Figure 2A multi-sensor sequential fusion estimation structure diagram provided by the present invention;

[0044] Figure 3 A multi-sensor sequential fusion estimation flow chart provided by the present invention;

[0045] Figure 4 A schematic diagram of plane tracking provided by the present invention;

[0046] Figure 5 A schematic diagram of mean square error of state estimation provided by the present invention;

[0047] Figure 6 A schematic diagram of a state estimation device for multi-sensor nonlinear system observation data fusion provided by the present invention. DETAILED DESCRIPTION

[0048] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the specific embodiments of the present invention and the corresponding drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0049] At present, in order to solve the problem of large computational burden caused by nonlinear high-dimensional observations and maintain global optimality of estimation, there is a batch weighted observation fusion state estimation method. However, this method still requires all observation data to arrive at the fusion center before it can be executed, which wastes a lot of waiting time. Moreover, the accuracy of the extended Kalman filter for nonlinear systems is low.

[0050] Sequential fusion estimation is an important method for multi-sensor information fusion state estimation. Since sequential fusion estimation is based on the order in which sensor data arrive at the fusion center, it has good real-time performance and reduces the computational burden compared to centralized fusion estimation.

[0051] The technical solutions provided by various embodiments of the present invention are described in detail below in conjunction with the accompanying drawings.

[0052] Figure 1 This is a flow chart of a state estimation method for multi-sensor nonlinear system observation data fusion in the present invention, which specifically includes the following steps:

[0053] S101: Acquire original observation data collected by each sensor in the multi-sensor nonlinear system on the observation target at the current moment.

[0054] In practical applications, a multi-sensor nonlinear system can first sense the observed target through each sensor to achieve purposes such as target tracking, and then each sensor can upload the collected original perception data to the server of the fusion center. Therefore, the server can usually obtain the original observation data of the observed target uploaded by each sensor according to the preset collection interval.

[0055] In general, a multi-sensor nonlinear system can be expressed as follows:

[0056] The state equation of a multi-sensor nonlinear system: x(k+1)=f(x(k),k)+ω(k).

[0057] Nonlinear observation equation for each sensor: j (k) = h j (x(k),k)+v j (k),j=1,…,N.

[0058] in, is the state vector of the multi-sensor nonlinear system at time k, is the nonlinear state function, is the process noise with variance Q(k), is the original observation data of the jth sensor, h j (x(k), k) is the nonlinear observation function of the jth sensor, v j (k) is the observation noise of the jth sensor and its variance is R j (k).

[0059] The server mentioned in the present invention may be a server located in a fusion center, or a device such as a desktop computer, a notebook computer, etc. that can execute the solution of the present invention. For the convenience of description, the following description is only based on the server as the execution subject.

[0060] S102: performing a second-order Taylor expansion approximation on the state of the observed target according to the nonlinear observation equation of each sensor, so as to linearize the nonlinear observation equation of each sensor and determine an approximate observation equation of each sensor.

[0061] Furthermore, in one or more embodiments of the present invention, the server may also perform a second-order Taylor expansion approximation on the nonlinear observation equation of each sensor by the following formula to determine the approximate observation equation of each sensor:

[0062] in, , is the state forecast estimate, H j (k) = is the measurement matrix of the jth sensor, U j (k) is the second-order expansion term, h j The i-th component of (x(k),k) is , is the coefficient matrix of the second-order expansion term of the point, tr[] is the trace of the matrix, P(k|k-1) is the state prediction error variance matrix of the multi-sensor nonlinear system, h j The i-th component of (x(k),k), is an m j ×1 column vector, where the i-th element is 1 and the rest are 0, m j is the dimension of the jth sensor observation data.

[0063] S103: According to the order of obtaining the original observation data of each sensor, the original observation data of each sensor are compressed in pairs in turn by weighted least square method based on the approximate observation equation of each sensor to obtain the compressed approximate observation equation.

[0064] S104: Determine the predicted state of the observed target at the next moment according to the state equation of the observed target; and correct the predicted state through the second-order extended Kalman filter algorithm according to the compressed approximate observation equation to estimate the state of the observed target at the next moment.

[0065] Specifically, the server uses the weighted least squares method to obtain the compressed approximate observation equation. The specific process is as follows:

[0066] First, perform weighted least squares compression on the original observation data. According to the approximate observation equation of each sensor, the weighted least squares principle is used to compress the approximate observation equation of each sensor in pairs in turn to obtain the compressed approximate observation equation. The compression process is as follows:

[0067] make Then we can get the new virtual observation equation: j (k) = H j (k)x(k)+v j (k). Where y j (k) = virtual observation data obtained by transformation based on the original observation data.

[0068] Based on the virtual observation equation obtained above, sequential weighted virtual observation data fusion is performed. The specific process is as follows:

[0069] Before the jth (1≤j≤N-1) weighted observation compression, the virtual observation data of the last compression is The virtual observation data y of the sensor recently acquired j+1 (k) is expanded, and the augmented virtual observation equation is obtained as follows:

[0070]

[0071] The superscript (j) indicates the jth compression. is the virtual observation data expanded before the jth compression, is the virtual observation data after the j-1th compression, is the original observation data after the j-1th compression, is the nonlinear observation function after the j-1th compression, is the second-order expansion term after the j-1th compression, is the observed noise after the j-1th compression, is the observation matrix after the j-1th compression.

[0072] For the convenience of description, the matrix can be abbreviated, so we can write And define make In the formula, r (j) (k) is a matrix The rank of is, n is the dimension of the state vector x(k). And we can get: In the formula, is the observation noise variance matrix after the j-1th compression, R j+1 (k) is the observation noise variance matrix of the j+1th sensor.

[0074] Then the augmented observation matrix can be Perform full rank decomposition, let is a column full rank matrix, is a row full rank matrix, that is, the observation matrix after the jth compression.

[0075] Apply weighted least squares method to the virtual observation data expanded before the jth compression Perform weighted observation fusion compression and dimensionality reduction:

[0076] in,

[0077] When j=N-1, the compression is completed. At this time, the final compressed virtual observation equation is:

[0078] By performing transformation based on the compressed virtual observation equation, the compressed approximate observation equation can be obtained.

[0079] The server can then reconstruct a new multi-sensor nonlinear system:

[0080]

[0081] in, f i (x(k),k) is the i-th component of f(x(k),k), is an n×1 column vector, where the i-th element is 1 and the rest are all 0. is the state filter estimate, P(k|k) is the filter error The variance matrix of .

[0082] Figure 2 is a multi-sensor sequential fusion estimation structure diagram in the present invention, Figure 3 This is a multi-sensor sequential fusion estimation flow chart in the present invention. Figure 2 and Figure 3 It can be seen that each sensor collects its own observation data and compresses them using the weighted least squares method. In the fusion center, the fusion estimation result is given based on the second-order extended Kalman filter.

[0083] After fusing the observation equations of each sensor, the server can use the Kalman one-step predictor to perform state prediction based on the following formula: P(k+1|k)=F(k)P(k|k)F T (k)+Q(k).

[0084] In the formula, the subscript "T" indicates that the matrix is ​​transposed.

[0085] Then, the Kalman filter is used to filter the predicted state and the compressed original observation data is used to correct the predicted state to complete the state estimation of the observed target:

[0086] In the formula, is the state estimation result of the observed target, = is the predicted state of the observed target, K(k+1) is the Kalman filter gain matrix for correcting the predicted state, and P(k+1|k+1) is the error covariance matrix of the Kalman filter.

[0087] based on Figure 1The state estimation method for multi-sensor nonlinear system observation data fusion shown in the figure first obtains the original observation data collected by each sensor in the multi-sensor nonlinear system on the observation target at the current moment, then performs a second-order Taylor expansion approximation on the state of the observation target according to the nonlinear observation equation of each sensor to linearize the nonlinear observation equation of each sensor and determine the approximate observation equation of each sensor, then according to the order of obtaining the original observation data of each sensor, the original observation data of each sensor are compressed in pairs in turn based on the approximate observation equation of each sensor by weighted least squares method to obtain the compressed approximate observation equation, finally, according to the state equation of the observation target, the predicted state of the observation target at the next moment is determined, and the predicted state is corrected by the second-order extended Kalman filter algorithm to estimate the state of the observation target at the next moment.

[0088] The present invention fuses the original observation data of each sensor in a multi-sensor nonlinear system by sequential weighted least squares method, so that in the transmission process of the original observation data of each sensor, sequential weighted pairwise fusion is performed according to the order of the original observation data that has been transmitted to the fusion center in real time, so that there is no need to wait for all the original observation data to arrive before fusion, saving waiting time and improving fusion efficiency. At the same time, the dimension of the observation equation is effectively reduced, the amount of calculation is significantly reduced, and the computational complexity of the second-order extended Kalman filter of the subsequent nonlinear system is further reduced, thereby improving the state estimation efficiency of the multi-sensor nonlinear system. The method has simple steps, is easy to operate, and has good real-time performance. Compared with the traditional centralized second-order extended Kalman filter method, the method is the same in estimation accuracy, that is, it has global optimality.

[0089] The present invention also provides an application embodiment of the state estimation method of the multi-sensor nonlinear system observation data fusion. First, a target tracking system with three sensors in a two-dimensional XY plane is simulated to obtain the original observation signal z j (k),j=1,2,3. System noise ω(k)=Γ(k)w(k), where the system parameters are as follows:

[0090]

[0091] In the formula, is the state vector. x(k) is the x-axis position of the target at time k, y(k) is the y-axis position at time k, α(k) is the speed at time k, and θ(k) is the angle at time k. is the angular velocity at time k, T is the sampling period, x j ,y j is the horizontal and vertical coordinates of the location of the jth sensor.

[0092] Assume that on the plane, sensor 1 is located at z1(x1,y1)=z1(1m,0), sensor 2 is located at z2(x2,y2)=z2(5.8m,0), and sensor 3 is located at z3(x3,y3)=z3(1m,4.8m). The observation matrix of the observation equation obtained by Taylor series expansion is:

[0093] In the simulation, the sampling period T = 0.11s, the initial state X(0) = [0.43, 0.14, 0.34, 0.14] T , the variance Q of the process noise w(k) w =diag(0.001,0.001). Observation noise v (j) The variance of (k) is assigned piecewise as shown in Table 1. The number of simulation steps is set to 200.

[0094] Table 1 Observation noise variance

[0095] scope <![CDATA[R1]]> <![CDATA[R2]]> <![CDATA[R3]]> x(k)∈(0.566,1.131] 2.818 2.230 2.465 x(k)∈[1.131,1.700) 1.972 2.733 1.311 x(k)∈[1.700,2.263) 2.673 3.345 2.815 x(k)∈[2.263,3.536) 2.454 3.436 2.149

[0096] According to the order in which the observation data of each sensor arrives at the fusion center, compression fusion is performed in sequence at the fusion center, and then the state estimation is performed using the second-order extended Kalman filter, such as Figure 4 shown. Figure 4 This is a schematic diagram of a plane tracking method in the present invention, wherein from top to bottom and from left to right are: x-axis position tracking diagram, y-axis position tracking diagram, speed tracking diagram, angle tracking diagram and angular velocity tracking diagram. The circle represents the centralized fusion filter value, the solid point represents the fusion filter value of the method of the present invention, and the solid line represents the true value. Figure 4 It is found that the tracking effects of centralized filter fusion and sequential weighted observation fusion overlap.

[0097] The estimation accuracy of the method of the present invention is further analyzed. The observations of the three sensors are centrally fused. The centralized fusion filter and the filter of the method of the present invention are plotted and compared at the same time. Figure 5 As shown, Figure 5 is a schematic diagram of mean square error of state estimation in the present invention, Figure 5 In the figure, from top to bottom and from left to right, they are: x-axis position mean square error (MSE), y-axis position MSE, velocity MSE, angle MSE and angular velocity MSE. The dotted line is the fusion estimation of the method of the present invention, and the circle is the centralized fusion estimation. It can be seen that the method of the present invention has the same estimation accuracy as the centralized fusion algorithm. Therefore, it has global optimality. It is suitable for target navigation, positioning and tracking problems with high reliability and high precision requirements.

[0098] The above is a state estimation method for multi-sensor nonlinear observation data fusion provided by one or more embodiments of the present invention. Based on the same idea, the present invention also provides a corresponding state estimation device for multi-sensor nonlinear observation data fusion, such as Figure 6 shown.

[0099] Figure 6 A schematic diagram of a state estimation device for multi-sensor nonlinear system observation data fusion provided by the present invention includes:

[0100] An acquisition module 201 is used to acquire the original observation data collected by each sensor of the multi-sensor nonlinear system on the observation target at the current moment;

[0101] An approximate observation equation determination module 202 is used to perform a second-order Taylor expansion approximation on the state of the observed target according to the nonlinear observation equation of each sensor, so as to linearize the nonlinear observation equation of each sensor and determine the approximate observation equation of each sensor;

[0102] The compression module 203 is used to compress the original observation data of each sensor in pairs in turn by weighted least square method based on the approximate observation equation of each sensor according to the order of obtaining the original observation data of each sensor, so as to obtain the compressed approximate observation equation;

[0103] The state estimation module 204 is used to determine the predicted state of the observed target at the next moment according to the state equation of the observed target; and to correct the predicted state through the second-order extended Kalman filter algorithm according to the compressed approximate observation equation to estimate the state of the observed target at the next moment.

[0104] The specific definition of the state estimation device for multi-sensor nonlinear system observation data fusion can be found in the above definition of the state estimation method for multi-sensor nonlinear system observation data fusion, which will not be repeated here. Each module in the above-mentioned state estimation device for multi-sensor nonlinear system observation data fusion can be implemented in whole or in part by software, hardware and a combination thereof. The above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory in the computer device in the form of software, so that the processor can call and execute the operations corresponding to the above modules.

[0105] The present invention also provides a computer-readable storage medium, which stores a computer program, which can be used to execute the above Figure 1 A state estimation method for multi-sensor nonlinear system observation data fusion is provided.

[0106] The present invention also provides a computer device. At the hardware level, the computer device includes a processor, an internal bus, a network interface, a memory, and a non-volatile memory. Of course, it may also include hardware required for other services. The processor reads the corresponding computer program from the non-volatile memory into the memory and then runs it to achieve the above Figure 1 A state estimation method for multi-sensor nonlinear system observation data fusion is provided.

[0107] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided by the present invention can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory or optical memory, etc. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0108] The technical features of the above embodiments may be arbitrarily combined. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of the present invention.

Claims

1. A state estimation method for multi-sensor nonlinear system observation data fusion, characterized in that: include: Obtain the original observation data collected by each sensor in the multi-sensor nonlinear system on the observation target at the current moment; According to the nonlinear observation equation of each sensor, the state of the observed target is approximated by a second-order Taylor expansion to linearize the nonlinear observation equation of each sensor and determine the approximate observation equation of each sensor; According to the order of obtaining the original observation data of each sensor, the original observation data of each sensor are compressed in pairs by weighted least square method based on the approximate observation equation of each sensor to obtain the compressed approximate observation equation; According to the state equation of the observed target, the predicted state of the observed target at the next moment is determined; and according to the compressed approximate observation equation, the predicted state is corrected through the second-order extended Kalman filter algorithm to estimate the state of the observed target at the next moment.

2. The state estimation method for multi-sensor nonlinear system observation data fusion according to claim 1, characterized in that: The second-order Taylor expansion approximation is performed on the state of the observed target according to the nonlinear observation equation of each sensor to linearize the nonlinear observation equation of each sensor and determine the approximate observation equation of each sensor, specifically including: The state of the observed target is approximated by a second-order Taylor expansion according to the nonlinear observation equation of each sensor through the following formula to determine the approximate observation equation of each sensor: Among them, z j (k)∈ is the original observation data of the jth sensor at time k, , is the state forecast estimate at time k, h j () is the nonlinear observation function of the jth sensor, H j (k) = is the measurement matrix of the jth sensor, U j (k) is the second-order expansion term, P(k|k-1) is the state prediction error variance matrix of the multi-sensor nonlinear system, x(k) is the state vector of the multi-sensor nonlinear system at time k, v j (k) is the observation noise of the jth sensor, tr[] is the trace of the matrix, h j The i-th component of (x(k),k) is , the coefficient matrix of the second-order expansion term, h j The i-th component of (x(k),k), is an m j ×1 column vector, whose i-th element is 1 and the rest are 0, m j is the dimension of the jth sensor observation data.

3. The state estimation method for multi-sensor nonlinear system observation data fusion according to claim 2, characterized in that: The method of compressing the observation equation of the sensor in sequence by the weighted least square method to obtain the compressed observation equation specifically includes: Let the virtual observation data be Get the virtual observation equation y j (k) = H j (k)x(k)+v j (k); For each compression, the virtual observation data of the last compression and the virtual observation data of the sensor acquired most recently are expanded to obtain an augmented virtual observation equation; Perform full rank decomposition on the augmented observation matrix in the augmented virtual observation equation to obtain the corresponding column full rank matrix and row full rank matrix; The weighted least squares method is applied to perform weighted observation fusion compression and dimension reduction on the virtual observation data expanded before the j-th compression by the following formula: Through multiple rounds of weighted observation fusion compression and dimensionality reduction, the final compressed virtual observation equation is obtained, and the inverse transformation is performed to obtain the compressed approximate observation equation; In the formula, is the virtual observation data after the jth compression, is the original observation data after the jth compression, is the nonlinear observation function of the sensor after the jth compression, The second-order expansion term after the j-th compression is: = is the observed noise after the jth compression, is the observation noise variance matrix after the jth compression, is the observation matrix after the jth compression, is the original observation data augmented by dimensional expansion before the jth compression, is the column full-rank matrix of the augmented observation matrix full-rank decomposition obtained by expanding the dimension before the jth compression, R j+1 (k) is the observation noise variance matrix of the j+1th sensor, is the nonlinear observation function of the sensor augmented by the dimension expansion before the jth compression, is the second-order expansion term obtained by expanding the dimension before the j-th compression, is the augmented observation noise obtained by dimensional expansion before the jth compression, It is the augmented virtual observation data obtained by dimensional expansion before the j-th compression. The superscript "T" indicates that the matrix is ​​transposed.

4. The state estimation method for multi-sensor nonlinear system observation data fusion according to claim 3, characterized in that: The method of correcting the predicted state according to the compressed approximate observation equation by using a second-order extended Kalman filter algorithm to estimate the state of the observed target at the next moment specifically includes: According to the compressed approximate observation equation finally obtained by compression, the multi-sensor nonlinear system is reconstructed by the following formula: The Kalman filter one-step predictor performs state prediction based on the following formula to obtain the predicted state: The predicted state is corrected by the compressed original observation data through the Kalman filter to estimate the state of the observed target: Where N is the number of sensors, f i (x(k),k) is the i-th component of f(x(k),k), is an n×1 column vector, where the i-th element is 1 and the rest are all 0. is the state filter estimate, P(k|k) is the filter error The variance matrix of is the state estimation result of the observed target, is the predicted state of the observed target, K(k+1) is the Kalman filter gain matrix for correcting the predicted state, and P(k+1|k+1)= is the error covariance matrix of the Kalman filter.

5. A state estimation device for multi-sensor nonlinear system observation data fusion, characterized in that: include: An acquisition module is used to acquire the original observation data collected by each sensor in the multi-sensor nonlinear system on the observation target at the current moment; An approximate observation equation determination module is used to perform a second-order Taylor expansion approximation on the state of the observed target according to the nonlinear observation equation of each sensor, so as to linearize the nonlinear observation equation of each sensor and determine the approximate observation equation of each sensor; A compression module is used to compress the original observation data of each sensor in pairs in turn by weighted least square method based on the approximate observation equation of each sensor according to the order of obtaining the original observation data of each sensor, so as to obtain the compressed approximate observation equation; The state estimation module is used to determine the predicted state of the observed target at the next moment based on the state equation of the observed target; and to correct the predicted state through the second-order extended Kalman filter algorithm based on the compressed approximate observation equation to estimate the state of the observed target at the next moment.

6. A computer-readable storage medium, characterized in that: The storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 4 is implemented.

7. A computer device, characterized in that: The method comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the method according to any one of claims 1 to 4 is implemented.

Images