UWB robust Kalman positioning algorithm based on dynamic reliability evaluation

Abstract

The invention discloses an ultra wide band (UWB) robust positioning method for a non-line-of-sight (NLOS) interference environment, and belongs to the technical field of wireless positioning. According to the method, firstly, all three-base-station combinations are generated through a permutation and combination method, the initial position of each group of base stations is solved through a trilateral positioning method, and a multi-dimensional coarse positioning point cloud is formed; then grouping the coarse positioning points according to the resolving participation condition of the base stations, acquiring representative position points of the base stations by adopting a central position estimation algorithm, and constructing a base station space distribution model; a mean vector and a covariance matrix of the point cloud are calculated, after a pseudo-inverse matrix is solved through SVD decomposition, the mahalanobis distance from each representative point to the distribution center is calculated, a dynamic threshold value is set according to the 95% confidence coefficient of chi-square distribution, a base station with the distance exceeding the threshold value is judged as an NLOS abnormal node, and data of the base station is abandoned; and finally, constructing a dual-stage anomaly suppression mechanism: screening reliable base stations based on geometric consistency at a measurement stage, dynamically adjusting the Kalman gain through an exponential gain constraint factor at a residual stage, constructing a dynamic noise model by fusing a geometric precision factor, and realizing position calculation by adopting improved extended Kalman filtering. According to the method, the positioning error can be effectively reduced in the NLOS interference environment, and a centimeter-level reliable positioning solution is provided for the fields of industrial Internet of Things or indoor positioning and the like.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of wireless sensor networks, and in particular to an UWB (Ultra-Wideband) positioning method based on geometric clustering dynamic reliability evaluation and Kalman filter improvement under NLOS (Non-Line-Of-Sight) conditions. BACKGROUND

[0002] Under the promotion of Industry 4.0 and digital twin technology, precise spatial perception capability is becoming the underlying infrastructure in the fields of intelligent manufacturing, smart energy and the like. Due to its special sub-nanosecond pulse modulation mechanism, the ultra-wideband (UWB) positioning technology exhibits a breakthrough space-time resolution performance - having a timestamp resolution accuracy of 0.3 nanoseconds, a spectrum expansion characteristic of more than 500 MHz, a multipath interference suppression gain of 30 dB, and a power consumption control of microwatt level, which provides a new positioning paradigm for high dynamic complex scenes. However, in the strong shielding environment such as industrial sites and underground pipe corridors, the electromagnetic wave transmission exists a dynamic coupling effect of line-of-sight (LOS) and non-line-of-sight (NLOS) propagation modes. Especially under the NLOS state, the path delay effect caused by signal diffraction attenuation will trigger the nonlinear expansion of the pseudo-range observation value, resulting in a meter-level drift of the positioning solution.

[0003] In the face of the positioning challenge in the NLOS propagation environment, the existing researches are concentrated in two types of technical routes: the first type is based on algorithm level noise suppression, which only reduces the error by optimizing at the algorithm level, and the upper limit of the accuracy is insufficient; the second type is based on environmental interaction noise suppression, which introduces external auxiliary information, and uses environmental interaction such as neural network, map information and the like to assist in suppressing the error, and the real-time performance and algorithm complexity are difficult to guarantee. Both of them are difficult to balance the environmental adaptability, real-time performance and accuracy requirements. SUMMARY

[0004] In order to solve the problems in the prior art, the present application provides a UWB robust Kalman positioning algorithm based on dynamic reliability evaluation, which uses ranging data and clustering to identify the base stations that may be in the NLOS state in the measurement period, and uses an improved adaptive Kalman filter to alleviate the error caused by the NLOS effect in positioning.

[0005] The present application provides a UWB robust Kalman positioning algorithm based on dynamic reliability evaluation, which includes:

[0006] The ranging data of all base stations in the current measurement period is obtained, for an UWB positioning system containing N (N>3) base stations, the base stations are numbered in order, and all three base station combinations are generated by permutation and combination method;

[0007] The three base station combination is solved by trilateration method to form a multi-dimensional coarse point cloud.

[0008] Each coarse measurement point in the multidimensional coarse measurement point cloud is grouped according to the base station participation status, and each coarse measurement point can be repeatedly assigned to the group corresponding to three base stations with different numbers;

[0009] The spatial centroid estimation algorithm is used to obtain the geometric mean center of each base station and calculate the mean center of the geometric mean center of each base station. The deviation between the geometric mean center and the mean center of each base station is evaluated based on Mahalanobis distance. The chi-square distribution confidence is used to dynamically identify NLOS abnormal base stations and discard the ranging data of NLOS abnormal base stations.

[0010] For ranging data from reliable base stations retained after discarding NLOS abnormal base stations, a two-stage robust Kalman filter is used to calculate the target location.

[0011] As a preferred embodiment, the geometric mean center of each base station is obtained using a spatial centroid estimation algorithm as follows:

[0012]

[0013] Among them, M i X represents the geometric mean center of base station i. i For each numbered base station, there is a set of points, and each set contains... One point, This represents the j-th element in the set with base station number i.

[0014] As a preferred embodiment, the statistical evaluation of the deviation between the geometric mean center and the mean center of each base station based on Mahalanobis distance includes:

[0015] Calculate the mean center of the geometric mean center of each base station;

[0016] For the geometric mean center of each base station, calculate the Mahalanobis distance between the geometric mean center and the mean center.

[0017] In a preferred embodiment, the mean center M0 is represented as:

[0018]

[0019] As a preferred embodiment, the geometric mean center M i Mahalanobis distance d between the mean center and the mean center M (M i The calculation formula for ) is as follows:

[0020]

[0021] Where S represents the covariance matrix.

[0022] As a preferred embodiment, dynamically determining NLOS abnormal base stations by chi-square distribution confidence includes:

[0023] The geometric mean center M i of the square Mahalanobis distance is considered to be subject to chi-square distribution with degree of freedom d:

[0024]

[0025] Construct a hypothesis test, the null hypothesis H0: elements come from the multivariate normal distribution; alternative hypothesis H1: elements are outliers;

[0026] The test statistic is the square Mahalanobis distance; given the significance level α = 0.05 (95% confidence), the critical value is:

[0027]

[0028] Wherein, is the cumulative distribution function of chi-square distribution;

[0029] If the geometric mean center M i of the base station numbered i as an element satisfies Then reject H0, determine that the geometric mean center M i is an outlier, and the base station numbered i is an NLOS base station, discarding d i (k) in the measurement period.

[0030] As a preferred embodiment, for the ranging data of reliable base stations retained after discarding NLOS abnormal base stations, the two-stage robust Kalman filter is used for positioning solution includes:

[0031] During in-plane motion, set the initial position and initial x and y axis velocities and the initial covariance matrix;

[0032] For each time k in the motion process, according to the state estimation of the last time and the state transition matrix, the prediction is as follows:

[0033] Wherein, is the predicted state, F is the state transition matrix, which describes the evolution of the system from the last time to the current time:

[0034] The covariance prediction formula is:

[0035] P k - = FP k-1 F T + Q

[0036] Wherein, P k-1is the covariance matrix of the last time, Q is the process noise covariance matrix, P k - is the predicted covariance matrix;

[0037] In the state update step, the current measurement value z k is used to adjust the predicted state estimate, the formula is:

[0038]

[0039] wherein, is the measurement value based on the predicted state, and H k is the observation matrix, used to map from the state space to the measurement space.

[0040] As a preferred embodiment, the observation matrix H k is calculated according to the positions of the effective base stations:

[0041] In each measurement period, after NLOS identification, the ranging data of the remaining LOS base stations after removing the NLOS abnormal base stations is denoted as i=1, 2…N' wherein, d i represents the ranging data of the remaining LOS base stations after removing the NLOS abnormal base stations, and N' is the number of effective base stations at the current time;

[0042] Using the nonlinear measurement model and the coordinates and ranging data of the LOS base stations, the observation matrix in the measurement period is obtained, which is used to update the estimated position at the next time; wherein the nonlinear measurement model is i represents the number corresponding to the LOS base station, (x, y) represents the target position of the tag to be measured, and (x i , y i ) represents the position of the base station with number i;

[0043] The observation matrix H k is represented as:

[0044]

[0045] wherein, d i represents the distance between the i-th base station and the target position.

[0046] As a preferred embodiment, in the process of positioning calculation using two-stage robust Kalman filtering, a dynamic adaptive measurement noise covariance matrix R k is introduced, and the Mahalanobis distance d i (M i ) between the geometric mean center M M of each base station and the mean center is used to construct At time k of Kalman filtering, Rk The setting is:

[0047]

[0048] Wherein, d M (M i ) is the geometric mean center M i of the base station of the number i, N' is the number of effective base stations after the identification process, and only the Mahalanobis distance of the effective (LOS state) base station at this moment is taken;

[0049] The measurement residual y k of the state update step is expressed as Wherein, (x k ,y k ) represents the predicted position of the target at the current moment k;

[0050] The adaptive measurement noise in the Kalman filter is expressed as:

[0051]

[0052] Wherein, y k is the difference between the observed measurement value and the predicted measurement value, GDOP k represents the GDOP value formed by the predicted position at the current moment and the base station;

[0053] Therefore, the adaptive measurement noise covariance matrix based on GDOP adjustment is introduced:

[0054] The GDOP formed by the predicted position at the moment k and the LOS base station in motion is expressed as:

[0055]

[0056] Wherein, H v is the design matrix formed by the LOS base station and the target position state estimation, and the calculation formula is:

[0057]

[0058] As a preferred embodiment, in the process of Kalman filtering, the Kalman gain is:

[0059]

[0060] The residual gain constraint mechanism is introduced, Wherein, c i is an exponential gain constraint factor, y i is the residual of the i th observation, and TH is a set threshold value, and the exponential gain constraint factor is constructed into a diagonal matrix L: ​

[0061]

[0062] Kalman filter gain obtained by correcting Kalman gain is expressed as:

[0063]

[0064] The state estimation is updated by the corrected Kalman gain and the measurement residual, which is expressed as:

[0065]

[0066] In the process of the Kalman filtering, the covariance matrix is updated, which is expressed as:

[0067]

[0068] Wherein, P k is the updated covariance matrix.

[0069] Compared with the prior art, the present application has the following beneficial effects:

[0070] 1. The present application proposes a UWB robust Kalman positioning algorithm based on dynamic reliability evaluation, which takes into account the accuracy and real-time performance. The algorithm establishes an online modeling mechanism for the spatial reliability of the base station: a set of geometric distribution points is constructed by trilateration combination solution, the degree of deviation of each base station data is quantified in real time by using Mahalanobis distance, NLOS base station identification is realized based on the dynamic threshold of chi-square test, and the data of the NLOS base station is removed, and an exponential gain constraint residual gain adjustment mechanism is used; the geometric dilution of precision (GDOP) is innovatively integrated to construct an environment adaptive noise model, providing core technical support for industrial level high reliability positioning.

[0071] 2. The Kalman filter proposed in the present application dynamically adjusts the observation matrix according to the LOS data of each measurement period, and the observation noise covariance matrix R is composed of d M (M i ) obtained by the first aspect of the present application. And a geometric accuracy adjustment factor is introduced, aiming to reduce the adverse effects of ranging data in poor GDOP position on accuracy due to base station geometric distribution. The solution position obtained by this method has good accuracy. BRIEF DESCRIPTION OF DRAWINGS

[0072] Figure 1 A flowchart of a UWB robust Kalman positioning algorithm based on dynamic reliability evaluation provided by the embodiment of the present application is provided.

[0073] Figure 2 A flowchart of a UWB abnormal ranging value identification method based on hypothesis testing provided by the embodiment of the present application is provided.

[0074] Figure 3 The improved robust Kalman filtering algorithm flow chart provided for the embodiment of the present application.

[0075] Figure 4 The position solution effect comparison schematic diagram of the scheme and the standard extended Kalman filter proposed for the embodiment of the present application. DETAILED DESCRIPTION

[0076] The present application will be further described below with reference to the drawings. The following embodiments are only used to more clearly illustrate the technical solutions of the present application, and cannot be used to limit the protection scope of the present application.

[0077] In combination Figure 1 To solve the problems in the prior art, the present application provides a UWB robust Kalman positioning algorithm based on dynamic reliability evaluation. The method identifies the base stations that may be in the NLOS state in the measurement period by using ranging data and clustering, and uses an improved adaptive Kalman filter to alleviate the error caused by NLOS in positioning. The method comprises the following steps:

[0078] Step 1: For a UWB positioning system containing N (N>3) base stations, the base stations are numbered in order, all three-base-station combinations are generated by permutation and combination, and the position point is solved by using the trilateration method for each group to form a multi-dimensional coarse measurement point cloud.

[0079] Step 2: Group the coarse measurement points according to the base station participation, each point can be repeatedly divided into three groups corresponding to the base stations with different numbers, and the spatial centroid estimation algorithm is used to obtain the geometric feature points of each base station, a total of N, to construct a base station spatial distribution model.

[0080] Step 3: Based on the statistical evaluation of the deviation degree of each feature point and the distribution center, the NLOS abnormal base station is dynamically determined by the chi-square distribution 95% confidence and the data is discarded.

[0081] Step 4: The processed data is positioned by using a two-stage robust Kalman filter: the measurement level filters the reliable base stations to construct the observation value, the residual level controls the state update by using an exponential gain constraint factor, and the geometric precision factor is used to dynamically adjust the noise parameter to obtain the precise positioning.

[0082] Figure 2 The flow chart of a UWB positioning method for identifying and mitigating NLOS based on clustering provided for the embodiment of the present application. As shown in the figure, Figure 2 The first aspect of the present application relates to a UWB signal NLOS identification method based on clustering, which comprises steps 1 to 3.

[0083] Further, the detailed steps of acquiring the coarse measurement positioning point in step 1 include:

[0084] At the measurement period k, the positioning tag coordinates are [x(k), y(k)], and the relationship between the tag node and the anchor node is:

[0085]

[0086] where {(x1, y1), …, (x n ,y n )} are the coordinates of n base stations, and d1(k) to d n (k) are the distances between the tag node and the anchor node.

[0087] The n ranging data are divided into groups of three each (extensible to more than three), and a total of different groups can be formed, and distinguished by the base station number combination. The base station numbers are 1, 2, …, N, and the number set is C ∈ {1, 2, …, N}.

[0088] In one measurement period, for each group of data, a point can be calculated using the trilateration method.

[0089] To simplify the formula, the (k) representing the ranging period is omitted below. For each group of data, the theoretical base station-to-target tag position distance formula is as follows:

[0090]

[0091] where a, b, and c represent the base station numbers, x a ,y a , x b ,y b , and x c ,y c are the positions of the corresponding base stations, and d a , d b , and d c are the theoretical distances from the target point to the base stations, and x and y are the coordinates to be solved.

[0092] Each group of data is solved using the trilateration method to obtain the corresponding preliminary coarse measurement point according to the distance formula, which is formulated as follows:

[0093]

[0094] where i represents the base station number. For the distance difference between the target point and multiple anchor points, the target point position can be determined by solving the equation set. Taking the a anchor point as the reference point, the distance difference between the a anchor point and the b anchor point can be expressed as:

[0095] d a 2 -d b 2= (x - x b ) 2 + (y - y b ) 2 - (x - x a ) 2 - (y - y a ) 2

[0096] Next, according to the distance difference, a linear equation set is constructed, and the coefficient matrix H can be represented as:

[0097]

[0098] The distance difference expansion formula constructs the constant term vector b in the linear equation set, and b can be represented as:

[0099]

[0100] The constructed linear equation set is:

[0101] H·X abc = b

[0102] Wherein, represents the coordinates of the to-be-solved point corresponding to the base station combination number a, b, and c. After sorting, we get:

[0103] X abc = (H T H) -1 H T b

[0104] Solving X can obtain the preliminary coarse measurement point corresponding to the group. The obtained preliminary coarse measurement point set is X, containing elements, and corresponding to each base station.

[0105] Further, the step 2 specifically includes the following contents:

[0106] Each coarse measurement point is calculated from the ranging data from 3 different base stations. All coarse measurement points are repeatedly grouped, each point is assigned to a set of 3 base station numbers it contains, and the geometric average center point of each group is calculated. Xi is the point set of each numbered base station, and each set contains points.

[0107] The basic formula for calculating the geometric average center is:

[0108]

[0109] Wherein, M i represents the obtained geometric average center, represents the jth element in the set of base stations numbered i.

[0110] Further, the step 3 specifically includes the following contents:

[0111] The obtained N geometric mean center Mahalanobis distances are calculated. Their geometric mean center is:

[0112]

[0113] The covariance matrix S∈ p×p Describes the variability and correlation of the data, using unbiased estimates:

[0114]

[0115] For each point M i , the Mahalanobis distance d M (M i ) is calculated:

[0116]

[0117] Further, the step 3 specifically includes the following contents:

[0118] The square Mahalanobis distance of M i is considered to be subject to chi-square distribution with degree of freedom d:

[0119]

[0120] The hypothesis test is constructed, the null hypothesis H0: the element comes from the multivariate normal distribution. The alternative hypothesis H1: the sample is an outlier. The test statistic is the square Mahalanobis distance. Given the significance level α = 0.05 (95% confidence level), the critical value is:

[0121]

[0122] Where, is the cumulative distribution function of chi-square distribution. If the element satisfies:

[0123]

[0124] H0 is rejected, M i is an outlier point, and the base station numbered i is a NLOS base station, which is discarded d i (k) in this period.

[0125] Figure 3 The flow chart of the improved robust Kalman filtering algorithm provided by the embodiment of the application. As shown in the figure, the second aspect of the application, Figure 3

[0126] ​An improved robust Kalman filter algorithm is used to adapt the ranging data processed in the first aspect of the invention for position solution, the method comprising step 4.

[0127] Further, step 4 specifically comprises the following:

[0128] The initial position x0, y0and initial x and y axis velocities v x0 and v y0 are:

[0129]

[0130] The initial covariance matrix is the identity matrix:

[0131] P0= I

[0132] Further, in the Kalman filter described in step 3,

[0133] For each time instant k in the motion process, the state estimate from the previous time instant is used to make a prediction according to the state transition matrix:

[0134]

[0135] where, is the predicted state. F is the state transition matrix, describing the evolution of the system from the previous time instant to the current time instant:

[0136]

[0137] Further, in the Kalman filter described in step 3, the covariance prediction formula is:

[0138] P k - = FP k-1 F T + Q

[0139] where, P k-1 is the covariance matrix from the previous time instant, and Q is the process noise covariance matrix. P k - is the predicted covariance matrix.

[0140] Further, in the Kalman filter described in step 3, in the update step, the predicted state estimate is adjusted using the current measurement z k , the formula is:

[0141]

[0142] where, is the measurement based on the predicted state. And H kH is the observation matrix used to map from state space to measurement space. Further, in the Kalman filter described in step 3, H k According to the position of the effective base station, the following is calculated:

[0143] In each measurement period, after NLOS identification, the ranging data of the remaining LOS anchor points is denoted as

[0144] where d i represents the ranging data of the remaining LOS anchor points after removing the NLOS anchor points. N' is the number of effective base stations at the current time.

[0145] Further, in the step of calculating the dynamic state observation matrix described in step 3,

[0146] Using the nonlinear ranging model and the coordinates of the LOS base stations and their ranging data, the observation matrix for this measurement period is obtained to update the estimated position at the next time. The measurement model is:

[0147]

[0148] where i represents the number corresponding to the LOS anchor point. For the nonlinear measurement model h(x, y), we obtain the Jacobian matrix H k by calculating the partial derivative of the measurement function. This matrix is used to map the changes in the state space to the measurement space.

[0149] For the ranging model, the Jacobian matrix is:

[0150]

[0151] Calculate the partial derivative to get:

[0152]

[0153] Further, in the step of calculating the dynamic state observation matrix described in step 3, the observation matrix has the following form:

[0154]

[0155] Since only the state observation of position information is involved, the last two columns of the observation matrix are 0.

[0156] Further, in the process of Kalman filter described in step 3, a dynamic adaptive measurement noise covariance matrix R k is introduced. i The Mahalanobis distance d M (M i ) of each point M kSince the independence of each base station is strong, the measurement noise correlation between them can be ignored. At time k, the measurement noise covariance matrix R k is set as:

[0157]

[0158] where d M is the Mahalanobis distance of the i-th base station, N' is the number of effective base stations after the identification process, and means only taking the Mahalanobis distance of the effective (LOS state) base station at this time. i i

[0159] Further, in the process of Kalman filtering described in step 3, the measurement residual of the update step is:

[0160]

[0161] where d i represents the distance between the i-th anchor point and the target position, and in this formula, (x i , y i ), i = 1, 2,..., N are the coordinates of each base station. (x k , y k ) represents the predicted position at the current time k.

[0162] Further, in the process of Kalman filtering described in step 3, the adaptive measurement noise is:

[0163]

[0164] where y k is the difference between the observed measurement value and the predicted measurement value, GDOP k represents the GDOP value formed by the predicted position at the current time and the base station. When the target is moving, the GDOP formed by the target and the anchor point amplifies the positioning error, and some positions with large GDOP increase the error. Therefore, an adaptive measurement noise covariance matrix based on GDOP adjustment is introduced:

[0165] The GDOP formed by the predicted position at time k and the LOS anchor point when moving can be represented as:

[0166]

[0167] where H v is the design matrix formed by the LOS anchor point and the target state estimation position, and the calculation formula is:

[0168]

[0169] ​​Further, in the process of Kalman filtering described in step 3, the Kalman gain is:

[0170]

[0171] Further, in the process of Kalman filtering described in step 3, a residual gain constraint mechanism is introduced to suppress the influence of abnormal observation values.

[0172]

[0173] Where c i is an exponential gain constraint factor, y i is the residual error of the i-th observation, and TH is the set threshold, generally 1.

[0174] The constraint factor is constructed into a diagonal matrix L:

[0175]

[0176] Further, in the process of Kalman filtering described in step 3, the modified Kalman filtering gain is:

[0177]

[0178] The state estimation is updated by the Kalman gain and the measurement residual error, and the formula is:

[0179]

[0180] Further, in the process of Kalman filtering described in step 3, the covariance matrix is updated, and the formula is:

[0181]

[0182] Where P k is the updated covariance matrix, indicating the new estimation uncertainty.

[0183] After the NLOS recognition processing of the ranging data, the improved adaptive Kalman filtering is used for solving, and finally the accurate positioning information can be obtained.

[0184] Figure 4A schematic diagram for comparing the position solution effect of the scheme and the standard Kalman filter is shown in the figure. The scene is set as shown in the figure, the receiver moves according to the green dashed line position, and the real-time position of the receiver is measured by each base station during the movement. When there is an obstacle between the base station and the receiver, the ranging data is disturbed. The data of the complete movement process is solved by the standard Kalman filter and the scheme, and the comparison is made. The root mean square error of the scheme is 0.12495m, and the average errors in the x direction and the y direction are 0.068492m and 0.070556m respectively. The root mean square error of the standard extended Kalman filter is 3.1243m, and the average errors in the x direction and the y direction are 1.4013m and 1.6387m respectively. It can be verified that the scheme has very good positioning accuracy.

[0185] The beneficial effects of the present application are: the present application combines the non-parametric hypothesis testing method in mathematical statistics with Kalman filtering, and uses Mahalanobis distance and chi-square distribution test to distinguish the ranging data in the NLOS state and discard it under the mixed condition of NLOS and LOS, and uses the improved robust Kalman filter for solution, which significantly improves the precision and ensures the reliability of UWB positioning. This breakthrough technology has important significance for the development of indoor positioning technology when the UWB positioning system faces harsh conditions such as physical environment mutation and dynamic mobile scene switching, and can still maintain centimeter-level continuous and stable positioning ability

[0186] The above is only the preferred embodiment of the present application, and it should be pointed out that for ordinary skilled persons in the art, some improvements and modifications can be made without departing from the technical principles of the present application, and these improvements and modifications should also be considered as the protection scope of the present application.

Claims

1. A UWB robust Kalman positioning algorithm based on dynamic reliability evaluation, characterized in that, The method comprises the following steps: Obtain ranging data of all base stations in the current measurement period, for an UWB positioning system with N(N>3) base stations, the base stations are numbered in order, and all three-base-station combinations are generated by permutation and combination method; Solve the target position by trilateration for each three-base-station combination to form a multi-dimensional coarse measurement point cloud; Group each coarse measurement point in the multi-dimensional coarse measurement point cloud according to the base station participation, and each coarse measurement point can be repeatedly divided into three groups corresponding to base stations with different numbers; Obtain the geometric mean center of each base station by spatial centroid estimation algorithm, calculate the mean center of the geometric mean centers of the base stations, evaluate the deviation of the geometric mean center of each base station from the mean center based on Mahalanobis distance, dynamically determine the NLOS abnormal base station by chi-square distribution confidence, and discard the ranging data of the NLOS abnormal base station; For the ranging data of the reliable base stations retained after discarding the NLOS abnormal base station, perform positioning calculation on the target position by double-stage robust Kalman filtering.

2. The UWB robust Kalman positioning algorithm based on dynamic reliability evaluation according to claim 1, characterized in that, The geometric mean center of each base station obtained by spatial centroid estimation algorithm is represented as: where M i represents the geometric mean center of the base stations numbered i, X i is a set of points for each numbered base station, each set containing points, represents the jth element in the set of base stations numbered i.

3. The UWB robust Kalman positioning algorithm based on dynamic reliability evaluation according to claim 2, characterized in that, The deviation of the geometric mean center of each base station from the mean center based on Mahalanobis distance includes: Calculate the mean center of the geometric mean centers of the base stations; For the geometric mean center of each base station, calculate the Mahalanobis distance between the geometric mean center and the mean center.

4. The UWB robust Kalman positioning algorithm based on dynamic reliability evaluation according to claim 3, characterized in that, The mean center M0 is represented as:

5. The UWB robust Kalman positioning algorithm based on dynamic reliability evaluation according to claim 3, characterized in that, Geometric mean center M i Mahalanobis distance d between the mean center and the point M (M i ) is calculated as follows: Wherein, S represents the covariance matrix.

6. The UWB robust Kalman positioning algorithm based on dynamic reliability evaluation according to claim 5, characterized in that, Dynamically determining the NLOS abnormal base station by chi-square distribution confidence includes: The squared Mahalanobis distance of the geometric mean center M i is considered to follow a chi-square distribution with d degrees of freedom: Construct a hypothesis test, the null hypothesis H0: the elements come from the multivariate normal distribution; the alternative hypothesis H1: the elements are outliers; The test statistic is the square Mahalanobis distance; given the significance level α=0.05 (95% confidence), the critical value is: wherein is the cumulative distribution function of the chi-square distribution; If the geometric mean center M i satisfies then H0is rejected, the geometric mean center M i is determined to be an outlier, and the base station numbered i is a NLOS base station, which is discarded d i (k) for the measurement period.

7. The UWB robust Kalman positioning algorithm based on dynamic reliability evaluation according to claim 1, characterized in that, For the ranging data of the reliable base stations retained after discarding the NLOS abnormal base station, perform positioning calculation by double-stage robust Kalman filtering includes: During in-plane motion, set the initial position and the initial x and y axis velocities and the initial covariance matrix; For each time instant k during the motion, the state estimate from the previous time instant k - 1 is predicted according to and the state transition matrix wherein, is the predicted state, F is the state transition matrix describing the evolution of the system from the previous time instant to the current time instant: The covariance prediction formula is: P k - = FP k-1 F T + Q where P k-1 is the covariance matrix of the previous time, Q is the process noise covariance matrix, P k - is the predicted covariance matrix; In the state update step, the current measurement z k is used to adjust the predicted state estimate, given by wherein, is a measurement based on the predicted state, while H k is an observation matrix for mapping from state space to measurement space.

8. The UWB robust Kalman positioning algorithm based on dynamic reliability evaluation according to claim 7, characterized in that, The observation matrix H k Calculation from the position of the active base station: In each measurement period, after NLOS identification, the ranging data after removing the NLOS abnormal base station is recorded as i = 1, 2…N' wherein, d i represents the ranging data of the remaining LOS base station after removing the NLOS abnormal base station, and N' is the number of effective base stations at the current time. Using a nonlinear measurement model and coordinates of the LOS base stations and ranging data thereof, an observation matrix in the measurement period is obtained to update an estimated position at a next time; wherein the nonlinear measurement model is i represents a number corresponding to the LOS base station, (x, y) represents a target position of the tag to be measured, and (x i ,y i ) represents a position of the base station numbered i; The observation matrix H k is represented as: where d i denotes the distance between the i-th base station and the target location.

9. The UWB robust Kalman positioning algorithm based on dynamic reliability evaluation according to claim 8, characterized in that, In the process of positioning calculation using a two-stage robust Kalman filter, a dynamic adaptive measurement noise covariance matrix R is introduced. k Using the geometric mean center M of each base station i Mahalanobis distance d between the mean center and the mean center M (M i )Build R k ; ； At time k in the Kalman filter, R k Set to: where d M (M i ) is the geometric mean center M i of the base stations of order i, and N' is the number of base stations that are valid after the identification process, and only the Mahalanobis distance of the base stations that are valid at this time (LOS state) is taken. In the state update step, the measurement residual y k is represented as where (x k ,y k ) represents the predicted position of the target at the current time instant k; Adaptive measurement noise in the Kalman filter is represented as: where y k is the difference between the observed measurement and the predicted measurement, GDOP k represents the GDOP value formed by the predicted position and the base station at the current time Therefore, an adaptive measurement noise covariance matrix based on GDOP adjustment is introduced: The GDOP formed by the predicted position of the motion at time k and the LOS base station is represented as: where H v is the design matrix formed by the LOS base stations and target position state estimates, and is computed as 10. The UWB robust Kalman positioning algorithm based on dynamic reliability evaluation according to claim 9, characterized in that, During the Kalman filtering process, the Kalman gain is: A residual gain constraint mechanism is introduced, where c i is an exponential gain constraint factor, y i is the residual of the i-th observation, and TH is a set threshold value, and the exponential gain constraint factor is configured as a diagonal matrix L: Kalman filter gain obtained by correcting the Kalman gain is expressed as: The state estimate is updated by a modified Kalman gain and measurement residual, expressed as: During the Kalman filtering process, the covariance matrix is updated, and the formula is represented as: where P k is the updated covariance matrix.

Images