Multi-sensor fusion positioning method

Abstract

The invention relates to the technical field of fusion positioning, in particular to a multi-sensor fusion positioning method, which comprises the following steps of: acquiring original positioning data from a plurality of sensors in real time, and preprocessing the positioning data; designing an independent local state estimation filter for each sensor, and outputting a local state estimation value; inputting each local state estimation value into a federated filter for multi-source information fusion, and outputting an optimal positioning result after fusion; and generating a fused positioning track according to an optimal positioning result output by federated filtering. According to the invention, an independent local state estimation filter is designed to carry out preprocessing and local state estimation on data of multi-source sensors such as an inertial measurement sensor, a GNSS receiver and a star sensor, a federated filtering architecture is adopted to carry out covariance weighted fusion, and the measurement precision and dynamic characteristics of each sensor are fully considered.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of fusion positioning technology, in particular, to a multi-sensor fusion positioning method. BACKGROUND

[0002] In the field of modern navigation and positioning, with the rapid development of high-precision positioning applications such as autonomous driving, unmanned aerial vehicles, intelligent transportation systems, etc., higher requirements are put forward for the precision, reliability, robustness and real-time performance of the positioning system. However, single sensors often have significant limitations in practical applications. For example, GNSS receivers can provide high-precision position information in open sky environments, but are easily affected by shielding or multipath effects in urban canyons, tunnels, underground or dense building groups, resulting in signal loss or increased positioning error; inertial measurement sensors have the advantages of strong autonomy, high output frequency, and good short-term precision, but their errors accumulate over time, leading to positioning drift over long periods of use; star sensors have the characteristics of all-weather, global domain, and no cumulative error, but are limited by weather, lighting, and observation conditions, and have low observation frequency and are easily disturbed by the environment. In addition, the data from different sensors usually arrive asynchronously at different clocks and sampling rates, and there are problems such as time asynchronization and inconsistent data frequencies. If effective time alignment and synchronization processing is not performed, it will directly affect the fusion performance. At the same time, the reliability of each sensor changes dynamically under different working conditions, and traditional simple fusion methods are difficult to cope with fluctuations in data quality, which can easily lead to unstable fusion results. Therefore, a multi-sensor fusion positioning method is provided. SUMMARY

[0003] The purpose of the present application is to provide a multi-sensor fusion positioning method to solve the problems of low positioning accuracy, poor reliability, weak environmental adaptability of single sensors and difficulty in asynchronous fusion of multi-sensor data as mentioned in the background.

[0004] To achieve the above-mentioned purpose, the present application provides a multi-sensor fusion positioning method, comprising the following steps:

[0005] S1, real-time acquisition of original positioning data from multiple sensors, and preprocessing of the positioning data;

[0006] S2, designing an independent local state estimation filter for each sensor, and outputting a local state estimation value;

[0007] S3, inputting each local state estimation value into a federated filter for multi-source information fusion, and outputting an optimal positioning result after fusion;

[0008] S4, generating a fused positioning trajectory according to the optimal positioning result output by the federated filter.

[0009] As a further improvement of the technical solution, in the S1, the plurality of sensors include an inertial measurement sensor, a GNSS receiver and a star sensor.

[0010] As a further improvement of the technical solution, in the S2, a separate local state estimation filter is designed for each sensor, including the following steps:

[0011] S2.1, selecting a filter type according to sensor characteristics;

[0012] S2.2, constructing an independent filter module for each sensor;

[0013] S2.3, initializing the state vector and covariance matrix of each filter according to the initial calibration data, and setting the process noise covariance matrix and the observation noise covariance matrix;

[0014] S2.4, for each local filter of each sensor, independently performing state prediction and observation update based on the recursive step of the filter;

[0015] S2.5, continuously outputting the local state and its covariance information estimated by the local filter, and interpolating the local states output by each filter according to a unified time reference.

[0016] As a further improvement of the technical solution, in the S2.4, the state prediction and observation update are independently performed based on the recursive step of the filter, including the following steps:

[0017] S2.41, predicting the state at the next time using the system dynamic model;

[0018] S2.42, using the state transition model to predict the uncertainty of the state;

[0019] S2.43, based on the current predicted state, calculating the expected observation value through the observation model;

[0020] S2.44, calculating the difference between the actual observation and the predicted observation value, i.e. the residual:

[0021] S2.45, judging the covariance matrix of the residual, which is used to quantify the observation error;

[0022] S2.46, in order to minimize the estimation error, calculating the Kalman gain;

[0023] S2.47, using the Kalman gain to correct the state prediction to obtain the final state estimation and update the covariance matrix to reflect the new uncertainty level.

[0024] As a further improvement of the technical solution, in the S2.5, the local states output by each filter are interpolated according to a unified time reference, including the following steps:

[0025] S2.51, determining a unified time reference and collecting state estimates of each filter output;

[0026] S2.52, defining a target time interval, constructing a target time sequence, and creating a data structure matching the target time sequence to save the interpolated state estimates;

[0027] S2.53, traversing each time point t in the target time sequence k , using linear interpolation to calculate the state estimate at time t k .

[0028] As a further improvement of the technical solution, in S3, the local state estimates are input into the federated filter for multi-source information fusion, and the optimal positioning result after fusion is output, including the following steps:

[0029] S3.1, constructing a federated filter structure, which includes a plurality of sub-filters and a main filter;

[0030] S3.2, using the main filter to receive the local state estimates and their covariance matrices output by all sub-filters in each fusion cycle, and performing synchronization processing under the unified time reference;

[0031] S3.3, performing weighted fusion of all local estimates based on the covariance weighted fusion criterion to generate a global optimal fused state estimate and its covariance;

[0032] S3.4, introducing an information distribution factor β i to weight the state estimates of each sub-filter according to their reliability, so as to suppress the influence of low reliability information on the fusion result when the data quality difference is large;

[0033] S3.5, after completing the weighted fusion, outputting the fused global state estimate and the fused covariance through the main filter.

[0034] As a further improvement of the technical solution, in S3.3, the global optimal fused state estimate and its covariance are generated by performing weighted fusion of all local estimates based on the covariance weighted fusion criterion, including the following steps:

[0035] S3.31, adding the inverses of the covariance matrices of each sub-filter, then inverting the result to obtain the fused covariance matrix P f ;

[0036] S3.32, for each sub-filter, constructing a fusion weight matrix according to its output covariance matrix;

[0037] S3.33, using the calculated fusion weight matrix and the state estimation vector of each sub-filter, substituting into the covariance weighted fusion formula to obtain the global optimal state estimation vector

[0038] S3.34, the state estimation vector and the fused covariance matrix P f are output as the fusion result.

[0039] As a further improvement of the technical solution, in S3.4, an information allocation factor is introduced in the weighted fusion process to weight the state estimation of each sub-filter according to the reliability, comprising the following steps:

[0040] S3.41, calculate the initial information matrix of each sub-filter;

[0041] S3.42, determine the information allocation factor β i of each sub-filter;

[0042] S3.43, weight the information matrix according to the reliability, calculate the fusion covariance matrix and the fusion state estimation.

[0043] As a further improvement of the technical solution, in S4, according to the optimal positioning result output by the federated filter, a fused positioning trajectory is generated, comprising the following steps:

[0044] S4.1, construct the data structure of the fused positioning trajectory as an ordered time series set;

[0045] S4.2, time series alignment and interpolation reconstruction are performed on the time series;

[0046] S4.3, use RTS smoother to smooth the trajectory, and use the observation information at future time to correct the state estimation at historical time by backward propagation;

[0047] S4.4, convert the position information in the smoothed fusion state from the filter working coordinate system to the map coordinate system;

[0048] S4.5, organize the time series processed by the above-mentioned processing into a trajectory data structure.

[0049] As a further improvement of the technical solution, in S4.3, RTS smoother is used to smooth the trajectory, and the observation information at future time is used to correct the state estimation at historical time by backward propagation, comprising the following steps:

[0050] S4.31, the total number of time points is N+1, and the filtering result at the last time is taken as its smoothing result, wherein N is the index value of the last time point;

[0051] S4.32, from time k is N-1, point by point in the reverse time direction, the smoothing gain matrix is calculated;

[0052] S4.33, the next time smoothing state and the current filtering state are used to calculate the current smoothing state;

[0053] S4.34, the smoothed covariance matrix is updated synchronously, and the modified uncertainty is reflected;

[0054] S4.35, the reverse recursion is executed in a loop to obtain a complete smoothing state sequence.

[0055] Compared with the prior art, the beneficial effects of the present application are:

[0056] 1. In the multi-sensor fusion positioning method, independent local state estimation filters are designed to preprocess and estimate the local state of multi-source sensors such as inertial measurement sensors, GNSS receivers and star sensors, and covariance weighted fusion is performed using a federal filter architecture, fully considering the measurement accuracy and dynamic characteristics of each sensor. In the fusion process, an information distribution factor is introduced for confidence weighting, which can effectively suppress the interference of low-confidence sensor data on the fusion result. Through multi-source complementation and weighted optimization, the overall positioning accuracy and anti-interference ability of the system in a dynamic environment are significantly improved.

[0057] 2. In the multi-sensor fusion positioning method, time synchronization and interpolation processing are performed on the asynchronous data of multiple sensors in the data preprocessing stage to ensure that the data of each sensor is aligned under a unified time reference. The coordination processing of different sampling rates of data is supported in the local filtering and federal fusion links, and an RTS smoother is introduced for backward smoothing processing in the final trajectory generation stage, which uses the observation information of the whole period to correct the historical state estimation in reverse, effectively reducing the filtering delay and estimation jitter, improving the smoothness and precision consistency of the trajectory. At the same time, the fusion result can be converted into a commonly used geographic coordinate system and the trajectory sequence can be reconstructed at a specified frequency to meet the needs of various application scenarios such as high-precision navigation, map matching and post-analysis. BRIEF DESCRIPTION OF DRAWINGS

[0058] Figure 1 The overall method flowchart of the present application is shown in the figure. DETAILED DESCRIPTION

[0059] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application.

[0060] Example: Please refer to Figure 1 As shown, this embodiment provides a multi-sensor fusion positioning method, including the following steps:

[0061] S1. Collect raw positioning data from multiple sensors in real time and preprocess the positioning data (the collected data arrives asynchronously under the clock or sampling rate of different sensors, and needs to be processed in time synchronization. Timestamp calibration and interpolation are performed on the data from different sensors to ensure that the data are aligned at the same time. Low-pass filtering or other filtering methods are used to remove noise from the raw signal).

[0062] In this embodiment, the multiple sensors include an inertial measurement sensor, a GNSS receiver, and a star sensor.

[0063] S2. Design an independent local state estimation filter for each sensor and output the local state estimate;

[0064] In this embodiment, an independent local state estimation filter is designed for each type of sensor, including the following steps:

[0065] S2.1 Select the filter type according to the sensor characteristics. When the sensor is an inertial measurement sensor, use state integration combined with an extended Kalman filter (EKF) for attitude estimation; when the sensor is a GNSS receiver, use a linear Kalman filter (KF) for position and velocity estimation; when the sensor is a star sensor, use an extended Kalman filter (EKF) for absolute attitude estimation.

[0066] S2.2 Build an independent filter module for each sensor to ensure that it has complete state prediction and observation update functions, and has input, processing and output interfaces to facilitate data interaction with other modules in the future;

[0067] S2.3. Initialize the state vector and covariance matrix of each filter based on the initial calibration data or external information (such as the first GNSS positioning result), and set the process noise covariance matrix and the observation noise covariance matrix to reflect the dynamic characteristics and measurement accuracy of the sensor.

[0068] S2.4 For each sensor's local filter, state prediction and observation update are performed independently based on the filter's recursive steps;

[0069] The filter-based recursive step independently performs state prediction and observation update, including the following steps:

[0070] S2.41. Predict the state at the next moment using the system dynamic model:

[0071] x k|k-1 =f(x) k-1|k-1 uk )+w k ;

[0072] where x k|k-1 denotes the state prediction at time k based on the information at the previous time, f(*) is the state transition function that maps the previous state x k-1|k-1 and the current control input u k to the current state, w k is the process noise;

[0073] S2.42, predict the uncertainty (covariance) of the state using the state transition model:

[0074]

[0075] where P k|k-1 is the covariance matrix of the predicted state, F k is the state transition matrix (e.g., the Jacobian matrix is used in EKF), T is the transpose operation, and Q k is the covariance matrix of the process noise;

[0076] S2.43, based on the currently predicted state x k|k-1 , compute the expected observation value y

[0077]

[0078] where h(*) is the observation function that maps the state vector to the observation space, and v k is the observation noise;

[0079] S2.44, compute the difference between the actual observation z k and the predicted observation value y k , i.e., the residual y :

[0080]

[0081] S2.45, judge the covariance matrix S k of the residual y , which quantifies the observation error:

[0082]

[0083] where H k is the observation matrix, and R k is the covariance matrix of the observation noise;

[0084] S2.46, to minimize the estimation error, compute the Kalman gain K k :

[0085]

[0086] S2.47, correct the state prediction with the Kalman gain to obtain the final state estimate x k| k(x k|k = x k|k-1 + K k y k ), while updating the covariance matrix P k|k to reflect the new level of uncertainty:

[0087] P k|k = (I - K k H k )P k|k-1 ;

[0088] where I is the identity matrix;

[0089] The above prediction and update steps are independently executed in each local filter, independent of the data of other sensors. After each update, the local filter outputs the current time state vector x k|k and the covariance matrix P k|k , and waits for the next sampling time to enter a new round of recursion;

[0090] S2.5, continuously output the local state estimated by the local filter (the attitude angle, angular velocity, linear acceleration and short-time position / velocity estimated by the inertial measurement sensor, the three-dimensional position and velocity estimated by the GNSS receiver, and the high-precision absolute attitude estimated by the star sensor) and its covariance information, and interpolate the local state output by each filter according to a unified time reference (time synchronization processing from S1);

[0091] Further, the local state output by each filter is interpolated according to a unified time reference (time synchronization processing from S1), including the following steps:

[0092] S2.51, determine the unified time reference (select a system master clock as the unified time reference (such as GPS PPS signal or system internal high-precision clock), and ensure that the data timestamps of all sensors are converted to the master clock), and collect the state estimates output by each filter;

[0093] S2.52, define a target time interval, construct a target time sequence, and create a data structure matching the target time sequence to save the interpolated state estimates, such as an array or a list;

[0094] S2.53, traverse each time point t k in the target time sequence, and use linear interpolation to calculate t kSimilarly, for each sub-filter's covariance matrix, interpolation is also needed according to the above method (interpolate the Cholesky decomposed vectors of the covariance matrix, then reconstruct back to the covariance matrix form).

[0095] S3, input each local state estimate into the federated filter for multi-source information fusion, and output the optimal positioning result after fusion;

[0096] In this embodiment, each local state estimate is input into the federated filter for multi-source information fusion, and the optimal positioning result after fusion is output, including the following steps:

[0097] S3.1, construct a federated filter structure, which includes a plurality of sub-filters (corresponding to inertial measurement sensors, GNSS receivers and star sensors respectively) and a main filter; the sub-filters are responsible for processing their own sensor data and outputting local state estimates, and the main filter is used to receive and fuse the output results of each sub-filter;

[0098] S3.2, use the main filter to receive the local state estimates and their covariance matrices output by all sub-filters in each fusion period, and perform synchronization processing under the unified time reference;

[0099] S3.3, based on the covariance weighted fusion criterion, weight and fuse all local estimates to generate a global optimal fused state estimate and its covariance;

[0100] Among them, based on the covariance weighted fusion criterion, weight and fuse all local estimates to generate a global optimal fused state estimate and its covariance, including the following steps:

[0101] S3.31, add the inverses of the covariance matrices of each sub-filter, then take the inverse of the result to obtain the fused covariance matrix P f ;

[0102]

[0103] Where n is the total number of sub-filters;

[0104] S3.32, for each sub-filter, construct a fusion weight matrix W i from its output covariance matrix P i (i represents the sub-filter index);

[0105]

[0106] S3.33, use the calculated fusion weight matrix and the state estimate vectors of each sub-filter to substitute into the covariance weighted fusion formula to obtain the global optimal state estimate vector

[0107]

[0108] S3.34, the state estimation vector and the fused covariance matrix P f As the output of the fusion result;

[0109] S3.4 Introducing an information allocation factor β during the weighted fusion process i The state estimates of each sub-filter are weighted by confidence to suppress the impact of low-confidence information on the fusion result when there are large differences in data quality; the information allocation factor can be dynamically adjusted based on historical performance evaluation, residual size or external confidence model;

[0110] Furthermore, an information allocation factor is introduced during the weighted fusion process to weight the state estimates of each sub-filter with confidence. This primarily addresses the issue of effectively suppressing the impact of low-confidence information on the fusion result when there are significant differences in the quality of data from multiple sensors. When there are significant differences in the quality of data provided by different sensors, simply applying uniform weighting to all sensor data may lead to low-quality or unreliable data interfering with the final fusion result, thereby reducing the overall positioning accuracy and reliability. By introducing an information allocation factor for each sub-filter, the weight of each sensor in the final fusion result can be dynamically adjusted based on the actual quality and historical performance of the data. This ensures that high-quality data receives higher weights, while low-quality or unreliable data is given lower weights or even effectively suppressed, thereby improving the accuracy and reliability of the fusion result. It also allows for flexible adaptation to changes in sensor performance under different working environments and conditions, making the entire multi-sensor fusion system more adaptable and robust.

[0111] The weighted fusion process introduces an information allocation factor to weight the state estimates of each sub-filter with confidence, including the following steps:

[0112] S3.41 Calculate the initial information matrix of each sub-filter (the information matrix is ​​the inverse of the covariance matrix);

[0113] S3.42 Determine the information allocation factor β for each sub-filter i ;

[0114] S3.43. Weight the information matrix according to its confidence level, and calculate the fusion covariance matrix and fusion state estimate;

[0115] The fusion covariance matrix is:

[0116]

[0117] The fusion state is estimated as follows:

[0118]

[0119] wherein W i (β) is the inverse weight matrix of the covariance weighted by the confidence;

[0120] S3.5, after the weighted fusion is completed, outputting the fused global state estimation and the fused covariance through the main filter.

[0121] S4, generating a fused positioning trajectory according to the optimal positioning result output by the federated filter;

[0122] In this embodiment, the fused positioning trajectory is generated according to the optimal positioning result output by the federated filter, including the following steps:

[0123] S4.1, constructing a data structure of the fused positioning trajectory as an ordered time sequence set, the set including an optimal state vector (including position (such as latitude, longitude and altitude or east-north-up coordinates), velocity, attitude angle (such as Euler angle or quaternion) key positioning information) output by the federated filter, a corresponding covariance matrix, and a time stamp (a unified clock reference, from S1 time synchronization);

[0124] S4.2, time sequence alignment and interpolation reconstruction are performed on the time sequence: whether the time sequence output by the fusion meets a user-specified trajectory update frequency (for example, 10 Hz, 50 Hz, etc.) is checked; if the fusion period is uneven or the frequency is lower than the target trajectory refresh rate, a time interpolation method is used to perform high-precision reconstruction under a unified time reference;

[0125] S4.3, an RTS smoother is used to smooth the trajectory, and the state estimation at a historical time is corrected by backward propagation using observation information at a future time, so as to reduce the estimation variance and improve the overall smoothness and accuracy of the trajectory;

[0126] The RTS smoother is used to smooth the trajectory, and the state estimation at a historical time is corrected by backward propagation using observation information at a future time, including the following steps:

[0127] S4.31, the total number of time points is N+1, and the filtering result at the last time is taken as the smoothing result thereof, wherein N is an index value of the last time point;

[0128] S4.32, starting from time k=N-1, the smoothing gain matrix B k is calculated point by point in the reverse time direction:

[0129]

[0130] wherein P k|k is the filtering covariance, is the inverse matrix of the prior covariance matrix;

[0131] S4.33, Smoothing the state at the next time instant (From the previous round of backward iteration) and the current filtered state Compute the current smoothed state

[0132]

[0133] where, The predicted state at k+1 time instant by the filter (i.e. the prediction step output in the forward filtering);

[0134] S4.34, Update the smoothed covariance matrix in sync Reflect the corrected uncertainty:

[0135]

[0136] where, P k|k is the filtered covariance matrix, T is the transpose operation, is the smoothed covariance at the next time instant, P k+1|k is the current predicted covariance, P k|k is the posterior covariance matrix;

[0137] S4.35, Loop the backward recursion to get the complete smoothed state sequence:

[0138] S4.4, Convert the position information in the smoothed fused state from the filter working coordinate system (e.g. ECEF, NED) to the user required geographic or map coordinate system, so as to meet the requirements of map display, navigation or subsequent data processing;

[0139] S4.5, Organize the time series processed above into a trajectory data structure, including time stamp (UTC / GPS time), position (longitude, latitude, height), velocity (east, north, up), attitude information (roll, pitch, heading angle or quaternion), positioning accuracy index (e.g. PDOP, square root of the diagonal elements of the fused covariance).

[0140] The basic principles, main features and advantages of the present application are shown and described above. It should be understood by those skilled in the art that the present application is not limited by the above embodiments, and the above embodiments and descriptions in the specification are only preferred examples of the present application and are not intended to limit the present application. Without departing from the spirit and scope of the present application, various changes and improvements can be made to the present application, and these changes and improvements all fall within the scope of the claimed present application.

Claims

1. A multi-sensor fusion positioning method, characterized in that, The method comprises the following steps: S1, collecting raw positioning data from multiple sensors in real time, and preprocessing the positioning data; S2, designing an independent local state estimation filter for each sensor, outputting a local state estimation value; S3, inputting each local state estimation value into a federated filter for multi-source information fusion, and outputting an optimal positioning result after fusion; S4, generating a fused positioning trajectory according to the optimal positioning result output by the federated filter.

2. The multi-sensor fusion positioning method of claim 1, wherein: In the S1, the multiple sensors include an inertial measurement sensor, a GNSS receiver and a star sensor.

3. The multi-sensor fusion positioning method of claim 1, wherein: In the S2, the independent local state estimation filter is designed for each sensor, comprising the following steps: S2.1, selecting a filter type according to the sensor characteristics; S2.2, constructing an independent filter module for each sensor; S2.3, initializing the state vector and covariance matrix of each filter according to the initial calibration data, and setting the process noise covariance matrix and the observation noise covariance matrix; S2.4, for each local filter of the sensor, independently performing state prediction and observation update based on the recursive steps of the filter; S2.5, continuously outputting the local state and its covariance information estimated by the local filter, and interpolating the local state output by each filter according to a unified time reference.

4. The multi-sensor fusion positioning method of claim 3, wherein: In the S2.4, the state prediction and observation update are independently performed based on the recursive steps of the filter, comprising the following steps: S2.41, predicting the state at the next time using the system dynamic model; S2.42, predicting the uncertainty of the state using the state transition model; S2.43, calculating the expected observation value based on the current predicted state through the observation model; S2.44, calculating the difference between the actual observation and the predicted observation value, i.e. the residual: S2.45, judging the covariance matrix of the residual to quantify the observation error; S2.46, to minimize the estimation error, calculating the Kalman gain; S2.47, using the Kalman gain to correct the state prediction to obtain the final state estimation and update the covariance matrix to reflect the new uncertainty level.

5. The multi-sensor fusion positioning method of claim 3, wherein: In the S2.5, the local state output by each filter is interpolated according to a unified time reference, comprising the following steps: S2.51, determining a unified time reference and collecting the state estimation output by each filter; S2.52, defining a target time interval, constructing a target time sequence, and creating a data structure matching the target time sequence to save the interpolated state estimation; S2.53, Traverse each time point t in the target time series k t is calculated using linear interpolation. k State estimation at time step.

6. The multi-sensor fusion positioning method of claim 1, wherein: In the S3, the local state estimation values are input into the federated filter for multi-source information fusion, and the optimal positioning result after fusion is output, comprising the following steps: S3.1, constructing a federated filter structure, which includes multiple sub-filters and a main filter; S3.2, using the main filter to receive the local state estimation and its covariance matrix output by all sub-filters in each fusion period, and performing synchronization processing under the unified time reference; S3.3, based on the covariance weighted fusion criterion, weighted fusion is performed on all local estimations to generate a global optimal fusion state estimation and its covariance. S3.4, Introducing information distribution factor β in the weighted fusion process i The state estimation of each sub-filter is weighted by confidence to suppress the influence of low-confidence information on the fusion result when the data quality difference is large. S3.5, output the fused global state estimation and fused covariance through the main filter after completing the weighted fusion.

7. The multi-sensor fusion positioning method of claim 6, wherein: In the S3.3, the weighted fusion is performed on all local estimations based on the covariance weighted fusion criterion to generate the global optimal fused state estimation and its covariance, including the following steps: S3.31, add the inverses of the individual sub-filter covariance matrices and then take the inverse of the result to obtain the fused covariance matrix P f ; S3.32, for each sub-filter, construct the fusion weight matrix according to the covariance matrix outputted by the sub-filter; S3.33, using the calculated fusion weight matrix and the state estimation vectors of each sub-filter, substitute into the covariance weighted fusion formula to obtain the globally optimal state estimation vector S3.34, the state estimate vector and the fused covariance matrix P f as the fusion result output.

8. The multi-sensor fusion positioning method of claim 6, wherein: In the S3.4, the information distribution factor is introduced in the weighted fusion process to weight the state estimations of the sub-filters according to the confidence, including the following steps: S3.41, calculate the initial information matrix of each sub-filter; S3.42, determining information distribution factors β of the sub-filters i ; S3.43, weight the information matrix according to the confidence, and calculate the fused covariance matrix and the fused state estimation.

9. The multi-sensor fusion positioning method of claim 1, wherein: In the S4, the optimal positioning result outputted by the federated filter is used to generate the fused positioning trajectory, including the following steps: S4.1, construct the data structure of the fused positioning trajectory as an ordered time sequence set; S4.2, perform time sequence alignment and interpolation reconstruction on the time sequence; S4.3, use the RTS smoother to smooth the trajectory, and use the observation information at the future time to correct the state estimation at the historical time through reverse propagation; S4.4, convert the position information in the smoothed fused state from the filter working coordinate system to the map coordinate system; S4.5, organize the time sequence processed through the above processing into the trajectory data structure.

10. The multi-sensor fusion positioning method of claim 9, wherein: In the S4.3, the RTS smoother is used to smooth the trajectory, and the observation information at the future time is used to correct the state estimation at the historical time through reverse propagation, including the following steps: S4.31, the total number of time points is N+1, and the filtering result at the last time is taken as the smoothing result, wherein N is the index value of the last time point; S4.32, starting from time k as N-1, calculate the smoothing gain matrix point by point in the reverse time direction; S4.33, use the smoothing state at the next time and the current filtering state to calculate the current smoothing state; S4.34, update the smoothed covariance matrix synchronously to reflect the corrected uncertainty; S4.35, perform reverse recursion in a loop to obtain the complete smoothing state sequence.

Images