Fast sensor data fusion method based on Sigma point belief propagation

Abstract

The present invention discloses a fast sensor data fusion method based on sigma point belief propagation, comprising the following steps: step (1), predicting the state and covariance of target k at time t; step (2), performing sigma point sampling on the predicted state; step (3), transferring sigma points on the sampled samples in step (2); step (4), calculating iterative data association within all sensors s in parallel; step (5), performing confidence calculation after completing the iterative data association within the sensor; step (6), estimating the state and covariance of each target k at time t+1 based on the distribution of the state of target k at time t+1. The method solves the problems of high computational complexity and poor scalability of traditional multi-sensor information fusion algorithms, and reduces computational complexity based on the implementation of the sigma point belief propagation algorithm.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-sensor information fusion and relates to a fast fusion algorithm of massive sensor data based on sigma point belief propagation. Background Art

[0002] Multi-sensor information fusion technology estimates time-varying states based on measurements from multiple sensors. In the civilian sector, for example, with the accelerated development of "smart cities" in my country, demand for security monitoring products is expected to grow rapidly. With the rapid development of cloud-based big data, the rapid fusion of massive amounts of sensor data holds great promise for future applications. Furthermore, multi-sensor information fusion technology can also play a vital role in smart homes, smart buildings, drones, robotics, and other fields.

[0003] Multi-sensor information fusion algorithms typically operate in embedded systems. However, embedded systems have limited resources, especially in scenarios where massive numbers of sensors are used. Therefore, developing a multi-sensor information fusion algorithm with low computational complexity is of great practical value. However, each additional sensor in the estimation system requires a change in the fusion framework of traditional information fusion algorithms, which requires algorithmic modifications and incurs additional costs. Scalability is also a requirement for practical applications. Multi-sensor information fusion algorithms based on belief propagation have relatively low computational complexity and excellent scalability. However, their message propagation implementation is based on particle filtering. As the number of particles increases, the computational complexity increases, and there are also issues such as particle degradation. Therefore, a fast sensor data fusion method based on sigma point belief propagation is proposed. Summary of the Invention

[0004] The purpose of the present invention is to address the deficiencies of the existing technology and propose a fast sensor data fusion method based on sigma point belief propagation, which reduces the computational difficulty and has excellent scalability, thereby avoiding particle degradation.

[0005] A fast sensor data fusion method based on sigma point belief propagation includes the following steps:

[0006] Step (1), predict the state and covariance of target k at time t;

[0007] Step (2), perform sigma point sampling on the predicted state;

[0008] Step (3), transferring the sigma points of the sampled samples in step (2) to obtain a set of sigma samples of the predicted measurement values ​​of the target k at time t+1;

[0009] Step (4), calculate the iterative data association within the sensor in parallel for all sensors s;

[0010] Step (5): After completing the iterative data association within the sensor, a confidence calculation is performed to obtain the distribution describing the state of the target k at time t+1;

[0011] Step (6): Estimate the state and covariance of each target k at time t+1 based on the distribution of the state of target k at time t+1.

[0012] Preferably, the step (4) includes the following sub-steps:

[0013] Step (4.1) measurement evaluation: use the sigma sampling of the predicted measurement value of target k at time t+1 to perform a preliminary evaluation of the correlation between the measurement data and target k, and obtain a set of weighted particles about the associated variables of target k at time t+1;

[0014] Step (4.2), the associated prior distribution, the weighted particles of the associated variables of target k at time t+1 are associated with the prior distribution to obtain another set of weighted particles;

[0015] Step (4.3), data association, data association is performed through a set of weighted particles obtained after prior distribution to obtain a set of weighted particles about the associated variables;

[0016] Step (4.4), measurement update, performing measurement update on the weighted particles with respect to the associated variables to obtain a set of weighted particles after measurement update;

[0017] Step (4.5), calculate the external information, and cyclically calculate the external information of the weighted particles after measurement update.

[0018] Preferably, the specific method of step (4.1) is:

[0019] In each sensor s, for each target k, measurement evaluation is performed, and the expression is as follows:

[0020]

[0021] The above formula uses the likelihood function and the prior information of the target measurement to evaluate the relevance of each measurement to the target. represents the correlation between target k and measurement m′ at time t+1, where s∈{1,...,S} represents the sensor index, S represents the number of sensors, t represents the time index, k∈{1,...,K} represents the target index, K represents the number of targets, q∈{1,...,Q} represents the iteration loop index outside the sensor, Q represents the maximum number of external iterations, represents the associated variable of target k at time t+1 in sensor s, express The value of represents the number of measurements at time t+1 in sensor s, j∈{0,1,...,2L} represents the index of the sigma point, L represents the state dimension of the target, z k,j (t+1|t) represents the predicted state x of target k at time t+1 k The j-th sigma point x at (t+1|t) k,j (t+1|t) is the sigma point after the observation function, z (s) (t+1) represents the measurement set at time t+1 in sensor s, and the initial value of the iterative loop outside the sensor is ω k,j Indicates the weight value of the sigma point,

[0022] Get a set of weighted particles about the associated variables of target k at time t+1 The weights indicated are used to evaluate the measurements.

[0023] Preferably, the specific method of step (4.2) is:

[0024] In each sensor s, the associated variable for each target k is The prior distribution of is expressed as follows:

[0025]

[0026] Represents an associated variable A prior distribution of , which describes a priori situation of the detection of target k, represents the detection probability of sensor s, μ (s) Represents the mean value of the number of clutter in sensor s, and a set of weighted particles is obtained The weights expressed are a priori assessments of the measurements.

[0027] Preferably, the specific method of step (4.3) is:

[0028] First, define k′∈{0,1,...,K} represents The value of, in each sensor s, for each target k, each measurement m, has

[0029] The formula for measuring message delivery to the target is:

[0030]

[0031] The target-to-measurement message passing formula is:

[0032]

[0033] The initial target-to-measurement message passing formula is:

[0034]

[0035] The above three formulas describe the process of message loop iteration in data association, where: represents the association variable of measurement m at time t+1 at sensor s, m represents the measurement index, p∈{1,...,P} represents the iteration loop index of data association within the sensor, and P represents the maximum number of iterations. Indicates the initial message from the sensor's internal target to the measurement, The message representing the pth iteration of the target measured by the sensor, represents the message from the target to the pth iteration of the measurement,

[0036] After completing the message iteration, the posterior probability of the associated variable is finally calculated. The message passing formula for the posterior probability of the associated variable is as follows:

[0037]

[0038] The weights of the associated variable values ​​describe the posterior probability between the target measurements,

[0039] Get a set of related variables Weighted particles The weights expressed are the posterior estimates of each measurement.

[0040] Preferably, the specific method of step (4.4) is:

[0041] After obtaining the posterior probability of the associated variable, the likelihood function is used to update the weight of each measurement particle. In each sensor s, for each target k, there is

[0042]

[0043] Finally, we get the weighted particles after a set of measurement updates. represents the posterior evaluation of the sigma point at the qth outer loop iteration.

[0044] Preferably, the specific method of step (4.5) is:

[0045] After the weights of the measurement particles in each sensor are updated, these weighted particles need to be treated as messages to interact and fuse with other sensors. Therefore, in each sensor s, for each target k, there is

[0046]

[0047] After reaching this step, you need to re-execute steps (4.1), (4.2), (4.3), (4.4), and (4.5) until q=Q. The initial value for the next sensor outer loop iteration.

[0048] Preferably, the specific method of step (5) is:

[0049] After all iterative message transmissions are completed between sensors, data fusion is performed using the messages from all sensors, that is, the weights of the measurement particles are updated. For each target k,

[0050]

[0051] f(z k,j (t+1|t)) represents the final posterior evaluation of the sigma point used to update the target state and covariance. Since ω k,j There are two values, so for the same particle, it also has a weight, which is used to calculate the mean and covariance respectively, thus obtaining a set of weighted particles:

[0052]

[0053] definition In addition, particle z k,j (t+1|t) is the particle x k,j (t+1|t) is obtained through nonlinear transformation, so z k,j The weight of (t+1|t) is equal to x k,j The weight of (t+1|t) is obtained, so a set of weighted particles is obtained:

[0054]

[0055] So we can describe x k The distribution of (t+1).

[0056] Preferably, the specific method of step (6) is:

[0057] After completing the data fusion, the fused data is used to estimate the state of the problem. The state of each target k at time t+1 is estimated as follows:

[0058]

[0059] The estimate of the state covariance of each target k at t+1 is:

[0060] BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 Flowchart of a method according to an embodiment of the present invention.

[0062] The present invention has the following characteristics and beneficial effects:

[0063] The above technical solution uses a graphical model to describe the data fusion problem. It is highly scalable and requires no algorithmic modifications. It not only consumes minimal land but also significantly reduces computational complexity. The Sigma implementation of belief propagation further reduces computational and spatial complexity, allowing it to process massive amounts of data while meeting real-time requirements. Therefore, while computational complexity increases with the number of particles, it still far outperforms existing methods and effectively addresses issues such as particle degradation. DETAILED DESCRIPTION

[0064] The principle of the method of the present invention is described in detail below with reference to the accompanying drawings.

[0065] The present invention provides a fast sensor data fusion method based on Sigma point belief propagation, such as Figure 1 As shown,

[0066] In this embodiment, it is assumed that there are K targets in total, and K is fixed and known. k (t) represents the state of target k∈{1,...,K} at time t.

[0067] First, consider the target model as:

[0068] x k (t) = Fx k (t-1)+v k (t-1) (1)

[0069] Among them, v k (t-1) is Gaussian white noise with zero mean, that is, v k (t-1)~N(0,Q k (t-1)), F is the state transfer matrix.

[0070] Assume there are S sensors, and the total number of sensors s∈{1,...,S} at time t is The measured value is The measurement is recorded as We define the measurement vector In order to distinguish which measurement the target k generates or which it misses, the following target-oriented association variables are defined at the sensor s:

[0071]

[0072] In order to indicate whether the measurement is from the target or from the clutter, the following measurement-oriented associated variables are defined at the sensor s:

[0073]

[0074] Consider the observation model of the target at sensor s as:

[0075]

[0076] in, represents the measurement of target k at sensor s at time t, μ k (t) is Gaussian white noise with zero mean, that is, μ k (t)~N(0,R k (t)). h(·) is a nonlinear function.

[0077] The initial state x of all targets k (0) and the initial covariance P k (0)Known.

[0078] like Figure 1 As shown, the following steps are included:

[0079] Step (1) predicts the state and covariance of target k at time t.

[0080] Specifically, for each target k, there is

[0081] x k (t+1|t)=Fx k (t|t) (5)

[0082] P k (t+1|t)=FP k (t|t)F T +Q(t) (6)

[0083] Among them, x k (t|t) represents the estimated state of target k at time t, x k(t+1|t) represents the predicted state of target k at time t+1, P k (t|t) represents the estimated covariance of target k at time t, P k (t+1|t) represents the predicted covariance of target k at time t+1, and Q(t) represents the process noise covariance. And, at the initial moment, there is x k (0|0)=x k (0), P k (0|0)=P k (0).

[0084] Step (2): perform sigma point sampling on the predicted state.

[0085] Specifically, for each target k, there are 2L+1 samples x k,j (t+1|t) and the corresponding mean weight and covariance weights use express.

[0086] The sample is calculated as follows:

[0087]

[0088] The weights corresponding to the samples are calculated as follows:

[0089]

[0090]

[0091] Where L represents the state dimension of the target and j∈{0,...,2L} is the index of the sample. is the jth row of the matrix RMS. 2 (L+κ)-L is a proportional parameter used to reduce the overall prediction error. α determines the x k The distribution of sigma points around (t+1|t) is usually set to a small positive number, such as 0.001. κ is also a scale parameter, usually set to 0. β is used to merge x k (t) Prior knowledge of the distribution. For Gaussian distribution, β = 2 is optimal.

[0092] Step (3): Transfer the sigma points of the sampled samples in step (2) to obtain a set of sigma samples of the predicted measurement values ​​of the target k at time t+1.

[0093] z k,j (t+1|t)=h(x k,j (t+1|t)) (10)

[0094] Specifically, k,j (t+1|t) is z k The sigma point sampling of (t+1) is also called unscented transformation. Thus, a set of sigma sampling of the predicted measurement value of target k at time t+1 is obtained. To simplify the expression, the weight value is recorded as

[0095]

[0096] When the mean is required, use When the covariance is required, use

[0097] Step (4) calculates the iterative data association within the sensor in parallel for all sensors s.

[0098] The following sub-steps are included:

[0099] Step (4.1) measurement evaluation: use the sigma sampling of the predicted measurement value of target k at time t+1 to evaluate the measurement data at sensor s at time t+1, and obtain a set of weighted particles about the associated variables of target k at time t+1;

[0100] In each sensor s, for each target k, there is

[0101]

[0102] Where q∈{1,...,Q} represents the iteration loop index outside the sensor, and Q represents the maximum number of external iterations. This formula uses the likelihood function and the sigma sampling of the predicted measurement value and its weight to make a preliminary assessment of the likelihood that each measurement originated from the target. The initial value of the iteration loop outside the sensor is also,

[0103]

[0104] Here represents the observation likelihood function of target k at time t+1. Represents a uniformly distributed probability density function of the measurement in the measurement space. m′∈{0,1,...,M t+1}express The value of . Definition So we get a set of weighted particles about the associated variables of target k at time t+1

[0105] In step (4.2), the associated prior distribution is performed on the weighted particles of the associated variables of target k at time t+1 to obtain another set of weighted particles.

[0106] In each sensor s, for each target k, there is

[0107]

[0108] This formula describes the random variable A prior distribution of . So we get a set of weighted particles

[0109] Step (4.3), data association, performs data association on a set of weighted particles obtained after prior distribution, and obtains a set of weighted particles about the associated variables after data association.

[0110] First, define k′∈{0,1,...,K} represents In each sensor s, for each target k, each measurement m, there is

[0111]

[0112] Equation (15) describes the calculation method of the message from the measurement to the target.

[0113]

[0114] Equation (16) describes the calculation method of the message from the target to the measurement.

[0115]

[0116] Equation (17) describes the calculation method of the initial iterative message from the target to the measurement so that multiple iterations of Equations (15) and (16) can be performed.

[0117] Where p∈{1,...,P} represents the iteration loop index of data association within the sensor, P represents the maximum number of iterations, Indicates the initial message from the sensor's internal target to the measurement, The message representing the pth iteration of the target measured by the sensor, represents the message from the target to the pth iteration of the measurement. In addition, is defined as follows:

[0118]

[0119] Equation (18) describes the uncertainty assumption: a target can produce at most one measurement, and a measurement has at most one source.

[0120] The weights of the associated variables are calculated as follows:

[0121]

[0122] Finally, we can get a set of related variables Weighted particles This set of particles describes the correlation between the target and the measurement.

[0123] Step (4.4), measurement update, uses the weighted particle pairs of the associated variables to update the measurement and obtain a set of weighted particles after measurement update.

[0124] In each sensor s, for each target k, there is

[0125]

[0126] Finally, we get the weighted particles after a set of measurement updates.

[0127] Step (4.5), calculate the external information, and cyclically calculate the external information of the weighted particles after measurement update.

[0128] In each sensor s, for each target k, there is

[0129]

[0130] After reaching this step, it is necessary to re-execute steps (4.1), (4.2), (4.3), (4.4), and (4.5) until q=Q.

[0131] Step (5) After completing the iterative data association within the sensor, the confidence calculation is performed to obtain the distribution describing the state of the target k at time t+1.

[0132] For each target k, we have

[0133]

[0134] Since in steps (4.1), (4.2), (4.3), (4.4), (4.5), (5) ω k,j There are two values, so for the same particle, it also has a weight, which is used to calculate the mean and covariance respectively. So we get a set of weighted particles:

[0135]

[0136] definition In addition, particle z k,j (t+1|t) is the particle x k,j (t+1|t) is obtained through nonlinear transformation, so z k,j The weight of (t+1|t) is equal to x k,jThe weight of (t+1|t). So we get a set of weighted particles:

[0137]

[0138] At this point, (24) can already describe x k The distribution of (t+1).

[0139] Step (10): Estimate the state and covariance of each target k at time t+1 based on the distribution of the state of target k at time t+1.

[0140] The state estimate for each target k at time t+1 is:

[0141]

[0142] The estimate of the state covariance of each target k at time t+1 is:

[0143]

Claims

1. A fast sensor data fusion method based on sigma point belief propagation, characterized by: The following steps are involved: Step (1), predict the state and covariance of target k at time t; Step (2), perform sigma point sampling on the predicted state; Step (3), transferring the sigma points of the sampled samples in step (2) to obtain a set of sigma samples of the predicted measurement values ​​of the target k at time t+1; Step (4), calculate the iterative data association within the sensor in parallel for all sensors s; The step (4) includes the following sub-steps: Step (4.1) measurement evaluation: use the sigma sampling of the predicted measurement value of target k at time t+1 to perform a preliminary evaluation of the correlation between the measurement data and target k, and obtain a set of weighted particles about the associated variables of target k at time t+1; Step (4.2), the associated prior distribution, the weighted particles of the associated variables of target k at time t+1 are associated with the prior distribution to obtain another set of weighted particles; Step (4.3), data association, uses the particles of the prior distribution and the particles of the measurement evaluation to perform data association, and finally obtains the posterior weighted particles about the associated variables; The specific method of step (4.3) is: First, define express The value of, in each sensor s, for each target k, each measurement m, has The formula for measuring message delivery to the target is: The target-to-measurement message passing formula is: The initial target-to-measurement message passing formula is: The above three formulas describe the process of message loop iteration in data association, where: represents the association variable of measurement m at time t+1 at sensor s, m represents the measurement index, p∈{1,...,P} represents the iteration loop index of data association within the sensor, and P represents the maximum number of iterations. Indicates the initial message from the sensor's internal target to the measurement, The message representing the pth iteration of the target measured by the sensor, represents the message from the target to the pth iteration of the measurement, After completing the message iteration, the posterior probability of the associated variable is finally calculated. The message passing formula for the posterior probability of the associated variable is as follows: The weights of the associated variable values ​​describe the posterior probability between the target measurements, Get a set of related variables Weighted particles The weights indicated are the posterior evaluations of each measurement; Step (4.4), measurement update, using the weighted particles of the associated variables to update the measurement, and obtain a set of weighted particles after measurement update; Step (4.5), calculate external information, and cyclically calculate the external information of the weighted particles after measurement update; Step (5): After completing the iterative data association within the sensor, a confidence calculation is performed to obtain the distribution describing the state of the target k at time t+1; Step (6): Estimate the state and covariance of each target k at time t+1 based on the distribution of the state of target k at time t+1.

2. The sensor data fast fusion method based on sigma point belief propagation according to claim 1 is characterized in that: The specific method of step (4.1) is: In each sensor s, for each target k, measurement evaluation is performed, and the expression is as follows: The above formula uses the likelihood function and the prior information of the target measurement to evaluate the relevance of each measurement to the target. represents the correlation between target k and measurement m′ at time t+1, where s∈{1,...,S} represents the sensor index, S represents the number of sensors, t represents the time index, k∈{1,...,K} represents the target index, K represents the number of targets, q∈{1,...,Q} represents the iteration loop index outside the sensor, Q represents the maximum number of external iterations, represents the associated variable of target k at time t+1 in sensor s, express The value of represents the number of measurements at time t+1 in sensor s, j∈{0,1,...,2L} represents the index of the sigma point, L represents the state dimension of the target, z k,j (t+1|t) represents the predicted state x of target k at time t+1 k The j-th sigma point x at (t+1|t) k,j (t+1|t) is the sigma point after the observation function, z (s) (t+1) represents the measurement set at time t+1 in sensor s, and the initial value of the iterative loop outside the sensor is ω k,j Indicates the weight value of the sigma point, A set of weighted particles about the associated variables of target k at time t+1 is obtained The weights indicated are used to evaluate the measurements.

3. The sensor data fast fusion method based on sigma point belief propagation according to claim 2 is characterized in that: The specific method of step (4.2) is: In each sensor s, the associated variable for each target k is The prior distribution of is expressed as follows: Represents an associated variable A prior distribution of , which describes a priori situation of the detection of target k, represents the detection probability of sensor s, μ (s) Represents the mean value of the number of clutter in sensor s, and a set of weighted particles is obtained The weights expressed are a priori assessments of the measurements.

4. The sensor data fast fusion method based on sigma point belief propagation according to claim 2 is characterized in that: The specific method of step (4.4) is: After obtaining the posterior probability of the associated variable, the likelihood function is used to update the weight of each measurement particle. In each sensor s, for each target k, the update formula of the measurement particle is as follows: Finally, we get the weighted particles after a set of measurement updates. represents the posterior evaluation of the sigma point at the qth outer loop iteration.

5. The sensor data fast fusion method based on sigma point belief propagation according to claim 2 is characterized in that: The specific method of step (4.5) is: After the weights of the measurement particles in each sensor are updated, these weighted particles need to be treated as messages to interact and fuse with other sensors. Therefore, in each sensor s, for each target k, there is After reaching this step, you need to re-execute steps (4.1), (4.2), (4.3), (4.4), and (4.5) until q=Q. The initial value for the next sensor outer loop iteration.

6. The sensor data fast fusion method based on sigma point belief propagation according to claim 2 is characterized in that: The specific method of step (5) is: After all iterative message transmissions are completed between sensors, data fusion is performed using the messages from all sensors, that is, the weights of the measurement particles are updated. For each target k, f(z k,j (t+1|t)) represents the final posterior evaluation of the sigma point used to update the target state and covariance. Since ω k,j There are two values, so for the same particle, it also has a weight, which is used to calculate the mean and covariance respectively, thus obtaining a set of weighted particles: definition In addition, particle z k,j (t+1|t) is the particle x k,j (t+1|t) is obtained through nonlinear transformation, so z k,j The weight of (t+1|t) is equal to x k,j The weight of (t+1|t) is obtained, so a set of weighted particles is obtained: So we can describe x k The distribution of (t+1).

7. The method for rapid sensor data fusion based on sigma point belief propagation according to claim 2, characterized in that: The specific method of step (6) is: After completing the data fusion, the fused data is used to estimate the state of the problem. The state of each target k at time t+1 is estimated as follows: The estimate of the state covariance of each target k at t+1 is:

Images