A weighted filtering state estimation method based on geometric relations
By employing geometrically weighted observation noise and suboptimal consistency filtering methods, the problems of decreased measurement accuracy and link anomalies in time-of-flight sensors were solved, achieving high-precision and robust target state estimation.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- NAT UNIV OF DEFENSE TECH
- Filing Date
- 2025-05-23
- Publication Date
- 2026-05-08
AI Technical Summary
Existing distributed state estimation methods suffer from decreased accuracy of time-of-flight sensor measurements as the target distance increases, and link anomalies lead to dimensionality reduction in the communication topology, affecting global estimation accuracy and consistency convergence, especially when the target is occluded.
A geometrically weighted observation noise method is introduced. By calculating the geometrically weighted consistency gain, the node weights are adjusted to ensure high estimation accuracy and robustness. A suboptimal Kalman consistency filter is used to reduce computational complexity.
Even when a single observation node performs poorly or the target is occluded, the system can still maintain high estimation accuracy and robustness while reducing computational load.
Smart Images
Figure CN120428238B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of target state estimation technology using multiple sensors, and in particular to a weighted filtering state estimation method based on geometric relationships. Background Technology
[0002] In the field of distributed state estimation, significant progress has been made in Kalman consensus filters based on multi-sensor networks. By integrating local observations with neighborhood consensus mechanisms, an incomplete graph communication architecture is constructed, enabling each node to exchange information only with neighboring sensors, thus enhancing the system's fault tolerance under link anomalies. Even if local communication is interrupted, the network can maintain basic state estimation functions through redundant paths, avoiding global paralysis caused by single-point failures in centralized architectures. This has significant application value in dynamic scenarios such as multi-agent target state estimation.
[0003] However, most existing distributed estimation methods assume a constant measurement noise covariance. For time-of-flight sensors, measurement accuracy decreases with increasing distance from the target, and the noise covariance is positively correlated with the distance from the target to the sensor. When the target is occluded, the effective observation distance of the occluded node increases significantly (or signal quality deteriorates). If fixed weights are still used, these anomalous observations will significantly affect the global estimation accuracy. Furthermore, link anomalies lead to dimensionality reduction in the communication topology, weakening the consistency accuracy between nodes and ultimately reducing the overall consistency convergence of the estimation. This indicates that current consistent Kalman filtering algorithms have an inherent contradiction between fault tolerance mechanisms and estimation accuracy in complex network environments, limiting their widespread application. Summary of the Invention
[0004] The purpose of this invention is to provide a geometrically weighted filtering state estimation method to address the problems existing in the prior art, such as the decrease in measurement accuracy of time-of-flight sensors as the distance to the target increases. This invention introduces a geometrically weighted observation noise method to ensure that the entire system still has high estimation accuracy and robustness when the observation effect of a single observation node is poor or the target is occluded. The optimal Kalman consensus filter increases computational complexity due to the introduction of consensus gain. This invention proposes a suboptimal distance weighting method to reduce the amount of computation while ensuring the accuracy of the estimation.
[0005] To achieve the above objectives, the present invention provides the following solution:
[0006] A weighted filtering state estimation method based on geometric relationships includes:
[0007] Acquire state estimation information of each UAV regarding ground targets;
[0008] The state estimation information is subjected to Kalman filtering to obtain the posterior estimate for each UAV;
[0009] For each drone's posterior estimate, asynchronous timestamp approximation alignment is performed;
[0010] Based on the aligned posterior estimate, geometric weighted consistency gain is calculated to obtain the weighted state estimate and consistency gain.
[0011] The weighted state estimate and the consistency gain are fused together to obtain the final target state estimate.
[0012] Optionally, obtaining state estimation information for each UAV regarding ground targets includes:
[0013] Treat each drone as an observation node;
[0014] Based on sensor data collected by the UAV from the target; wherein, the sensor data includes: the target distance measured by lidar, the yaw and pitch angle when the optical axis of the optoelectronic pod is aligned with the target, and the UAV attitude;
[0015] Based on the sensor data, the position of the target in the camera volume coordinate system is obtained;
[0016] The velocity of the target is obtained by calculating the difference between the states at adjacent sampling times.
[0017] Based on the target's velocity and its position in the camera's body coordinate system, the UAV obtains state estimation information of the ground target.
[0018] Optionally, obtaining the target's position in the camera's volume coordinate system based on the sensor data includes:
[0019] Based on the target distance, obtain the target's coordinates in the camera coordinate system;
[0020] Based on the target's coordinates in the camera coordinate system, and combined with the yaw and pitch angles when the optical axis of the optoelectronic pod is aligned with the target and the UAV's attitude, the target's position in the camera body coordinate system is obtained.
[0021] Optionally, performing Kalman filtering on the state estimation information to obtain the posterior estimate for each UAV includes:
[0022] Based on the state estimation information, the target motion state equation is constructed to obtain a prior estimate of the target.
[0023] Construct the observation equations for the UAV;
[0024] Based on the observation equations of the UAVs and prior estimates of the targets, the Kalman filtering algorithm is applied to each UAV. Obtain the posterior estimate of the drone at time k.
[0025] Optionally, for each drone's posterior estimate, asynchronous timestamp approximation alignment includes:
[0026] For each UAV's posterior estimate, approximately synchronized on the system's standard timestamps are performed to obtain an aligned posterior estimate for each UAV.
[0027] Optionally, based on the aligned posterior estimate, a geometrically weighted consensus gain is calculated to obtain the weighted state estimate and consensus gain, including:
[0028] The drone performs consistent Kalman estimation based on the aligned posterior estimates broadcast by neighboring nodes to obtain the consistency factor and consistency weight;
[0029] The consistency gain is calculated based on the consistency factor and consistency weight.
[0030] Based on the consistency gain, the aligned posterior estimate is weighted to obtain the weighted state estimate.
[0031] Optionally, the weighted state estimate and the consistency gain are fused and calculated, including:
[0032] Multiply the weighted state estimate of each drone node by the corresponding consistency gain;
[0033] The weighted state estimates of all nodes, multiplied by the consistency gain, are summed to obtain the final target state estimate.
[0034] The beneficial effects of this invention are as follows:
[0035] This application provides a weighted filtering state estimation method based on geometric relationships, in which each UAV reaches a consensus on its own and its neighboring nodes' estimates of the target. The observation noise is calculated based on the distance from the laser sensor to the target. When a node's target observation is obstructed, its weights can be adjusted to maintain high accuracy in the sensor network's target estimation. Attached Figure Description
[0036] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0037] Figure 1 This is a comparative diagram showing whether the target state estimation of the present invention is used when a target is occluded at a certain node according to an embodiment of the present invention; wherein, (a) is the original estimation and (b) is the method proposed in the present invention;
[0038] Figure 2 This is a schematic diagram illustrating the impact of an embodiment of the present invention on consistency and convergence speed; where (a) is γ = 0.1, (b) is γ = 0.3, and (c) is γ = 0.6.
[0039] Figure 3 This is a schematic diagram illustrating the relationship between observation distance and fusion weight in an embodiment of the present invention;
[0040] Figure 4 This is a schematic diagram of a weighted filtering state estimation method based on geometric relationships according to an embodiment of the present invention. Detailed Implementation
[0041] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0042] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.
[0043] like Figure 4 As shown, this embodiment proposes a weighted filtering state estimation method based on geometric relationships, including:
[0044] Step 1. Obtain state estimation information of each UAV regarding ground targets;
[0045] Step 2. Apply Kalman filtering to the state estimation information of the weighted filtering state estimation method based on geometric relationships to obtain the posterior estimate for each UAV;
[0046] Step 3. For the posterior estimate of each UAV, perform asynchronous timestamp approximation alignment;
[0047] Step 4. Based on the aligned posterior estimate, perform geometric weighted consistency gain calculation to obtain the weighted state estimate and consistency gain;
[0048] Step 5. Combine the weighted state estimate and the consistency gain to obtain the final target state estimate.
[0049] Furthermore, obtaining state estimation information for each UAV regarding ground targets includes:
[0050] Treat each drone as an observation node;
[0051] Sensor data collected by UAVs; among which, the state estimation method based on geometric relationships uses sensor data including: target distance measured by lidar, yaw and pitch angle when the optical axis of the optoelectronic pod is aligned with the target, and UAV attitude;
[0052] A weighted filtering state estimation method based on geometric relationships is used to obtain the target's position in the camera's volumetric coordinate system from sensor data.
[0053] The velocity of the target is obtained by calculating the difference between the states at adjacent sampling times.
[0054] The weighted filtering state estimation method based on geometric relationships obtains the state estimation information of the UAV on the ground target by using the target's velocity and position in the camera's volume coordinate system.
[0055] Furthermore, the weighted filtering state estimation method based on geometric relationships, using sensor data, obtains the target's position in the camera's volumetric coordinate system, including:
[0056] A weighted filtering state estimation method based on geometric relationships is used to estimate the target distance and obtain the target's coordinates in the camera coordinate system.
[0057] Based on the target's coordinates in the camera coordinate system, and combined with the yaw and pitch angles when the optical axis of the optoelectronic pod is aligned with the target and the UAV's attitude, the target's position in the camera body coordinate system is obtained.
[0058] Specifically, in this embodiment, step 1, taking the state estimation of a ground target by multiple UAVs as an example, treats the multi-UAV system as a multi-sensor network, with each UAV as an observation node. Each node estimates the state of the target, and a consistent estimate is obtained through distributed information fusion. In the sensor network, at time t, node i obtains the target's coordinates in the world coordinate system through coordinate transformation based on data from the lidar sensor, pod attitude, and UAV attitude. The specific implementation process is as follows:
[0059] Consider a scenario where multiple quadcopter drones equipped with electro-optical pods form a sensor network to estimate the state of a ground target, with each drone considered as an observation node. Assume that the distance to the target measured by the lidar of drone i is D, and the yaw and pitch angles when the optical axis of the electro-optical pod is aligned with the target are... The drone's attitude is [x,y,z,θ,σ,φ]. The detected target's pixel coordinates are P. uv =[u,v], the conversion from pixel coordinates to the normalized plane:
[0060]
[0061] Where K is the camera intrinsic parameter matrix, P cEach column represents the projection of the three axes of the normalized planar coordinate system onto the camera coordinate system. Therefore, the target's coordinates in the camera coordinate system are P. c =P c D. Combining the drone's attitude and the relative position of the pod, determine the target's position in the aircraft's coordinate system. The target's velocity is calculated by subtracting the states at adjacent sampling times. Finally, the velocity of UAV i relative to target t is obtained. k State estimation x at time 1 i (t k ).
[0062] Furthermore, Kalman filtering is applied to the state estimation information of the weighted filtering state estimation method based on geometric relationships to obtain the posterior estimate of each UAV.
[0063] A weighted filtering state estimation method based on geometric relationships is used to estimate the state information, construct the target motion state equation, and obtain a priori estimate of the target.
[0064] Construct the observation equations for the UAV;
[0065] Based on the observation equations of the UAVs and prior estimates of the targets, the Kalman filtering algorithm is applied to each UAV. Obtain the posterior estimate of the drone at time k.
[0066] Specifically, in this embodiment, step 2. describes the target state motion equation and the observation equation of each node according to the local Kalman filter estimation method, where the observation noise ν is positively correlated with the distance from the node to the target. For each observation node, a prior estimate is first calculated using the target's motion equation. The observed value z is obtained using the observation equation. i (t), and finally the posterior estimate is obtained by Kalman filtering. Update the covariance matrix and Kalman gain; as follows:
[0067] Establish state equations for the target motion Where ω is the system noise, the observation equation for each node is z = Hx + μ, where H is the observation matrix and μ is the distance-related noise. A nonlinear relationship is established as μ = Φ(ρ)ν, where the noise varies with distance. For each node, the Kalman filter algorithm is used to obtain the posterior estimate of node i at time k. Then broadcast it to neighboring nodes and proceed to step 3.
[0068] Furthermore, for the posterior estimate of each drone, asynchronous timestamp approximation alignment includes:
[0069] For each UAV's posterior estimate, approximately synchronized on the system's standard timestamps are performed to obtain an aligned posterior estimate for each UAV.
[0070] Specifically, in this embodiment, step 3. Due to the use of distributed consistency filtering, each node needs to perform asynchronous timestamp alignment before fusing neighbor node data, and perform approximate synchronization on the system standard timestamp to obtain t. k Posterior estimates of each node at time t. Specifically as follows:
[0071] At time (t) k-1 ,t k During the time interval, all drones provided an estimate, and at t k The previous neighbor node's estimate is accepted, at t k The data is fused at different times to obtain a consistent state estimate of the target. Therefore, the problem that needs to be solved in asynchronous consistency estimation is that each UAV receives measurement sets from its neighboring nodes at different times. Then, solve: Proceed to step 4.
[0072] Furthermore, based on the aligned posterior estimate, a geometrically weighted consensus gain is calculated to obtain the weighted state estimate and consensus gain, including:
[0073] The drone performs consistent Kalman estimation based on the aligned posterior estimates broadcast by neighboring nodes to obtain the consistency factor and consistency weight;
[0074] Consistency factor and consistency weight of the weighted filtering state estimation method based on geometric relations; Calculate the consistency gain of the weighted filtering state estimation method based on geometric relations.
[0075] The geometrically based weighted filtering state estimation method uses consistency gain to weight the aligned posterior estimate and obtain the weighted state estimate.
[0076] Specifically, in this embodiment, step 4. In the distributed sensor network, node i only communicates with its neighbor node j, that is, it receives the posterior state estimate from node j. Simultaneously, the consistency gain c is calculated using the distance-weighted method. ij , which represents the consistency compensation factor of sensor i relative to j; specifically as follows:
[0077] Node i performs a consistent Kalman estimation based on the messages broadcast by its neighboring nodes, using the following formula:
[0078]
[0079] Among them, Ki This is the Kalman gain, which can be obtained from the minimum covariance matrix. The key points to consider are the consistency factor γ and the consistency weight c. ij The consistency factor γ is an adjustment parameter that affects the convergence speed and stability of consistency. The consistency compensation factor c for node i relative to j... ij It is a measured distance ρ from node i and node j to the target node respectively. i and ρ j The function. Then, consistency weights are constructed by applying the following nonlinear characteristic function if the target is occluded. Otherwise, the value is 1, and the operation is performed on these two distances:
[0080]
[0081] Figure 3 The relationship between the distance to the target and the weight of the three drones is shown. It can be seen that under normal circumstances, the weight decreases smoothly as the distance increases. However, when there is occlusion, such as when drone 2 is set to infinite distance, the weight becomes 0 according to the formula.
[0082] Different choices are only possible when the characteristic function is monotonically decreasing with respect to the distance ρ. The smoothness, continuity, and boundedness of the hyperbolic cosine function make it a good choice for generating compensation factors; proceed to step 5.
[0083] Furthermore, the weighted state estimate and the consistency gain are fused and calculated, including:
[0084] Multiply the weighted state estimate of each drone node by the corresponding consistency gain;
[0085] The weighted state estimates of all nodes, multiplied by the consistency gain, are summed to obtain the final target state estimate.
[0086] Specifically, in this embodiment, step 5. Obtain the final consensus posterior estimate by fusing the state estimates of node i and node j after distance weighting. Simultaneously, parameters such as the covariance matrix, Kalman gain, and consensus gain are updated, and the estimate of node i is broadcast to neighboring nodes, eventually leading to consensus among all nodes.
[0087] Figure 1 This is a comparative diagram showing whether the target state estimation of the present invention is used when a target is occluded at a certain node in this embodiment; Figure 1 (a) shows the results of the three UAVs not running the proposed algorithm. Due to the occlusion of the observed target by UAV 3 and the lack of consistent weighting, the estimated state remains at the last valid estimate, which affects the state estimates of other UAVs. Figure 1(b) shows the results of applying the proposed algorithm, which demonstrates that even if the estimates of a single node are abnormal, the system can maintain high estimation accuracy through dynamic weighting.
[0088] According to the general consistent Kalman filtering method, the Kalman gain K can be obtained by solving for the trace tr(M) that minimizes the covariance matrix. i Then K i Substituting back into the original equation, we can obtain the updated posterior covariance matrix, which is derived from the following formula:
[0089]
[0090] The main complexity lies in The calculation of γ. When adjusting the consistency rate of the estimates, a smaller γ is tended to be used to ensure stability. Therefore, compared to P, The magnitude in the expression becomes negligible, and ignoring This can significantly reduce computational complexity. Therefore, using an approximate suboptimal solution, the following algorithm is given:
[0091]
[0092] M = FPF T +KΦ(ρ)R(Φ(ρ)) T K T
[0093]
[0094] For its stability analysis, the error-related Lyapunov function is selected.
[0095] V(η)=η T M -1 η
[0096] Its difference can be obtained
[0097]
[0098] in,
[0099]
[0100] Λ1 is a symmetric positive definite matrix. Λ2 and Λ3 are real symmetric matrices, and therefore have real eigenvalues. Let λ min (Λ1),λ min (Λ2),λ max (Λ3) represents the minimum or maximum eigenvalue of the corresponding Λ1, Λ2, and Λ3. From the above statement, it can be seen that λ min (Λ1)>0 and λ max (Λ3)>0. Therefore, simple linear algebra shows that:
[0101]
[0102] This means that in order to ensure that the error dynamics are globally asymptotically stable, it is necessary to:
[0103] λ min (Λ1)+γλ min (Λ2)-γ 2 λ max (Λ3)>0
[0104]
[0105] Therefore, when 0 < γ < γ * This ensures that the system is globally stable.
[0106] Figure 2 This is a schematic diagram illustrating the impact of this embodiment on consistency and convergence speed; Figure 2 (a) represents a smaller γ value, indicating that the system converges slowly with significant deviations. Figure 2 (b) represents a suitable value of γ, resulting in rapid system convergence with minimal error. Figure 2 (c) is greater than γ * The system diverges at the value of γ.
[0107] This embodiment proposes a geometrically weighted observation noise method to replace constant observation noise, ensuring that the entire system maintains high estimation accuracy and robustness even when the observation performance of a single observation node is poor or the target is occluded. Furthermore, a suboptimal consistency filtering method is proposed to reduce computational complexity while maintaining estimation accuracy.
[0108] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Various modifications and improvements made to the technical solutions of the present invention by those skilled in the art without departing from the spirit of the present invention should fall within the protection scope defined by the claims of the present invention.
Claims
1. A weighted filtering state estimation method based on geometric relationships, characterized in that, include: Acquire state estimation information of each UAV regarding ground targets; The state estimation information is subjected to Kalman filtering to obtain the posterior estimate for each UAV; For each drone's posterior estimate, asynchronous timestamp approximation alignment is performed; Based on the aligned posterior estimate, geometric weighted consistency gain is calculated to obtain the weighted state estimate and consistency gain. Consistency weights are constructed by applying the following nonlinear characteristic function if the target is occluded. Otherwise, the value is 1. The operation is performed on these two distances: ； The weighted state estimate and the consistency gain are fused together to obtain the final target state estimate.
2. The weighted filtering state estimation method based on geometric relationships according to claim 1, characterized in that, Obtaining state estimation information for each UAV regarding ground targets includes: Treat each drone as an observation node; Based on sensor data collected by the UAV from the target; wherein, the sensor data includes: the target distance measured by lidar, the yaw and pitch angle when the optical axis of the optoelectronic pod is aligned with the target, and the UAV attitude; Based on the sensor data, the position of the target in the camera volume coordinate system is obtained; The velocity of the target is obtained by calculating the difference between the states at adjacent sampling times. Based on the target's velocity and its position in the camera's body coordinate system, the UAV obtains state estimation information of the ground target.
3. The weighted filtering state estimation method based on geometric relationships according to claim 2, characterized in that, Based on the sensor data, obtaining the target's position in the camera's volume coordinate system includes: Based on the target distance, obtain the target's coordinates in the camera coordinate system; Based on the target's coordinates in the camera coordinate system, and combined with the yaw and pitch angles when the optical axis of the optoelectronic pod is aligned with the target and the UAV's attitude, the target's position in the camera body coordinate system is obtained.
4. The weighted filtering state estimation method based on geometric relationships according to claim 1, characterized in that, The state estimation information is then subjected to Kalman filtering to obtain the posterior estimate for each UAV, including: Based on the state estimation information, the target motion state equation is constructed to obtain a priori estimate of the target; Construct the observation equations for the UAV; Based on the observation equations of the UAVs and prior estimates of the targets, the Kalman filter algorithm is used for each UAV to obtain the UAV's observations at the target location. Posterior estimation at time step.
5. The weighted filtering state estimation method based on geometric relationships according to claim 1, characterized in that, For each drone's posterior estimate, asynchronous timestamp approximation alignment includes: For each UAV's posterior estimate, approximately synchronized on the system's standard timestamps are performed to obtain an aligned posterior estimate for each UAV.
6. The weighted filtering state estimation method based on geometric relationships according to claim 1, characterized in that, Based on the aligned posterior estimate, a geometrically weighted consensus gain is calculated to obtain the weighted state estimate and consensus gain, including: The drone performs consistent Kalman estimation based on the aligned posterior estimates broadcast by neighboring nodes to obtain the consistency factor and consistency weight; The consistency gain is calculated based on the consistency factor and consistency weight. Based on the consistency gain, the aligned posterior estimate is weighted to obtain the weighted state estimate.
7. The weighted filtering state estimation method based on geometric relationships according to claim 1, characterized in that, The fusion calculation of the weighted state estimate and the consistency gain includes: Multiply the weighted state estimate of each drone node by the corresponding consistency gain; The weighted state estimates of all nodes, multiplied by the consistency gain, are summed to obtain the final target state estimate.