Indoor Positioning Method and System Based on Multi-Source Constraint Loose Coupling Asynchronous INS / UWB

Abstract

This invention provides a multi-source constrained loosely coupled asynchronous INS / UWB indoor positioning method and system, relating to the field of wireless indoor positioning technology. The method includes: acquiring distance observation sequences from UWB base stations and using the EM algorithm and K-Medoids clustering algorithm for optimization to obtain the LOS measurement values ​​of the UWB base stations; collecting acceleration and rotation speed information of mobile nodes, constructing an INS dynamic model and pre-integrating it to obtain the INS pre-integration result; calculating UWB position observation values ​​based on the LOS measurement values ​​of all valid base stations, and performing a validity check by combining the INS pre-integration result; if valid, fusing the UWB position observation values ​​and the INS pre-integration result using an improved adaptive extended Kalman filter to obtain the indoor positioning result; if invalid, using the INS pre-integration result as the indoor positioning result. This invention can ensure the stability and reliability of positioning accuracy in complex indoor environments.

Description

Technical Field

[0001] This invention relates to the field of wireless indoor positioning technology, and in particular to an indoor positioning method and system based on multi-source constrained loosely coupled asynchronous INS / UWB. Background Technology

[0002] Indoor positioning technology has a wide range of applications, and with the upgrading of industry demands, the demand for accurate and efficient indoor positioning services continues to grow. In indoor environments where satellite signals cannot reach, the Global Navigation Satellite System (GNSS) struggles to achieve high-precision, continuous positioning output. Therefore, various combined positioning technologies have been widely explored and applied.

[0003] Among these, the joint positioning scheme of Inertial Navigation System (INS) and Ultra-Wideband (UWB) has become a research hotspot due to their complementary advantages. INS estimates position by integrating its own collected acceleration information and is unaffected by external interference; UWB positioning technology calculates position based on distance measurement and can achieve centimeter-level positioning accuracy in line-of-sight (LOS) scenarios, making it a reliable positioning technology.

[0004] In existing technologies, the joint positioning method of INS and UWB improves positioning performance by combining the advantages of both: it uses the correction information provided by UWB to suppress the error growth of INS, and at the same time, it uses INS to assist UWB in achieving more accurate positioning in complex environments, thereby obtaining accurate location information and providing a feasible technical path for solving the indoor positioning problem.

[0005] While existing INS / UWB joint positioning technology can meet basic positioning needs, it still has significant shortcomings in complex indoor environments, making it difficult to achieve stable and high-precision positioning results. Its non-line-of-sight (NLOS) error suppression capability is insufficient. NLOS environments are prevalent in indoor scenarios, requiring positioning signals to propagate around obstacles through reflection and refraction. This results in NLOS errors in the measurements acquired by UWB base stations, and current technologies are not thorough enough in identifying and suppressing these errors, directly affecting the accuracy of positioning results. Furthermore, INS suffers from inherent error accumulation. INS relies on integration calculations to obtain position estimates; over long-term use, integration errors accumulate, leading to a gradual decrease in positioning accuracy. Existing solutions have not effectively addressed this core weakness.

[0006] Furthermore, due to insufficient system adaptability and robustness, the indoor environmental signal transmission conditions are complex and changeable. Existing joint positioning systems lack flexible adaptive adjustment mechanisms and are difficult to match dynamically changing environmental noise. At the same time, when faced with sudden interference or sensor malfunctions, the system has weak fault tolerance, large fluctuations in positioning error, and cannot continuously output stable positioning results, which restricts its application in real complex scenarios. Summary of the Invention

[0007] Addressing the core challenges faced by existing INS / UWB fusion positioning technologies in complex indoor environments—namely, the decrease in UWB positioning accuracy due to non-line-of-sight (NLOS) interference, the impact of INS integration error accumulation on positioning stability, and the insufficient adaptability of traditional filtering algorithms—this invention proposes a multi-source constrained loosely coupled asynchronous INS / UWB indoor positioning method and system based on UWB data preprocessing, INS positioning modeling, and an improved Adaptive Extended Kalman Filter (AEKF). The aim is to effectively suppress NLOS errors and eliminate INS accumulated errors, ensuring the stability and reliability of positioning accuracy in complex indoor environments.

[0008] On the one hand, this invention proposes an indoor positioning method based on multi-source constrained loosely coupled asynchronous INS / UWB, which includes the following process:

[0009] The distance observation values ​​of several UWB base stations to the mobile node are acquired in real time, forming a distance observation sequence for each UWB base station;

[0010] For each UWB base station's distance observation sequence, the expectation-maximization (EM) algorithm and the K-Medoids clustering algorithm are used to process the distance observation sequence, and the optimal result of the two processing methods is selected to obtain the current LOS measurement value of the UWB base station.

[0011] Acceleration and rotation speed information of the moving node are collected, Euler angles are used to construct the INS dynamic model, and pre-integration calculation is performed based on the acceleration and rotation speed information to obtain the INS pre-integration result at the current time.

[0012] UWB base stations that have obtained LOS measurements at the current time are considered as valid base stations. The UWB location observation value at the current time is calculated based on the LOS measurements of all valid base stations at the current time. The validity of the UWB location observation value at the current time is verified by combining the INS pre-integration results at the current time.

[0013] If the test result is valid, the improved adaptive extended Kalman filter is used to fuse the current UWB position observation and INS pre-integration result to obtain the indoor positioning result of the mobile node.

[0014] If the test result is invalid, the INS pre-integration result at the current moment will be used as the indoor positioning result of the mobile node.

[0015] Furthermore, for each UWB base station's distance observation sequence, the Expectation-Maximization (EM) algorithm and the K-Medoids clustering algorithm are used to process the distance observation sequence, and the optimal result of the two processing methods is selected to obtain the current LOS measurement value of the UWB base station. The specific method is as follows:

[0016] For the The distance observation sequence of each UWB base station is processed using the expectation-maximization (EM) algorithm to generate the first UWB observation value.

[0017] The distance observation sequence is preprocessed using the K-Medoids clustering algorithm to generate the second UWB observation value;

[0018] The smaller UWB observation value between the first and second UWB observation values ​​is selected as the LOS measurement value of the UWB base station at the current time.

[0019] Furthermore, the above refers to the first The method for generating the first UWB observation value by processing the distance observation sequence of a UWB base station using the Expectation-Maximization (EM) algorithm is as follows:

[0020] For the The UWB base station in the first Distance observation value obtained during the second observation The measurement error is defined as the distance observed. and the The UWB base station in the first The true distance to the moving node at the time of the second observation The difference between them;

[0021] The measurement error is modeled using a Gaussian mixture model to obtain a probability density model of the measurement error;

[0022] The parameter set of the probability density model Recorded as ,in For the first The covariance matrix of Gaussian components; For the first The mean of the Gaussian components; For the first The weights of each Gaussian component;

[0023] According to the Distance observation sequences of UWB base stations, defining parameter sets. The log-likelihood function;

[0024] The log-likelihood function is solved using the Expectation-Maximization (EM) algorithm to obtain the parameter set. The optimal estimate is as follows:

[0025] In each iteration, the E-Step and M-Step are executed sequentially.

[0026] No. The E-Step of the round of iteration is: based on the first... After each iteration, the parameter estimates are updated, and the posterior probability of each distance observation belonging to each Gaussian component is calculated.

[0027] No. The M-Step of the iterative execution round is as follows: Update the weights, covariance matrix, and variance of each Gaussian component according to the posterior probability, as the result of the first iteration. The parameter estimates after each iteration;

[0028] After each iteration, the difference between the parameter estimate updated in the current iteration and the parameter estimate updated in the previous iteration is calculated. The iteration terminates when the calculated difference is lower than the preset threshold for two consecutive iterations.

[0029] The parameter set obtained in the last iteration In this process, the Gaussian component with the smallest covariance matrix is ​​selected, and the mean of this Gaussian component is used as the UWB measurement value under the LOS environment.

[0030] If distance observation value If the distance observation is greater than the UWB measurement value under the LOS environment, then the UWB measurement value under the LOS environment is taken as the first UWB observation value; otherwise, the distance observation value is taken as the first UWB observation value. This is the first UWB observation.

[0031] Furthermore, the specific method for preprocessing the distance observation sequence using the K-Medoids clustering algorithm to generate the second UWB observation value is as follows:

[0032] For the The distance observation sequence of each UWB base station is set to have 2 clusters, corresponding to LOS and NLOS measurements respectively, and the initial cluster center of each cluster is randomly selected.

[0033] For each round of clustering, perform the following operations:

[0034] Each distance observation in the distance observation sequence is taken as a data item, and the Euclidean distance between each data item and the current cluster centers is calculated. The data item is then assigned to the cluster to which the nearest cluster center belongs.

[0035] For each cluster, iterate through all data items in the cluster, calculate the sum of the squared distances from all data items in the cluster to the currently traversed data item, and select the data item with the smallest sum of squared distances as the new cluster center of the cluster.

[0036] Calculate the Euclidean distance between the new cluster center and the previous cluster center. If the Euclidean distance is less than the predetermined convergence threshold, terminate the clustering process; otherwise, start the next round of clustering.

[0037] After clustering, two cluster centers are obtained corresponding to the LOS and NLOS measurements, and the cluster center corresponding to the LOS measurement is used as the second UWB observation.

[0038] Furthermore, the specific method for acquiring the acceleration and rotational speed information of the moving node, constructing an INS dynamic model using the Euler angle method, and performing pre-integration calculations based on the acceleration and rotational speed information to obtain the INS pre-integration result at the current moment is as follows:

[0039] The acceleration and rotation speed information of the mobile node are collected using the inertial measurement unit (IMU) mounted on the mobile node.

[0040] Define the state vector of INS as follows:

[0041] ;

[0042] in, for The state vector of INS at any given time; The Euler angles of the moving node; and These are the velocity and position components of the moving node in the navigation coordinate system; and These are the inherent biases in measurements from gyroscopes and accelerometers, respectively.

[0043] The error state vector of INS is defined as follows:

[0044] ;

[0045] in, for The error state vector of INS at time 1; Let be the Euler angle error vector of the moving node; This is the velocity error vector of the moving node in the navigation coordinate system; This is the position error vector of the moving node in the navigation coordinate system; This is the bias error vector of the gyroscope; This is the bias error vector of the accelerometer;

[0046] The collected rotation speed information is pre-integrated to obtain the attitude angle of the mobile node in the IMU coordinate system. The attitude angle is then transformed to the navigation coordinate system using the direction cosine matrix to obtain the attitude of the mobile node.

[0047] The collected accelerations are pre-integrated to obtain the velocity and position of the moving node;

[0048] The obtained attitude, velocity, and position of the mobile node are used as the INS pre-integration results at the current time step.

[0049] Furthermore, the specific method for considering UWB base stations that have obtained LOS measurements at the current time as valid base stations, calculating the UWB location observation value at the current time based on the current LOS measurements of all valid base stations, and combining the INS pre-integration results at the current time to verify the validity of the UWB location observation value at the current time is as follows:

[0050] UWB base stations that have obtained the LOS measurement value at the current time are considered as valid base stations, and the number of valid base stations at the current time is counted.

[0051] Based on the current LOS measurements of all valid base stations, the UWB location observation at the current time is calculated using least squares estimation.

[0052] If the number of valid base stations at the current time meets the preset conditions, then the convergence verification of the UWB location observation value at the current time is performed; otherwise, the UWB location observation value at the current time is determined to be invalid.

[0053] The convergence verification is as follows: obtain the base station coordinates of all valid base stations at the current time, and combine the LOS measurement values ​​of all valid base stations at the current time with the UWB position observation values ​​at the current time to calculate the least squares estimated residual sum of squares. If the calculated residual sum of squares is not greater than the preset residual threshold, then the position consistency verification is further performed by combining the INS pre-integration results at the current time; otherwise, the UWB position observation values ​​at the current time are determined to be invalid.

[0054] The position consistency check is performed by calculating the Euclidean distance difference between the current UWB position observation and the position of the moving node in the current INS pre-integration result. If the Euclidean distance difference is not greater than a preset position deviation threshold, the current UWB position observation is determined to be valid; otherwise, the current UWB position observation is determined to be invalid.

[0055] Furthermore, the specific method for fusing the current UWB location observation and INS pre-integration results using an improved adaptive extended Kalman filter to obtain the indoor positioning result of the mobile node is as follows:

[0056] Initialize the state error covariance matrix And set the window size of the sliding estimation window. Forgetting factor adaptively updated by filtering ;

[0057] Based on the error state vector of the INS, the error state transition equation and the error state observation equation are established.

[0058] ;

[0059] ;

[0060] in, From Time's up The state error prediction vector at time 1; for The posterior estimate vector of the INS state error at time 1; express The observation prediction vector at time; Represents a nonlinear state transition function; Represents a nonlinear observation function; for Time-based process noise, , express The process noise covariance matrix at time step; for Measure noise at all times. , express The measurement noise covariance matrix at time point;

[0061] For the nonlinear state transition function in The posterior estimate vector of the INS state error at time 1 Perform a Taylor expansion at the given point, and label the coefficients of the first-order terms after the expansion as the state transition Jacobian matrix. ;

[0062] For the nonlinear observation function from Time's up State error prediction vector at time step Perform a Taylor expansion at the given location, and label the coefficients of the first-order terms after the expansion as the observed Jacobian matrix. ;

[0063] Based on the state transition Jacobian matrix , for from Time's up State error prediction vector at time step and state error covariance matrix Perform forward prediction; the prediction model is:

[0064] ;

[0065] ;

[0066] in, for The state error covariance matrix at time t;

[0067] Based on the observed Jacobian matrix ,calculate The new vector at time and new information vector covariance matrix , is represented as:

[0068] ;

[0069] ;

[0070] in, for UWB position observation at time; for The measurement noise covariance matrix at time point;

[0071] Using from Time's up State error covariance matrix at time 1 New information vector covariance matrix And the observation Jacobian matrix ,calculate Kalman gain at time step ;

[0072] right The new vector at time Perform the chi-square test and calculate. Test statistic at time ,like Then it is believed The UWB position observation at time t is detected as the LOS value; if Then it is believed The UWB position observation at time t was detected as an NLOS value; To test the threshold;

[0073] Using a sliding estimation window and a forgetting factor The noise covariance of the UWB location observations detected as LOS values ​​is adaptively estimated, and the LOS scene is updated based on the estimation results. Measurement noise covariance matrix at time process noise covariance matrix ;

[0074] Using a sliding estimation window, the mean of the innovation vector is statistically analyzed for UWB location observations detected as NLOS values, and the updated LOS values ​​are then analyzed based on the statistical results. Measurement noise covariance matrix at time Perform error compensation and update the NLOS scenario. Measurement noise covariance matrix at time Then, the NLOS scenario is updated according to the filtering convergence criterion. Process noise covariance matrix at time step ;

[0075] Will The measurement noise covariance matrix updated at each time step under either the LOS or NLOS scenario is used as... Measurement noise covariance matrix at time ,Will The updated process noise covariance matrix at each time step under either LOS or NLOS scenarios is used as... Process noise covariance matrix at time step ;

[0076] use Process noise covariance matrix at time step Recalculate from Time's up State error covariance matrix at time 1 And thus obtain from Time's up State error prediction vector at time step ;

[0077] Based on the recalculated state error covariance matrix ,renew Kalman gain at time step and utilize Time-information vector Perform posterior estimation and covariance update of the INS state error to obtain... The posterior estimate vector of the INS state error at time 1 With the state error covariance matrix The state error covariance matrix is ​​shown below. The recursive estimation process for the next time step;

[0078] use The posterior estimate vector of the INS state error at time 1 The INS pre-integration results are corrected, and then loosely coupled and fused with the current UWB position observations to obtain... Indoor positioning results of constantly moving nodes.

[0079] Furthermore, the method utilizes a sliding estimation window and a forgetting factor. The noise covariance of the UWB location observations detected as LOS values ​​is adaptively estimated, and the LOS scene is updated based on the estimation results. Measurement noise covariance matrix at time process noise covariance matrix The specific method is as follows:

[0080] according to UWB position observations at time 10:00 With From Time's up State error prediction vector at time step ,calculate Residual vector at time step ;

[0081] Within the sliding estimation window, using Residual vector at time step and the front of the window The residual vector at each historical moment is calculated. Residual covariance matrix at time step ;

[0082] Within the sliding estimation window, using The new vector at time and the front of the window The information vector at each historical moment is calculated. The new covariance matrix at time 1 ;

[0083] Get Residual covariance matrix at time step and to Adaptive estimation of the measurement noise covariance matrix at time step is performed to obtain... Estimated value of the measurement noise covariance matrix at time step ;

[0084] based on The new covariance matrix at time 1 and Kalman gain ,right The process noise covariance matrix at time step is adaptively estimated to obtain... Estimated value of process noise covariance matrix at time step ;

[0085] based on Estimated value of the measurement noise covariance matrix at time step Estimated values ​​of process noise covariance matrix Utilizing the forgetting factor To each Measurement noise covariance matrix at time process noise covariance matrix Perform adaptive updates to obtain the LOS scenario. Measurement noise covariance matrix at time process noise covariance matrix .

[0086] Furthermore, by utilizing a sliding estimation window, the mean of the innovation vector is statistically analyzed for the UWB location observations detected as NLOS values, and the updated LOS scene is then analyzed based on the statistical results. Measurement noise covariance matrix at time Perform error compensation and update the NLOS scenario. Measurement noise covariance matrix at time Then, the NLOS scenario is updated according to the filtering convergence criterion. Process noise covariance matrix at time step The specific method is as follows:

[0087] Within the sliding estimation window, using The new vector at time and the front of the window The information vector at each historical moment is calculated. Statistical mean of the innovation vector at time step ;

[0088] use Statistical mean of the innovation vector at time step The adaptive update obtained in the LOS scenario Time-based measurement noise covariance matrix Perform NLOS error compensation to obtain the updated NLOS scenario. Measurement noise covariance matrix at time ;

[0089] Updated based on NLOS scenario Measurement noise covariance matrix at time Calculate the adjustment index based on the filtering convergence criterion. And adjust the indicators accordingly. Update NLOS scenarios Process noise covariance matrix at time step .

[0090] On the other hand, this invention proposes an indoor positioning system based on multi-source constrained loosely coupled asynchronous INS / UWB, which includes:

[0091] The UWB data acquisition module is used to acquire distance observation values ​​of mobile nodes from several UWB base stations in real time, forming a distance observation sequence for each UWB base station.

[0092] The NLOS suppression processing module is used to process the distance observation sequence of each UWB base station using the expectation-maximization (EM) algorithm and the K-Medoids clustering algorithm respectively, and selects the best of the two processing results to obtain the current LOS measurement value of the UWB base station.

[0093] The IMU data acquisition module is used to collect acceleration and rotation speed information of the moving node;

[0094] The IMU pre-integration processing module is used to construct the INS dynamic model using the Euler angle method and perform pre-integration calculations based on the acceleration and rotation speed information to obtain the INS pre-integration result at the current moment.

[0095] The UWB location calculation module is used to treat UWB base stations that have obtained the LOS measurement value at the current time as valid base stations, and calculate the UWB location observation value at the current time based on the LOS measurement values ​​at the current time of all valid base stations.

[0096] The INS / UWB fusion positioning module is used to combine the INS pre-integration result at the current moment to verify the validity of the UWB position observation at the current moment. If the verification result is valid, an improved adaptive extended Kalman filter is used to fuse the UWB position observation at the current moment and the INS pre-integration result to obtain the indoor positioning result of the mobile node. If the verification result is invalid, the INS pre-integration result at the current moment is used as the indoor positioning result of the mobile node.

[0097] The beneficial effects of adopting the above technical solution are as follows:

[0098] (1) A two-stage NLOS error suppression architecture was constructed: In the initial UWB data processing stage, the method of this invention initially suppressed NLOS interference through a hybrid processing mechanism combining the Expectation-Maximization (EM) algorithm and the K-Mediods clustering algorithm, achieving preliminary identification and filtering of non-line-of-sight propagation errors. In the back-end data fusion stage, the method of this invention adopted an improved adaptive extended Kalman filter (AEKF) and introduced a hypothesis testing method based on the chi-square test of the innovation to perform secondary NLOS discrimination on the INS / UWB positioning difference and dynamically adjust the noise covariance matrix, forming a multi-constraint error control system. Through the improved adaptive extended Kalman filter, a differentiated noise covariance update mechanism for LOS / NLOS environments was constructed. In dynamic multipath, signal occlusion, and sudden interference scenarios, this method can effectively suppress filter divergence and maintain stable positioning performance.

[0099] (2) A robust adaptive filtering update mechanism was designed: In the LOS scenario, the method of this invention utilizes residual information to adaptively update the covariance of process noise and observation noise. In the NLOS scenario, the covariance matrix is ​​compensated to reduce the NLOS observation weights. Finally, the filter state is updated, and the fusion result is used to correct the INS accumulated error, effectively alleviating the filter divergence problem caused by measurement anomalies. In the LOS environment, the positioning accuracy is outstanding due to the complementary advantages of INS and UWB and the filtering optimization. In the NLOS environment, the NLOS interference is effectively suppressed through data preprocessing and secondary recognition mechanisms, the positioning accuracy is maintained at an excellent level, and the INS accumulated error is eliminated, maintaining a stable high-precision output, which is superior to existing algorithms.

[0100] (3) Verify the effectiveness of the algorithm through a multi-scenario verification system: By simulating scenarios in which the NLOS error follows a Gamma distribution and a Gaussian distribution, and covering the changes in NLOS probability, mean error, and standard deviation of error, and comparing it with existing algorithms such as PR-PF, SHFAF, and RIEKF, as well as conducting field tests in indoor scenarios with obstacles and random pedestrians, it has been confirmed that the method of the present invention exhibits robust performance in target node localization, providing a feasible solution for high-precision indoor positioning systems.

[0101] In summary, this invention employs a loosely coupled architecture and adaptive filtering design, which maintains low computational complexity while ensuring positioning accuracy, meets the real-time requirements of indoor dynamic positioning, and provides a feasible engineering solution for high-precision indoor positioning systems. Attached Figure Description

[0102] Figure 1 This is a flowchart of the multi-source constrained loosely coupled asynchronous INS / UWB indoor positioning method in this embodiment;

[0103] Figure 2 This is a schematic diagram of the process of the multi-source constrained loosely coupled asynchronous INS / UWB indoor positioning method in this embodiment;

[0104] Figure 3 The CDF curves of the positioning error of the method in Example 1 compared with those of PR-PF, SHFAF, and RIEKF algorithms are shown when the NLOS error follows a gamma distribution.

[0105] Figure 4 This is a schematic diagram of the root mean square error of the method in Example 1 under different types of NLOS error probabilities (0.1-0.9) when the NLOS error follows a gamma distribution.

[0106] Figure 5 This is a schematic diagram of the root mean square error of the method in Example 1 under the average value (4m-10m) of different NLOS errors when the NLOS error follows a gamma distribution.

[0107] Figure 6 This is a schematic diagram of the root mean square error of the method in Example 1 under different standard deviations (0.5m-1.9m) of NLOS errors when the NLOS error follows a gamma distribution.

[0108] Figure 7 The CDF curves of the positioning error of the method in Example 1 compared with those of PR-PF, SHFAF, and RIEKF algorithms are shown when the NLOS error follows a Gaussian distribution.

[0109] Figure 8 This is a schematic diagram of the root mean square error of the method in Example 1 under different types of NLOS error probabilities (0.1-0.8) when the NLOS error follows a Gaussian distribution.

[0110] Figure 9 This is a schematic diagram of the root mean square error of the method in Example 1 under the average value (3m-9m) of different NLOS errors when the NLOS error follows a Gaussian distribution.

[0111] Figure 10 This is a schematic diagram of the root mean square error of the method in Example 1 under different standard deviations (4m-10m) of NLOS when the NLOS error follows a Gaussian distribution;

[0112] Figure 11 This is a structural diagram of the multi-source constrained loosely coupled asynchronous INS / UWB indoor positioning system in this embodiment. Detailed Implementation

[0113] To facilitate understanding of this application, specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and embodiments. The following embodiments are illustrative of the invention but are not intended to limit its scope. Rather, these embodiments are provided to provide a more thorough and complete understanding of the disclosure of this application.

[0114] Example 1:

[0115] This embodiment presents a multi-source constrained loosely coupled asynchronous INS / UWB indoor positioning method, such as... Figure 1 As shown, the method includes the following steps:

[0116] The distance observation values ​​of several UWB base stations to the mobile node are acquired in real time, forming a distance observation sequence for each UWB base station.

[0117] The distance observation value is: under the condition that the mobile node and the UWB base station keep time synchronized, the one-way arrival time (ToA) of the UWB signal from the mobile node to the UWB base station is measured, and the measured one-way arrival time is converted into the distance observation value according to the electromagnetic wave propagation speed.

[0118] In this embodiment, as Figure 2 As shown, using an ultra-wideband receiver installed on the mobile node, the data from each UWB base station to the mobile node is obtained. The results of this observation Each distance observation value is used to form a distance observation sequence for each UWB base station.

[0119] For each UWB base station's distance observation sequence, the expectation-maximization (EM) algorithm and the K-Medoids clustering algorithm are used to process the distance observation sequence, and the optimal result of the two processing methods is selected to obtain the current time LOS measurement value of the UWB base station.

[0120] In this embodiment, in order to distinguish between LOS and NLOS data, the Expectation-Maximization (EM) algorithm and the K-Medoids clustering algorithm are combined to process the distance observation sequence observed by the UWB base station, and the smaller value in the processing result is selected as the LOS measurement value to achieve preliminary NLOS suppression processing.

[0121] The specific method for obtaining the current LOS measurement value of each UWB base station by processing the distance observation sequence using both the Expectation-Maximization (EM) algorithm and the K-Medoids clustering algorithm, and selecting the optimal result from the two processing methods, is as follows:

[0122] For the The distance observation sequence of each UWB base station is processed using the Expectation-Maximization (EM) algorithm to generate the first UWB observation value.

[0123] The for the first The method for generating the first UWB observation value by processing the distance observation sequence of a UWB base station using the Expectation-Maximization (EM) algorithm is as follows:

[0124] For the The UWB base station in the first Distance observation value obtained during the second observation The measurement error is defined as the distance observed. and the The UWB base station in the first The true distance to the moving node at the time of the second observation The difference between them.

[0125] The actual distance value For: the The UWB base station and mobile node in the 1st The Euclidean distance at the time of the second observation, in this embodiment, is the true distance value. It is calculated based on the base station coordinates of the UWB base station and the actual location of the mobile node.

[0126] The measurement error is modeled using a Gaussian Mixture Model (GMM) to obtain a probability density model of the measurement error.

[0127] ;

[0128] in, For the first The UWB base station in the first The distance observation value obtained during the second observation; Distance observation value The probability density function; For the first The weights of the Gaussian components, and In this embodiment, the number of Gaussian components is 2, representing LOS error and NLOS error respectively. For the first The mean of the Gaussian components; For the first The covariance matrix of Gaussian components; Indicates For the mean, The variance of the first Each Gaussian component, at a distance from the observed value The probability density at that location.

[0129] The parameter set of the probability density model Recorded as .

[0130] According to the Distance observation sequences of UWB base stations, defining parameter sets. The log-likelihood function.

[0131] ;

[0132] in, For observations at a given distance Under the condition, parameter set The log-likelihood function.

[0133] The log-likelihood function is solved using the Expectation-Maximization (EM) algorithm to obtain the parameter set. The optimal estimate.

[0134] The specific process of solving the log-likelihood function using the Expectation-Maximization (EM) algorithm is as follows:

[0135] In each iteration, the E-Step and M-Step are executed sequentially.

[0136] No. The E-Step of the round of iteration is: based on the first... After each iteration, the parameter estimates are updated, and the posterior probability of each distance observation belonging to each Gaussian component is calculated.

[0137] ;

[0138] in, To achieve a given observation distance value Under the conditions, Belongs to the The first Gaussian component Posterior probability; , , All are the first The parameter estimates after each iteration represent, respectively After the first iteration update The weights, covariance matrix, and variance of each Gaussian component; in this embodiment, the posterior probability is the core calculation result of the E-Step in the Expectation Maximization (EM) algorithm, used to measure the degree to which a single observation belongs to a certain Gaussian component.

[0139] No. The M-Step of the iterative execution round is as follows: Update the weights, covariance matrix, and variance of each Gaussian component according to the posterior probability, as the result of the first iteration. The parameter estimates after rounds of iterative updates.

[0140] ;

[0141] ;

[0142] ;

[0143] in, Indicates the first After the first iteration update The weights of each Gaussian component; Indicates the first After the first iteration update The covariance matrix of Gaussian components; Indicates the first After the first iteration update The variance of each Gaussian component.

[0144] After each iteration, the difference between the parameter estimate updated in the current iteration and the parameter estimate updated in the previous iteration is calculated. The iteration terminates when the calculated difference is lower than the preset threshold for two consecutive iterations.

[0145] In this embodiment, the first step is completed. After each round of iteration, calculate respectively and The difference and The difference and and The difference is calculated, and it is determined whether the three calculated differences are lower than the preset threshold. The iteration terminates when all the calculated differences are lower than the preset threshold in two consecutive iterations.

[0146] It should be noted that since the weights, covariance matrix, and variance have different dimensions, they each require corresponding preset thresholds. However, in practical applications, to simplify parameter configuration and reduce computational complexity, this embodiment uses the mean difference as a benchmark, and the convergence of the mean difference is used as the primary criterion for iteration termination. Therefore, the range of the preset threshold used for comparison with the mean difference is set to... The unit is meters (m). Preferably, in this embodiment, the preset threshold is set to... .

[0147] The parameter set obtained in the last iteration In this study, the Gaussian component with the smallest covariance matrix is ​​selected, and the mean of this Gaussian component is used as the UWB measurement value under the LOS environment.

[0148] If distance observation value If the distance observation is greater than the UWB measurement value under the LOS environment, then the UWB measurement value under the LOS environment is taken as the first UWB observation value; otherwise, the distance observation value is taken as the first UWB observation value. This is the first UWB observation.

[0149] In this embodiment, the UWB measurements and distance observations under the LOS environment are compared. For distance observations Perform NLOS error correction.

[0150] The distance observation sequence was preprocessed using the K-Medoids clustering algorithm to generate the second UWB observation value.

[0151] The specific method for preprocessing the distance observation sequence using the K-Medoids clustering algorithm to generate the second UWB observation value is as follows:

[0152] For the The distance observation sequence of each UWB base station was used, with a cluster size of 2, corresponding to LOS and NLOS measurements respectively, and the initial cluster center of each cluster was randomly selected.

[0153] For each round of clustering, perform the following operations:

[0154] Each distance observation in the distance observation sequence is treated as a data item, and the Euclidean distance between each data item and the current cluster centers is calculated. The data item is then assigned to the cluster to which the nearest cluster center belongs.

[0155] For each cluster, iterate through all data items in the cluster, calculate the sum of the squared distances from all data items in the cluster to the currently traversed data item, and select the data item with the smallest sum of squared distances as the new cluster center for the cluster.

[0156] Calculate the Euclidean distance between the new cluster centers and the previous cluster centers. If the Euclidean distance is less than a predetermined convergence threshold, this embodiment sets the convergence threshold to... If the clustering process fails, the clustering process terminates; otherwise, the next round of clustering begins.

[0157] After clustering, two cluster centers are obtained corresponding to the LOS and NLOS measurements, and the cluster center corresponding to the LOS measurement is used as the second UWB observation.

[0158] In this embodiment, only the cluster centers with smaller values ​​corresponding to the LOS measurement values ​​are selected as the second UWB observations, while the cluster centers corresponding to the NLOS measurement values ​​are not included in the subsequent localization calculation.

[0159] The smaller UWB observation value between the first and second UWB observation values ​​is selected as the LOS measurement value of the UWB base station at the current time.

[0160] In this embodiment, as Figure 2 As shown, by comparing the output localization results of the Expectation Maximization (EM) algorithm and the K-Mediods clustering algorithm, i.e., the first UWB observation... Second UWB observation .

[0161] Choose the smaller UWB observation value as the current time. The LOS measurement value of each UWB base station is expressed as follows:

[0162] ;

[0163] in, For the first The UWB base station in the first The corresponding UWB observation value at the time of the observation; For the first The UWB base station in the first The first UWB observation value corresponding to the next observation; For the first The UWB base station in the first The second UWB observation value corresponding to the next observation.

[0164] Acceleration and rotational speed information of the mobile node (MN) are collected, an INS dynamic model is constructed using Euler angles, and pre-integration calculation is performed based on the acceleration and rotational speed information to obtain the INS pre-integration result at the current time.

[0165] The method for collecting the acceleration and rotational speed information of the mobile node (MN), constructing an INS dynamic model using the Euler angle method, and performing pre-integration calculations based on the acceleration and rotational speed information to obtain the INS pre-integration result at the current moment is as follows:

[0166] The acceleration and rotation speed information of the mobile node are collected using the inertial measurement unit (IMU) mounted on the mobile node.

[0167] It should be noted that the inertial measurement unit (IMU) and the navigation calculation algorithm constitute the inertial navigation system (INS). The IMU, mounted on the mobile node, includes a gyroscope and an accelerometer, used to collect the angular velocity (i.e., rotational speed information) and acceleration of the mobile node, respectively. The navigation calculation algorithm calculates the position, velocity, and attitude of the mobile node by integrating and recursively applying the collected information.

[0168] To achieve INS positioning modeling, this embodiment uses the Euler angle method to construct an INS dynamic model. The attitude of the moving node is described by defining three basic rotational degrees of freedom (roll, pitch, and yaw), and corresponding state vectors and error state vectors are established for subsequent INS / UWB fusion positioning.

[0169] Specifically, the state vector of INS is defined as follows:

[0170] ;

[0171] in, for The state vector of INS at any given time; Let be the Euler angles of the moving node, and , , and These represent the three basic rotational degrees of freedom of the moving node in space: roll, pitch, and yaw. and To represent the velocity and position components of the moving node in the navigation coordinate system, this embodiment establishes a northeast-downward coordinate system as the navigation coordinate system, with a fixed point in space as the origin. , ; This represents the northward velocity component of the moving node in the northeast-down coordinate system; This represents the eastward velocity component of MN in the northeast-down coordinate system; This represents the downward velocity component of MN in the northeast-down coordinate system; This represents the northward position component of MN in the northeast-down coordinate system; This represents the eastward position component of MN in the northeast-down coordinate system; This represents the downward position component of MN in the northeast-down coordinate system; and These are the inherent biases in gyroscope and accelerometer measurements, respectively.

[0172] The error state vector of INS is defined as follows:

[0173] ;

[0174] in, for The error state vector of INS at time 1; Let be the Euler angle error vector of the moving node; This is the velocity error vector of the moving node in the navigation coordinate system; This is the position error vector of the moving node in the navigation coordinate system; This is the bias error vector of the gyroscope; This is the bias error vector of the accelerometer.

[0175] The collected rotation speed information is pre-integrated to obtain the attitude angle of the mobile node in the IMU coordinate system. The attitude angle is then transformed to the navigation coordinate system using the direction cosine matrix to obtain the attitude of the mobile node.

[0176] In this embodiment, the attitude pre-integration is performed on the angular velocities acquired by the IMU over a period of time to obtain the attitude angles of the mobile node in the local coordinate system (i.e., the IMU coordinate system). The origin of the IMU coordinate system is the measurement center of the IMU, and the X, Y, and Z axes of the IMU coordinate system are aligned with the measurement axes of the gyroscope and accelerometer inside the IMU, respectively. The attitude of the mobile node is then obtained by transforming the coordinate system to the global coordinate system (i.e., the navigation coordinate system) using the direction cosine matrix.

[0177] The direction cosine rotation matrix is ​​represented as:

[0178] ;

[0179] in, This is the direction cosine rotation matrix from the IMU coordinate system to the navigation coordinate system, used to transform the local attitude information from the IMU coordinate system to the navigation coordinate system. This represents the initial attitude rotation matrix of the mobile node in the IMU coordinate system; and These are the rotation matrices of the mobile node around the X, Y, and Z axes in the IMU coordinate system, corresponding to the roll, pitch, and yaw attitudes of the mobile node. and They are defined as follows:

[0180] , , ;

[0181] The collected accelerations are pre-integrated to obtain the velocity and position of the moving node.

[0182] The obtained attitude, velocity, and position of the mobile node are used as the INS pre-integration results at the current time step.

[0183] UWB base stations that have obtained LOS measurements at the current time are considered valid base stations. The UWB location observation value at the current time is calculated based on the LOS measurements of all valid base stations at the current time. The validity of the UWB location observation value at the current time is verified by combining the INS pre-integration results at the current time.

[0184] The specific method for considering UWB base stations that have obtained LOS measurements at the current time as valid base stations, calculating the UWB location observation value at the current time based on the current LOS measurements of all valid base stations, and combining the INS pre-integration results at the current time to verify the validity of the UWB location observation value at the current time is as follows:

[0185] UWB base stations that have obtained the LOS measurement value at the current time are considered as valid base stations, and the number of valid base stations at the current time is counted.

[0186] It should be noted that during the process of processing the distance observation sequence of each UWB base station using both the Expectation-Maximization (EM) algorithm and the K-Medoids clustering algorithm, and then selecting the optimal result from the two methods (i.e., preliminary NLOS suppression processing), both the EM algorithm and the K-Medoids clustering algorithm can fail. If both fail, the UWB base station cannot output a LOS measurement value at that time, and is therefore considered an invalid base station. Only base stations that successfully output LOS measurements are considered valid base stations and participate in subsequent calculations.

[0187] Based on the current LOS measurements of all valid base stations, the UWB location observation at the current time is calculated using least squares estimation.

[0188] In this embodiment, least squares estimation is used to solve for the UWB location observations. This involves obtaining the known locations of all valid base stations and finding an optimal location that minimizes the sum of the squares of the differences between the calculated distances from this optimal location to each base station and the corresponding LOS measurements. This optimal location is then the UWB location observation for the current moment. The validity of this optimal location is then verified by combining it with the INS pre-integration results for the current moment. If a valid UWB location observation exists at the current moment, the UWB location observation and the INS pre-integration results (i.e., the position, velocity, and attitude of the mobile node) are input into an improved adaptive extended Kalman filter (AEKF) for fusion. A chi-square test is used to identify the NLOS environment, and the noise covariance matrix is ​​adaptively adjusted to obtain a high-precision indoor positioning result for the mobile node in the global coordinate system. If no valid UWB location observation exists at the current moment, the INS pre-integration results are directly used as the positioning result for the current moment, achieving seamless integration of UWB / INS combined positioning.

[0189] If the number of valid base stations at the current time meets the preset conditions, then the convergence verification of the UWB location observation value at the current time is performed; otherwise, the UWB location observation value at the current time is determined to be invalid.

[0190] In this embodiment, the number of UWB base stations that can output valid LOS measurements after preliminary NLOS suppression processing at the current time is counted. If the number of UWB base stations counted meets the preset condition, that is, not less than 3, the convergence of the solution result is further verified; if the number of UWB base stations counted is less than 3, the UWB position observation value at the current time is directly determined to be invalid.

[0191] The convergence verification is as follows: obtain the base station coordinates of all valid base stations at the current time, and combine the LOS measurement values ​​of all valid base stations at the current time with the UWB position observation values ​​at the current time to calculate the least squares estimated residual sum of squares. If the calculated residual sum of squares is not greater than the preset residual threshold, then the position consistency verification is further performed by combining the INS pre-integration results at the current time; otherwise, the UWB position observation values ​​at the current time are determined to be invalid.

[0192] In this embodiment, the theoretical distance of each effective base station is calculated based on the base station coordinates of all effective base stations at the current time and the current UWB location observation value. Then, based on the current LOS measurement value of each effective base station, the residual square of each effective base station is calculated. Finally, the least squares estimated residual square sum is obtained by summing the residual squares of all effective base stations. The preset residual threshold ranges from 0.3m² to 0.8m², and is preferably 0.5m² in this embodiment.

[0193] The position consistency check is performed by calculating the Euclidean distance difference between the current UWB position observation and the position of the moving node in the current INS pre-integration result. If the Euclidean distance difference is not greater than a preset position deviation threshold, the current UWB position observation is determined to be valid; otherwise, the current UWB position observation is determined to be invalid.

[0194] The preset position deviation threshold ranges from 2m to 5m, and is preferably 3m in this embodiment.

[0195] If the test result is valid, an improved adaptive extended Kalman filter is used to fuse the current UWB position observation and INS pre-integration result to obtain the indoor positioning result of the mobile node.

[0196] If the test result is invalid, the INS pre-integration result at the current moment will be used as the indoor positioning result of the mobile node.

[0197] In this embodiment, the difference between the UWB location observation and the location in the INS pre-integration result is input into an improved adaptive extended Kalman filter (AEKF). The NLOS is identified twice by using a chi-square test based on statistical analysis of the difference between the predicted value and the actual observation value at each time point. The noise covariance matrix is ​​adaptively adjusted for LOS and NLOS environments respectively, and the INS positioning is finally corrected using the filtering result.

[0198] The specific method for fusing the current UWB location observation and INS pre-integration results using an improved adaptive extended Kalman filter to obtain the indoor positioning result of the mobile node is as follows:

[0199] Initialize the state error covariance matrix And set the window size of the sliding estimation window. Forgetting factor adaptively updated by filtering .

[0200] In this embodiment, the window size of the sliding estimation window is... As preset empirical parameters, with Increasing the value results in greater system stability, but this enhancement comes at the cost of increased computational demands. Conversely, decreasing... A value of will speed up the system's response, but it will also make it more susceptible to noise interference. Therefore, this embodiment will... The value of the forgetting factor is set to 10. This is a preset empirical parameter used in the adaptive update process of the filtered noise covariance to balance the weights of historical and current observation data in noise estimation. With... Increasing the value of historical data increases its weight and improves the stability of the filtering system, but reduces its response speed to dynamic environmental changes; conversely, decreasing the value of historical data increases its weight and improves its stability. The value of will increase the weight of the current observation data and speed up the filter's response to changes in environmental noise, but it will also make the filter more susceptible to interference from instantaneous observation noise. Therefore, in this embodiment... The value range is 0.95-0.99, with a preferred value of 0.98.

[0201] Based on the error state vector of the INS, the error state transition equation and the error state observation equation are established.

[0202] ;

[0203] ;

[0204] in, From Time's up The state error prediction vector at time step, i.e., based on The prediction obtained from the posterior time result Prior estimate vector of INS state error at time step; for The posterior estimate vector of the INS state error at time 1; express The observation prediction vector at time; This represents a nonlinear state transition function used to describe the recursive relationship of the INS system state over time. This represents a nonlinear observation function used to establish a mapping relationship between the INS system state and UWB position observations; for Time-based process noise, , express The process noise covariance matrix at time step is used to describe the uncertainty introduced by environmental disturbances, high-frequency noise from sensors, and other factors during the state transition of the INS system. for Measure noise at all times. , express The measurement noise covariance matrix at time is used to describe the uncertainties introduced by factors such as ranging error and NLOS interference during UWB observation.

[0205] For the nonlinear state transition function in The posterior estimate vector of the INS state error at time 1 Perform a Taylor expansion at the given point, and label the coefficients of the first-order terms after the expansion as the state transition Jacobian matrix. .

[0206] For the nonlinear observation function from Time's up State error prediction vector at time step Perform a Taylor expansion at the given location, and label the coefficients of the first-order terms after the expansion as the observed Jacobian matrix. .

[0207] ;

[0208] ;

[0209] Based on the state transition Jacobian matrix , for from Time's up State error prediction vector at time step and state error covariance matrix Perform forward prediction; the prediction model is:

[0210] ;

[0211] ;

[0212] in, for The state error covariance matrix at time t.

[0213] Based on the observed Jacobian matrix ,calculate The new vector at time and new information vector covariance matrix , is represented as:

[0214] ;

[0215] ;

[0216] in, for UWB position observation at time; for The measurement noise covariance matrix at time.

[0217] Using from Time's up State error covariance matrix at time 1 New information vector covariance matrix And the observation Jacobian matrix ,calculate Kalman gain at time step Used for weighted fusion of state prediction and observation information, represented as:

[0218] ;

[0219] In this embodiment, during the data fusion process, each time step uses data based on the current... The new vector at time The chi-square test is used to identify NLOS interference in UWB location observations. If the statistical test value falls within the validation threshold, the observation is determined to have been acquired in a LOS environment; otherwise, the observation is considered to have NLOS interference. Under ideal conditions, the innovation vector... It should possess the characteristics of zero-mean Gaussian white noise, and when the UWB position observations are disturbed by external factors, the innovation vector... The zero-mean property will also be destroyed.

[0220] right The new vector at time Perform the chi-square test and calculate. Test statistic at time ,like Then it is believed The UWB position observation at time t is detected as the LOS value; if Then it is believed The UWB position observation at time t was detected as an NLOS value.

[0221] The test statistic for:

[0222] ;

[0223] The pair The new vector at time The process of performing the chi-square test is as follows:

[0224] The hypothesis test is constructed as follows:

[0225] ;

[0226] in, As per the null hypothesis, the corresponding UWB location observations come from the LOS environment; As an alternative hypothesis, the corresponding UWB location observations are from the NLOS environment; This represents a chi-square distribution with 2 degrees of freedom, where 2 degrees of freedom corresponds to the two-dimensional planar position coordinates of the UWB position observations.

[0227] The test threshold For example, under a chi-square distribution with 2 degrees of freedom, the critical value corresponding to the significance level of this test is calculated as follows:

[0228] ;

[0229] in, This indicates the probability of a correct detection of a UWB location observation originating from a LOS environment; This represents the false alarm probability, which is the acceptable probability of misclassifying a LOS environment as an NLOS environment during the test. This false alarm probability is the significance level of this hypothesis test. Let be the probability density function of a chi-square distribution with 2 degrees of freedom, and let be the two-dimensional planar position coordinates of the UWB position observations with 2 degrees of freedom.

[0230] Test statistic With threshold The comparison yields the NLOS recognition result, which is represented as follows:

[0231] ;

[0232] The results of the chi-square test described above serve as the basis for subsequent processing of the noise covariance matrix. To ensure the robustness of the filtering algorithm and prevent outliers from causing estimation bias and performance oscillations, the entire process from residual vector calculation to adaptive estimation and updating of the noise covariance only uses UWB observations detected as LOS values; NLOS values ​​are not involved in any calculation steps of this process. In subsequent processes, if... (NLOS environment): Compensation measurement noise covariance matrix, synchronous compensation process noise covariance matrix; if (LOS environment): Adaptive estimation of process noise covariance matrix.

[0233] Using a sliding estimation window and a forgetting factor The noise covariance of the UWB location observations detected as LOS values ​​is adaptively estimated, and the LOS scene is updated based on the estimation results. Measurement noise covariance matrix at time process noise covariance matrix .

[0234] In this embodiment, for UWB location observations detected as LOS values, the following complete residual calculation, residual covariance and innovation covariance statistics, measurement / process noise covariance adaptive estimation and update process are performed, as follows:

[0235] according to UWB position observations at time 10:00 With From Time's up State error prediction vector at time step (Right now (Prior prediction of state error at time step), calculation Residual vector at time step .

[0236] In this embodiment, if the UWB location observation is detected as a LOS value, the residual vector is calculated during the state update process using the difference between the actual observation and the predicted observation. , is represented as:

[0237] ;

[0238] Within the sliding estimation window, using Residual vector at time step and the front of the window The residual vector at each historical moment is calculated. Residual covariance matrix at time step .

[0239] ;

[0240] in, Indicates the first time within the sliding estimation window The residual vector at time step; the sliding estimation window takes the current time step. Time and the past The residual vectors at each historical moment are statistically analyzed within a window, with the window size being... These are the system's preset empirical parameters.

[0241] Within the sliding estimation window, using The new vector at time and the front of the window The information vector at each historical moment is calculated. The new covariance matrix at time 1 .

[0242] ;

[0243] in, Indicates the first time within the sliding estimation window The information vector at any given moment.

[0244] Get Residual covariance matrix at time step and to Adaptive estimation of the measurement noise covariance matrix at time step is performed to obtain... Estimated value of the measurement noise covariance matrix at time step , is represented as:

[0245] ;

[0246] based on The new covariance matrix at time 1 and Kalman gain ,right The process noise covariance matrix at time step is adaptively estimated to obtain... Estimated value of process noise covariance matrix at time step , is represented as:

[0247] ;

[0248] in, It is a mathematical expectation operator used for statistical estimation of the second moment of a random variable; for Process noise at any given moment.

[0249] based on Estimated value of the measurement noise covariance matrix at time step Estimated values ​​of process noise covariance matrix Utilizing the forgetting factor To each Measurement noise covariance matrix at time process noise covariance matrix Perform adaptive updates to obtain the LOS scenario. Measurement noise covariance matrix at time process noise covariance matrix .

[0250] ;

[0251] ;

[0252] Using a sliding estimation window, the mean of the innovation vector is statistically analyzed for UWB location observations detected as NLOS values, and the updated LOS values ​​are then analyzed based on the statistical results. Measurement noise covariance matrix at time Perform error compensation and update the NLOS scenario. Measurement noise covariance matrix at time Then, the NLOS scenario is updated according to the filtering convergence criterion. Process noise covariance matrix at time step .

[0253] In this embodiment, if an observation is detected as an NLOS value, it does not participate in the residual statistics and covariance adaptive estimation process of the LOS value. Instead, based on the noise covariance obtained by the LOS scene adaptive update, the change in the innovation covariance is further considered. By compensating and increasing the value of the measurement noise covariance matrix, the weight of NLOS outlier observations in the filter update is reduced, and the value of the measurement noise covariance matrix is ​​increased accordingly. The specific process is as follows:

[0254] Within the sliding estimation window, using The new vector at time and the front of the window The information vector at each historical moment is calculated. Statistical mean of the innovation vector at time step .

[0255] use Statistical mean of the innovation vector at time step The adaptive update obtained in the LOS scenario Time-based measurement noise covariance matrix Perform NLOS error compensation to obtain the updated NLOS scenario. Measurement noise covariance matrix at time , is represented as:

[0256] ;

[0257] Updated based on NLOS scenario Measurement noise covariance matrix at time Calculate the adjustment index based on the filtering convergence criterion. And adjust the indicators accordingly. Update NLOS scenarios Process noise covariance matrix at time step .

[0258] Specifically, by adjusting the measurement noise covariance matrix as described above, the updated [matrix] in the current NLOS scenario is [achieved / restored]. Measurement noise covariance matrix at time This is considered an accurate value to achieve optimal filtering performance. Under the most stringent filtering convergence criterion:

[0259] ;

[0260] in, Represents the trace of a matrix; Indicates multiplication operation; To adjust the indicators, the specific representation is as follows:

[0261] ;

[0262] like This indicates that the actual noise of the current INS system is greater than the noise setting of the current INS system, and the process noise of the INS system needs to be increased to prevent filter divergence; if This indicates that the actual noise of the current INS system is equal to the set noise of the current INS system, and the filter has strictly converged; if If the actual noise of the current INS system is less than the set noise of the current INS system, then the filter has converged.

[0263] In summary, to prevent filter divergence, the updated filter in the NLOS scenario... Process noise covariance matrix at time step for:

[0264] ;

[0265] in, express The process noise covariance matrix before the update time step is typically considered an intrinsic property of the system (or a slowly changing quantity) during the Kalman filter recursion. Therefore, the process noise covariance matrix at each time step is passed to the next time step, i.e., here... Process noise covariance matrix before time update In fact The process noise covariance matrix obtained after adaptive update The adjusted process noise covariance matrix is ​​used to re-predict the state error covariance matrix, thus completing the adaptive correction of the prediction covariance.

[0266] Will The measurement noise covariance matrix updated at each time step under either the LOS or NLOS scenario is used as... Measurement noise covariance matrix at time ,Will The updated process noise covariance matrix at each time step under either LOS or NLOS scenarios is used as... Process noise covariance matrix at time step .

[0267] use Process noise covariance matrix at time step Recalculate from Time's up State error covariance matrix at time 1 And thus obtain from Time's up State error prediction vector at time step .

[0268] Based on the recalculated state error covariance matrix ,renew Kalman gain at time step and utilize Time-information vector Perform posterior estimation and covariance update of the INS state error to obtain... The posterior estimate vector of the INS state error at time 1 With the state error covariance matrix The state error covariance matrix is ​​shown below. The recursive estimation process for the next time step.

[0269] use The posterior estimate vector of the INS state error at time 1 The INS pre-integration results are corrected, and then loosely coupled and fused with the current UWB position observations to obtain... Indoor positioning results of constantly moving nodes.

[0270] In this embodiment, the estimated Time-state error (including the posterior estimate vector of INS state error) With error covariance matrix Feedback is sent to the INS system to correct the output of the INS system. The position, velocity, and attitude at each moment (i.e., the INS pre-integration results) are then loosely coupled and fused with the current UWB position observations to finally output the moving node in the global coordinate system. High-precision indoor positioning results are achieved in real time. The improved adaptive extended Kalman filter enables loosely coupled INS / UWB fusion positioning, effectively suppressing the influence of NLOS errors and improving the system's positioning accuracy and robustness.

[0271] Example 2:

[0272] This embodiment comprehensively verifies the effectiveness and superiority of the multi-source constrained loosely coupled asynchronous INS / UWB indoor positioning method proposed in Embodiment 1 (hereinafter referred to as the Embodiment 1 method) through both simulation verification and real experimental verification.

[0273] The simulation verification uses Nave Go to build the simulation environment. The mobile node MN is equipped with an IMU and a UWB receiver. The movement trajectory of the mobile node MN is limited to the same height plane and includes multiple sharp turns to simulate complex dynamic motion. The anchor nodes of the UWB positioning system are randomly distributed in a designated square area and are at the same horizontal height as MN. MN calculates the distance to each anchor node using the time of arrival (TOA) measurement technology. During the simulation, line-of-sight (LOS) noise following a Gaussian distribution and non-line-of-sight (NLOS) noise using gamma and Gaussian distributions respectively are set to simulate the signal propagation uncertainty under different NLOS conditions. At the same time, random NLOS probability values ​​are generated and compared with a preset threshold to simulate the randomness of noise occurrence. The INS simulation parameters include: random walk, bias, correlation time, frequency, and other indicators for both the gyroscope and accelerometer. The UWB simulation parameters include: the number of anchor nodes and the operating frequency. The corresponding measurement noise and NLOS error correlation parameters are set for the gamma distribution and Gaussian distribution, respectively. Monte Carlo simulation is used for training. The root mean square error (RMSE) and cumulative distribution function (CDF) are used as performance evaluation indicators and compared with three comparison algorithms: PR-PF, SHFAF, and RIEKF.

[0274] The lobby on the first floor of the school's main building was selected as the actual experimental site. The experimental area measures 4.8m long and 4.2m wide. Four UWB base stations were set up with coordinates of (0m, 2.40m), (2.40m, 4.20m), (4.80m, 2.40m), and (2.40m, 0m). The initial position of MN was (0.60m, 1.20m), and the moving speed was 0.20m / s. Obstacles were placed in the experimental area, and pedestrians were arranged to walk randomly to ensure the randomness of the noise. The UWB equipment used in the experiment was model D-DWM-PG1.7, and the INS equipment integrated a high-precision gyroscope and accelerometer, which can measure the attitude angle, angular velocity change, and linear acceleration of each axis of the moving object.

[0275] The simulation experiment process is as follows: set the motion trajectory and noise distribution parameters of MN, initialize the INS simulation parameters and UWB simulation parameters, run the method of Example 1 and the comparison algorithms, such as: Position Resetting Particle Filter (PR-PF), Strong Tracking Huber-based Adaptive Filter (SHFAF), Robust Iterative Extended Kalman Filter (RIEKF), collect the localization results of each algorithm, calculate the RMSE and CDF indices, and compare and analyze the performance differences.

[0276] The actual experimental procedure is as follows: set up base stations and experimental areas, initialize MN devices and motion parameters, start INS and UWB data acquisition, move MN on the preset trajectory and record positioning data, run various algorithms to process data, and analyze positioning errors and stability.

[0277] Figure 3 The cumulative distribution function of the localization error of each algorithm is shown when the NLOS error follows a gamma distribution. Experimental results show that the 90th percentile errors of PR-PF, SHFAF, and RIEKF do not exceed 4.383m, 3.411m, and 3.146m, respectively, while the 90th percentile error of the method in Example 1 is controlled within 0.470m, which is significantly lower than that of the comparison algorithm, demonstrating a very strong NLOS error suppression capability.

[0278] Figure 4 , Figure 5 , Figure 6The root mean square error (RMSE) of the method in Example 1 is presented under different scenarios with varying NLOS error probabilities (0.1-0.9), average NLOS errors (4m-10m), and standard deviations of NLOS errors (0.5m-1.9m) when the NLOS error follows a gamma distribution. Experimental results show that the average errors of the method in Example 1 are 0.331m, 0.261m, and 0.244m in the above three scenarios, respectively, demonstrating its stable positioning advantage under complex gamma-distributed NLOS interference.

[0279] Figure 7 The cumulative distribution function of the localization error of each algorithm is shown when the NLOS error follows a Gaussian distribution. Experimental results show that the 90th percentile errors of PR-PF, SHFAF, and RIEKF do not exceed 5.436m, 4.033m, and 3.914m, respectively, and the 90th percentile error of the method in Example 1 is less than 0.418m, maintaining extremely high localization accuracy even under Gaussian distributed NLOS interference scenarios.

[0280] Figure 8 , Figure 9 , Figure 10 The root mean square error (RMSE) of the method in Example 1 is presented under different scenarios when the NLOS error follows a Gaussian distribution, including different NLOS error probabilities (0.1-0.8), average NLOS errors (3m-9m), and standard deviations of different NLOS errors (4m-10m). Experimental results show that the average errors of the method in Example 1 are 0.439m, 0.238m, and 0.243m in the above three scenarios, respectively, demonstrating a significant advantage and verifying the adaptability of the method in Example 1 to NLOS interference with different distribution characteristics.

[0281] Example 3:

[0282] This embodiment presents an indoor positioning system based on multi-source constrained loosely coupled asynchronous INS / UWB, such as... Figure 11 As shown, the system includes:

[0283] The UWB data acquisition module is used to acquire distance observation values ​​of mobile nodes from several UWB base stations in real time, forming a distance observation sequence for each UWB base station.

[0284] The NLOS suppression processing module is used to process the distance observation sequence of each UWB base station using the expectation-maximization (EM) algorithm and the K-Medoids clustering algorithm respectively, and selects the better result of the two processing methods to obtain the current LOS measurement value of the UWB base station.

[0285] The IMU data acquisition module is used to collect acceleration and rotation speed information of the moving node.

[0286] The IMU pre-integration processing module is used to construct the INS dynamic model using the Euler angle method and perform pre-integration calculations based on the acceleration and rotation speed information to obtain the INS pre-integration result at the current moment.

[0287] The UWB location calculation module is used to treat UWB base stations that have obtained the LOS measurement value at the current time as valid base stations, and calculate the UWB location observation value at the current time based on the current LOS measurement values ​​of all valid base stations.

[0288] The INS / UWB fusion positioning module is used to combine the INS pre-integration result at the current moment to verify the validity of the UWB position observation at the current moment. If the verification result is valid, an improved adaptive extended Kalman filter is used to fuse the UWB position observation at the current moment and the INS pre-integration result to obtain the indoor positioning result of the mobile node. If the verification result is invalid, the INS pre-integration result at the current moment is used as the indoor positioning result of the mobile node.

[0289] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope defined by the present invention.

Claims

1. A multi-source constrained loosely coupled asynchronous INS / UWB indoor positioning method, characterized in that, This method includes the following steps: The distance observation values ​​of several UWB base stations to the mobile node are acquired in real time, forming a distance observation sequence for each UWB base station; For each UWB base station's distance observation sequence, the expectation-maximization (EM) algorithm and the K-Medoids clustering algorithm are used to process the distance observation sequence, and the optimal result of the two processing methods is selected to obtain the current LOS measurement value of the UWB base station. Acceleration and rotation speed information of the moving node are collected, Euler angles are used to construct the INS dynamic model, and pre-integration calculation is performed based on the acceleration and rotation speed information to obtain the INS pre-integration result at the current time. UWB base stations that have obtained LOS measurements at the current time are considered as valid base stations. The UWB location observation value at the current time is calculated based on the LOS measurements of all valid base stations at the current time. The validity of the UWB location observation value at the current time is verified by combining the INS pre-integration results at the current time. If the test result is valid, the improved adaptive extended Kalman filter is used to fuse the current UWB position observation and INS pre-integration result to obtain the indoor positioning result of the mobile node. If the test result is invalid, the INS pre-integration result at the current moment will be used as the indoor positioning result of the mobile node.

2. The indoor positioning method based on multi-source constrained loosely coupled asynchronous INS / UWB according to claim 1, characterized in that, The specific method for obtaining the current LOS measurement value of each UWB base station by processing the distance observation sequence using both the Expectation-Maximization (EM) algorithm and the K-Medoids clustering algorithm, and selecting the optimal result from the two processing methods, is as follows: For the The distance observation sequence of each UWB base station is processed using the expectation-maximization (EM) algorithm to generate the first UWB observation value. The distance observation sequence is preprocessed using the K-Medoids clustering algorithm to generate the second UWB observation value; The smaller UWB observation value between the first and second UWB observation values ​​is selected as the LOS measurement value of the UWB base station at the current time.

3. The indoor positioning method based on multi-source constrained loosely coupled asynchronous INS / UWB according to claim 2, characterized in that, The for the first The method for generating the first UWB observation value by processing the distance observation sequence of a UWB base station using the Expectation-Maximization (EM) algorithm is as follows: For the The UWB base station in the first Distance observation values ​​obtained during the second observation The measurement error is defined as the distance observed. and the The UWB base station in the first The true distance to the moving node at the time of the second observation The difference between them; The measurement error is modeled using a Gaussian mixture model to obtain a probability density model of the measurement error; The parameter set of the probability density model Recorded as ,in For the first The covariance matrix of Gaussian components; For the first The mean of the Gaussian components; For the first The weights of each Gaussian component; According to the Distance observation sequences of UWB base stations, defining parameter sets. The log-likelihood function; The log-likelihood function is solved using the Expectation-Maximization (EM) algorithm to obtain the parameter set. The optimal estimate is as follows: In each iteration, the E-Step and M-Step are executed sequentially. No. The E-Step of the round of iteration is: based on the first... After each iteration, the parameter estimates are updated, and the posterior probability of each distance observation belonging to each Gaussian component is calculated. No. The M-Step of the iterative execution round is as follows: Update the weights, covariance matrix, and variance of each Gaussian component according to the posterior probability, as the result of the first iteration. The parameter estimates after each iteration; After each iteration, the difference between the parameter estimate updated in the current iteration and the parameter estimate updated in the previous iteration is calculated. The iteration terminates when the calculated difference is lower than the preset threshold for two consecutive iterations. The parameter set obtained in the last iteration In this process, the Gaussian component with the smallest covariance matrix is ​​selected, and the mean of this Gaussian component is used as the UWB measurement value under the LOS environment. If distance observation value If the value is greater than the UWB measurement value under the LOS environment, then the UWB measurement value under the LOS environment is taken as the first UWB observation value. Conversely, the distance from the observed value will be... This is the first UWB observation.

4. The indoor positioning method based on multi-source constrained loosely coupled asynchronous INS / UWB according to claim 3, characterized in that, The specific method for preprocessing the distance observation sequence using the K-Medoids clustering algorithm to generate the second UWB observation value is as follows: For the The distance observation sequence of each UWB base station is set to have 2 clusters, corresponding to LOS and NLOS measurements respectively, and the initial cluster center of each cluster is randomly selected. For each round of clustering, perform the following operations: Each distance observation in the distance observation sequence is taken as a data item, and the Euclidean distance between each data item and the current cluster centers is calculated. The data item is then assigned to the cluster to which the nearest cluster center belongs. For each cluster, iterate through all data items in the cluster, calculate the sum of the squared distances from all data items in the cluster to the currently traversed data item, and select the data item with the smallest sum of squared distances as the new cluster center of the cluster. Calculate the Euclidean distance between the new cluster center and the previous cluster center. If the Euclidean distance is less than the predetermined convergence threshold, terminate the clustering process; otherwise, start the next round of clustering. After clustering, two cluster centers are obtained corresponding to the LOS and NLOS measurements, and the cluster center corresponding to the LOS measurement is used as the second UWB observation.

5. The indoor positioning method based on multi-source constrained loosely coupled asynchronous INS / UWB according to claim 4, characterized in that, The method for collecting the acceleration and rotation speed information of the moving node, constructing an INS dynamic model using the Euler angle method, and performing pre-integration calculations based on the acceleration and rotation speed information to obtain the INS pre-integration result at the current moment is as follows: The acceleration and rotation speed information of the mobile node are collected using the inertial measurement unit (IMU) mounted on the mobile node. Define the state vector of INS as follows: ； in, for The state vector of INS at any given time; The Euler angles of the moving node; and These are the velocity and position components of the moving node in the navigation coordinate system; and These are the inherent biases in measurements from gyroscopes and accelerometers, respectively. The error state vector of INS is defined as follows: ； in, for The error state vector of INS at time 1; Let be the Euler angle error vector of the moving node; This is the velocity error vector of the moving node in the navigation coordinate system; This is the position error vector of the moving node in the navigation coordinate system; This is the bias error vector of the gyroscope; This is the bias error vector of the accelerometer; The collected rotation speed information is pre-integrated to obtain the attitude angle of the mobile node in the IMU coordinate system. The attitude angle is then transformed to the navigation coordinate system using the direction cosine matrix to obtain the attitude of the mobile node. The collected accelerations are pre-integrated to obtain the velocity and position of the moving node; The obtained attitude, velocity, and position of the mobile node are used as the INS pre-integration results at the current time step.

6. The indoor positioning method based on multi-source constrained loosely coupled asynchronous INS / UWB according to claim 5, characterized in that, The specific method for considering UWB base stations that have obtained LOS measurements at the current time as valid base stations, calculating the UWB location observation value at the current time based on the current LOS measurements of all valid base stations, and combining the INS pre-integration results at the current time to verify the validity of the UWB location observation value at the current time is as follows: UWB base stations that have obtained the LOS measurement value at the current time are considered as valid base stations, and the number of valid base stations at the current time is counted. Based on the current LOS measurements of all valid base stations, the UWB location observation at the current time is calculated using least squares estimation. If the number of valid base stations at the current time meets the preset conditions, then the convergence of the UWB location observation value at the current time is verified. Conversely, the UWB position observation at the current moment is determined to be invalid. The convergence verification is as follows: obtain the base station coordinates of all valid base stations at the current time, and combine the LOS measurement values ​​of all valid base stations at the current time with the UWB position observation values ​​at the current time to calculate the least squares estimated residual sum of squares. If the calculated residual sum of squares is not greater than the preset residual threshold, then the position consistency verification is further performed by combining the INS pre-integration results at the current time; otherwise, the UWB position observation values ​​at the current time are determined to be invalid. The position consistency verification is as follows: calculate the Euclidean distance difference between the current UWB position observation value and the position of the moving node in the current INS pre-integration result; if the Euclidean distance difference is not greater than the preset position deviation threshold, the current UWB position observation value is determined to be valid. Conversely, the UWB position observation at the current moment is determined to be invalid.

7. The indoor positioning method based on multi-source constrained loosely coupled asynchronous INS / UWB according to claim 6, characterized in that, The specific method for fusing the current UWB location observation and INS pre-integration results using an improved adaptive extended Kalman filter to obtain the indoor positioning result of the mobile node is as follows: Initialize the state error covariance matrix And set the window size of the sliding estimation window. Forgetting factor adaptively updated by filtering ; Based on the error state vector of the INS, the error state transition equation and the error state observation equation are established. ； ； in, From Time's up The state error prediction vector at time 1; for The posterior estimate vector of the INS state error at time 1; express The observation prediction vector at time; Represents a nonlinear state transition function; Represents a nonlinear observation function; for Time-based process noise, , express The process noise covariance matrix at time step; for Measure noise at all times. , express The measurement noise covariance matrix at time point; For the nonlinear state transition function in The posterior estimate vector of the INS state error at time 1 Perform a Taylor expansion at the given point, and label the coefficients of the first-order terms after the expansion as the state transition Jacobian matrix. ; For the nonlinear observation function from Time's up State error prediction vector at time step Perform a Taylor expansion at the given location, and label the coefficients of the first-order terms after the expansion as the observed Jacobian matrix. ; Based on the state transition Jacobian matrix , for from Time's up State error prediction vector at time step and state error covariance matrix Perform forward prediction; the prediction model is: ； ； in, for The state error covariance matrix at time t; Based on the observed Jacobian matrix ,calculate The new vector at time and new information vector covariance matrix , is represented as: ； ； in, for UWB position observation at time; for The measurement noise covariance matrix at time point; Using from Time's up State error covariance matrix at time 1 New information vector covariance matrix And the observation Jacobian matrix ,calculate Kalman gain at time step ; right The new vector at time Perform the chi-square test and calculate. Test statistic at time ,like Then it is believed The UWB position observation at time t is detected as the LOS value; if Then it is believed The UWB position observation at time t was detected as an NLOS value; To test the threshold; Using a sliding estimation window and a forgetting factor The noise covariance of the UWB location observations detected as LOS values ​​is adaptively estimated, and the LOS scene is updated based on the estimation results. Measurement noise covariance matrix at time process noise covariance matrix ; Using a sliding estimation window, the mean of the innovation vector is statistically analyzed for UWB location observations detected as NLOS values, and the updated LOS values ​​are then analyzed based on the statistical results. Measurement noise covariance matrix at time Perform error compensation and update the NLOS scenario. Measurement noise covariance matrix at time Then, the NLOS scenario is updated according to the filtering convergence criterion. Process noise covariance matrix at time step ; Will The measurement noise covariance matrix updated at each time step under either the LOS or NLOS scenario is used as... Measurement noise covariance matrix at time ,Will The updated process noise covariance matrix at each time step under either LOS or NLOS scenarios is used as... Process noise covariance matrix at time step ; use Process noise covariance matrix at time step Recalculate from Time's up State error covariance matrix at time 1 And thus obtain from Time's up State error prediction vector at time step ; Based on the recalculated state error covariance matrix ,renew Kalman gain at time step and utilize Time-information vector Perform posterior estimation and covariance update of the INS state error to obtain... The posterior estimate vector of the INS state error at time 1 With the state error covariance matrix The state error covariance matrix is ​​shown below. The recursive estimation process for the next time step; use The posterior estimate vector of the INS state error at time 1 The INS pre-integration results are corrected, and then loosely coupled and fused with the current UWB position observations to obtain... Indoor positioning results of constantly moving nodes.

8. The indoor positioning method based on multi-source constrained loosely coupled asynchronous INS / UWB according to claim 7, characterized in that, The use of sliding estimation window and forgetting factor The noise covariance of the UWB location observations detected as LOS values ​​is adaptively estimated, and the LOS scene is updated based on the estimation results. Measurement noise covariance matrix at time process noise covariance matrix The specific method is as follows: according to UWB position observations at time 10:00 With From Time's up State error prediction vector at time step ,calculate Residual vector at time step ; Within the sliding estimation window, using Residual vector at time step and the front of the window The residual vector at each historical moment is calculated. Residual covariance matrix at time step ; Within the sliding estimation window, using The new vector at time and the front of the window The information vector at each historical moment is calculated. The new covariance matrix at time 1 ; Get Residual covariance matrix at time step and to Adaptive estimation of the measurement noise covariance matrix at time step is performed to obtain... Estimated value of the measurement noise covariance matrix at time step ; based on The new covariance matrix at time 1 and Kalman gain ,right The process noise covariance matrix at time step is adaptively estimated to obtain... Estimated value of process noise covariance matrix at time step ; based on Estimated value of the measurement noise covariance matrix at time step Estimated values ​​of process noise covariance matrix Utilizing the forgetting factor To each Measurement noise covariance matrix at time process noise covariance matrix Perform adaptive updates to obtain the LOS scenario. Measurement noise covariance matrix at time process noise covariance matrix .

9. The indoor positioning method based on multi-source constrained loosely coupled asynchronous INS / UWB according to claim 8, characterized in that, The method utilizes a sliding estimation window to perform innovation vector mean statistics on the UWB location observations detected as NLOS values, and updates the LOS scene based on the statistical results. Measurement noise covariance matrix at time Perform error compensation and update the NLOS scenario. Measurement noise covariance matrix at time Then, the NLOS scenario is updated according to the filtering convergence criterion. Process noise covariance matrix at time step The specific method is as follows: Within the sliding estimation window, using The new vector at time and the front of the window The information vector at each historical moment is calculated. Statistical mean of the innovation vector at time step ; use Statistical mean of the innovation vector at time step The adaptive update obtained in the LOS scenario Time-based measurement noise covariance matrix Perform NLOS error compensation to obtain the updated NLOS scenario. Measurement noise covariance matrix at time ; Updated based on NLOS scenario Measurement noise covariance matrix at time Calculate the adjustment index based on the filtering convergence criterion. And adjust the indicators accordingly. Update NLOS scenarios Process noise covariance matrix at time step .

10. A multi-source constrained loosely coupled asynchronous INS / UWB indoor positioning system, used to implement the multi-source constrained loosely coupled asynchronous INS / UWB indoor positioning method according to any one of claims 1-9, characterized in that, The system includes: The UWB data acquisition module is used to acquire distance observation values ​​of mobile nodes from several UWB base stations in real time, forming a distance observation sequence for each UWB base station. The NLOS suppression processing module is used to process the distance observation sequence of each UWB base station using the expectation-maximization (EM) algorithm and the K-Medoids clustering algorithm respectively, and selects the best of the two processing results to obtain the current LOS measurement value of the UWB base station. The IMU data acquisition module is used to collect acceleration and rotation speed information of the moving node; The IMU pre-integration processing module is used to construct the INS dynamic model using the Euler angle method and perform pre-integration calculations based on the acceleration and rotation speed information to obtain the INS pre-integration result at the current moment. The UWB location calculation module is used to treat UWB base stations that have obtained the LOS measurement value at the current time as valid base stations, and calculate the UWB location observation value at the current time based on the LOS measurement values ​​at the current time of all valid base stations. The INS / UWB fusion positioning module is used to combine the INS pre-integration result at the current moment to verify the validity of the UWB position observation at the current moment. If the verification result is valid, an improved adaptive extended Kalman filter is used to fuse the UWB position observation at the current moment and the INS pre-integration result to obtain the indoor positioning result of the mobile node. If the verification result is invalid, the INS pre-integration result at the current moment is used as the indoor positioning result of the mobile node.

Images