PDR-assisted UWB cooperative positioning method in NLOS scene in underground confined space

Abstract

The invention discloses a PDR-assisted UWB cooperative positioning method in an NLOS scene in an underground confined space, and belongs to the technical field of positioning, and the method comprises the steps: building a quaternary state hypothesis model which comprises an LOS state and an NLOS state of a fixed base station, and an LOS state and an NLOS state of a mobile base station; identifying whether each base station is in an NLOS state based on the CMD determinant, and respectively defining the fixed base station and the mobile base station in the LOS state as a first fixed base station and a first mobile base station; position information of a to-be-positioned label is obtained based on a PDR technology, first distance information from a first fixed base station and a first mobile base station to the to-be-positioned label is obtained by fusing UWB, and a state equation and a measurement equation of extended Kalman filtering are established; and predicting and updating errors generated by positioning as system state vectors to obtain posterior estimation information, and correcting the position of the to-be-positioned label in real time. According to the invention, the positioning precision of the underground closed scene is improved.

Description

Technical Field

[0001] The present invention belongs to the field of positioning technology, and in particular relates to a PDR-assisted UWB collaborative positioning method in an NLOS scenario in an underground confined space. Background Art

[0002] In underground projects, accurate positioning is crucial to ensure personnel safety and improve work efficiency. Currently, a variety of positioning technologies are used to locate underground personnel and equipment, including RFID positioning, ZigBee positioning, Wi-Fi positioning, Bluetooth positioning, and UWB positioning. RFID is difficult to integrate into mobile devices and has a short range; ZigBee technology has unstable signals, a small positioning range, and poor security; Wi-Fi positioning technology is easily affected by environmental interference, and when there are many access points, the positioning effect is poor.

[0003] For example, the Chinese patent document with publication number CN117061991A provides an underground personnel positioning system and positioning method based on UWB, and the system includes: an underground personnel positioning card, an underground vehicle, an underground vehicle positioning card, a positioning device, a specific Bluetooth beacon, a positioning base station and a positioning server; the positioning device and the specific Bluetooth beacon are arranged inside the underground vehicle, and the specific Bluetooth beacon is bound to the underground vehicle; the underground personnel positioning card is used for underground personnel to carry with them, and is communicated with the positioning device; the underground vehicle positioning card is installed on the outside of the underground vehicle, and is connected to the positioning device through serial port communication; the positioning base station is arranged on the ground and is communicated with the positioning server, the underground personnel positioning card and the underground vehicle positioning card, and the positioning server is registered with the bound specific Bluetooth beacon and the underground vehicle.

[0004] In the existing technology, the positioning of underground personnel is highly dependent on Bluetooth beacons. However, there may be various electromagnetic interference sources in underground confined spaces such as underground mines, which may affect the transmission of Bluetooth signals, thereby reducing positioning accuracy. In contrast, UWB positioning technology has become the preferred underground confined space positioning solution due to its excellent multipath resolution, strong anti-interference ability and low power consumption characteristics. However, the underground confined space environment is complex, and multipath effects and object blocking problems are common, resulting in the significant impact of UWB ranging performance on NLOS propagation. Therefore, how to identify and suppress NLOS errors has become a problem that must be solved to improve the positioning accuracy of underground confined spaces.

[0005] Another example is the Chinese patent document with publication number CN113038377A, which discloses a one-dimensional precise positioning method for underground mines based on UWB technology C-TWR, including the following steps: deploying UWB positioning base stations underground, using the C-TWR process to obtain two measurement values ​​(the distance from the identification card to UWB locator A and UWB locator B), preliminarily correcting the errors of the two-way data, determining the one-dimensional linear direction of the identification card, and using a one-dimensional Kalman filter to obtain the final distance value. This invention can achieve higher underground positioning accuracy, has good real-time positioning, can realize blind spot-free positioning, and meets the technical requirements for underground applications in coal mines.

[0006] However, the above-mentioned existing technologies can only achieve positioning in one-dimensional linear direction in underground confined spaces, and the positioning accuracy is low. Therefore, a positioning method is needed that can effectively identify the NLOS path in underground confined spaces, suppress NLOS errors, and improve the reliability and accuracy of personnel positioning. Summary of the invention

[0007] In order to solve the above problems, the present invention provides a PDR-assisted UWB collaborative positioning method in a NLOS scenario in an underground confined space to solve the problems in the prior art.

[0008] In order to achieve the above-mentioned invention object, the present invention proposes a PDR-assisted UWB collaborative positioning method in a NLOS scenario in an underground confined space, comprising:

[0009] S1: Establishing a four-element state hypothesis model, wherein the four-element state hypothesis model includes the LOS state and the NLOS state of the fixed base station, and the LOS state and the NLOS state of the mobile base station;

[0010] S2: Identify whether each of the fixed base station or the mobile base station is in the NLOS state based on the CMD determinant, and define the fixed base station and the mobile base station in the LOS state as a first fixed base station and a first mobile base station, respectively;

[0011] S3: Acquire location information of the tag to be located based on the PDR technology, acquire first distance information from the first fixed base station and the first mobile base station to the tag to be located based on the location information and the UWB positioning technology, and establish a state equation and a measurement equation of an extended Kalman filter based on the first distance information;

[0012] S4: taking the error generated by positioning as the system state vector, predicting and updating the system state vector based on the state equation and the measurement equation to obtain a posteriori estimation information, and correcting and outputting the position of the tag to be positioned in real time based on the a posteriori estimation information.

[0013] Furthermore, establishing the four-element state hypothesis model includes the following steps:

[0014] Based on the first formula, the weight w of the mobile base station to the positioning tag is calculated i , the first formula is: Among them, D e is the ranging error, R e The relative positioning error is obtained by obtaining the distance information between the fixed base station or the mobile base station and the tag to be positioned, and the distance information is modeled to generate a four-element state hypothesis model, wherein the four-element state hypothesis model includes the LOS state hypothesis H of the fixed base station. 1 and NLOS state assumption H 2 , and the LOS state of the mobile base station is assumed to be H 3 and NLOS state assumption H 4 ;

[0015] Based on the second formula, the distance information d under the four state assumptions is obtained. i , the second formula is: Among them, r i is the measured distance from the ith fixed base station or mobile base station to the tag to be located obtained based on the TOA positioning algorithm, e i is the system noise, which satisfies Gaussian distribution, e i ~N(0,σ 2 ), n i is the forward error caused by NLOS propagation, e is the error of the mobile base station itself, and n i >>e i .

[0016] Further, calculating the ranging error and the relative positioning error includes the following steps:

[0017] The distance measurement error D is calculated based on the third formula e , the third formula is: Among them, r a and r b represents the real distance and the estimated distance between the mobile base station and the tag to be located respectively, R is the communication radius, and the relative positioning error R is calculated based on the fourth formula e , the fourth formula is: Among them, (x i ,y i ) is the actual coordinate of the ith mobile base station, is the estimated coordinates of the i-th tag to be located, and n is the number of mobile base stations.

[0018] Further, identifying whether each of the fixed base station or the mobile base station is in the NLOS state based on the CMD determinant includes the following steps:

[0019] Determine the CMD matrix, which is: Wherein, A and B are the fixed base stations, T 1 is the mobile base station, T 2 is the tag to be located, d 2 (A,B)=d 2 (B, A), and both represent the square of the Euclidean distance between the fixed base station A and the fixed base station B, d 2 (A,T 1 ) = d 2 (T 1 , A), and both represent the fixed base station A and the mobile base station T 1 The square of the distance, d 2 (A,T 2 ) = d 2 (T 2 , A), and both represent the fixed base station A and the tag to be located T 2 The square of the distance, d 2 (B,T 1 ) = d 2 (T 1 , B), and both represent the fixed base station B and the mobile base station T 1 The square of the distance, d 2 (B,T 2 ) = d 2 (T 2 , B), and both represent the fixed base station B and the tag to be located T 2 The square of the distance, d 2 (T 1 ,T 2 ) = d 2 (T 2 ,T 1 ), and both represent the mobile base station T 1 and the tag to be located T 2 The square of the distance;

[0020] Determine the determinant D(A, B, T) of the CMD matrix based on the fifth formula: 1 ,T 2 ), the fifth formula is: det is the determinant calculation formula, and the determinant D(A, B, T 1 ,T 2), if the determinant is 0 and the rank is less than a first threshold, it indicates that the fixed base station or the mobile base station is in an NLOS state, otherwise it is in a LOS state.

[0021] Furthermore, obtaining the location information of the tag to be located based on the PDR technology includes the following steps:

[0022] Obtain the location information of the tag to be located at time k, the location information includes the current horizontal coordinate and vertical coordinate of the pedestrian where the tag to be located is located, the step length l and the direction angle θ, and calculate the horizontal coordinate x at the kth time based on the sixth formula k and the ordinate y k , the sixth formula is: Among them, (x k-1 ,y k-1 ) is the position coordinate of the pedestrian at time k-1, and the step length l is calculated based on the seventh formula, which is: Wherein, h is the height of the pedestrian, P is the step frequency of the pedestrian, and a, b and c are the first parameter, the second parameter and the third parameter respectively.

[0023] Further, establishing the state equation includes the following steps:

[0024] The system state vector X is obtained based on the eighth formula t , the eighth formula is: X t =[e x(t) ,e y(t) ,e l(t) ,e θ(t) ] T , where e x(t) 、e y(t) 、e l(t) and e θ(t) are the position error, step error and angular error on the x-axis and y-axis respectively, and a state equation is established based on the ninth formula, which is: t+1 =f t X t +W t , where X t+1 is the system state vector at time t+1, f t is the system state transfer matrix, θ t is the direction angle of the pedestrian at time t, W t is the system dynamic noise, including the position noise w on the x-axis and y-axis x and w y , step noise w l and directional noise w θ , where wx ~N(0,2),w y ~N(0,2),w l ~N(0,1),w θ ~N(0,4).

[0025] Furthermore, the following steps are included before establishing the measurement equation:

[0026] The initial measurement equation is established based on the tenth formula, which is: Z = H t X t +V t , where Z is the system measurement information, H t is the measurement coefficient matrix, V t is the measurement noise, where V t ~N(0,R t ), R t is the measurement noise covariance matrix, and the measurement coefficient matrix is: in, is the location information of the first fixed base station at time t, is the location information of the tag to be located, is the location information of the virtual base station calculated based on the PDR technology at time t, is the distance information of the i-th first fixed base station or the first mobile base station calculated based on the PDR algorithm at time t, and the measurement noise covariance matrix is:

[0027] The state assumption under the four-element state assumption model is binarized based on the eleventh formula, and the eleventh formula is: η i,t is the state of the i-th fixed base station or the mobile base station at time t, and a first matrix is ​​generated based on the binarization result, wherein the first matrix is Based on the first matrix, base stations in the NLOS state are identified and eliminated to obtain a transition measurement equation, which is: t =η t Z, where Z t is the measurement information at time t.

[0028] Furthermore, establishing the measurement equation includes the following steps:

[0029] The transition measurement equation is dimensionally simplified to obtain a new measurement equation, which is: in, is the distance measurement information between the i-th first fixed base station and the tag to be located obtained based on the UWB positioning technology, The distance information between the first mobile base station and the tag to be located is obtained based on the UWB positioning technology;

[0030] Based on the twelfth formula, the The twelfth formula is: Based on the thirteenth formula, the The thirteenth formula is: Based on the fourteenth formula, the The fourteenth formula is: in, is the location information of the first mobile base station at time t.

[0031] Furthermore, the prediction phase includes the following steps:

[0032] Based on the fifteenth formula, the system state vector predicted at time t-1 is calculated. The fifteenth formula is: in, is the predicted system state vector at time t-1, f t / t-1 is the system state transfer matrix, and the mean square error matrix P of the prediction at time t-1 to time t is calculated based on the sixteenth formula t / t-1 , the sixteenth formula is: Among them, the is the transpose of the system state transfer matrix, Q t-1 is the system noise at time t-1, P t-1 is the mean square error matrix predicted at time t-1.

[0033] Furthermore, the update phase includes the following steps:

[0034] Based on the seventeenth formula, the filter gain K of the extended Kalman filter is obtained: t , the seventeenth formula is: in, is the transpose of the measurement coefficient matrix, and the measurement residual e is calculated based on the eighteenth formula t , the eighteenth formula is: Based on the nineteenth formula, the system state vector Correction is performed to obtain the state correction matrix The nineteenth formula is: The mean square error matrix P predicted based on the twentieth formula t / t-1 Update to obtain the updated mean square error matrix The 20th formula is: The updated state correction matrix and the mean square error matrix are used as a posteriori estimation information.

[0035] Compared with the prior art, the beneficial effects of the present invention are at least as follows:

[0036] The present invention proposes an NLOS algorithm based on the CMD determinant statistical hypothesis. The simulation verifies that the method can effectively identify fixed base stations or mobile base stations in an NLOS environment. Based on the recognition effect, the fixed base stations and mobile base stations with large errors in the NLOS situation are discarded, and the fixed base stations and mobile base stations with small errors in the LOS situation are selected. The improved EKF algorithm is used to integrate the ranging data from the fixed base station and the mobile base station, as well as the distance information derived by the PDR algorithm, to provide more accurate positioning information for the tag to be located. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 A flowchart of the steps of a PDR-assisted UWB collaborative positioning method in a NLOS scenario in an underground confined space according to the present invention;

[0038] Figure 2 A two-dimensional collaborative positioning scene model diagram of the present invention;

[0039] Figure 3 This is a diagram showing the recognition rate experimental results of the NLOS path of the present invention;

[0040] Figure 4 This is a NLOS recognition result diagram of the present invention;

[0041] Figure 5 The NLOS identification flow chart based on the CMD determinant of the present invention;

[0042] Figure 6 This is a flow chart of PDR-assisted UWB combined with mobile base station fusion positioning of the present invention;

[0043] Figure 7 It is the positioning trajectory diagram of the present invention;

[0044] Figure 8 is a trajectory error diagram of the present invention;

[0045] Fig. 9 is the positioning rate of the tag to be positioned of the present invention;

[0046] Fig.10 This is a relationship diagram between the positioning error and the number of mobile base stations of the present invention. DETAILED DESCRIPTION

[0047] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0048] It is understood that the terms "first", "second", etc. used in this application can be used to describe various elements in the present invention, but unless otherwise specified, these elements are not limited by these terms. These terms are only used to distinguish a first element from another element. For example, without departing from the scope of this application, a first xx script can be referred to as a second xx script, and similarly, a second xx script can be referred to as a first xx script.

[0049] like Figure 1 As shown, a PDR-assisted UWB collaborative positioning method in a NLOS scenario in an underground confined space includes:

[0050] S1: Establish a four-element state hypothesis model, which includes the LOS state and NLOS state of a fixed base station, and the LOS state and NLOS state of a mobile base station;

[0051] In order to facilitate the understanding of the present invention, a two-dimensional collaborative positioning scenario model is established. The model includes a fixed base station, a mobile base station and a tag to be positioned. The tag to be positioned can be a mobile phone carried by a person, etc.

[0052] Specifically, Figure 2 As shown in the figure, it is a two-dimensional collaborative positioning scenario model diagram. A, B, C and D are fixed base stations and can communicate with each other. 1 、T 2 、T 3 and T 4 They are tags carried by underground workers. There are three situations in which the base station and the tag communicate:

[0053] Case 1: In the case of LOS (line-of-sight transmission), that is, when there are no obstacles between the transmitter and the receiver in the signal propagation path, it is possible to determine the location information of the person when there are at least 3 fixed base stations. Figure 2 It can be seen that the label T 2 You can determine your location through fixed base stations B, C and D.

[0054] Case 2: Label T 3 Can only communicate with fixed base station C and mobile base station T 1 and mobile base station T 2When transmitting positioning signals, the fixed base station B is blocked by obstacles, resulting in NLOS transmission (non-line-of-sight transmission). That is, there are obstacles in the process of signal propagation from the transmitter to the receiver, which prevents the signal from propagating in a straight line to the receiver, increasing the positioning error. At this time, the tag T 3 is the label to be located.

[0055] Known T 1 、T 2 The location can act as a mobile base station, and the mobile base station T 2 Can be located with the tag T 3 Transmit positioning signals between base stations C, D and mobile base station T 2 It can be determined that T 3 The position of the tag to be located T is obtained through the EKF filtering algorithm and the PDR algorithm. 3 The precise location of

[0056] T 4 Through fixed base station C, fixed base station D and mobile base station T 2 To determine its own position coordinates, it is known that T 3 Location, but T 3 The location information is obtained through two mobile base stations, and the NLOS detection and recognition algorithm of CMD is needed to determine T 3 Can it be used as a mobile base station? If it is determined that T 3 In LOS state, use T 3 The provided positioning information is discarded otherwise.

[0057] Case 3: Tag T to be located 1 With mobile base station T 3 It can transmit signals with the fixed base station A, but it still lacks a base station to determine the location information. In this case, the PDR algorithm can be used to locate the tag and obtain its precise location.

[0058] The underground closed environment is complex and the sudden NLOS situation is serious, with situation 2 and situation 3 being the most common. The present invention mainly provides solutions to the above two situations.

[0059] In a two-dimensional positioning scenario, when measuring the distance between a mobile base station and the tag to be located, there may be obstacles in the signal propagation path, and the signal may take a non-straight path to reach the receiver, which will increase the propagation time and cause ranging errors. After the mobile base station is introduced, the relative positioning error is accumulated during the positioning process due to the uncertainty of its own position accuracy.

[0060] The weight of the mobile base station for the tag to be located is determined by two errors: ranging error and relative positioning error. The error is introduced to establish a quaternary state hypothesis model, and the distance from the fixed base station or mobile base station to the tag to be located is modeled to generate distance information under four base station state hypotheses. The four base station state hypotheses include fixed base stations in LOS and NLOS states, and mobile base stations in LOS and NLOS states.

[0061] S2: Identify whether each fixed base station or mobile base station is in the NLOS state based on the CMD determinant, and define the fixed base station and mobile base station in the LOS state as the first fixed base station and the first mobile base station, respectively.

[0062] Specifically, after obtaining the four-state hypothesis model, it is necessary to identify the path and determine which of the above four states the path is in. The present invention identifies and detects the NLOS path based on the CMD determinant, and detects whether the path between the base station and the tag to be located is in the NLOS state. By identifying the NLOS path, inaccurate measurements caused by multipath effects and occlusions can be excluded or corrected, thereby improving the accuracy of the positioning system.

[0063] The effectiveness of the algorithm proposed in the present invention can be verified by simulation experiments using MATLAB. It is assumed that the experimental scene is set to a 50×200 underground confined space two-dimensional positioning scene range, and 4 fixed base stations, 3 mobile base stations and 1 tag node to be located are arranged. The 3 mobile base stations are randomly distributed within the positioning range of the 4 base stations. The system noise is a Gaussian white noise with a mean of 0 and a variance of 1. The NLOS error is generated using a uniform random distribution. The NLOS error is set to be positively correlated with the distance d from the fixed base station or the mobile base station to the tag to be located, and is uniformly randomly generated in [Sd, 2Sd]. S is a scale factor. The larger the scale factor, the larger the NLOS error.

[0064] In order to make the simulation results closer to the real confined space positioning environment, the present invention designed 20 different experiments, each of which included the number and scale factor changes of various NLOS paths. In order to ensure the credibility of the simulation results, each set of settings was run 500 times, and the average value was taken as the final experimental data for NLOS path identification, as shown in Figure 2. Figure 3 The figure shows the experimental results of the recognition rate of the NLOS path.

[0065] from Figure 3It can be seen that as the scale factor S increases, the NLOS recognition efficiency gradually improves. When the scale factor S is equal to 1, the NLOS recognition rate reaches its peak. This is because a larger scale factor leads to a larger NLOS error, that is, there are more satisfied states. When the number of NLOS paths is 0, the NLOS recognition rate is 0, indicating that the proposed algorithm can successfully identify the number of paths without NLOS errors.

[0066] like Figure 4 As shown, this is the NLOS recognition result diagram. Figure 4 The recognition results of LOS and NLOS situations of fixed base stations and mobile base stations in an underground confined space environment are shown. In the 20th group of experiments, the scale factor value is 1, and NLOS noise interference is artificially added to the fixed base stations and mobile base stations. It can be seen that the recognition results of the proposed algorithm are consistent with the actual situation.

[0067] S3: Based on the PDR technology, the location information of the tag to be located is obtained. Based on the location information, the first distance information from the first fixed base station and the first mobile base station to the tag to be located is obtained by fusing the UWB positioning technology. Based on the first distance information, the state equation and measurement equation of the extended Kalman filter are established.

[0068] Specifically, PDR technology can estimate the position of pedestrians, but PDR is a time series-based estimation method. The initial small error may accumulate over time, causing the deviation of the position estimate to gradually increase. At the same time, the acquired position information may need to fuse data from multiple sensors. The algorithm selection and implementation in the fusion process may introduce additional errors, and errors may also be generated in the UWB positioning process. The present invention regards the error generated by PDR and UWB fusion positioning and ranging as the system state vector, and then combines the prior information of the NLOS recognition result with the improved EKF algorithm to predict and correct the system state vector, thereby obtaining more accurate position information.

[0069] The state equation and the measurement equation together constitute the system dynamic model. The state equation describes the evolution of the system state vector over time, and the measurement equation describes the process of how to obtain actual observation data from the system state vector. The system dynamic model is the core component of the EKF filtering algorithm.

[0070] S4: The error generated by positioning is used as the system state vector, and the system state vector is predicted and updated based on the state equation and the measurement equation to obtain the posterior estimation information. Based on the posterior estimation information, the position of the tag to be positioned is corrected in real time and output.

[0071] Specifically, the present invention combines the observation data in the system dynamic model, that is, the measurement equation, to correct the prediction of the system state vector. The ranging information of the base station in the NLOS state is discarded in the observation data. Only the difference between the ranging information from the fixed base station and the mobile base station to the tag to be located in the LOS state and the ranging information calculated by the PDR is compared to update the ranging information. The LOS path usually provides more reliable measurements. The posterior estimate obtained in the present invention includes the updated system state vector and the mean square error matrix. The error is corrected by the two to make it smaller and smaller, thereby improving the reliability of the positioning result and obtaining accurate location information of the pedestrian.

[0072] The present invention proposes an NLOS algorithm based on the CMD determinant statistical hypothesis. The simulation verifies that the method can effectively identify fixed base stations or mobile base stations in an NLOS environment. Based on the recognition effect, the fixed base stations and mobile base stations with large errors in the NLOS situation are discarded, and the fixed base stations and mobile base stations with small errors in the LOS situation are selected. The improved EKF algorithm is used to integrate the ranging data from the fixed base station and the mobile base station, as well as the distance information derived by the PDR algorithm, to provide more accurate positioning information for the tag to be located.

[0073] As a preferred technical solution of the present invention, establishing a four-element state hypothesis model includes the following steps:

[0074] Based on the first formula, the weight w of the mobile base station to the positioning tag is calculated i , the first formula is: Among them, D e is the ranging error, R e The relative positioning error is obtained by obtaining the distance information between the fixed base station or the mobile base station and the tag to be located, and the distance information is modeled to generate a four-element state hypothesis model. The four-element state hypothesis model includes the LOS state hypothesis H of the fixed base station 1 and NLOS state assumption H 2 , and the LOS state of the mobile base station is assumed to be H 3 and NLOS state assumption H 4 .

[0075] Based on the second formula, the distance information d under the four state assumptions is obtained. i , the second formula is: Among them, r i is the measured distance from the ith fixed base station or mobile base station to the tag to be located based on the TOA positioning algorithm, e i is the system noise, which satisfies Gaussian distribution, e i ~N(0,σ 2 ), n iis the forward error caused by NLOS propagation, e is the error of the mobile base station itself, and n i >>e i .

[0076] Specifically, the weight is determined by the ranging error and the relative positioning error. The larger the weight, the greater the error of the mobile base station, the lower the availability, and the easier it is to be abandoned when locating the tag to be located. The four-element state hypothesis model divides underground base stations into four types: H 1 and H 2 denote the LOS and NLOS state assumptions of the fixed base station, respectively, and H 3 and H 4 They represent the LOS and NLOS states of the mobile base station, respectively. It is assumed that fixed base stations are not prone to the two errors, so no weight is assigned. However, in the process of ranging and positioning, the probability of errors occurring in mobile base stations is relatively high, so they are assigned certain weights. In the NLOS environment, the forward error caused by NLOS propagation is much greater than the system noise, that is, n i >>e i .

[0077] Calculating the ranging error and relative positioning error includes the following steps:

[0078] Calculate the distance measurement error D based on the third formula e , the third formula is: Among them, r a and r b They represent the real distance and estimated distance between the mobile base station and the tag to be located, respectively, R is the communication radius, and the relative positioning error R is calculated based on the fourth formula e , the fourth formula is: Among them, (x i ,y i ) is the actual coordinate of the ith mobile base station, is the estimated coordinates of the i-th tag to be located, and n is the number of mobile base stations.

[0079] Specifically, the ranging method uses the TOA positioning algorithm. The ranging error refers to the difference between the measured distance and the actual distance. The relative positioning error refers to the difference between the position estimated by the positioning system and the actual position of the target object within a certain period of time. This error is usually cumulative, such as sensor bias, time synchronization error, etc. The identification and correction of ranging errors can improve the accuracy of a single measurement and help obtain closer to the actual distance information at a specific moment. The evaluation and correction of relative positioning errors help improve the stability and reliability of the positioning system in long-term operation.

[0080] Identifying whether each fixed base station or mobile base station is in the NLOS state based on the CMD determinant includes the following steps:

[0081] Determine the CMD matrix, which is: Among them, A and B are fixed base stations, T 1 is a mobile base station, T 2 is the label to be located, d 2 (A,B)=d 2 (B, A), and both represent the square of the Euclidean distance between fixed base station A and fixed base station B, d 2 (A,T 1 ) = d 2 (T 1 , A), and both represent fixed base station A and mobile base station T 1 The square of the distance, d 2 (A,T 2 ) = d 2 (T 2 , A), and both represent the fixed base station A and the tag to be located T 2 The square of the distance, d 2 (B,T 1 ) = d 2 (T 1 , B), and both represent fixed base station B and mobile base station T 1 The square of the distance, d 2 (B,T 2 ) = d 2 (T 2 , B), and both represent the fixed base station B and the tag to be located T 2 The square of the distance, d 2 (T 1 ,T 2 ) = d 2 (T 2 ,T 1 ), and both represent mobile base stations T 1 and the tag to be located T 2 The square of the distance.

[0082] Determine the determinant D(A, B, T) of the CMD matrix based on the fifth formula 1 ,T 2 ), the fifth formula is: det is the determinant calculation formula, get the determinant D(A,B,T 1 ,T 2 ), if the determinant is 0 and the rank is less than the first threshold, it indicates that the fixed base station or the mobile base station is in the NLOS state, otherwise it is in the LOS state.

[0083] Specifically, the NLOS identification flow chart based on the CMD determinant is as follows: Figure 5As shown, first determine whether the base station is a fixed base station, so that in this case, the error of the mobile base station is introduced, and a four-element state hypothesis model is established based on the error. The upper bound of the LOS value of the fixed base station and the upper bound of the LOS value of the mobile base station are obtained by utilizing the CMD property. The upper bound of the LOS value represents the highest communication quality that can be achieved between the fixed base station or the mobile base station and the receiving device in the most ideal communication environment, that is, the line-of-sight environment. By calculating the upper bound of the LOS value, it can be checked whether the tag to be located is in a non-line-of-sight environment, and then a four-element hypothesis test model is established to determine whether the fixed base station or the mobile base station is in the NLOS state, that is, in the non-line-of-sight environment. If not, the location information of the fixed base station and the mobile base station in the LOS state will be involved in the location solution of the tag to be located.

[0084] The recognition of NLOS signals is realized by using two important theorems of CMD. First, when (A, B, T 1 ,T 2 ) The four elements are distributed in two-dimensional space. If the CMD determinant is 0, it means that the six edges between the tag to be located and the fixed base station and the mobile base station are not independent, and the rows or columns of the matrix are linearly related. Second, if the determinant is 0 and the rank is less than a certain threshold, in geometric positioning, the rank of the signal propagation matrix (such as the arrival time or arrival angle matrix) is related to the number of observable independent signal paths. If the rank is less than the minimum required number of independent paths, this may mean that not all base station signals can reach the receiver independently, which may be a sign of an NLOS signal. This property is used to identify NLOS signals.

[0085] Acquiring the location information of the tag to be located based on the PDR technology includes the following steps:

[0086] Get the location information of the tag to be located at time k, which includes the current horizontal and vertical coordinates, step length l and direction angle θ of the pedestrian where the tag to be located is located. Calculate the horizontal coordinate x at time k based on the sixth formula k and the ordinate y k , the sixth formula is: Among them, (x k-1 ,y k-1 ) is the position coordinate of the pedestrian at time k-1, and the step length l is calculated based on the seventh formula, which is: Wherein, h is the height of the pedestrian, P is the pedestrian's step frequency, a, b and c are the first parameter, the second parameter and the third parameter respectively.

[0087] Specifically, the PDR algorithm is a relative positioning algorithm. Its basic principle is to combine the data information provided by the accelerometer and gyroscope to perceive the gait characteristics of pedestrians in real time. By monitoring the pedestrian's step frequency, the algorithm can estimate the pedestrian's step length and determine the pedestrian's direction of travel. The position of the pedestrian at the previous moment is used, combined with the current step length prediction and direction angle information. The PDR algorithm is used to estimate the horizontal and vertical coordinates of the pedestrian at time k through the ninth formula. The pedestrian step length model in the tenth formula adopts the classic step length linear model. The first parameter, the second parameter and the third parameter in the formula are the parameters of the pedestrian step length model, which are obtained through least squares fitting. Among them, a=0.371, b=0.227 and c=1.050.

[0088] Establishing the state equation includes the following steps:

[0089] Based on the eighth formula, the system state vector X is obtained t , the eighth formula is: X t =[e x(t) ,e y(t) ,e l(t) ,e θ(t) ] T , where e x(t) 、e y(t) 、e l(t) and e θ(t) are the position error, step error and angular error on the x-axis and y-axis respectively. The state equation is established based on the ninth formula, which is: t+1 =f t X t +W t , where X t+1 is the system state vector at time t+1, f t is the system state transfer matrix, θ t is the direction angle of the pedestrian at time t, W t is the system dynamic noise, including the position noise w on the x-axis and y-axis x and w y , step noise w l and directional noise w θ , where w x ~N(0,2),w y ~N(0,2),w l ~N(0,1),w θ ~N(0,4).

[0090] Specifically, the state equation: X t+1 =f t X t +W tIt is generated with the assistance of PDR and combined with UWB positioning technology. The state equation is used to describe the system from a state X t To another state X t+1 In PDR, the state may include the horizontal axis position coordinates, step size, and direction angle. Combined with the high-precision position information provided by UWB, the PDR estimation error can be corrected to improve the overall positioning accuracy.

[0091] The following steps are included before establishing the measurement equation:

[0092] The initial measurement equation is established based on the tenth formula, which is: Z = H t X t +V t , where Z is the system measurement information, H t is the measurement coefficient matrix, V t is the measurement noise, where V t ~N(0,R t ), R t is the measurement noise covariance matrix, and the measurement coefficient matrix is: in, is the location information of the first fixed base station at time t, is the location information of the tag to be located, is the location information of the virtual base station calculated based on PDR technology at time t, is the distance information calculated by the PDR algorithm at time t for the i-th first fixed base station or the first mobile base station, and the measurement noise covariance matrix is:

[0093] Based on the eleventh formula, the state assumption under the four-element state assumption model is binarized. The eleventh formula is: η i,t is the state of the i-th fixed base station or mobile base station at time t, and a first matrix is ​​generated based on the binarization result. The first matrix is Based on the first matrix, base stations in the NLOS state are identified and eliminated to obtain the transition measurement equation, which is: Z t =η t Z, where Z t is the measurement information at time t.

[0094] Specifically, the fixed base stations and mobile base stations are screened and H 2 and H 4 The base station in the state is selected 1 and H 3The base stations in the state provide positioning information for the tags to be located, reducing the number of base stations with NLOS errors. The fixed base stations and mobile base stations in the LOS state are artificially set to 1, and the fixed base stations and mobile base stations in the NLOS state are set to 0. After NLOS identification, the base stations in the LOS state are retained. In order to simplify the calculation, all base stations in the NLOS state are discarded, and only the base stations in the LOS state are selected to establish the measurement equation. The base station matrix η t The rows whose row vectors are all 0 are deleted to reduce the dimension of the matrix and the amount of calculation. At this time, the ranging information provided by the base stations obtained are all fixed base stations and mobile base stations in the LOS state.

[0095] H t and R t The dimension of is determined by the dimension of the ranging information from the fixed base station and the mobile base station to the tag to be located. It makes full use of the ranging information provided by the fixed base station and the mobile base station and has good robustness.

[0096] Establishing the measurement equation includes the following steps:

[0097] The transition measurement equation is simplified to obtain a new measurement equation, which is: in, is the distance information between the i-th first fixed base station and the tag to be located obtained based on the UWB positioning technology, It is the distance measurement information between the first mobile base station and the tag to be located obtained based on the UWB positioning technology.

[0098] Calculation based on the twelfth formula The twelfth formula is: Calculation based on the thirteenth formula The thirteenth formula is: Calculated based on the 14th formula The fourteenth formula is: in, is the location information of the first mobile base station at time t.

[0099] Specifically, the new measurement equation is obtained by identifying the NLOS state. Based on this identification effect, the fixed base stations and mobile base stations with large errors in the NLOS situation are discarded, and the fixed base stations and mobile base stations with small errors in the LOS situation are selected. The ranging information is updated by comparing the difference between the ranging information from the fixed base station and the mobile base station to the tag to be located and the ranging information calculated by PDR. The PDR system may produce large cumulative errors after long-term operation. By regularly comparing with the ranging information of the base station, these errors can be discovered and corrected in time. When there is a significant difference between the ranging information of the base station and the ranging information calculated by PDR, further diagnosis and verification can be triggered, thereby improving the reliability of the entire positioning system.

[0100] The forecasting phase includes the following steps:

[0101] Based on the fifteenth formula, the system state vector predicted at time t-1 is calculated. The fifteenth formula is: in, is the predicted system state vector at time t-1, f t / t-1 is the system state transfer matrix, and the mean square error matrix P of the prediction at time t-1 to time t is calculated based on the sixteenth formula t / t-1 , the sixteenth formula is: in, is the transpose of the system state transfer matrix, Q t-1 is the system noise at time t-1, P t-1 is the mean square error matrix predicted at time t-1.

[0102] Specifically, the EKF algorithm combines the target state with the state prediction value and the distance information prediction value through two steps of prediction and update to obtain an accurate estimate of the target position. First, the prediction process is explained. The Extended Kalman Filter (EKF) is a filter used for nonlinear system state estimation. At the beginning of the filter, the predicted system state vector is calculated. The twentieth formula: It is also a state equation, which requires an initial state estimate, that is, the predicted system state vector at time t-1 And this It can be calculated by the above state equation, and the prediction process of calculating EKF is iterative, and the above steps are repeated at each time step to update the state estimate and the mean square error matrix.

[0103] The update phase consists of the following steps:

[0104] Based on the seventeenth formula, the filter gain K of the extended Kalman filter is obtained t, the seventeenth formula is: in, is the transpose of the measurement coefficient matrix, and the measurement residual e is calculated based on the 18th formula t , the eighteenth formula is: Based on the nineteenth formula, the predicted system state vector Correction is performed to obtain the state correction matrix The nineteenth formula is: Based on the 20th formula, the predicted mean square error matrix P t / t-1 Update and obtain the updated mean square error matrix The twentieth formula is: The updated state correction matrix and mean square error matrix are used as the posterior estimation information.

[0105] Specifically, Figure 6 As shown in the figure, it is a flow chart of PDR-assisted UWB combined with mobile base station fusion positioning. The positioning process starts with obtaining the user's initial heading angle and position. This information can be input by the user or inherited from the previous positioning results. The PDR system uses sensor data, such as step frequency and step length, heading angle, etc., to infer the user's movement to update the PDR position. Fixed base stations and mobile base stations use UWB technology to provide high-precision ranging information, and the selection of fixed base stations and mobile base stations utilizes the NLOS identification algorithm based on CMD determinant and statistical assumptions for NLOS identification. Only fixed base stations or mobile base stations in the LOS state are used. These ranging information is used to correct the position calculated by PDR to improve the positioning accuracy. Based on the predicted system state vector and mean square error matrix, the optimized collaborative positioning filtering algorithm, namely the EKF algorithm, is used to perform a priori estimation and real-time correction on the system state vector, thereby accurately extracting the mean square error matrix, and the generated information is fed back to the PDR system to correct the existing errors, thereby achieving high-precision and high-reliability positioning information.

[0106] The performance of the PDR-assisted UWB collaborative positioning method proposed based on the NLOS situation in underground confined spaces is analyzed. The pedestrian trajectory data provided by the public database on the Internet is used. During the MATLAB simulation, NLOS noise is randomly added to the positioning path. During underground positioning, moving workers and moving vehicles cause occlusion during the positioning process. Three UWB fixed base stations A, B, and C are set, and their positions are (0,0), (200,0), and (200,50) respectively. There are 10 tags to be located and the maximum transmission distance between tags is set to 20m. The pedestrian walking speed is set to a constant position of 1.2m / s, and the step size is fixed to 0.6m.

[0107] like Figure 7As shown in the figure, it is a positioning trajectory diagram. By analyzing the trajectory results of sub-graphs (a) and (b), it can be seen that when the fixed base stations A, B and C are blocked, the algorithm proposed in the present invention regards the tags with known positions as mobile base stations, creates a LOS situation for the tags to be located, provides positioning information, reduces the positioning blind area, and provides position correction for the PDR, so that the positioning result is closer to the real trajectory.

[0108] like Figure 8 As shown in the figure, it is a trajectory error diagram. The errors of the algorithm proposed in the present invention in the x-direction and the y-direction are both less than 0.2m, and the errors of the trajectory points are concentrated within the range of 0.1m. This is because in the positioning process, the mobile base station is added to provide positioning information and correct the error of the PDR algorithm. The positioning effect is significantly better than the UWB+PDR positioning algorithm, and the positioning information in the LOS situation provided by the mobile base station with small error is effectively utilized to reduce the interference caused by the NLOS situation.

[0109] like Fig. 9 As shown in the figure, it is the positioning rate of the tag to be located. It can be seen that under the premise of a constant number of fixed base stations, the positioning rate of the tag to be located is steadily improved by gradually introducing mobile base stations. When the number of mobile base stations is 0, after the fixed base stations and PDR provide positioning information, the location information of some of the tags to be located can also be obtained. These positioning tags can be used as mobile base stations to provide positioning information for other tags to be located, so the more tags to be located, the higher the positioning rate. However, with the increase in the number of mobile base stations, this growth benefit is not linear, but shows a decreasing trend, showing a saturation effect. The line trend tends to be stable.

[0110] like Fig.10 As shown in the figure, it is a relationship diagram between positioning error and the number of mobile base stations. The average value of positioning error shows a trend of first decreasing and then being relatively stable. This is because the NLOS recognition result is used as prior information, and only mobile base stations in the LOS state are used. The errors contained in these base stations are small, and the accuracy of the tags to be located is relatively high when providing positioning information. However, as the number of mobile base stations increases, the positioning complexity increases, and the positioning accuracy of the tags to be located will also decrease slightly. Therefore, the more mobile base stations there are, the better the effect will not be. In fact, if the number of mobile base stations is increased excessively, the trend change of the positioning rate is not obvious, and the positioning accuracy may even decline.

[0111] The present invention proposes a NLOS recognition algorithm based on the Kaleman-Mager (CMD) determinant, and uses an improved extended Kalman filter (EKF) algorithm to realize pedestrian dead reckoning (PDR) assisted UWB collaborative positioning. Experimental verification shows that the proposed algorithm highly integrates the tracking of personnel in underground confined spaces with the real trajectory, and the error of the trajectory point is concentrated within the range of 0.1m. The proposed algorithm can effectively identify the NLOS path, while suppressing the influence of the NLOS error, thereby improving the reliability and accuracy of the precision of personnel positioning.

[0112] It should be understood that, although each step in the flow chart of each embodiment of the present invention is shown in sequence according to the indication of the arrow, these steps are not necessarily performed in sequence according to the order indicated by the arrow. Unless there is a clear explanation in the present invention, the execution of these steps does not have strict order restrictions, and these steps can be performed in other orders. Moreover, at least a portion of the steps in each embodiment may include a plurality of sub-steps or a plurality of stages, and these sub-steps or stages are not necessarily performed at the same time, but can be performed at different times, and the execution order of these sub-steps or stages is not necessarily performed in sequence, but can be performed in turn or alternately with at least a portion of other steps or sub-steps or stages of other steps.

[0113] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program. The above-mentioned program can be stored in a non-volatile computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0114] The technical features of the above-mentioned embodiments can be combined arbitrarily. In order to make the description concise, not all possible combinations of the technical features in the above-mentioned embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0115] The above embodiments only express several implementation methods of the present invention, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the patent of the present invention. It should be pointed out that, for those of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present invention, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

[0116] The above are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A PDR-assisted UWB collaborative positioning method in a NLOS scenario in an underground confined space, characterized in that: The method comprises the following steps: S1: Establishing a four-element state hypothesis model, wherein the four-element state hypothesis model includes the LOS state and the NLOS state of the fixed base station, and the LOS state and the NLOS state of the mobile base station; S2: Identify whether each of the fixed base station or the mobile base station is in the NLOS state based on the CMD determinant, and define the fixed base station and the mobile base station in the LOS state as a first fixed base station and a first mobile base station, respectively; S3: Acquire location information of the tag to be located based on the PDR technology, acquire first distance information from the first fixed base station and the first mobile base station to the tag to be located based on the location information and the UWB positioning technology, and establish a state equation and a measurement equation of an extended Kalman filter based on the first distance information; S4: taking the error generated by positioning as the system state vector, predicting and updating the system state vector based on the state equation and the measurement equation to obtain a posteriori estimation information, and correcting and outputting the position of the tag to be positioned in real time based on the a posteriori estimation information.

2. The method according to claim 1, characterized in that: The establishment of the four-element state hypothesis model includes the following steps: Based on the first formula, the weight w of the mobile base station to the positioning tag is calculated i , the first formula is: Among them, D e is the ranging error, R e For relative positioning error, obtain the distance information between the fixed base station or the mobile base station and the tag to be located, model the distance information to generate a four-element state hypothesis model, the four-element state hypothesis model includes the LOS state hypothesis H1 and the NLOS state hypothesis H2 of the fixed base station, and the LOS state hypothesis H3 and the NLOS state hypothesis H4 of the mobile base station; Based on the second formula, the distance information d under the four state assumptions is obtained. i , the second formula is: Among them, r i is the measured distance from the ith fixed base station or mobile base station to the tag to be located obtained based on the TOA positioning algorithm, e i is the system noise, which satisfies Gaussian distribution, e i ~N(0,σ 2 ), n i is the forward error caused by NLOS propagation, e is the error of the mobile base station itself, n i >>e i .

3. The method according to claim 2, characterized in that Calculating the ranging error and relative positioning error includes the following steps: The distance measurement error D is calculated based on the third formula e , the third formula is: Among them, r a and r b represents the real distance and the estimated distance between the mobile base station and the tag to be located respectively, R is the communication radius, and the relative positioning error R is calculated based on the fourth formula e , the fourth formula is: Among them, (x i ,y i ) is the actual coordinate of the ith mobile base station, is the estimated coordinates of the i-th tag to be located, and n is the number of mobile base stations.

4. The method according to claim 1, characterized in that: Identifying whether each of the fixed base stations or the mobile base stations is in the NLOS state based on the CMD determinant comprises the following steps: Determine the CMD matrix, which is: Among them, A and B are the fixed base stations, T1 is the mobile base station, T2 is the tag to be located, d 2 (A,B)=d 2 (B, A), and both represent the square of the Euclidean distance between the fixed base station A and the fixed base station B, d 2 (A,T1)=d 2 (T1, A), and both represent the square of the distance between the fixed base station A and the mobile base station T1, d 2 (A,T2)=d 2 (T2, A), and both represent the square of the distance between the fixed base station A and the tag T2 to be located, d 2 (B,T1)=d 2 (T1, B), and both represent the square of the distance between the fixed base station B and the mobile base station T1, d 2 (B,T2) = d 2 (T2, B), and both represent the square of the distance between the fixed base station B and the tag to be located T2, d 2 (T1,T2)=d 2 (T2, T1), and both represent the square of the distance between the mobile base station T1 and the tag to be located T2; The determinant D(A, B, T1, T2) of the CMD matrix is ​​determined based on the fifth formula, which is: det is a determinant calculation formula, and the rank of the determinant D(A, B, T1, T2) is obtained. If the determinant is 0 and the rank is less than the first threshold, it means that the fixed base station or the mobile base station is in the NLOS state, otherwise it is in the LOS state.

5. The method according to claim 1, characterized in that Acquiring the location information of the tag to be located based on the PDR technology includes the following steps: Obtain the location information of the tag to be located at time k, the location information includes the current horizontal coordinate and vertical coordinate of the pedestrian where the tag to be located is located, the step length l and the direction angle θ, and calculate the horizontal coordinate x at the kth time based on the sixth formula k and the ordinate y k , the sixth formula is: Among them, (x k-1 ,y k-1 ) is the position coordinate of the pedestrian at time k-1, and the step length l is calculated based on the seventh formula, which is: Wherein, h is the height of the pedestrian, P is the step frequency of the pedestrian, and a, b and c are the first parameter, the second parameter and the third parameter respectively.

6. The method according to claim 1, characterized in that Establishing the state equation includes the following steps: The system state vector X is obtained based on the eighth formula t , the eighth formula is: Among them, e x(t) 、e y(t) 、e l(t) and are the position error, step error and angular error on the x-axis and y-axis respectively, and a state equation is established based on the ninth formula, which is: t+1 =f t X t +W t , where X t+1 is the system state vector at time t+1, f t is the system state transfer matrix, θ t is the direction angle of the pedestrian at time t, W t is the system dynamic noise, including the position noise w on the x-axis and y-axis x and w y , step noise w l and directional noise w θ , where w x ~N(0,2),w y ~N(0,2),w l ~N(0,1),w θ ~N(0,4).

7. The method according to claim 6, characterized in that The following steps are included before establishing the measurement equation: The initial measurement equation is established based on the tenth formula, which is: Z = H t X t +V t , where Z is the system measurement information, H t is the measurement coefficient matrix, V t is the measured noise, where V t ~N(0,R t ), R t is the measurement noise covariance matrix, and the measurement coefficient matrix is: in, is the location information of the first fixed base station at time t, is the location information of the tag to be located, is the location information of the virtual base station calculated based on the PDR technology at time t, is the distance information of the i-th first fixed base station or the first mobile base station calculated based on the PDR algorithm at time t, and the measurement noise covariance matrix is: The state assumption under the four-element state assumption model is binarized based on the eleventh formula, and the eleventh formula is: η i,t is the state of the i-th fixed base station or the mobile base station at time t, and a first matrix is ​​generated based on the binarization result, wherein the first matrix is Based on the first matrix, base stations in the NLOS state are identified and eliminated to obtain a transition measurement equation, which is: t =η t Z, where Z t is the measurement information at time t.

8. The method according to claim 7, characterized in that Establishing the measurement equation includes the following steps: The transition measurement equation is dimensionally simplified to obtain a new measurement equation, which is: in, is the distance measurement information between the i-th first fixed base station and the tag to be located obtained based on the UWB positioning technology, The distance information between the first mobile base station and the tag to be located is obtained based on the UWB positioning technology; Based on the twelfth formula, the The twelfth formula is: Based on the thirteenth formula, the The thirteenth formula is: Based on the fourteenth formula, the The fourteenth formula is: in, is the location information of the first mobile base station at time t.

9. The method according to claim 8, characterized in that The forecasting phase includes the following steps: Based on the fifteenth formula, the system state vector predicted at time t-1 is calculated. The fifteenth formula is: in, is the predicted system state vector at time t-1, f t / t-1 is the system state transfer matrix, and the mean square error matrix P of the prediction at time t-1 to time t is calculated based on the sixteenth formula t / t-1 , the sixteenth formula is: Among them, the is the transpose of the system state transfer matrix, Q t-1 is the system noise at time t-1, P t-1 is the mean square error matrix predicted at time t-1.

10. The method according to claim 9, characterized in that The update phase consists of the following steps: Based on the seventeenth formula, the filter gain K of the extended Kalman filter is obtained: t , the seventeenth formula is: in, is the transpose of the measurement coefficient matrix, and the measurement residual e is calculated based on the eighteenth formula t , the eighteenth formula is: Based on the nineteenth formula, the system state vector predicted Correction is performed to obtain the state correction matrix The nineteenth formula is: The mean square error matrix P predicted based on the twentieth formula t / t-1 Update to obtain the updated mean square error matrix The 20th formula is: The updated state correction matrix and the mean square error matrix are used as a posteriori estimation information.

Images