Distributed Kalman filtering multi-node cooperative positioning method based on average consensus

Abstract

The invention discloses a distributed Kalman filtering multi-node cooperative tracking and positioning method and system based on average consensus, and relates to the technical field of networking positioning and tracking. According to the method, the detection packet loss problem and the communication packet loss problem in the wireless sensor network are innovatively proposed, validity check is carried out on a measured value, and a proper method is selected from four methods of elimination, repetition, prediction and estimation to carry out data processing; for an asynchronous network, a global update period is set, local posteriori estimations at the same time point are obtained in each period, confidence values of node data are set according to error characteristics of measurement data through an average consensus algorithm, and the local posteriori estimations are fused, so that the problem of asynchronous measurement in the asynchronous sensing network is solved; the information fusion step is optimized, and higher estimation precision is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-node networking positioning, and specifically to a distributed Kalman filtering multi-node collaborative positioning method based on average consensus. Background Art

[0002] At present, wireless sensor networks composed of multiple nodes in a self-organizing manner are widely used. Wireless sensor networks mostly adopt a distributed control method, that is, each node has both independent routing and host functions. There is no network central control point similar to a base station. The status of nodes is equal, which makes the wireless sensor network very robust and resistant to destruction. However, in actual engineering, due to different initial sampling times, different sampling cycles and information transmission delays of each node, the measurement sampling time of each node is different, resulting in the measurement information reaching each node being asynchronous. This network is called an asynchronous sensor network. In actual asynchronous wireless sensor network applications, packet loss often occurs due to network delays, network congestion, sensor failures, etc. Packet loss can be divided into two main types: ① Communication packet loss within the minefield network node, that is, the data packet sent by the node in the network to the neighboring node is lost; ② Detection packet loss, that is, the measurement data of the network node is lost. The packet loss problem will lead to reduced estimation accuracy or even divergence of the estimated value. Summary of the invention

[0003] The present invention proposes a distributed Kalman filter multi-node collaborative positioning method based on average consensus to solve the following problems:

[0004] 1) Asynchronous multi-node target tracking problem

[0005] In the collaborative positioning of wireless sensor networks (WSNs), due to the different initial sampling time, sampling period and information transmission delay of each node detector, the measurement sampling time of each node is different, resulting in the measurement information arriving at each node being asynchronous.

[0006] 2) Communication packet loss and detection packet loss issues

[0007] In actual wireless sensor network applications, packet loss often occurs due to network delay, network congestion, sensor failure, etc. Packet loss can be divided into two types: ① Communication packet loss within WSN nodes, that is, the data packets sent by nodes in the network to neighboring nodes are lost; ② Detection packet loss, that is, the measurement data of network nodes is lost. Packet loss can reduce the estimation accuracy or even lead to divergence of the estimation value.

[0008] The technical solution to realize the present invention is: a distributed Kalman filter multi-node collaborative positioning method based on average consensus, comprising the following steps:

[0009] Step 1: Multiple sensors form a WSN through self-organizing networking. Each sensor is regarded as a node. WSN senses, collects and processes information in the coverage area in real time in a collaborative way. In WSN, node i at time t k Get target azimuth With latitude and longitude information Node j at time t k Get target azimuth With latitude and longitude information Then, through multi-node collaborative calculation, we can get the value of node i at time t k The measured value of node j at time t k , go to step 2.

[0010] Step 2: Check the validity of the measured values ​​obtained in step 1, and select one of the four methods of elimination, duplication, prediction, and estimation to process the invalid data to obtain valid measured values. Go to step 3.

[0011] Step 3: According to the EKF state estimation rule, the motion equation and observation equation of the target state are constructed in node i; for node i: the motion equation is used to calculate the state of node i at time t k The local prior estimate of Based on the local prior estimate The observation equation is used to calculate the node i at time t k The local posterior estimate of Go to step 4.

[0012] Step 4: Estimate based on local posterior Node i advances forward through the state transition function to align with the end point t of GUP E , and obtain the time-aligned local posterior estimate Go to step 5.

[0013] Step 5: In WSN, the state estimation fusion weight W is calculated by combining the distance between the target and the node, the number of neighbors of the node and the measurement time point. ij , W ij is the fusion weight of node j relative to node i, and proceed to step 6.

[0014] Step 6: Node i broadcasts the current time t k Time-aligned local a posteriori estimation Send to neighbor nodes and estimate the fusion weight W according to the state ij The time t is calculated based on the average consensus algorithm k The global posterior estimate X k|k , realizing multi-node collaborative positioning in WSN.

[0015] Compared with the prior art, the present invention has the following significant advantages:

[0016] 1) In the local filtering stage, the present invention performs data processing on packet loss or misidentification situations, effectively suppresses the positioning error diffusion problem caused by observation loss or packet loss, and improves the stability of the system.

[0017] 2) After obtaining the local a posteriori estimate, the present invention pushes the state estimate of each node forward to the end point of the GUP, thereby realizing time-aligned local estimation. It is suitable for periodically updating the monitoring system and effectively solves the measurement asynchronous problem.

[0018] 3) In the fusion filter stage, the present invention adopts the average consensus algorithm to optimize the information fusion process, sets the confidence value of each node data according to the accuracy characteristics of the measurement data, integrates the local posterior estimates of each node after time alignment, forms a consistent global posterior estimate, and achieves higher estimation accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 This is a flow chart of the distributed Kalman filtering multi-node collaborative positioning method based on average consensus of the present invention.

[0020] Figure 2 This is the single node positioning error diagram of the present invention.

[0021] Figure 3 This is the positioning error diagram for multi-node networking. DETAILED DESCRIPTION

[0022] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0023] Combination Figure 1 The distributed Kalman filter multi-node collaborative positioning method based on average consensus described in the present invention comprises the following steps:

[0024] Step 1: Multiple sensors form a WSN through self-organizing networking. Each sensor is regarded as a node. WSN senses, collects and processes information in the coverage area in real time in a collaborative way. In WSN, node i at time t k Get target azimuth With latitude and longitude information Node j at time t k Get target azimuth With latitude and longitude information Then, through multi-node collaborative calculation, we can get the value of node i at time t k The measured value of node j at time t kThe specific steps of multi-node collaborative calculation are as follows:

[0025] Step 11: Node i at time t k From coordinates by The angle draws a ray, and node j is at time t k From coordinates by The angle draws out a ray and establishes the ray equation:

[0026]

[0027] Where l represents the extension of the ray along the direction vector (cos(r)+sin(r)), l≥0, (N i (l),E i (l)) represents the ray equation established at node i; (N j (l),E j (l)) represents the ray equation established at node j; represents node i at time t k Get the target azimuth; represents node j at time t k Get the target azimuth; represents node i at time t k The longitude and latitude information obtained; represents node j at time t k The longitude and latitude information obtained.

[0028] Step 12: Solve the ray equation for the intersection point and obtain the intersection point coordinates (u m ,v m ), and then average the coordinates of the intersection points to get the measured value. The formula is as follows:

[0029]

[0030] Among them, m means there are m intersection points in total; u m represents the horizontal coordinate of the intersection point m; v m represents the ordinate of the intersection point m; Indicates the measured value.

[0031] Go to step 2.

[0032] Step 2: Check the validity of the measured value obtained in step 1, as follows:

[0033] In actual application scenarios, although the target's motion pattern cannot be predicted, it is known that the target's speed and acceleration have certain limits. The target's speed and acceleration are checked at this time. If the target's speed and acceleration exceed the known limits, or the measurement value is lost, the invalid data is processed by one of the four methods: elimination, repetition, prediction, and estimation, to obtain a valid measurement value. The four methods are as follows:

[0034] a) Elimination: If node i is at time t k If the lth measurement is invalid and the number of neighbor nodes of node i is less than 3, then this invalid measurement value will not be used in the network positioning.

[0035] b) Repeat: If node i is at time t k If the lth measurement is invalid, it will be replaced by the time t k-1 The measured value

[0036] c) Prediction: If node i is at time t k If the lth measurement is invalid and the number of neighbors of the node is less than 3, then the time t k-1 The global posterior estimate X k-1|k-1 As a valid measurement

[0037] d) Estimation: If node i is at time t k If the lth measurement is invalid and the number of neighbors of the node is less than 3, then the time t k-1 The global posterior estimate X k-1|k-1 Calculate the time t by Kalman filtering k The local state estimate of

[0038] In actual wireless sensor network applications, packet loss often occurs due to network delay, network congestion, sensor failure, etc. Whether it is communication packet loss or detection packet loss, it will lead to reduced accuracy of the estimated value or even divergence of the estimated value. The present invention innovatively proposes a method for introducing a validity check on the measured value. According to the known limitations of the speed and acceleration of the target, the target speed and acceleration are checked, and invalid data is processed by one of the four methods of elimination, repetition, prediction, and estimation to obtain valid measurement values, thereby reducing the impact of packet loss on the estimation accuracy, improving positioning performance, and increasing the accuracy of obtaining target information.

[0039] Go to step 3.

[0040] Step 3: According to the EKF state estimation rule, the motion equation and observation equation of the target state are constructed in node i. For node i: calculate the state of node i at time tk The local prior estimate of Then through the local prior estimate Calculate the node i at time t k The local posterior estimate of The details are as follows:

[0041] Step 31: According to the node i at time t k The effective measurement value Calculate the nonlinear function h(x) at time t k The Jacobian matrix With the nonlinear function f(x) at time t k The Jacobian matrix

[0042] Step 32: Construct the motion equation and measurement equation of the target state, calculate the state prediction and error covariance prediction, and the calculation equation is:

[0043]

[0044] Where f(·) represents the state transfer function, X k-1|k-1 Indicates time t k-1 The global a posteriori estimate of ; represents node i at time t k process noise; represents node i at time t k The local prior estimate of k-1|k-1 Indicates time t k-1 The global error covariance of represents node i at time t k The prediction error covariance of represents node i at time t k The process noise covariance matrix of Represents the nonlinear function f(x) at time t k The Jacobian matrix of ; T denotes the transpose.

[0045] Step 33: Calculate the Kalman gain, update the state estimate and error covariance, and the calculation formula is as follows:

[0046]

[0047]

[0048] in, represents node i at time t k The Kalman gain of represents node i at time t k The prediction error covariance of represents node i at time tk A local prior estimate of ; represents node i at time t k Local state estimation of ; represents the effective measurement value; h() represents the nonlinear function; is the node i at time t k The updated error covariance of Represents the nonlinear function h() of node i at time t k The Jacobian matrix of ; I represents the identity matrix.

[0049] Go to step 4.

[0050] Step 4: Estimate the local posterior obtained in step 3 Node i advances forward through the state transition function to align with the end point t of GUP E , and obtain the time-aligned local posterior estimate The calculation formula is as follows:

[0051]

[0052] Among them, f E (·) represents the local posterior estimate Advance to GUP end point t E The state transfer function; Δt represents the time interval; represents node i at time t k The local a posteriori estimate of ; Represents a local a posteriori estimate of temporal alignment.

[0053] In actual engineering, due to the different initial sampling time of each node, different sampling cycle and information transmission delay, the measurement values ​​reaching each node are asynchronous. In this asynchronous wireless sensor network, the present invention innovatively proposes a method of introducing a global update cycle. After each node obtains a local a posteriori estimate, it pushes the local a posteriori estimate forward to the end point t of GUP. E , thereby achieving time-aligned local a posteriori estimation, this method is suitable for periodically updated monitoring systems and effectively solves the measurement asynchrony problem.

[0054] Go to step 5.

[0055] In step 5, the state estimation fusion weight W is calculated by combining the distance between the target and the node, the number of neighbors of the node and the measurement time point ij , the specific steps are as follows:

[0056] Step 51, calculate the initial confidence value of the node:

[0057] Since the measured value is related to the distance between the target and the node, the number of neighbors of the node and the measurement time, the initial confidence value of node i is calculated as follows:

[0058] C i =t'×w t +n'×w n +d'×w d

[0059] where t', n' and d' are the normalized values ​​of the measurement time, the number of node neighbors, and the distance between the target and the node; w t , w n and w d is the corresponding weight; C i Represents the initial confidence value of node i.

[0060] Step 52: Fusion weight calculation:

[0061] Using the initial confidence value C of a single node i To dynamically adjust the fusion weights to more accurately reflect the importance of each node and the strength of the adjacency relationship.

[0062]

[0063] Among them, C i represents the initial confidence value of node i; C j represents the initial confidence value of node j, C n represents the initial confidence value of the neighboring node of node i; n represents the neighboring node of node i, W ii represents the fusion weight of node i itself; W ij represents the fusion weight of node j relative to node i; β represents the weight used to adjust W ii With W ij Specific gravity; N i represents the neighbor node set of node i, and A represents all nodes in the WSN.

[0064] Since the measurement value in actual engineering is related to the distance between the target and the node, the number of neighbors of the node and the measurement time, the closer the distance between the target and the node, the more accurate the measurement value; the more neighbors the node has, the more accurate the measurement value; the newer the measurement time, the more accurate the measurement value. The present invention innovatively proposes a method of introducing fusion weights during information fusion. When using the average consensus algorithm to perform information fusion, the fusion weight of the more accurate local posterior estimation is increased according to the characteristics of the measurement value, and the local posterior estimation of each node after time alignment is integrated to achieve higher positioning accuracy.

[0065] Go to step 6.

[0066] Step 6: Estimate the fusion weight W according to the state obtained in step 5ij , calculated based on the average consensus algorithm, t k The global posterior estimate X at time k|k , the specific steps are as follows:

[0067] Step 61: Information exchange:

[0068] In WSN, node i broadcasts the current time t k Time-aligned local a posteriori estimation Send to neighbor nodes;

[0069] Step 62: Status update:

[0070] The update rule is as follows:

[0071]

[0072] in, represents node i at time t k The consensus state variables of the ξ+1th iteration;

[0073] represents node i at time t k The consensus state variables of the ξth iteration; represents node j at time t k The consensus state variables of the ξth iteration; represents node i at time t k The consensus error covariance matrix of the ξ+1th iteration; represents node i at time t k The consensus error covariance matrix of the ξth iteration; represents node j at time t k The consensus error covariance matrix of the ξth iteration; ξ represents the average consensus algorithm at time t k The ξth iteration during the GUP; W ii represents the fusion weight of node i itself; W ij represents the fusion weight of node j relative to node i; N i Represents the set of neighbor nodes of node i;

[0074] Step 63: Convergence test:

[0075] When the absolute value of the difference between the ξ+1th iteration and the ξth iteration is less than the set value ε, the state estimation is considered to have converged, and the time t k The global posterior estimate X k|k , calculated as follows:

[0076]

[0077] in, represents node i at time t k The consensus state variables of the ξ+1th iteration;

[0078] represents node i at time t k The consensus state variable of the ξth iteration; ξ represents the average consensus algorithm at time t k The ξth iteration during the GUP; ε represents the convergence tolerance; X k|k Indicates that node i is at t k The global a posteriori estimate of time.

[0079] Finally, we get the time t k The global posterior estimate X k|k , realizing multi-node collaborative positioning in WSN.

[0080] Example 1

[0081] Considering the limited communication range of each node and the detection range of the acoustic array (the communication distance and detection distance are both within 300m), 5 nodes were selected to conduct the test in an area of ​​450m×200m. The target drove along the set straight path at an average speed of 36km / h. The experimental scene is as follows Figure 3 In order to simulate the situation where the number of nodes that detect the target is uncertain, the target's trajectory is set to be unable to be detected by 5 nodes throughout the entire process, that is, there will be a situation where there are less than 5 nodes for network positioning, and the number of neighbors of each node is different.

[0082] The local a posteriori estimation results of the five observation stations are as follows: Figure 2 As shown in the figure, the blue line represents the actual trajectory of the target, and the dotted line represents the local a posteriori estimation of each node for each step of the target. As can be seen from the figure, each node starts to estimate the local state only when the target enters the detection range of each node. At the same time, when the detection distance is far, the error is large due to signal attenuation and environmental interference. In addition, since the number of neighbors of node 3 is large, the positioning accuracy is higher, which verifies the influence of the number of node neighbors on the confidence value of the measurement value.

[0083] Each node calculates the global posterior estimate through the average consensus algorithm, and the result is as follows Figure 3 As shown in Figure 2, the blue line represents the actual trajectory of the target, and the red asterisk marks the global posterior estimation of each node after iteration. Figure 2 and Figure 3 It can be seen that each node significantly improves the estimation accuracy of the target position by combining the local posterior information of neighboring nodes through iterative calculation.

Claims

1. A distributed Kalman filter multi-node collaborative positioning method based on average consensus, characterized in that: The steps include: Step 1: Multiple sensors form a WSN through self-organizing networking. Each sensor is regarded as a node. WSN senses, collects and processes information in the coverage area in real time in a collaborative way. In WSN, node i at time t k Get target azimuth With latitude and longitude information Node j at time t k Get target azimuth With latitude and longitude information Then, through multi-node collaborative calculation, we can get the value of node i at time t k The measured value of node j at time t k The measured value goes to step 2; Step 2: Check the validity of the measured values ​​obtained in step 1, and select one of the four methods of elimination, duplication, prediction, and estimation to process the invalid data to obtain valid measured values. Go to step 3; Step 3: According to the EKF state estimation rule, the motion equation and observation equation of the target state are constructed in node i; for node i: the motion equation is used to calculate the state of node i at time t k The local prior estimate of Based on the local prior estimate The observation equation is used to calculate the node i at time t k The local posterior estimate of Go to step 4; Step 4: Estimate based on local posterior Node i advances forward through the state transition function to align with the end point t of GUP E , and obtain the time-aligned local posterior estimate Go to step 5; Step 5: In WSN, the state estimation fusion weight W is calculated by combining the distance between the target and the node, the number of neighbors of the node and the measurement time point. ij , W ij is the fusion weight of node j relative to node i, and proceed to step 6; Step 6: Node i broadcasts the current time t k Time-aligned local a posteriori estimation Send to neighbor nodes and estimate the fusion weight W according to the state ij The time t is calculated based on the average consensus algorithm k The global posterior estimate X k|k , realizing multi-node collaborative positioning in WSN.

2. The distributed Kalman filter multi-node collaborative positioning method based on average consensus according to claim 1 is characterized in that: In step 2, the measurement values ​​obtained in step 1 are checked for validity, and invalid data are processed by one of the four methods: elimination, duplication, prediction, and estimation, to obtain valid measurement values. The details are as follows: a) Elimination: If node i is at time t k If the lth measurement is invalid and the number of neighbor nodes of node i is less than 3, then this invalid measurement value will not be used in network positioning; b) Repeat: If node i is at time t k If the lth measurement is invalid, it will be replaced by the time t k-1 The measured value c) Prediction: If node i is at time t k If the lth measurement is invalid and the number of neighbors of the node is less than 3, then the time t k-1 The global posterior estimate X k-1|k-1 As a valid measurement d) Estimation: If node i is at time t k If the lth measurement is invalid and the number of neighbors of the node is less than 3, then the time t k-1 The global posterior estimate X k-1|k-1 Calculate the time t by Kalman filtering k The local state estimate of 3. The distributed Kalman filter multi-node collaborative positioning method based on average consensus according to claim 2 is characterized in that: In step 3, according to the EKF state estimation rule, the motion equation and observation equation of the target state are constructed in node i. For node i: calculate node i at time t k The local prior estimate of Then through the local prior estimate Calculate the node i at time t k The local posterior estimate of The details are as follows: Step 31: According to the node i at time t k The effective measurement value Calculate the nonlinear function h(x) at time t k The Jacobian matrix With the nonlinear function f(x) at time t k The Jacobian matrix Step 32: Construct the motion equation and measurement equation of the target state, calculate the state prediction and error covariance prediction, and the calculation equation is: Where f(·) represents the state transfer function, X k-1|k-1 Indicates time t k-1 The global a posteriori estimate of ; represents node i at time t k process noise; represents node i at time t k A local prior estimate of ; P k-1|k-1 Indicates time t k-1 The global error covariance of represents node i at time t k The prediction error covariance of represents node i at time t k The process noise covariance matrix of Represents the nonlinear function f(x) of node i at time t k The Jacobian matrix of ; T represents the transpose; Step 33: Calculate the Kalman gain, update the state estimate and error covariance, and the calculation formula is as follows: in, represents node i at time t k The Kalman gain of represents node i at time t k The prediction error covariance of represents node i at time t k A local prior estimate of ; represents node i at time t k Local state estimation of ; represents the effective measurement value; h() represents the nonlinear function; is the node i at time t k The updated error covariance of Represents the nonlinear function h() of node i at time t k The Jacobian matrix of ; I represents the identity matrix.

4. The distributed Kalman filter multi-node collaborative positioning method based on average consensus according to claim 1, characterized in that: In step 4, the local posterior estimate obtained in step 3 is Node i advances forward through the state transition function to align with the end point t of GUP E , and obtain the time-aligned local posterior estimate The calculation formula is as follows: Among them, f E (·) represents the local posterior estimate Advance to GUP end point t E The state transfer function; Δt represents the time interval; represents node i at time t k The local a posteriori estimate of ; Represents a local a posteriori estimate of temporal alignment.

5. The distributed Kalman filter multi-node collaborative positioning method based on average consensus according to claim is characterized in that: In step 5, the state estimation fusion weight W is calculated by combining the distance between the target and the node, the number of neighbors of the node and the measurement time point ij , the specific steps are as follows: Step 51, calculate the initial confidence value of the node: Since the measurement information is related to the distance between the target and the node, the number of neighbors of the node and the measurement time, the initial confidence value of node i is calculated as follows: C i =t'×w t +n'×w n +d'×w d where t', n' and d' are the normalized values ​​of the measurement time, the number of node neighbors, and the distance between the target and the node, and w t 、w n and w d is the corresponding weight; C i represents the initial confidence value; Step 52: Fusion weight calculation: Using the initial confidence value C of node i i To dynamically adjust the fusion weights to more accurately reflect the importance of each node and the strength of the adjacency relationship; Among them, C i represents the initial confidence value of node i; C j represents the initial confidence value of node j, C n represents the initial confidence value of the neighboring node of node i; n represents the neighboring node of node i, W ii represents the fusion weight of node i itself; W ij represents the fusion weight of node j relative to node i; β represents the weight used to adjust W ii With W ij Specific gravity; N i represents the neighbor node set of node i, and A represents all nodes in the WSN.

6. The distributed Kalman filter multi-node collaborative positioning method based on average consensus according to claim 5 is characterized in that: In step 6, the state estimation fusion weight W obtained in step 5 is used ij , calculated based on the average consensus algorithm, t k The global a posteriori estimate X at time k|k , the specific steps are as follows: Step 61: Information exchange: In WSN, node i broadcasts the current time t k Time-aligned local a posteriori estimation Send to neighbor nodes; Step 62: Status update: The update rule is as follows: in, represents node i at time t k The consensus state variables of the ξ+1th iteration; represents node i at time t k The consensus state variables of the ξth iteration; represents node j at time t k The consensus state variables of the ξth iteration; represents node i at time t k The consensus error covariance matrix of the ξ+1th iteration; represents node i at time t k The consensus error covariance matrix of the ξth iteration; represents node j at time t k The consensus error covariance matrix of the ξth iteration; ξ represents the average consensus algorithm at time t k The ξth iteration during the GUP; W ii represents the fusion weight of node i itself; W ij represents the fusion weight of node j relative to node i; N i Represents the set of neighbor nodes of node i; Step 63: Convergence test: When the absolute value of the difference between the ξ+1th iteration and the ξth iteration is less than the set value ε, the state estimation is considered to have converged, and the time t k The global posterior estimate X k|k , calculated as follows: in, represents node i at time t k The consensus state variables of the ξ+1th iteration; represents node i at time t k The consensus state variable of the ξth iteration; ξ represents the average consensus algorithm at time t k The ξth iteration during the GUP; ε represents the convergence tolerance; X k|k Indicates that node i is at t k The global a posteriori estimate of time; Finally, we get the time t k The global posterior estimate X k|k , realizing multi-node collaborative positioning in WSN.

Images