Bayesian data fusion vehicle positioning method and system based on Kalman filter

Abstract

The invention discloses a Bayesian data fusion vehicle positioning method and system based on a Kalman filter. The method comprises the following steps: firstly, preprocessing positioning data acquired by a vehicle sensor positioning system; carrying out forward filtering on the preprocessed data through a Kalman filter; inputting the filtered data into a Bayesian fusion module for data fusion; and finally, carrying out backward filtering output on the fused data through a Kalman filter. By combining the Bayesian estimation with the Kalman filter, the method can dynamically process the data abnormity while denoising and estimating the sensor data, and improves the precision and robustness of vehicle positioning.

Description

Technical Field

[0001] The present invention relates to the field of vehicle positioning technology, and in particular to a vehicle positioning method and system based on multi-sensor data fusion, which specifically combines a Kalman filter and a Bayesian estimation to improve the positioning accuracy and reliability of an autonomous driving vehicle in a complex environment. Background Art

[0002] The continuous development of autonomous driving technology has made vehicle positioning one of the core elements affecting the safety and reliability of the system. However, in actual application scenarios, the environment around the vehicle is complex and changeable, such as densely populated areas with high-rise buildings in cities, tunnels, and bad weather, which will have a great impact on the accuracy of vehicle positioning. These environmental interferences usually limit the performance of a single sensor (such as GPS, lidar, or camera), which in turn affects the positioning accuracy of the vehicle. Therefore, how to use multi-sensor data fusion to achieve high-precision and stable vehicle positioning is an important challenge in current research and engineering applications. Summary of the invention

[0003] The purpose of the present invention is to solve the uncertainty and inconsistency of the data of each sensor node in the sensor network data fusion, especially to ensure the high accuracy and stability of vehicle positioning in complex environments such as urban high-rise buildings and tunnels. To this end, a data fusion vehicle positioning method and system based on Kalman filter and Bayesian estimation is proposed.

[0004] A first aspect of the present invention provides a vehicle positioning method based on a Bayesian data fusion of a Kalman filter, the method comprising the following steps:

[0005] Preprocessing the positioning data collected by the vehicle sensor positioning system;

[0006] The preprocessed data is forward filtered through a Kalman filter;

[0007] The filtered data enters the Bayesian fusion module for data fusion;

[0008] The fused data is back-filtered and output through a Kalman filter.

[0009] A second aspect of the present invention provides a Bayesian data fusion vehicle positioning system based on a Kalman filter, comprising:

[0010] A data preprocessing module, used to preprocess the positioning data collected by the vehicle sensor positioning system;

[0011] A forward filtering module is used to perform forward filtering on the preprocessed data through a Kalman filter;

[0012] A data fusion module is used to input the filtered data into the Bayesian fusion module for data fusion;

[0013] The backward filtering module is used to perform backward filtering and output of the fused data through a Kalman filter.

[0014] The third aspect of the present invention provides a Bayesian data fusion vehicle positioning device based on a Kalman filter, comprising a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, they are used to implement a Bayesian data fusion vehicle positioning method based on a Kalman filter.

[0015] According to a fourth aspect of the present invention, a computer-readable storage medium is provided, on which a program is stored. When the program is executed by a processor, a Bayesian data fusion vehicle positioning method based on a Kalman filter is implemented.

[0016] Beneficial effects of the present invention: The present invention proposes a vehicle positioning method and system based on a combination of a Kalman filter and a Bayesian estimation, which is intended to cope with the uncertainty, inconsistency and complexity of the external environment in multi-sensor data. By combining the Bayesian estimation with the Kalman filter, the present invention can not only denoise and estimate the sensor data, but also dynamically handle data anomalies to improve the accuracy and robustness of vehicle positioning. Furthermore, the sensor data fusion of the present application also takes into account the reliability of the sensor, that is, dynamically allocates weights according to the reliability of each sensor, and fuses to obtain preliminary location information, and the weights are determined by the difference between the measured values ​​and their respective variances. The weight ratio of the sensors reflects their relative reliability in state estimation. When the difference between the measured values ​​of two sensors is large, the sensor with a smaller difference will be given a higher weight. The Bayesian method described in the present application can very effectively identify the inconsistency of sensor data, thereby reflecting the true state of the measured value. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 This is a flow chart of a method according to an embodiment of the present application.

[0018] Figure 2 This is the flow chart of the multi-sensor data fusion algorithm based on Kalman filter.

[0019] Figure 3 This is a system structure diagram of an embodiment of the present application. DETAILED DESCRIPTION

[0020] In order to make the purpose and technical solution of the embodiment of the present invention more clear, the technical solution of the embodiment of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of the embodiment of the present invention.

[0021] like Figure 1 As shown, the embodiment of the present application proposes a data fusion vehicle positioning method based on Kalman filter and Bayesian estimation. First, the positioning data collected by the vehicle sensor positioning system is preprocessed; then the preprocessed data is forward filtered through the Kalman filter; then the filtered data is input into the Bayesian fusion module for data fusion; finally, the fused data is backward filtered and output through the Kalman filter to ensure the high accuracy and stability of the vehicle positioning information. The specific steps are as follows:

[0022] Step 1. Data preprocessing

[0023] First, the original positioning data collected by the vehicle sensors is obtained, including data from sensors such as GPS, IMU and LiDAR. These data may be affected by noise and external environment during the collection process, so they need to be preprocessed. Through the preprocessing step, obvious noise and outliers in the data can be removed, thereby improving the accuracy of subsequent processing.

[0024] Step 2. Forward filtering

[0025] The pre-processed data is forward filtered through the Kalman filter. In this process, the Kalman filter uses the system state equation and observation equation to predict and update the sensor data, thereby filtering out noise and interference and obtaining preliminary denoised data. This step ensures that the positioning data of each sensor is more accurate and reliable, laying the foundation for subsequent data fusion.

[0026] Step 3. Data fusion

[0027] The denoised data enters the Bayesian fusion module. In this module, the posterior probability of each sensor is calculated, and the weight is dynamically assigned according to the reliability of each sensor, and the initial location information is obtained by fusion. The role of Bayesian estimation is to adjust the contribution of each sensor in the fusion process according to its current measurement accuracy, so as to make the fusion result more reliable. Especially when there is inconsistency in some sensor data, Bayesian estimation can effectively reduce the impact of unreliable data on the final positioning result.

[0028] Step 4. Post-filtering

[0029] The fused preliminary position information is smoothed by a backward Kalman filter. The purpose of backward filtering is to further eliminate residual measurement errors and ensure that the final vehicle positioning information has high accuracy and stability. Through backward filtering, the data can be further optimized in time series, making the vehicle positioning results smoother and more continuous, which is suitable for high-precision application scenarios such as autonomous driving.

[0030] In one example, the method of preprocessing the vehicle positioning data collected by the vehicle multi-sensor positioning system in step (1) comprises the following steps:

[0031] S1. Data verification: Check the consistency and validity of the data from each sensor. By calculating the deviation of each data point, use the 3-sigma rule to determine whether there are any abnormal points. If the deviation of a data point exceeds three times the standard deviation, it is considered as abnormal data and removed. This method helps detect and remove abnormal data points caused by sensor failure, data loss or external interference, thereby improving the overall quality and reliability of the data.

[0032] S2. Time synchronization: Time synchronization processing is performed on data from different sensors. Assuming that the data collected by each sensor has different timestamps, these data need to be aligned to the same time axis for fusion processing. To this end, linear interpolation is used to interpolate the data:

[0033]

[0034] In the formula, and are two adjacent timestamps, is the target time point.

[0035] This method is used to estimate the data value at the target time point and ensure that the data from different sensors are fused at the same timestamp, thus avoiding position errors caused by time asynchrony.

[0036] S3. Coordinate transformation: coordinate transformation of data from different sensors to facilitate subsequent data fusion. For example, GPS and IMU data are usually in different coordinate systems and need to be unified into the vehicle coordinate system for effective fusion. To this end, the homogeneous coordinate transformation formula is needed:

[0037]

[0038] In the formula, is the coordinate in the IMU coordinate system, It is the transformation matrix from the IMU coordinate system to the vehicle coordinate system.

[0039] The coordinate transformation process needs to consider the installation position and direction deviation of the sensor. Each sensor installation will have a certain error, which can be measured through calibration experiments and compensated into the coordinate transformation matrix.

[0040] In one example, the step (2) of forward filtering the preprocessed data through a Kalman filter is performed using the following steps:

[0041] S1. State prediction: First, based on the preprocessed sensor data, a state transition model of the multi-sensor system is constructed to predict the state at the current moment to obtain the prior state at the next moment. The state transition model is usually expressed as:

[0042]

[0043] In the formula, represents the predicted prior state estimate, is the state transfer matrix, which represents the state change relationship of the system from the previous moment to the current moment. is the control input matrix, For control input.

[0044] The purpose of this step is to make a preliminary prediction of the system state based on the state at the previous moment and the current control input.

[0045] S2. Covariance prediction: Based on the state prediction, the uncertainty of the predicted state is further described and the error covariance matrix is ​​calculated. The prediction formula is as follows:

[0046]

[0047] In the formula, represents the predicted covariance matrix, is the process noise covariance matrix, which describes the inherent uncertainty of the system. The covariance prediction step is used to measure the reliability and accuracy of the system state prediction.

[0048] S3. Kalman gain calculation: The calculation of Kalman gain is the core of the entire filtering process, which determines the weighted ratio of the predicted state and the observed data in the state update. The calculation is as follows:

[0049]

[0050] In the formula, is the Kalman gain, is the observation matrix, which represents the mapping relationship from the state vector to the observation vector. is the observation noise covariance matrix.

[0051] The role of the Kalman gain is to determine the relative weight between the observed data and the predicted state, thereby making the state estimation more accurate.

[0052] S4. State update: According to the observed data and Kalman gain, the prior state is corrected to obtain the posterior state estimate. The state update formula is as follows:

[0053]

[0054] In the formula, is the observed value, To predict the observed value, Represents the residual between the predicted observation and the actual observation.

[0055] Through state updating, the system combines predicted information and actual observed information to obtain a more accurate state estimate.

[0056] S5. Covariance update: The last step is to update the error covariance matrix to reflect the uncertainty of the system after the state update. The formula for covariance update is as follows:

[0057]

[0058] in, is the identity matrix, represents the updated error covariance matrix.

[0059] This matrix is ​​used to describe the accuracy of the current state estimation. The smaller the covariance, the more accurate the state estimation.

[0060] In one example, step (3) uses an improved Bayesian data fusion algorithm. Bayesian reasoning is a statistical data fusion algorithm based on the conditional or posterior probability of Bayes' theorem, which can estimate the unknown n-dimensional state vector X through the known vector Z.

[0061] Simplified Bayesian (SB) representation: Assuming a state space, the Bayesian estimator provides a method to calculate the posterior (conditional) probability distribution, the probability at time k is , k groups of measurements are known And the prior distribution is as follows:

[0062]

[0063] in is the likelihood function - based on a given sensor measurement model; is the prior distribution function - the model of the given transformation system; the denominator It is just a normalization term to prove that the probability density function is normalized.

[0064] Probability density function Describes the probabilistic information of Z about X, which is a sensor-dependent objective function based on observations. If the available information about the state variable X is independent of previous observations, the likelihood function can be used to improve it to provide more accurate results. If the prior information about the variable X is encapsulated as a prior probability, it is not based on observed data, which is subjective.

[0065] Due to the uncertainty caused by noise, the information provided by the sensor is usually modeled as an average value that is close to the true value, and the uncertainty of the noise is expressed according to the variance of the measurement and the operating parameters of the sensor. Probabilistic sensor models make it easier to determine the statistical characteristics of the acquired data. When the measured quantity X of the state is known, this probabilistic model can obtain the probability distribution of the sensor Z. This distribution is specific to a specific sensor node and can be determined experimentally. Gaussian distribution is the most commonly used distribution to represent sensor uncertainty, and the formula is as follows:

[0066]

[0067] in Representative Therefore, if there are two sensor nodes modeled using equation (9), then according to Bayesian theory, the fusion mean of the two sensors can be obtained by the maximum a posteriori probability estimation (MAP):

[0068]

[0069] in and are the standard deviations of sensors 1 and 2 respectively, from which the fusion variance is obtained:

[0070]

[0071] Sensors often provide false data due to factors such as sensor failure, inherent limitations, and complex environments. However, the simplified Bayesian method (SB) described above cannot effectively handle false data. This method produces the same weighted average from equation (10) and does not consider whether the data from each sensor is true. The posterior probability distribution always has a smaller variance than any single distribution. The simplified Bayesian method does not have a mechanism to determine whether the data from certain sensors is false, and may therefore lead to incorrect estimates.

[0072] In order to solve the above problems, an improved Bayesian method (MB) is proposed in this example, which takes into account the inconsistency of measurement. Figure 2 As shown:

[0073] S1. Establish a system model: First, establish a comprehensive system model, including the sensor noise model, state space model, and the collaborative relationship between sensors.

[0074] There are three main types of sensors used in the system: GPS sensor, IMU (Inertial Measurement Unit) sensor and LIDAR sensor. Each sensor has its own specific noise characteristics when collecting data, which will affect the accuracy of the sensor output data. Therefore, in the fusion process, these noises must be taken into account and weighted according to the measurement error of each sensor.

[0075] Furthermore, the GPS sensor receives multiple satellite signals to calculate the distance to the satellite, thereby determining the vehicle's geographic location. The measurement error of GPS is usually assumed to be Gaussian, and the noise standard deviation is Reflects its accuracy. The accuracy of GPS signals will be affected by environmental factors (such as tall buildings, weather, etc.), resulting in a position error usually between 1 and 10 meters. In the GPS sensor model, the measurement error of GPS follows a normal distribution, and its probability density function can be expressed as:

[0076]

[0077] Furthermore, the IMU sensor measures acceleration, angular velocity and other information through accelerometers and gyroscopes to help estimate the vehicle's speed, position and attitude. The measurement error of the IMU mainly comes from random drift and bias. Over time, the error will accumulate, resulting in long-term accuracy degradation. The noise of the IMU is also assumed to be Gaussian distributed, and its noise standard deviation is Represents the measurement error of acceleration and angular velocity. The noise characteristics of IMU usually include random walk noise and zero bias error, which need to be considered during data fusion. The probability density function of IMU is expressed as:

[0078]

[0079] Furthermore, the LIDAR sensor calculates the distance between the object and the sensor by emitting a laser beam and measuring the time it takes for the laser to be reflected back, thereby generating high-precision 3D point cloud data. The measurement error of LIDAR also usually conforms to the Gaussian distribution, and its noise standard deviation is It will increase with the change of measurement distance. At close distance, the measurement error of LIDAR is small, while at long distance, the error will increase. The measurement error of LIDAR can also be expressed by a normal distribution model, and its probability density function is:

[0080]

[0081] By modeling the noise of each sensor, an error distribution is defined for each sensor's measurement. These noise models provide the basis for subsequent Kalman filtering and Bayesian data fusion. The standard deviation of the noise for each sensor (e.g. , , ) are adjusted according to the actual sensor performance and usage environment. The fusion algorithm will weight the sensor data according to these noise models to ensure that the final state estimate is as accurate as possible.

[0082] S2. Simulating sensor noise input: In this step, the noise input from different sensors is simulated so that the measurement error of each sensor can be taken into account during the data fusion process. This is done by specifying the standard deviation for each sensor. , the noise of sensor outputs can be simulated and their impact on state estimation can be adjusted in subsequent fusion.

[0083] S3. According to Estimation of the measured value at time State value at time: This step is based on Sensor measurement value at time , estimated The vehicle status value at the moment And the fused state estimate In order to make the state estimation as accurate as possible, the Kalman filter algorithm is used to predict the state of the vehicle.

[0084] S4. Estimation State value variance at time: At this stage, it is necessary to The variance of the state estimate at the moment is calculated to describe the uncertainty of the state estimate. The state variance of each sensor Indicates the accuracy of the predicted state and the variance after fusion It represents the accuracy after fusion of multiple sensor data.

[0085] S5. Calculate weight ratio: Calculate the weight of each sensor according to the difference between the sensor measurements and their respective variances.

[0086] The weight ratio of sensors reflects their relative reliability in state estimation. When the difference between the measurements of two sensors is large, the sensor with smaller difference will be given a higher weight. In the improved Bayesian method, the fused state estimate depends on the weighted values ​​of the sensors, which are calculated based on the difference and variance between the measurements. Formula (15) represents the probability density function of the fusion of two sensor data. This formula combines the noise level and measurement difference of the sensor to calculate the weight of each sensor data.

[0087]

[0088]

[0089] As can be seen in formula (15), this improvement leads to the variance of individual distribution and factor Proportional, factor It is given by the following formula (16):

[0090]

[0091] parameter is the maximum expected difference in sensor values. The larger the difference in sensor measurements, the greater the factor The larger the value, the greater the variance. The maximum a posteriori probability estimate (MAP) of remains unchanged, but the variance of the fused posterior probability distribution changes.

[0092] Therefore, depending on the squared difference of the two sensor measurements compared to a single Gaussian distribution, the variance of the posterior probability distribution may increase or decrease, and the improved Bayesian method can very effectively identify inconsistencies in sensor data, thereby reflecting the true state of the measurements.

[0093] S6. Loop through each sensor data: In this step, the algorithm performs weighted processing on the data of each sensor. The weight of the sensor is calculated in the previous step, and on this basis, the filtered state estimate and variance is calculated. New sensor data is processed each loop in order to continuously optimize the state estimate.

[0094] S7. Apply filters to the data before fusion: This step applies filters to the raw sensor data before fusion to remove possible noise and inaccurate data. This step ensures that only high-quality data is used for the final fusion, thereby improving the accuracy of the final positioning result.

[0095] S8. Calculate the fusion state: This step fuses the filtered data. According to the weight calculation above, the fusion state The weighted data from each sensor is combined to obtain the optimal state estimate. This is an important step because it ensures that the data from all sensors can be effectively fused according to their reliability to obtain the optimal state estimate.

[0096] S9. Apply filters to the fused data: The last step is to further filter the fused data to ensure that the fused state estimate has high accuracy and stability. The filtering process will eliminate any residual errors in the data fusion process, thereby further optimizing the positioning results.

[0097] In one example, the preliminary position information obtained by fusion in step (4) is back-filtered through a Kalman filter to further improve the smoothness and accuracy of the data. The following specific steps are used for back-filtering:

[0098] S1. Reverse state prediction: First, the state is predicted from the back to the front in the time series, combined with the preliminary estimate obtained by the forward filtering process, and corrected using the reverse state transfer model. The state prediction formula is:

[0099]

[0100] in, represents the inverse matrix of the state transfer matrix, is the estimated value of the state at the next moment.

[0101] Inverse state prediction gradually corrects the system state from back to front in the time series to reduce the impact of cumulative errors. In complex environments of autonomous driving, such as dense urban areas or tunnels, forward filtering may accumulate certain errors due to various noise interferences. Through inverse state prediction, the state information at the end of the time series can be effectively used to improve the current state estimation, thereby enhancing the robustness and stability of the entire system.

[0102] S2. Covariance smoothing: Covariance calculation is performed on the uncertainty in the inverse state prediction, and the error covariance matrix is ​​smoothed. The covariance smoothing calculation is used to ensure that the error is minimized in the reverse direction of the time series and reduce the impact of measurement errors.

[0103]

[0104] in, is the covariance matrix after backward smoothing, reflecting the accuracy after the state update, and is the Kalman gain and observation matrix in forward filtering.

[0105] The purpose of covariance smoothing is to further reduce the uncertainty of state estimation in inverse prediction. By processing the covariance matrix, the system can effectively reduce the uncertainty caused by sensor noise or environmental complexity, making the final state estimation more stable and accurate. Especially in the case of multiple observation error accumulation, covariance smoothing can significantly reduce error propagation and ensure that the system output is consistent and has a high degree of confidence.

[0106] S3. Reverse Kalman gain calculation: The calculation of the reverse Kalman gain is similar to the forward filtering process, but in the opposite direction, and is used to further smooth and correct the forward estimation results.

[0107]

[0108] in, It is the reverse Kalman gain, and its main function is to combine the error between the reverse prediction and the forward observation to achieve the final positioning smoothness.

[0109] In actual environments, the trust of different sensors may be different, especially when some sensor data has large noise or the environment is complex. The inverse Kalman gain can ensure that the observed data and prediction results are effectively weighted during the state update process, and finally obtain the optimal state estimate. The dynamic characteristics of this gain calculation enable the system to flexibly adjust the trust of different data sources in different scenarios, thereby improving the overall robustness and stability.

[0110] S4. State update: The state update combines the information of the reverse prediction and the observation data, and ensures that the final state estimate is closer to the actual state by correcting the prediction residual.

[0111]

[0112] in, is the observed value, reflecting the difference between forward and reverse filtering, so as to make a bidirectional correction to the state estimate in combination with the actual observation information. This step is crucial to the final effect of the filtering process. Through state updating, the system can minimize the difference between the prediction and the actual observation, thereby obtaining a more accurate state estimate. Especially in the application scenario of vehicle positioning, state updating can ensure that the error accumulation is gradually reduced in different time steps, ensuring the high accuracy and high stability of the system.

[0113] Through the above steps, backward filtering not only effectively combines the preliminary results of forward filtering and data fusion, but also ensures that the vehicle positioning results remain smooth, continuous and highly accurate throughout the entire time series through inverse correction, state update and covariance adjustment.

[0114] The embodiments of the present application are particularly suitable for vehicle positioning needs in complex environments, such as densely populated urban areas, tunnels, and other scenes, and can provide more reliable high-precision positioning information for the autonomous driving system. Through the bidirectional smoothing processing of backward filtering, the system can adaptively adjust in a dynamic environment, further improving the robustness and reliability of vehicle positioning, thereby providing strong support for high-precision applications such as autonomous driving.

[0115] In another embodiment, Figure 3 As shown, the Bayesian data fusion vehicle positioning system based on Kalman filter includes:

[0116] A data preprocessing module, used to preprocess the positioning data collected by the vehicle sensor positioning system;

[0117] A forward filtering module is used to perform forward filtering on the preprocessed data through a Kalman filter;

[0118] A data fusion module is used to input the filtered data into the Bayesian fusion module for data fusion;

[0119] The backward filtering module is used to perform backward filtering and output of the fused data through a Kalman filter.

[0120] In another embodiment, a Bayesian data fusion vehicle positioning device based on a Kalman filter includes a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, they are used to implement a Bayesian data fusion vehicle positioning method based on a Kalman filter.

[0121] In another embodiment, a computer-readable storage medium stores a program, which, when executed by a processor, implements a Bayesian data fusion vehicle positioning method based on a Kalman filter.

[0122] In order to verify the optimization performance of this application, encoders are used to measure the number of wheel revolutions and steering of the trolley. A Hall sensor is installed on the right wheel and an optical encoder is installed on the left wheel. The microcontroller MCU collects and processes the data to make unified instructions to ensure that the two sensors receive the same instructions. According to the sampling time of the data and the uniform motion of the vehicle, the moving distance of each wheel can be calculated. Due to the uncertainty and inconsistency of the data collected by the two sensors, the microcontroller MCU is required to send these data to MATLAB to execute the proposed algorithm.

[0123] The main noise sources inside the sensor are Noise and thermal noise (also called white noise). In order to simulate the internal noise source of each sensor, it is assumed that under the condition of additive Gaussian white noise, the standard deviation of the noise of the Hall sensor and the optical encoder is 2.260cm and 2.378cm respectively. In addition, it is assumed that the car moves at a uniform linear speed of 7.8cm / s, and the approximate interference error is 0.493cm / s. The sampling period is 0.5s and the movement time is 20s.

[0124] The performance of the algorithm is evaluated by the following four indicators:

[0125] (1) CPU running time (TIME): It represents the total processing time consumed by the algorithm to estimate the entire moving position of the car. Of course, the smaller the running time, the better.

[0126] (2) Residual Sum of Squares (RSS): It represents the sum of the squares of the difference between the theoretical position value and the estimated position value of the car at each sampling point. The smaller the residual sum of squares (RSS), the more accurate the algorithm. The formula is as follows:

[0127]

[0128] (3) Variance (P): It represents the variance of the car's estimated value. The variance reflects the performance of the filter in each algorithm. Correlation coefficient: It represents the relationship between the estimated value and the theoretical value under each algorithm. The range of the correlation coefficient is .

[0129] (4) Discriminant function (CF): A decision method for calculating the discriminant function is to estimate the utility of each technology. According to the importance of each indicator, a weight is assigned to each of the CPU running time (TIME), residual sum of squares (RSS), and variance (P). , and the sum of the three weights is 1. The experimental calculation value under each technology is To express, then the discriminant function (CF) under each technology is obtained by the following formula:

[0130]

[0131] in , and Represent the weights of TIME, RSS and P respectively. These weights can be adjusted according to different situations. Here, it is assumed that , , . , and Respectively represent the calculated values ​​of TIME, RSS and P obtained in each experiment. , and Respectively represent the maximum calculated values ​​of TIME, RSS and P obtained in each experiment. In order for the algorithm to produce accurate estimates with the minimum variance in the shortest time, the goal is to minimize the discriminant function value.

[0132] Table 1 summarizes the average results of 500 executions of the three indicators described above. It can be seen from Table 1 that the improved Yes algorithm (MB) has the smallest running time compared to other algorithms, but it has the largest RSS. The variance of the estimated value of the algorithm with the filter is the lowest, which can prove the effectiveness of the Kalman filter. More importantly, the estimated value of the embodiment of the present application is almost proportional to the theoretical value, with a correlation coefficient of 0.99.

[0133] Table 1:

[0134]

[0135] In summary, the present invention proposes to combine the Kalman filter and the Bayesian fusion algorithm, and proves that the Kalman filter can improve the residual sum of squares (RSS) and the variance of the estimated value (P), and solve the uncertainty and inconsistency problems of sensor data. The proposed technology is applied to vehicle position estimation, especially in complex environments such as urban high-rise buildings and tunnels to ensure high accuracy and stability of vehicle positioning.

[0136] The contents described in the embodiments of this specification are merely an enumeration of implementation forms of the inventive concept. The protection scope of the present invention should not be regarded as limited to the specific forms described in the embodiments. The protection scope of the present invention also extends to equivalent technical means that can be conceived by those skilled in the art based on the inventive concept.

Claims

1. A Bayesian data fusion vehicle positioning method based on Kalman filter, characterized by The method comprises the following steps: Preprocessing the positioning data collected by the vehicle sensor positioning system; The preprocessed data is forward filtered through a Kalman filter; The filtered data enters the Bayesian fusion module for data fusion; The fused data is back-filtered and output through a Kalman filter; The Bayesian fusion module performs data fusion specifically by calculating the posterior probability of each sensor, dynamically allocating weights according to the reliability of each sensor, and fusing to obtain preliminary location information; The weights are determined by the differences between the measurements and their respective variances.

2. The Bayesian data fusion vehicle positioning method based on Kalman filter according to claim 1 is characterized in that: The positioning data includes data from GPS, IMU and lidar sensors.

3. The Bayesian data fusion vehicle positioning method based on Kalman filter according to claim 2 is characterized in that: Preprocessing of positioning data specifically includes: Data verification: Check the consistency and validity of the data from each sensor. By calculating the deviation of each data point, use the three sigma principle to determine whether there are any abnormal points. Time synchronization: time synchronization processing of data from different sensors; Coordinate transformation: perform coordinate transformation on data from different sensors to facilitate subsequent data fusion.

4. The Bayesian data fusion vehicle positioning method based on Kalman filter according to claim 1 is characterized in that: In the forward filtering, the Kalman filter predicts and updates the sensor data using the system state equation and the observation equation, thereby filtering out noise and interference to obtain preliminary denoised data.

5. The vehicle positioning method based on Bayesian data fusion of Kalman filter according to claim 1 is characterized in that: The fused preliminary position information is smoothed by a backward Kalman filter to further eliminate residual measurement errors.

6. Bayesian data fusion vehicle positioning system based on Kalman filter, characterized in that: include: A data preprocessing module, used to preprocess the positioning data collected by the vehicle sensor positioning system; A forward filtering module is used to perform forward filtering on the preprocessed data through a Kalman filter; A data fusion module is used to input the filtered data into the Bayesian fusion module for data fusion; The backward filtering module is used to perform backward filtering and output of the fused data through a Kalman filter.

7. Bayesian data fusion vehicle positioning device based on Kalman filter, characterized in that: It comprises a memory and one or more processors, wherein the memory stores executable codes, and when the one or more processors execute the executable codes, they are used to implement the Bayesian data fusion vehicle positioning method based on the Kalman filter as described in any one of claims 1 to 5.

8. A computer-readable storage medium, characterized in that: A program is stored thereon, and when the program is executed by a processor, the Bayesian data fusion vehicle positioning method based on a Kalman filter described in any one of claims 1 to 5 is implemented.

Images