A Multi-Traffic Entity Information Fusion Method for Autonomous Transportation Systems

Abstract

This invention relates to the field of traffic state perception technology, and more particularly to a multi-traffic subject information fusion method for autonomous traffic systems. The method includes step S1, fusing image data and point cloud data; and step S2, estimating the vehicle subject state. This invention combines Euclidean clustering with a YOLOv model to process and fuse data from different dimensions. Based on a confidence-optimized data fusion method, it achieves accurate detection of long-distance, partially occluded vehicle subjects. It utilizes D-S evidence theory to determine the data hypothesis with the highest confidence, thereby obtaining reliable vehicle subject position data as state data, effectively ensuring sensor data fusion and authenticity assessment. By extending the state vector and using a nonlinear system model to describe the position and motion state of the vehicle subject, it achieves accurate estimation and composite localization of vehicle position, effectively handling the fusion of nonlinear systems and multi-source information, and improving the accuracy and stability of vehicle position estimation.

Description

Technical Field

[0001] This invention relates to the field of traffic condition perception technology, and in particular to a method for information fusion of multiple traffic entities for autonomous traffic systems. Background Technology

[0002] With the rapid development of emerging technologies such as artificial intelligence, the Internet of Things, and communication, Intelligent Transportation Systems (ITS) are currently transforming towards a model of integrated development among different transportation entities, evolving towards autonomy. To adapt to the emerging trend of multi-entity collaborative perception, Autonomous Transportation Systems (ATS), characterized by self-sensing, self-adaptation, self-learning, and self-organization, are becoming a new trend in future transportation system development. However, since current ITS cannot adequately meet the demands for accurate perception and efficient decision-making in future autonomous transportation environments, there is a need for breakthroughs and innovations in state fusion solutions. Further research is required on multi-entity information fusion for ATS.

[0003] Information fusion methods for multiple traffic entities utilize data from various sensors and vehicles with different levels of autonomy in the Internet of Vehicles (IoV) to comprehensively analyze data using computer vision and machine learning algorithms. This provides accurate environmental perception and real-time decision support to optimize traffic management and autonomous driving functions. In recent years, the increasing penetration rate of autonomous vehicles and the widespread application of IoV communication technology in scenarios such as vehicle-road-cloud collaboration, vehicle-to-vehicle collaboration, and vehicle-road collaboration have enabled interconnection and interoperability among multiple traffic entities, providing a technological foundation for improving their autonomy. However, due to the inconsistency in semantic expression among traffic entities with different levels of autonomy and the difficulty in adapting sensory and computing resources, effective collaboration among different traffic entities remains a challenge. Therefore, how to leverage real-time shared and interactive multi-traffic entity data information based on autonomous transportation systems, actively construct effective lightweight perception datasets, and provide data support for integrated sensory and computing networks to ultimately achieve information fusion among multiple traffic entities is a hot research area worthy of attention.

[0004] Patent document CN113077101A discloses a digital system and method for energy internet dispatch management. The energy subsystem couples the power system with natural gas and heating networks to form a centralized and distributed energy service network. The information subsystem includes an information acquisition layer, an information transmission layer, and an information processing layer. The business subsystem is used to share energy consumption data with users, conduct energy-saving monitoring and assessment, and optimize energy efficiency. The multi-flow fusion subsystem is used to fuse energy flow, information flow, and business flow, integrate the information system with the power system, natural gas system, and electrified transportation system, and coordinate multi-coupled collaborative production operations. The internal and external fusion subsystem is used to coordinate multi-source data entities with external entities for real-time data sharing and interaction. It is evident that existing traffic dataset construction technologies are overly redundant, increasing the load on industrial control computers and failing to meet real-time requirements in urban road network scenarios. Inconsistent semantic expressions among traffic entities with different levels of autonomy and difficulty in adapting synergistic computing resources further hinder the effective collaboration between different traffic entities using traditional state fusion methods, which are limited by fixed scenarios, leading to decreased data fusion efficiency. Summary of the Invention

[0005] To address this, the present invention provides a multi-traffic subject information fusion method for autonomous transportation systems, which overcomes the problem in the prior art that lacks composite positioning and state estimation of vehicle subject positions, makes it difficult to effectively guarantee the fusion of nonlinear data and multi-source information, and results in low vehicle positioning accuracy.

[0006] To achieve the above objectives, this invention provides a method for multi-transport entity information fusion in autonomous transportation systems, comprising:

[0007] Image data and point cloud data are fused together, whereby...

[0008] Based on the constructed data acquisition scenario, vehicle datasets and KITTI datasets are acquired, the datasets including image data and corresponding point cloud data;

[0009] Optimize the YOLOvx model, where,

[0010] The vehicle dataset and the KITTI dataset are subjected to lightweight processing to obtain output feature data. The image data in the output feature data is processed by multi-scale pyramid convolution to obtain the target feature map. The lightweight processing is to filter the vehicle dataset and the KITTI dataset using a lightweight convolutional neural network GhostNet and depthwise separable convolution to obtain output feature data.

[0011] Point cloud clustering detection is performed on the point cloud data in the output feature data to generate vehicle clustering detection results and obtain the target three-dimensional point cloud data of the vehicle body.

[0012] The detection boxes of the target 3D point cloud data and the target feature map are matched, the target is detected according to the matching result, and the target detection result is corrected according to the point cloud clustering detection result to output the fused detection box;

[0013] Vehicle main body state estimation, among which,

[0014] The initial position data of the vehicle body output by each sensor is obtained based on Euclidean distance;

[0015] The initial location data is updated to the target location data by estimating the location data based on the DS theory and correcting the location data according to the target adaptive factor.

[0016] The target location data is processed using an improved multi-layer UKF to obtain composite positioning data of the vehicle body.

[0017] Specifically, the noise changes of each sensor are monitored to update the error sequence and covariance matrix of the observed noise when the sensor noise changes dynamically or abnormally.

[0018] Further, performing point cloud clustering detection on the point cloud data in the output feature data includes,

[0019] The point cloud data is subjected to Voxel downsampling filtering to obtain initial point cloud data;

[0020] Perform RANSAC plane fitting on the initial point cloud data to obtain each data point and plane equation parameters;

[0021] Calculate the distance between each data point, group points whose distance is within a set threshold into the same class, and perform Euclidean clustering on the initial point cloud data to obtain the target 3D point cloud data.

[0022]

[0023] Where, x i Let x be the x-coordinate of the i-th point. j Let x be the x-coordinate of the j-th point, and y be the y-coordinate of the j-th point i Let y be the y-coordinate of the i-th point. j Let z be the y-coordinate of the j-th point, and z be the z-coordinate of the j-th point. i Let z be the z-coordinate of the i-th point. j Let D be the z-axis coordinate of the j-th point. ij The distance is Euclidean.

[0024] Furthermore, matching the target 3D point cloud data and the detection bounding boxes of the target feature map includes,

[0025] Projecting the points in the 3D detection box onto the image generates a 2D bounding box;

[0026] The intersection and union of the two two-dimensional detection boxes are calculated to obtain the matching result.

[0027] Further, target detection is performed based on the matching results, wherein,

[0028] The IoU value is calculated based on the matching results, where,

[0029]

[0030] union = S ii +S j -intersetion (3)

[0031]

[0032] Where "intersection" represents the intersection between the mapped bounding box and the 2D visual detection box, and the coordinates of the top-left corner of the bounding box are... The coordinates of the bottom right corner are The coordinates of the top left corner of the 2D visual inspection box are The coordinates of the bottom right corner are In equation (3), union represents the union of two two-dimensional detection boxes; S ii S represents the area of ​​the mapped point cloud frame. j The area of ​​the detection box in the 2D image is represented by IoU, which represents the actual correlation between the detection box in the point cloud and the image.

[0033] The IoU value is compared with a fusion threshold for target detection;

[0034] When the IoU value is less than the fusion threshold, the fused detection box is output;

[0035] Specifically, when the IoU value is greater than or equal to the fusion threshold, the point cloud clustering detection result is obtained to correct the target detection.

[0036] Furthermore, correcting the target detection result based on the point cloud clustering detection result includes:

[0037] Obtain the point cloud clustering detection results;

[0038] When the point cloud clustering detection result is not a vehicle, a corrected confidence probability is calculated based on the Sigmoid function, where,

[0039] The formula for the Sigmoid function is as follows:

[0040]

[0041] Among them, P out P is the corrected confidence probability. old The value is the confidence probability before correction, and c is the bias value. The bias value is used to ensure that the sigmoid function value is in the interval where it is greater than 0 and differentiable.

[0042] Based on the corrected confidence probability, the detection boxes of the target 3D point cloud data and the target feature map are rematched to output the fused detection boxes.

[0043] Furthermore, the initial position data of the vehicle body output by each sensor, obtained based on Euclidean distance, includes:

[0044] Calculate the Euclidean distance between the data from each sensor and normalize it to obtain a normalized distance matrix;

[0045] The confidence level is calculated based on the normalized distance matrix.

[0046] The vehicle's main body position output by each sensor with the highest confidence level is selected as the state data;

[0047] in,

[0048]

[0049]

[0050] D conf (d ij )=1-D Norm (d ij (9)

[0051] Where, θ i Let θ represent the data acquired by the i-th sensor. j Let n represent the data acquired by the j-th sensor. s d(θ) represents the number of sensors. i ) represents θ i Euclidean distance between D(d) and other data ij ) represents the distance matrix, D Norm (d ij D represents the normalized distance matrix. conf (d ij () indicates the confidence level.

[0052] Furthermore, the location data estimated based on DS theory includes,

[0053] The roadside sensing subject is extended to the state vector of the vehicle subject by extending the state vector;

[0054] The position data and motion state of the vehicle body are obtained through a nonlinear model;

[0055] in,

[0056]

[0057] in, This represents the distance between the vehicle and the roadside sensing subject at time k for the i-th vehicle. Let X represent the state vector of the i-th vehicle at time k. RPP Indicates the location of the roadside sensing subject. This represents the extended state vector of the vehicle body. Let F represent the extended state vector of the i-th vehicle body at time k+1, F be the state transition matrix, G be the control matrix, and ω be the vector. k This represents zero-mean Gaussian white noise, and its covariance matrix is ​​expressed as: I is the identity matrix, υ k To observe the noise, it is assumed that it follows a zero-mean Gaussian normal distribution with covariance R. Let represent the observed distance between the i-th vehicle and the roadside sensing object at time k+1. Represents a nonlinear observation function. This represents the position coordinates of the i-th vehicle body at time k. Set it to 0.7.

[0058] Further, correcting the location data according to the target adaptation factor includes,

[0059] Determine the objective function for fusion correction, where,

[0060] J = minσ 2 (37)

[0061] Where J represents the target of the fusion correction, σ 2 Indicates the total mean squared error;

[0062] Calculate the total mean square error, and select the adaptive factor corresponding to the minimum total mean square error as the target adaptive factor, where...

[0063]

[0064] Where, n batch W represents the maximum batch size of the time series; i Y represents the adaptive factor, μ represents the coefficient of each sensor, and Y represents the adaptive factor. iY is the vehicle's main state vector output by the DS evidence theory. i * This is the vehicle's main state vector output by IUKF;

[0065] Target location data is calculated based on the target adaptive factor and the location data, wherein,

[0066]

[0067] Among them, W i * X is the target adaptive factor. i For location data, This is the target location data.

[0068] Furthermore, processing the target location data according to the improved multi-layer UKF includes:

[0069] Calculate the initial mean and initial covariance of the vehicle's main state, where,

[0070]

[0071]

[0072] in, This represents the initial mean value of the vehicle's main body state. Let represent the state vector of the i-th vehicle at the initial moment. The initial covariance matrix representing the main state of the vehicle;

[0073] Generate 2n+3 Sigma points, where,

[0074]

[0075] Where λ satisfies n+1+λ>0, n=4 This represents the first Sigma point of the i-th vehicle body at time k. This represents the (j+1)th Sigma point of the i-th vehicle body at time k. This represents the (l+1)th Sigma point of the i-th vehicle body at time k. Represents the initial covariance matrix;

[0076] State prediction is performed on the Sigma points to obtain prediction results for k+1 Sigma points. The weight coefficients and prior covariance matrices for each Sigma point are calculated, where...

[0077]

[0078] in, Let l represent the predicted state vector corresponding to the sigma point l of the i-th vehicle body at time k. W represents the prior estimated state vector of the i-th vehicle at time k. l i This represents the weight coefficient of the state of the i-th vehicle entity after the update at the l-th sigma point. Let Q represent the prior estimate covariance of the i-th vehicle's main state at time k+1. ω The prior covariance matrix of the noise is given by the covariance matrix of the new Sigma points, which is used to update the initial covariance matrix. Thus, the prediction result and prior covariance matrix at time k+1 are obtained;

[0079] The predicted distance between the vehicle and the roadside sensing object is calculated using a nonlinear observation function, whereby...

[0080]

[0081] in, υ represents the prior estimate of the distance between the i-th vehicle and the roadside sensing subject at time k. k To observe noise;

[0082] The estimated value of the vehicle's main state is updated based on the observed value and the prior estimate to obtain the composite positioning data.

[0083] Furthermore, updating the error sequence and covariance matrix of the observation noise includes,

[0084] Obtain the state vector and the nonlinear observation function;

[0085] The error sequence is determined based on the nonlinear observation function, wherein,

[0086]

[0087] Where e represents the error sequence of the nonlinear observation function;

[0088] Calculate the covariance matrix of the noise of the nonlinear observation function, where,

[0089]

[0090] Among them, E(ee) T ) represents the noise covariance matrix of the nonlinear observation function;

[0091] The covariance of the noise of the nonlinear observation function is calculated based on the covariance matrix, where,

[0092]

[0093] Among them, E(ee) T ) represents the noise covariance of the nonlinear observation function;

[0094] The covariance matrix of the observed noise is updated, where,

[0095]

[0096] Among them, R new This represents the value obtained by updating the covariance matrix of the observation noise at time k+1.

[0097] Compared with existing technologies, the beneficial effects of this invention are as follows: By establishing an autonomous transportation system data collection scenario for urban intersections, it achieves more intelligent, efficient, and safe urban traffic management; by combining Euclidean clustering algorithm with YOLOx model to process and fuse data from different dimensions, it proposes a confidence-optimized data fusion method to achieve accurate detection of long-distance, partially occluded vehicle entities; by combining the lightweight characteristics of GhostNet and the parameter reduction effect of depthwise separable convolution, it improves the accuracy and performance of the model through multi-scale pyramid convolution and batch normalization, thereby optimizing the vehicle target detection model to achieve better accuracy and performance while maintaining high efficiency; by eliminating outliers in sensor data, it obtains accurate sensor output data, and uses the combination rules of DS evidence theory to determine the data hypothesis with the highest confidence, thereby obtaining reliable vehicle entity position data as part of the state data, effectively ensuring sensor data fusion and authenticity assessment; by extending the state vector and nonlinear system model to describe the position and motion state of the vehicle entity, it achieves accurate estimation and composite localization of vehicle position, effectively handling nonlinear systems and multi-source information fusion, and improving the accuracy and stability of vehicle position estimation. Attached Figure Description

[0098] Figure 1 This is a flowchart illustrating the multi-traffic entity information fusion method for autonomous transportation systems according to an embodiment of the present invention.

[0099] Figure 2 This is a schematic diagram of the lightweight sensing method according to an embodiment of the present invention;

[0100] Figure 3 This is a schematic diagram illustrating the normalization process performed in an embodiment of the present invention;

[0101] Figure 4 This is a schematic diagram of the voxel downsampling process according to an embodiment of the present invention;

[0102] Figure 5 This is a schematic diagram of RANSAC plane fitting according to an embodiment of the present invention;

[0103] Figure 6 This is a flowchart of the vehicle multi-source data fusion process based on confidence optimization according to an embodiment of the present invention;

[0104] Figure 7 This is a flowchart of the method for updating the observation error sequence according to an embodiment of the present invention. Detailed Implementation

[0105] To make the objectives and advantages of the present invention clearer, the present invention will be further described below with reference to embodiments; it should be understood that the specific embodiments described herein are merely for explaining the present invention and are not intended to limit the present invention.

[0106] Preferred embodiments of the present invention will now be described with reference to the accompanying drawings. Those skilled in the art should understand that these embodiments are merely illustrative of the technical principles of the present invention and are not intended to limit the scope of protection of the present invention.

[0107] It should be noted that in the description of this invention, the terms "upper", "lower", "left", "right", "inner", "outer", etc., which indicate directions or positional relationships, are based on the directions or positional relationships shown in the accompanying drawings. This is only for the convenience of description and is not intended to indicate or imply that the device or element must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, it should not be construed as a limitation of this invention.

[0108] Furthermore, it should be noted that, in the description of this invention, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.

[0109] Please see Figure 1 The diagram shown is a structural schematic of a multi-traffic subject information fusion method for autonomous transportation systems according to an embodiment of the present invention. The present invention provides a multi-traffic subject information fusion method for autonomous transportation systems, including:

[0110] Step S1 involves fusing the image data and point cloud data, where...

[0111] Step S11: Based on the constructed data acquisition scenario, obtain the vehicle dataset and the KITTI dataset, wherein the dataset includes image data and corresponding point cloud data;

[0112] Step S12, optimize the YOLOvx model, where,

[0113] The vehicle dataset and the KITTI dataset are subjected to lightweight processing to obtain output feature data. The image data in the output feature data is then processed using multi-scale pyramid convolution to obtain the target feature map.

[0114] The lightweight processing involves using a lightweight convolutional neural network, GhostNet, and depthwise separable convolutions to filter the vehicle dataset and the KITTI dataset to obtain output feature data.

[0115] Step S13: Perform point cloud clustering detection on the point cloud data in the output feature data to generate vehicle clustering detection results and obtain the target three-dimensional point cloud data of the vehicle body.

[0116] Step S14: Match the target 3D point cloud data and the detection box of the target feature map, perform target detection based on the matching result, and correct the target detection result based on the point cloud clustering detection result to output the fused detection box;

[0117] Step S2, vehicle main body state estimation, wherein,

[0118] Step S21: Obtain the initial position data of the vehicle body output by each sensor based on Euclidean distance;

[0119] Step S22: Estimate the position data according to the DS theory, and correct the position data according to the target adaptive factor to obtain the target position data, so as to update the initial position data to the target position data;

[0120] Step S23: Process the target location data according to the improved multi-layer UKF to obtain the composite positioning data of the vehicle body;

[0121] Specifically, the noise changes of each sensor are monitored to update the error sequence and covariance matrix of the observed noise when the sensor noise changes dynamically or abnormally.

[0122] Autonomous transportation systems possess the ability to evolve and improve self-sensing, self-adaptation, self-learning, self-decision-making, self-repair, and self-evolution. As a new generation of transportation systems with high intelligence, high flexibility, high adaptability, high openness, and high autonomy, it will become a new trend in the development of future transportation systems. Cooperation and collaboration among different transportation entities are the foundation for building ATS, and its realization depends on an integrated network of communication, sensing, and computing.

[0123] Vehicle-road-cloud integration technology is an intelligent transportation system that deeply integrates vehicles, road infrastructure, and cloud platforms. Through the close cooperation of on-board sensors, roadside communication units, and high-performance cloud computing centers, it enables real-time data collection, rapid processing, and efficient utilization. Vehicles can obtain real-time road conditions, pedestrian information, and vehicle information, while road infrastructure is responsible for sensing the environment, assisting in positioning, and transmitting instructions. The cloud acts as the brain, performing big data analysis, path planning, and decision support for autonomous driving strategies.

[0124] Traffic dataset construction technology integrates multi-source heterogeneous data and employs efficient data collection, cleaning, labeling, and fusion methods to generate high-quality, real-time updated traffic datasets that comprehensively and accurately reflect traffic conditions, providing fundamental support for the development, optimization, and application of intelligent transportation systems.

[0125] Traffic information fusion technology refers to a technical means of integrating, processing, and analyzing traffic data from different sources and of different types to obtain more comprehensive and accurate traffic situation perception and prediction capabilities. These multi-source heterogeneous data effectively integrate fragmented traffic data through multi-level and multi-dimensional information fusion algorithms, eliminating redundancy and improving data quality.

[0126] In step S11, an autonomous transportation system urban intersection data collection scenario is established, with any intersection as a separate sensing node, containing roadside sensing entities and vehicle entities as underlying sensing and execution units. The roadside sensing entities are equipped with sensing devices such as lidar, cameras, and GPS receivers, and are also equipped with V2X communication equipment to meet the load requirements of vehicle-to-infrastructure communication at a single intersection and communication at multiple intersections. The roadside sensing capability covers the entire intersection and extends to a 40-meter range at the intersection entrance. The communication distance coverage needs to be close to the intersection. All vehicle entities traveling in the urban road network scenario have sufficient holographic perception and precise control capabilities and can acquire lightweight data shared by the roadside sensing entities in real time.

[0127] This embodiment establishes an autonomous transportation system urban intersection data collection scenario to achieve smarter, more efficient, and safer urban traffic management. By combining Euclidean clustering with the YOLOx model, it processes and fuses data from different dimensions. A confidence-optimized data fusion method is proposed to achieve accurate detection of vehicles at long distances and with partial occlusion. The lightweight characteristics of GhostNet and the parameter reduction effect of depthwise separable convolution are combined. Multi-scale pyramid convolution and batch normalization are used to improve the model's accuracy and performance, optimizing the vehicle target detection model to maintain efficiency while achieving better accuracy and performance. By eliminating outliers in sensor data, accurate sensor output data is obtained. The combination rules of DS evidence theory are used to determine the data hypothesis with the highest confidence, thus obtaining reliable vehicle location data as part of the state data. This effectively ensures sensor data fusion and authenticity assessment. By extending the state vector and using a nonlinear system model to describe the vehicle's position and motion state, accurate estimation and composite localization of the vehicle's position are achieved. This effectively handles nonlinear systems and multi-source information fusion, improving the accuracy and stability of vehicle position estimation.

[0128] See Figure 2 As shown, it is a structural schematic diagram of the lightweight sensing method according to an embodiment of the present invention;

[0129] Specifically, the accuracy of the model is improved by combining multi-scale pyramid convolution (PyConv) and ReLU activation function. GhostNet is used as the lightweight core to modify the backbone network of YOLOx. For the input feature layer, a part of the real feature layer is first generated by standard convolution. Then, depthwise separating convolution and constant transformation are used to linearly process each feature layer to obtain another part of the feature layer. Finally, the two parts of the feature layer are concatenated to form the output feature layer of the Ghost module.

[0130] See Figure 3 As shown, it is a schematic diagram of normalization processing in an embodiment of the present invention;

[0131] Specifically, to prevent the loss of image information caused by nonlinear operations, the feature maps are batch normalized after the second Ghost module instead of using the ReLU activation function.

[0132] See Figure 4 and Figure 5 As shown, Figure 4 This is a schematic diagram of the voxel downsampling process according to an embodiment of the present invention. Figure 5 This is a schematic diagram of RANSAC plane fitting according to an embodiment of the present invention;

[0133] Specifically, in step S13, performing point cloud clustering detection on the point cloud data in the output feature data includes,

[0134] The point cloud data is subjected to Voxel downsampling filtering to obtain initial point cloud data;

[0135] Perform RANSAC plane fitting on the initial point cloud data to obtain each data point and plane equation parameters;

[0136] Calculate the distance between each data point, group points whose distance is within a set threshold into the same class, and perform Euclidean clustering on the initial point cloud data to obtain the target 3D point cloud data.

[0137]

[0138] Where, x i Let x be the x-coordinate of the i-th point, and y be the y-coordinate of the i-th point i Let z be the y-coordinate of the i-th point, and z be the z-coordinate of the i-th point. i Let D be the z-coordinate of the i-th point. ij The distance is Euclidean.

[0139] To supplement this, 3D vehicle target data acquired using LiDAR is used. Voxel downsampling filtering is applied to the point cloud to remove redundant data. The point cloud is then fitted using the Random Sample Consensus (RANSAC) algorithm to obtain spatial geometric plane equation parameters. Euclidean clustering is performed on the 3D point cloud to obtain the vehicle target's point cloud cluster data. Axis-aligned bounding boxes are then used to process the target cluster data, yielding information such as the target's position and size. The set of data points to be queried in the point cloud space is defined as P. A 3D KD-Tree object is created based on the features of P. One feature point is selected in P, and its nearest neighbor is found using a nearest neighbor search algorithm. The Euclidean distance D between points Pi and Pj is calculated. ij The three-dimensional features of the vehicle are obtained through Euclidean clustering algorithm. In this embodiment, clustering radius thresholds suitable for different scenarios are set according to the distance range between the vehicle and the roadside sensing subject, as shown in Table 1. Table 1 is a table showing the relationship between clustering radius thresholds and distance.

[0140] Table 1. Relationship between clustering radius threshold and distance.

[0141]

[0142] If the number of points in the clustered point cloud set Q is within the defined valid range, then the clustered point cloud set Q is valid. At the same time, a vehicle clustering result is generated, and the vehicle 3D point cloud data, i.e. the target 3D point cloud data, is output.

[0143] By effectively processing the 3D vehicle target data acquired by LiDAR, information such as the vehicle's position and size can be extracted, enabling effective identification and processing of vehicle targets.

[0144] See Figure 6 As shown, it is a flowchart of the vehicle multi-source data fusion based on confidence optimization according to an embodiment of the present invention;

[0145] Specifically, in step S14, matching the detection bounding boxes of the target 3D point cloud data and the target feature map includes:

[0146] Projecting the points in the 3D detection box onto the image generates a 2D bounding box;

[0147] The intersection and union of the two two-dimensional detection boxes are calculated to obtain the matching result.

[0148] The R-Tree algorithm is used to associate and match detection boxes in point clouds and images. First, a frame of image data and point cloud data, including 3D and 2D detection boxes, are input into the algorithm. Second, the i-th 3D vehicle target detection result and the j-th 2D vehicle target detection result are selected, and the initial values ​​of parameters i and j are set to 1. Third, the points in the i-th 3D detection box are projected onto the image to generate a 2D bounding box. Finally, the algorithm calculates the values ​​of the 2D bounding box and the j-th 2D detection box. By comparing the IoU value of the two boxes with a threshold, the category information of the detection result can be determined.

[0149] Specifically, target detection is performed based on the matching results, wherein...

[0150] The IoU value is calculated based on the matching results, where,

[0151]

[0152] union = S ii +S j -intersetion (3)

[0153]

[0154] Where "intersection" represents the intersection between the mapped bounding box and the 2D visual detection box, and the coordinates of the top-left corner of the bounding box are... The coordinates of the bottom right corner are The coordinates of the top left corner of the 2D visual inspection box are The coordinates of the bottom right corner are In equation (3), union represents the union of two two-dimensional detection boxes; S ii S represents the area of ​​the mapped point cloud frame. j Represents the area of ​​the detection box in a two-dimensional image;

[0155] The IoU value is compared with a fusion threshold for target detection;

[0156] When the IoU value is less than the fusion threshold, the fused detection box is output;

[0157] Specifically, when the IoU value is greater than or equal to the fusion threshold, the point cloud clustering detection result is obtained to correct the target detection.

[0158] Target detection is achieved by comparing the calculated IoU with the fusion threshold, and the results detected by YOLOx are corrected by the point cloud clustering results. If the point cloud clustering detection result is a non-vehicle, the confidence probability of the target is corrected according to the Sigmoid() function.

[0159] Specifically, in step S14, correcting the target detection result based on the point cloud clustering detection result includes:

[0160] Obtain the point cloud clustering detection results;

[0161] When the point cloud clustering detection result is not a vehicle, a corrected confidence probability is calculated based on the Sigmoid function, where,

[0162] The formula for the Sigmoid function is as follows:

[0163]

[0164] Among them, P out P is the corrected confidence probability. old The value is the confidence probability before correction, and c is the bias value. The bias value is used to ensure that the sigmoid function value is in the interval where it is greater than 0 and differentiable.

[0165] Based on the corrected confidence probability, the detection boxes of the target 3D point cloud data and the target feature map are rematched to output the fused detection boxes.

[0166] In this implementation, the IoU value between the bounding box and the visual detection box is recalculated based on the improved confidence probability function, threshold comparison and cluster detection are performed again, the label of the two-dimensional composite box is updated, and the fused detection box is output.

[0167] Specifically, in step S21, obtaining the initial position data of the vehicle body output by each sensor based on Euclidean distance includes:

[0168] Calculate the Euclidean distance between the data from each sensor and normalize it to obtain a normalized distance matrix;

[0169] The confidence level is calculated based on the normalized distance matrix.

[0170] The vehicle's main body position output by each sensor with the highest confidence level is selected as the state data;

[0171] in,

[0172]

[0173] D conf (d ij )=1-D Norm (d ij (9)

[0174] Where, θ i Let n represent the data acquired by the i-th sensor. s d(θ) represents the number of sensors. i ) represents θ i Euclidean distance between D(d) and other data ij ) represents the distance matrix, D Norm (d ij D represents the normalized distance matrix. conf (d ij () indicates the confidence level.

[0175] Outlier elimination based on Euclidean distance measures the similarity of data. The smaller the Euclidean distance between sensor data, the higher the similarity of the data and the higher the authenticity of the sensor. The smaller the distance between different data, the higher the authenticity of the data. The confidence function of data acquired by different sensors reflects the authenticity of the data. Once outliers appear in the data, the value of the confidence function will decrease and its distribution will become more dispersed. Setting a threshold is to eliminate outliers, and its value is shown in formula (10). According to formula (11), outliers in the data can be eliminated.

[0176] γ=D conf_mean ·5% (10)

[0177] D conf_mean -γ≤D conf (d ij )≤D conf_mean +γ (11)

[0178] Where γ is the anomaly threshold, D conf_mean It's D conf (d ij The mean of ) once D conf (d ij If the value is within the interval shown in formula (11), then retain D. conf (d ijAnd eliminate outliers. Therefore, eliminate outliers according to formula (6) to formula (11) to obtain the correct data output by different sensors. At the same time, initialize the confidence probability of each hypothesis through Basic Probability Assignment (BPA) and calculate the hypothesis with the highest confidence probability using the basic combination rules of DS evidence theory. The combination rules are shown in formula (12) and (13).

[0179]

[0180] Wherein, K represents the proposition in distance estimation, reflecting the confidence of the evidence in proposition A. In this embodiment, K is the degree of conflict between different sensors. The larger the coefficient K is, the more inconsistent the conclusions about distance acquisition between various data sources are. Once K=1, the data acquired by each sensor are completely opposite, the individual pieces of evidence are completely conflicting, and the combination rule fails.

[0181] Finally, the vehicle body position output by each sensor with the highest confidence level is used as one of the components of the state data.

[0182] Specifically, in step S22, estimating the location data based on the DS theory includes:

[0183] The roadside sensing subject is extended to the state vector of the vehicle subject by extending the state vector;

[0184] The position data and motion state of the vehicle body are obtained through a nonlinear model;

[0185] in,

[0186]

[0187]

[0188] in, This represents the distance between the vehicle and the roadside sensing subject at time k for the i-th vehicle. Let X represent the state vector of the i-th vehicle at time k. RPP Indicates the location of the roadside sensing subject. The extended state vector representing the main body of the vehicle is, i.e. The resulting extended vector Let F represent the extended state vector of the i-th vehicle body at time k+1, F be the state transition matrix, G be the control matrix, and ω be the vector. k This represents zero-mean Gaussian white noise, and its covariance matrix is ​​expressed as: I is the identity matrix, υ k To observe the noise, it is assumed that it follows a zero-mean Gaussian normal distribution with covariance R. Let represent the observed distance between the i-th vehicle and the roadside sensing object at time k+1. Represents a nonlinear observation function. Set it to 0.7.

[0189] In this embodiment, the roadside sensing subject is extended to the state vector of the vehicle subject, and then the actual position of the vehicle subject estimated by DS theory is used instead of GPS information as the input of IUKF. In the proposed model, the actual position of the roadside sensing subject and the state vector are nonlinear, and the position of the roadside sensing subject is extended to the state vector of the vehicle subject.

[0190] Specifically, in step S22, correcting the position data according to the target adaptive factor includes:

[0191] Determine the objective function for fusion correction, where,

[0192] J = minσ 2 (37)

[0193] Where J represents the target of the fusion correction, σ 2 Indicates the total mean squared error;

[0194] Calculate the total mean square error, and select the adaptive factor corresponding to the minimum total mean square error as the target adaptive factor, where...

[0195]

[0196] Where, n batch W represents the maximum batch size of the time series; i Y represents the adaptive factor, μ represents the coefficient of each sensor, and Y represents the adaptive factor. i Y is the vehicle's main state vector output by the DS evidence theory. i * This is the vehicle's main state vector output by IUKF;

[0197] Target location data is calculated based on the target adaptive factor and the location data, wherein,

[0198]

[0199] Among them, W i * X is the target adaptive factor. i For location data, This is the target location data.

[0200] In this embodiment, the adaptive factor with the minimum total mean square error, i.e., the target adaptive factor, is obtained through Equation 40. A fusion correction method based on the target adaptive factor is adopted as a link between DS evidence theory and the multi-layered improved UKF. To improve the synergy between distance estimation and IUKF and the stability of the system, a correction method based on the fusion target adaptive factor is proposed as a link between DS evidence theory and IUKF.

[0201] By using a fusion correction method based on target adaptive factors, information from different data sources can be effectively integrated to improve the accuracy and stability of vehicle position estimation, thereby improving the performance and reliability of intelligent connected vehicle systems.

[0202] According to the multivariate function of the extreme value theory, the adaptive factor with the smallest total mean square error can also be calculated by formula (40). The adaptive factor is multiplied by the vehicle body position output by each sensor to obtain the correction value, which is then fed back to the DS evidence theory.

[0203] Finally, the vehicle's main state estimate is updated using a Kalman filter, as shown in Equation (15-31). The fused intelligent connected vehicle state vector information is output as follows:

[0204]

[0205] in, and Let be the coordinates of the main body position of car i at time k+1. Let be the instantaneous speed of car i at time k+1. Let i be the vehicle acceleration at time k+1. Let be the heading angle of vehicle i at time k+1.

[0206] Specifically, in step S23, processing the target location data according to the improved multi-layer UKF includes:

[0207] Calculate the initial mean and initial covariance of the vehicle's main state, where,

[0208]

[0209] in, This represents the initial mean value of the vehicle's main body state. The initial covariance matrix representing the main state of the vehicle;

[0210] Generate 2n+3 Sigma points, where,

[0211]

[0212] Where λ satisfies n+1+λ>0, n=4, This represents the first Sigma point of the i-th vehicle body at time k. This represents the (j+1)th Sigma point of the i-th vehicle body at time k. This represents the (l+1)th Sigma point of the i-th vehicle body at time k. Represents the initial covariance matrix;

[0213] State prediction is performed on the Sigma points to obtain prediction results for k+1 Sigma points. The weight coefficients and prior covariance matrices for each Sigma point are calculated, where...

[0214]

[0215] in, Let l represent the predicted state vector corresponding to the sigma point l of the i-th vehicle body at time k. W represents the prior estimated state vector of the i-th vehicle at time k. l i This represents the weight coefficient of the state of the i-th vehicle entity after the update at the l-th sigma point. This represents the prior estimate covariance of the i-th vehicle's main state at time k+1. The prior covariance matrix of the new Sigma point is used to update the initial covariance matrix. Thus, the prediction result and prior covariance matrix at time k+1 are obtained;

[0216] The predicted distance between the vehicle and the roadside sensing object is calculated using a nonlinear observation function, whereby...

[0217]

[0218] in, This represents the prior estimate of the distance between the i-th vehicle and the roadside sensing subject at time k.

[0219] The estimated value of the vehicle's main state is updated based on the observed value and the prior estimate to obtain the composite positioning data.

[0220] In the unscented transformation, the number of Sigma points increases to 2(n+1)+1=2n+3. In the extended composite positioning model, only position and distance information are considered, and velocity and heading angle are not considered. Therefore, n is set to 4.

[0221] The estimation results of the covariance matrix between the vehicle's main state vectors at different times are shown in Equation (25), and the estimation results of the covariance matrix between the vehicle's main state vector and the observation vector are shown in Equation (26).

[0222]

[0223] in, Let represent the prior estimate of the covariance between the i-th vehicle body state and the (k+1)-th vehicle body state. Q represents the prior estimate of the covariance between the i-th vehicle state, the (k+1)-th vehicle state, and the distance between the vehicle and the roadside sensing subject. dd This represents the process noise covariance matrix between the state vectors of different vehicle entities.

[0224] Finally, the vehicle's main state estimate is updated using a Kalman filter, as shown in Equation (27). The distance observation at time k+1 is shown in Equation (28).

[0225]

[0226]

[0227] in, Let K be the Kalman gain of the i-th vehicle body at time k+1; This represents the state covariance of the i-th vehicle at time k+1; Let represent the state vector of the i-th vehicle body at time k+1.

[0228] At the k-th time point, a sampling strategy is used to select N sampling points to obtain the expected value and variance of the sampling point set. Then, after updating the vehicle main state in the upper-layer UKF, the weights of the lower-layer UKF are calculated according to the PF principle and normalized. This process is represented by formula (29).

[0229]

[0230] in, This represents the average weight of the fusion of states in the upper-level UKF. This represents the covariance weights of the fusion of states in the upper-level UKF.

[0231] The initial values ​​of the estimated values ​​are shown in Equation (30), and the covariance of the lower-level UKF at time k+1 is shown in Equation (30).

[0232]

[0233] in, Let represent the mean of the main state of the i-th vehicle at time k+1; Let Q represent the covariance of the i-th vehicle's main state at time k+1. ω Let be the covariance matrix of the noise. The subsequent steps of the lower-level UKF are the same as those of the upper-level UKF based on Equations (19) to (26).

[0234] By combining the advantages of particle filtering and Kalman filtering, this multi-layer unscented Kalman filtering method achieves composite estimation and state update of the vehicle's main position. Steps such as sampling strategy, weight calculation, and covariance update help improve the accuracy and stability of the estimation, making the vehicle position estimation more reliable and accurate.

[0235] See Figure 7 The diagram shows a flowchart of the observation error sequence update method according to an embodiment of the present invention. Because sensor noise at urban intersections varies over time, when the covariance matrix of the observation noise undergoes dynamic or abnormal changes, it becomes impossible to accurately estimate the state vector and its covariance of the vehicle, which severely impacts the positioning accuracy of the vehicle. Therefore, an observation error sequence update method is designed to address the aforementioned problem.

[0236] The error sequence is determined according to the observation equation (16) and is expressed by formula (32).

[0237]

[0238] Where e represents the error sequence of the observation equation. Substituting formula (32) into formula (16), the error sequence obtained by combining the vehicle main observation equation is expressed as formula (33).

[0239] Specifically, updating the error sequence and covariance matrix of the observation noise includes,

[0240] Obtain the state vector and the nonlinear observation function;

[0241] The error sequence is determined based on the nonlinear observation function, wherein,

[0242]

[0243] Where e represents the error sequence of the nonlinear observation function;

[0244] Calculate the covariance matrix of the noise of the nonlinear observation function, where,

[0245]

[0246] Among them, E(ee) T ) represents the noise covariance matrix of the nonlinear observation function;

[0247] The covariance of the noise of the nonlinear observation function is calculated based on the covariance matrix, where,

[0248]

[0249] Among them, E(ee) T) represents the noise covariance of the nonlinear observation function;

[0250] The covariance matrix of the observed noise is updated, where,

[0251]

[0252] Among them, R new This represents the value obtained by updating the covariance matrix of the observation noise at time k+1.

[0253] This observation error sequence update method can respond promptly to the dynamic changes in sensor noise at urban intersections based on the update of the error sequence of the observation equation and the noise covariance matrix, thereby improving the accurate estimation of the vehicle's main state vector and thus improving the positioning accuracy of the vehicle.

[0254] The technical solution of the present invention has been described above with reference to the preferred embodiments shown in the accompanying drawings. However, it will be readily understood by those skilled in the art that the scope of protection of the present invention is obviously not limited to these specific embodiments. Without departing from the principles of the present invention, those skilled in the art can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will all fall within the scope of protection of the present invention.

[0255] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for information fusion among multiple traffic entities in an autonomous transportation system, characterized in that, include, Step S1 involves fusing the image data and point cloud data, where... Step S11: Based on the constructed data acquisition scenario, obtain the vehicle dataset and the KITTI dataset, wherein the dataset includes the image data and the corresponding point cloud data; Step S12, optimize the YOLOvx model, where, The vehicle dataset and the KITTI dataset are subjected to lightweight processing to obtain output feature data. The image data in the output feature data is then processed using multi-scale pyramid convolution to obtain the target feature map. The lightweight processing involves using a lightweight convolutional neural network, GhostNet, and depthwise separable convolutions to filter the vehicle dataset and the KITTI dataset to obtain output feature data. Step S13: Perform point cloud clustering detection on the point cloud data in the output feature data to generate vehicle clustering detection results and obtain the target three-dimensional point cloud data of the vehicle body. Step S14: Match the target 3D point cloud data and the detection box of the target feature map, perform target detection based on the matching result, and correct the target detection result based on the point cloud clustering detection result to output the fused detection box; In step S14, correcting the target detection result based on the point cloud clustering detection result includes: Obtain the point cloud clustering detection results; When the point cloud clustering detection result is not a vehicle, a corrected confidence probability is calculated based on the Sigmoid function, where, The formula for the Sigmoid function is as follows: Among them, P out P is the corrected confidence probability. old The value is the confidence probability before correction, and c is the bias value. The bias value is used to ensure that the sigmoid function value is in the interval where it is greater than 0 and differentiable. The detection boxes of the target 3D point cloud data and the target feature map are rematched based on the corrected confidence probability to output the fused detection boxes; Step S2, vehicle main body state estimation, wherein, Step S21: Obtain the initial position data of the vehicle body output by each sensor based on Euclidean distance; In step S21, obtaining the initial position data of the vehicle body output by each sensor based on Euclidean distance includes: Calculate the Euclidean distance between the data from each sensor and normalize it to obtain a normalized distance matrix; The confidence level is calculated based on the normalized distance matrix. The vehicle's main body position output by each sensor with the highest confidence level is selected as the state data; in, D conf (d ij )=1-D Norm (d ij ) (9) Where, θ i Let θ represent the data acquired by the i-th sensor. j Let n represent the data acquired by the j-th sensor. s d(θ) represents the number of sensors. i ) represents θ i Euclidean distance between D(d) and other data ij ) represents the distance matrix, D Norm (d ij D represents the normalized distance matrix. conf (d ij () indicates the confidence level; Step S22: Estimate the position data according to the DS theory, and correct the position data according to the target adaptive factor to obtain the target position data, so as to update the initial position data to the target position data; In step S22, correcting the location data according to the target adaptive factor includes: Determine the objective function for fusion correction, where, J=minσ 2 (37) Where J represents the target of the fusion correction, σ 2 Indicates the total mean squared error; Calculate the total mean square error, and select the adaptive factor corresponding to the minimum total mean square error as the target adaptive factor, where... Where, n batch W represents the maximum batch size of the time series; i Y represents the adaptive factor. i Y is the vehicle's main state vector output by the DS evidence theory. i * The vehicle's main state vector is output by IUKF, and μ represents the coefficient of each sensor. Target location data is calculated based on the target adaptive factor and the location data, wherein, Among them, W i * X is the target adaptive factor. i For location data, For target location data; Step S23: Process the target location data according to the improved multi-layer UKF to obtain the composite positioning data of the vehicle body; Specifically, the noise changes of each sensor are monitored to update the error sequence and covariance matrix of the observed noise when the sensor noise changes dynamically or abnormally.

2. The multi-transportation entity information fusion method for autonomous transportation systems according to claim 1, characterized in that, In step S13, performing point cloud clustering detection on the point cloud data in the output feature data includes, The point cloud data is subjected to Voxel downsampling filtering to obtain initial point cloud data; Perform RANSAC plane fitting on the initial point cloud data to obtain each data point and plane equation parameters; Calculate the distance between each data point, group points whose distance is within a set threshold into the same class, and perform Euclidean clustering on the initial point cloud data to obtain the target 3D point cloud data. Among them, x i Let x be the x-coordinate of the i-th point. j Let x be the x-coordinate of the j-th point, and y be the y-coordinate of the j-th point. i Let y be the y-coordinate of the i-th point. j Let z be the y-coordinate of the j-th point. i Let z be the z-axis coordinate of the i-th point. j Let D be the z-axis coordinate of the j-th point. ij The distance is Euclidean.

3. The multi-transportation entity information fusion method for autonomous transportation systems according to claim 1, characterized in that, In step S14, matching the detection bounding boxes of the target 3D point cloud data and the target feature map includes: Projecting the points in the 3D detection box onto the image generates a 2D bounding box; The intersection and union of the two two-dimensional detection boxes are calculated to obtain the matching result.

4. The multi-transportation entity information fusion method for autonomous transportation systems according to claim 3, characterized in that, Target detection is performed based on the matching results, wherein... The IoU value is calculated based on the matching results, where, union=S ii +S j -intersetion (3) Where "intersection" represents the intersection between the mapped bounding box and the 2D visual detection box, and the coordinates of the top-left corner of the bounding box are... The coordinates of the bottom right corner are The coordinates of the top left corner of the 2D visual inspection box are The coordinates of the bottom right corner are In equation (3), union represents the union of two two-dimensional detection boxes; S ii S represents the area of ​​the mapped point cloud frame. j The area of ​​the detection box in the 2D image is represented by IoU, which represents the actual correlation between the detection box in the point cloud and the image. The IoU value is compared with a fusion threshold for target detection; When the IoU value is less than the fusion threshold, the fused detection box is output; Specifically, when the IoU value is greater than or equal to the fusion threshold, the point cloud clustering detection result is obtained to correct the target detection.

5. The multi-transportation entity information fusion method for autonomous transportation systems according to claim 1, characterized in that, In step S22, estimating location data based on DS theory includes, The roadside sensing subject is extended to the state vector of the vehicle subject by extending the state vector; The position data and motion state of the vehicle body are obtained through a nonlinear model; in, in, This represents the distance between the vehicle and the roadside sensing subject at time k for the i-th vehicle. Let X represent the state vector of the i-th vehicle at time k. RPP Indicates the location of the roadside sensing subject. This represents the extended state vector of the vehicle body. Let F represent the extended state vector of the i-th vehicle body at time k+1, F be the state transition matrix, G be the control matrix, and ω be the vector. k This represents zero-mean Gaussian white noise, and its covariance matrix is ​​expressed as: I is the identity matrix, υ k To observe the noise, it is assumed that it follows a zero-mean Gaussian normal distribution with covariance R. Let represent the observed distance between the i-th vehicle and the roadside sensing object at time k+1. Represents a nonlinear observation function. This represents the position coordinates of the i-th vehicle body at time k. Set it to 0.

7.

6. The multi-transportation entity information fusion method for autonomous transportation systems according to claim 1, characterized in that, In step S23, processing the target location data according to the improved multi-layer UKF includes: Calculate the initial mean and initial covariance of the vehicle's main state, where, in, This represents the initial mean value of the vehicle's main body state. Let represent the state vector of the i-th vehicle at the initial moment. The initial covariance matrix representing the main state of the vehicle; Generate 2n+3 Sigma points, where, Where λ satisfies n+1+λ>0, n=4, This represents the first Sigma point of the i-th vehicle body at time k. This represents the (j+1)th Sigma point of the i-th vehicle body at time k. This represents the (l+1)th Sigma point of the i-th vehicle body at time k. Represents the initial covariance matrix; State prediction is performed on the Sigma points to obtain prediction results for k+1 Sigma points. The weight coefficients and prior covariance matrices for each Sigma point are calculated, where... in, Let l represent the predicted state vector corresponding to the sigma point l of the i-th vehicle body at time k. W represents the prior estimated state vector of the i-th vehicle at time k. l i This represents the weight coefficient of the state of the i-th vehicle entity after the update at the l-th sigma point. Let G represent the prior estimate covariance of the i-th vehicle's main state at time k+1, and let G be the control matrix. ω The prior covariance matrix of the noise is given by the covariance matrix of the new Sigma points, which is used to update the initial covariance matrix. Thus, the prediction result and prior covariance matrix at time k+1 are obtained, and F is the state transition matrix; The predicted distance between the vehicle and the roadside sensing object is calculated using a nonlinear observation function, whereby... in, υ represents the prior estimate of the distance between the i-th vehicle and the roadside sensing subject at time k. k To observe noise; The estimated value of the vehicle's main state is updated based on the observed value and the prior estimate to obtain the composite positioning data.

7. The multi-transportation entity information fusion method for autonomous transportation systems according to claim 6, characterized in that, The error sequence and covariance matrix for updating the observation noise include, Obtain the state vector and the nonlinear observation function; The error sequence is determined based on the nonlinear observation function, wherein, Where e represents the error sequence of the nonlinear observation function, and h() represents the nonlinear observation function; Calculate the covariance matrix of the noise of the nonlinear observation function, where, Among them, E(ee) T ) represents the noise covariance matrix of the nonlinear observation function; The covariance of the noise of the nonlinear observation function is calculated based on the covariance matrix, where, Among them, E(ee) T ) represents the noise covariance of the nonlinear observation function; The covariance matrix of the observed noise is updated, where, Among them, R new This represents the value obtained by updating the covariance matrix of the observation noise at time k+1.

Images