Method for enhancing UWB positioning based on two-stage algorithm

Abstract

The invention relates to the technical field of UWB positioning, and discloses a method for enhancing UWB positioning based on a two-stage algorithm, and the method comprises the steps: constructing a TDoA signal model, obtaining a distance difference equation, carrying out the rough positioning of a tag address according to the distance difference equation, and obtaining a rough positioning result (xcoarse, ycoarse) of the tag address; and a state space model is defined, accurate positioning is carried out based on the state space model according to the rough positioning result (xcoarse, ycoarse), and an accurate positioning result (xfinal, yfinal) is obtained. According to the invention, the UWB and the dual-stage positioning algorithm are combined, so that the problems of non-line-of-sight interference, multipath effect, clock synchronization requirement, calculation efficiency, balance between positioning precision and anti-interference capability and the like are effectively solved, and the performance and reliability of the positioning system are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of UWB positioning, and particularly to a method for enhancing UWB positioning based on a two-stage algorithm. Background Art

[0002] At present, UWB technology has been applied in fields such as smartphones, automotive digital keys, and industrial Internet of Things, and has great growth potential in the future. UWB positioning technology is mainly applied in fields such as high-precision positioning, radar, and data transmission. Specific application scenarios include end-to-end file transfer, anti-collision, sub-meter indoor positioning, directional remote control, automotive digital keys, automatic access control, and casualty search. The UWB technology has technical characteristics such as ultra-wide bandwidth, insensitivity to channel fading, and strong penetration ability, making it perform excellently in data transmission, precise positioning, and radar applications.

[0003] However, there are some problems in the application of UWB technology, such as 1. Limited positioning accuracy. Non-line-of-sight (NLOS) interference: The signal attenuates severely when penetrating metal and concrete, resulting in ranging errors (which may reach several meters). Multipath effect: Signal reflection / scattering in complex environments (such as indoors) leads to deviation in time delay estimation. 2. Short transmission distance. Frequency band limitation: The frequency band is 6 - 9 GHz, and high-frequency signals have poor penetration ability, and the effective distance is usually < 100 meters (in outdoor open environments). 3. High cost. Hardware: The price of UWB chips is significantly higher than that of Bluetooth / Wi-Fi modules. Deployment: It is necessary to densely deploy base stations (usually 1 base station per 100 ㎡), increasing the system cost. 4. High algorithm complexity. Signal processing: It is necessary to process nanosecond-level pulse signals, and the algorithms are complex (such as TDOA / TOA calculation and multipath suppression). Real-time requirement: In dynamic scenarios (such as robot navigation), low-latency resolution (< 10 ms) is required.

[0004] Therefore, there is an urgent need for a method for enhancing UWB positioning based on a two-stage algorithm to solve problems such as non-line-of-sight interference, multipath effect, clock synchronization requirements, calculation efficiency, and the balance between positioning accuracy and anti-interference ability. Summary of the Invention

[0005] The purpose of the present invention is to provide a method for enhancing UWB positioning based on a two-stage algorithm, aiming to solve the above problems.

[0006] The present invention provides a method for enhancing UWB positioning based on a two-stage algorithm, including:

[0007] Construct a TDoA signal model to obtain a distance difference equation, and roughly locate the tag address according to the distance difference equation to obtain a rough positioning result (x coarse , y coarse );

[0008] Define the state space model, and based on the state space model, perform precise positioning according to the rough positioning result (x coarse , y coarse ), and obtain the precise positioning result (x final , y final ).

[0009] Preferably, construct a TDoA signal model to obtain a distance difference equation, including: setting the tag position as (x, y), setting the number of base stations as N, the base station positions as (xi, yi), the signal propagation time as t i , the speed of light as c, and the distance as d i , then the distance equation is:

[0010]

[0011] where, ∈ i is the measurement noise, and ∈ i obeys a Gaussian distribution ∈ i ~N(0, σ 2 );

[0012] Set the base station position of base station 1 as (x1, y1), and obtain the time difference Δt according to the base station position of base station 1 i1 ;

[0013] According to the time difference Δt i1 obtain the distance difference equation, and the distance difference equation is:

[0014]

[0015] where, ∈ i1 = ∈ i - ∈1.

[0016] Preferably, perform rough positioning on the tag address according to the distance difference equation to obtain the rough positioning result (x coarse , y coarse ) of the tag address, including:

[0017] Introduce an intermediate variable Convert the distance difference equation into a linear form to obtain a linear expression of the distance difference equation:

[0018]

[0019] where, interference term

[0020] Preferably, perform rough positioning on the tag address according to the distance difference equation to obtain the rough positioning result (x coarse , y coarse ), and further include:

[0021] Convert the linear expression of the distance difference equation into the matrix form AX = b; where,

[0022]

[0023] Through the least-factorial solution X = (A T A) -1 A T B, obtain the rough positioning result (x coarse , y coarse ) of the tag address.

[0024] Preferably, define a state space model, including:

[0025] Set the target to move in a two-dimensional plane, and the state vector X k is expressed as:

[0026]

[0027] where, (x k , y k ) represents the position coordinates of the target at time k, represents the velocity component of the target.

[0028] Preferably, based on the state space model, perform precise positioning according to the rough positioning result (x coarse , y coarse ) to obtain the precise positioning result (x final , y final ), and further include:

[0029] Set the target to perform uniform motion in a two-dimensional plane, then the process equation is obtained as:

[0030] x k = Fx k-1 + w k , F represents the state transition matrix, and w k represents the process noise;

[0031] where, the state transition matrix F is expressed as:

[0032] Δt represents the time step;

[0033] The process noise w k is expressed as:

[0034] Q represents the process noise covariance matrix, and its expression is:

[0035] represents the acceleration noise variance.

[0036] Preferably, based on the state space model, precise positioning is performed according to the rough positioning result (x coarse , y coarse ), and the precise positioning result (x final , y final ) is obtained. It further includes:

[0037] An observation variable is determined according to the time difference Δt i1 , and the observation variable is:

[0038]

[0039] Z k represents the observation variable.

[0040] Preferably, based on the state space model, precise positioning is performed according to the rough positioning result (x coarse , y coarse ), and the precise positioning result (x final , y final ) is obtained. It further includes:

[0041] The observation equation is linearized, and the observation function h i (X k ) is expressed as:

[0042]

[0043] Preferably, based on the state space model, precise positioning is performed according to the rough positioning result (x coarse , y coarse ), and the precise positioning result (x final , y final ) is obtained. It further includes:

[0044] For each observation component h i , the partial derivatives of its position components x k and y k are:

[0045]

[0046] The partial derivative with respect to the velocity component is zero;

[0047] The final Jacobian matrix is expressed as:

[0048]

[0049] Preferably, based on the state space model, precise positioning is performed according to the rough positioning result (x coarse , y coarse ), and the precise positioning result (xfinal , y final ), further comprising:

[0050] Determine the state at time k

[0051] Covariance P k|k-1 = FP k-1|k-1 F T + Q;

[0052] Calculate the Kalman gain as:

[0053]

[0054] where R is the observation noise covariance matrix;

[0055] Perform state update,

[0056]

[0057] Obtain (x final , y final ) through the updated state.

[0058] The present invention aims to solve technical problems such as non-line-of-sight interference, multipath effect, clock synchronization requirements, computational efficiency, and the balance between positioning accuracy and anti-interference ability by leveraging the advantages of the combination of UWB and the two-stage positioning algorithm (TPTP), thereby improving the performance and reliability of the positioning system.

[0059] The present invention optimizes UWB positioning in a two-stage manner. In the first stage, the positioning error can be around 1m, and in the second stage, precise positioning can achieve an error of around 0.3m. By simple formula processing, part of the error is eliminated, and the positioning accuracy and precision are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only embodiments of the present invention, and those of ordinary skill in the art can also obtain other drawings according to the provided drawings without creative efforts.

[0061] Figure 1 is a schematic flowchart of a method for enhancing UWB positioning based on a two-stage algorithm according to the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0062] Next, the technical solutions in the embodiments of the present application will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in the present application without creative efforts belong to the scope of protection of the present application.

[0063] Currently, UWB (Ultra Wide Band) technology has been applied in fields such as smart phones, automotive digital keys, and industrial Internet of Things, and has great growth potential in the future. UWB positioning technology is mainly applied in fields such as high-precision positioning, radar, and data transmission. Specific application scenarios include end-to-end file transfer, anti-collision, sub-meter indoor positioning, directional remote control, automotive digital keys, automatic access control, and casualty search. UWB technology has technical characteristics such as ultra-wide bandwidth, insensitivity to channel fading, and strong penetration ability, making it perform excellently in data transmission, precise positioning, and radar applications.

[0064] However, there are some problems regarding the application of UWB technology, such as:

[0065] 1. Non-line-of-sight (NLOS) interference problem:

[0066] In a complex indoor environment, signals may propagate through non-line-of-sight paths such as reflection and refraction, resulting in the measured distance being greater than the actual distance and generating positioning errors. In the rough positioning stage of the TPTP algorithm, the residual weighted algorithm is used to identify NLOS measurement data, and in the fine positioning stage, the position estimation is optimized through the SMC framework and the SABO algorithm, effectively reducing the influence of NLOS interference. Among them, the TPTP algorithm is a positioning algorithm based on Time Difference of Arrival (TDOA).

[0067] 2. Multipath effect:

[0068] UWB signals may generate multipath signals during propagation, resulting in deviations in the measured time or distance. UWB technology itself has the characteristics of high bandwidth and narrow pulses, which can resolve multipath signals. Combined with the optimization process of the TPTP algorithm, the influence of the multipath effect on the positioning accuracy is further reduced.

[0069] 3. Clock synchronization requirements:

[0070] The TOA algorithm has extremely high requirements for clock synchronization, while the TDOA algorithm only requires synchronization between base stations. The TPTP algorithm uses an algorithm with lower requirements for clock synchronization in the rough positioning stage and improves the positioning accuracy through an optimized algorithm in the fine positioning stage, reducing the dependence on clock synchronization.

[0071] 4. Computational efficiency:

[0072] Traditional positioning algorithms may require a large amount of computing resources and are difficult to achieve real-time positioning. The TPTP algorithm optimizes the computing process through two-stage processing, improves the computing efficiency, and is suitable for real-time applications.

[0073] 5. Balance between positioning accuracy and anti-interference ability:

[0074] UWB technology provides high-precision positioning ability, but is vulnerable to interference in complex environments. The TPTP algorithm combines the high precision of UWB technology and anti-interference algorithms to achieve a balance between positioning accuracy and anti-interference ability, and improves the stability and reliability of the system.

[0075] The present invention aims to provide a method of combining UWB positioning algorithm with two-stage positioning algorithm (TPTP). Through physical layer signal design and algorithm optimization, technical problems such as non-line-of-sight interference, multipath effect, clock synchronization requirements, and computing efficiency are solved.

[0076] As Figure 1 shown, the present invention provides a method for enhancing UWB positioning based on a two-stage algorithm, including:

[0077] S100, constructing a TDoA signal model to obtain a distance difference equation, and roughly positioning the tag address according to the distance difference equation to obtain a rough positioning result (x coarse , y coarse ).

[0078] In some embodiments of the present application, constructing a TDoA signal model to obtain a distance difference equation includes: setting the tag position as (x, y), setting the number of base stations as N, the base station positions as (xi, yi), the signal propagation time as t i , the speed of light as c, and the distance as d i , then the distance equation is:

[0079]

[0080] wherein, ∈ i is measurement noise, and ∈ i obeys a Gaussian distribution ∈ i ~N(0, σ 2 );

[0081] Setting the base station position of base station 1 as (x1, y1), obtaining the time difference Δt according to the base station position of base station 1 i1 ;

[0082] The expression of the time difference Δt i1 is:

[0083]

[0084] According to the distance equation, the equation can be obtained as follows:

[0085] d i -d1 = c·Δt i1 +(∈ i -∈1);

[0086] Expanding the above equation gives the distance difference equation, and the distance difference equation is:

[0087]

[0088] wherein, ∈ i1 = ∈ i -∈1.

[0089] It can be understood that by constructing a TDoA signal model and deriving the distance difference equation, the time difference information between multiple base stations can be effectively utilized, thereby improving the positioning accuracy. This method not only considers the measurement noise in the signal propagation process, but also reduces the influence of non-line-of-sight (NLOS) interference and multipath effects on the positioning result through the time difference calculation between base stations.

[0090] In some embodiments of the present application, the tag address is roughly located according to the distance difference equation to obtain a rough positioning result (x coarse , y coarse ), including:

[0091] Introduce an intermediate variable Convert the distance difference equation into a linear form. Substitute the intermediate ratio variable R1 into both sides of the distance difference equation and expand to obtain (x - x i ) 2 +(y - y i ) 2 =(R1 + c·Δt i1 +∈ i1 ), 2 After arrangement, the linear expression of the distance difference equation can be obtained:

[0092]

[0093] wherein, Interference term

[0094] The interference term T remaining after assuming and eliminating in the subtraction of two statistics i , is 0.

[0095] It is understandable that by converting the distance difference equation into a linear form and using statistical methods to eliminate interference terms, a rough positioning of the tag location is achieved. This method is relatively simple computationally and can quickly provide an approximate positioning result, laying a foundation for subsequent precise positioning. In addition, by reasonably designing the process of statistical subtraction, the influence of measurement noise on the positioning result is effectively reduced, further improving the stability and reliability of the positioning.

[0096] In some embodiments of the present application, based on the distance difference equation, a rough positioning of the tag address is performed to obtain a rough positioning result (x coarse , y coarse ) of the tag address, and further includes:

[0097] Converting the linear expression of the distance difference equation into a matrix form AX = b; where,

[0098]

[0099]

[0100] By solving the least squares solution X = (A T A) -1 A T b, a rough positioning result (x coarse , y coarse ) of the tag address is obtained.

[0101] It is understandable that by converting the linear expression of the distance difference equation into a matrix form and using the least squares solution for solving, the calculation process can be further simplified and the calculation efficiency can be improved. This conversion makes the positioning algorithm more adaptable to large-scale data processing and real-time positioning requirements.

[0102] S200, defining a state space model, and based on the state space model, performing precise positioning according to the rough positioning result (x coarse , y coarse ) to obtain a precise positioning result (x final , y final ).

[0103] In some embodiments of the present application, defining a state space model includes:

[0104] Setting that the target moves in a two-dimensional plane, the state vector includes position and velocity, and the state vector X k is expressed as:

[0105]

[0106] Where, (x k , y k ) represents the position coordinates of the target at time k, Represents the velocity component of the target.

[0107] It can be understood that the present application can more comprehensively describe the motion state of the target. By not only considering the position information of the target but also introducing velocity information, the positioning accuracy and anti-interference ability are improved. Through the establishment of the state space model, the prediction and tracking of the target motion trajectory can be realized, further enhancing the stability and reliability of the UWB positioning system. In addition, this technical solution also considers the continuity of the target motion state, avoiding the jump of the positioning result caused by the single measurement error, thereby improving the smoothness and accuracy of the positioning result.

[0108] In some embodiments of the present application, based on the state space model, precise positioning is performed according to the rough positioning result (x coarse , y coarse ), and the precise positioning result (x final , y final ) is obtained, and it further includes:

[0109] Assume that the target moves at a constant speed in the two-dimensional plane, then the process equation is obtained as:

[0110] x k = Fx k-1 + w k , where F represents the state transition matrix and w k represents the process noise;

[0111] Among them, the state transition matrix F is expressed as:

[0112] Δt represents the time step;

[0113] The process noise w k is expressed as:

[0114] Q represents the process noise covariance matrix, and its expression is:

[0115] represents the acceleration noise variance.

[0116] It can be understood that by assuming that the target moves at a constant speed in the two-dimensional plane and using the process equation to describe the evolution of the target state over time, the actual motion of the target can be more accurately reflected. The state transition matrix F in the process equation precisely depicts the relationship between the target position and velocity over time, and the introduction of the process noise wk takes into account various uncertainty factors that may exist in the actual environment, making the model closer to the actual situation. In addition, by defining the process noise covariance matrix Q and considering the acceleration noise variance, the anti-interference ability and positioning accuracy of the model are further improved.

[0117] In some embodiments of the present application, based on the state space model, precise positioning is performed according to the rough positioning result (x coarse , y coarse ), and the precise positioning result (x final , y final ) is obtained. It further includes:

[0118] Determine the observation variable according to the time difference Δt i1 . The observation variable is:

[0119]

[0120] Z k represents the observation variable.

[0121] It can be understood that by using the time difference △ti1 to determine the observation variable Zk, the accuracy of the positioning system is further enhanced. As a bridge connecting the theoretical model and the actual observation data, the observation variable can directly reflect the target position information. In this method, the observation variable is combined with the state space model, and through continuous iteration and optimization, the true position of the target can be gradually approximated, thereby achieving high-precision positioning.

[0122] In some embodiments of the present application, based on the state space model, precise positioning is performed according to the rough positioning result (x coarse , y coarse ), and the precise positioning result (x final , y final ) is obtained. It further includes:

[0123] Perform linearization processing on the observation equation,

[0124] Then the observation function Substituting into the formula gives:

[0125]

[0126] It can be understood that by performing linearization processing on the observation equation, the calculation process can be simplified and the calculation efficiency can be improved. The linearized observation equation is easier to solve and can quickly obtain the precise positioning result. In addition, the linearization processing helps to reduce the model error and improve the positioning accuracy. In this method, after linearizing the observation function hi(Xk) and substituting it into the formula, the state space model can be further used for iteration and optimization to obtain a more accurate positioning result.

[0127] In some embodiments of the present application, based on the state space model, precise positioning is performed according to the rough positioning result (x coarse , y coarse)Perform precise positioning to obtain the precise positioning result (x final , y final ), and further includes:

[0128] For each observation component h i , the partial derivatives of its position components x k and y k are:

[0129]

[0130] The partial derivative with respect to the velocity component is zero;

[0131] The final Jacobian matrix is expressed as:

[0132]

[0133] In some embodiments of the present application, based on the state space model, precise positioning is performed according to the rough positioning result (x coarse , y coarse ), to obtain the precise positioning result (x final , y final ), and further includes:

[0134] Determine the state at time k

[0135] Covariance P k|k-1 = FP k-1|k-1 F T + Q;

[0136] Calculate the Kalman gain as:

[0137]

[0138] where R is the observation noise covariance matrix;

[0139] Perform state update,

[0140]

[0141] Obtain (x final , y final ) through the updated state.

[0142] It can be understood that by introducing the Kalman filter algorithm, this method can update the state estimation in real time and continuously correct the state variables using the observation data, thereby improving the continuity and stability of positioning. The calculation of the Kalman gain fully considers the influence of the observation noise, making the positioning result more robust. In the state update stage, by combining the previous state prediction and the current observation data, the position of the target can be estimated more accurately.

[0143] The present invention effectively solves technical problems such as non-line-of-sight interference, multipath effects, clock synchronization requirements, computational efficiency, and the balance between positioning accuracy and anti-interference ability through the advantages of the combination of UWB and the two-stage positioning algorithm (TPTP), improving the performance and reliability of the positioning system.

[0144] Those skilled in the art should understand that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0145] The present application is described with reference to the flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or block in the flowchart and / or block diagram can be implemented by computer program instructions, and the combination of the processes and / or blocks in the flowchart and / or block diagram can also be implemented. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate a device for implementing the functions specified in Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple blocks.

[0146] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory generate a manufactured article including an instruction device, and the instruction device implements the functions specified in Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple blocks.

[0147] These computer program instructions can also be loaded onto a computer or other programmable data processing device, so that a series of operation steps are executed on the computer or other programmable device to generate a computer-implemented process. Therefore, the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple blocks.

[0148] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the above embodiments, those of ordinary skill in the art should understand that modifications or equivalent substitutions can still be made to the specific embodiments of the present invention. Any modification or equivalent substitution that does not depart from the spirit and scope of the present invention shall be covered by the protection scope of the claims of the present invention.

Claims

1. A method for enhancing UWB positioning based on a two-stage algorithm, characterized in that Including: Construct a TDoA signal model to obtain the distance difference equation. Based on the distance difference equation, roughly locate the tag address to obtain the rough positioning result (x coarse , y coarse ) of the tag address; Define a state space model, and based on the state space model, perform precise positioning according to the rough positioning result (x coarse , y coarse ) to obtain a precise positioning result (x final , y final ).

2. The method for enhancing UWB positioning based on a two-stage algorithm according to claim 1, wherein Construct the TDoA signal model to obtain the distance difference equation, including: setting the tag position as (x, y), setting the number of base stations as N, the base station positions as (xi, yi), and the signal propagation time as t i , the speed of light as c, and the distance as d i , then the distance equation is: where, ∈ i is the measurement noise, and ∈ i obeys a Gaussian distribution ∈ i ~ N(0, σ 2 ); Set the base station location of base station 1 as (x1, y1), and obtain the time difference Δt according to the base station location of base station 1 i1 ; According to the time difference Δt i1 a distance difference equation is obtained, and the distance difference equation is as follows: where, ∈ i1 = ∈ i - ∈1.

3. The method for enhancing UWB positioning based on a two-stage algorithm according to claim 2, wherein Roughly locate the tag address according to the distance difference equation to obtain the rough location result (x coarse , y coarse ) of the tag address, including: Introduce an intermediate variable Convert the distance difference equation into a linear form to obtain a linear expression of the distance difference equation: Among them, interference item 4. The method for enhancing UWB positioning based on a two-stage algorithm according to claim 3, wherein Roughly locate the tag address according to the distance difference equation to obtain the rough positioning result (x coarse , y coarse ) of the tag address. It also includes: Transform the linear expression of the range difference equation into the matrix form AX = b; where, Obtain a rough positioning result of the label address through the minimum factorial solution X = (A T A) -1 A T b, getting (x coarse , y coarse ).

5. The method for enhancing UWB positioning based on a two-stage algorithm according to claim 1, wherein Define the state space model, including: Set the target to move in a two-dimensional plane, and the state vector X k is expressed as: Among them, (x k , y k ) represents the position coordinates of the target at time k, represents the velocity component of the target.

6. The method for enhancing UWB positioning based on a two-stage algorithm according to claim 5, characterized in that Performing precise positioning based on the rough positioning result (x coarse , y coarse ) according to the state space model to obtain the precise positioning result (x final , y final ), further comprising: Set the target to move at a constant speed in a two-dimensional plane, and the process equation is obtained as: x k = Fx k-1 + w k , where F represents the state transition matrix and w k represents the process noise; Where, the state transition matrix F is expressed as: Δt represents the time step; Process noise w k Denoted as: Q represents the process noise covariance matrix, and its expression is: represents the acceleration noise variance.

7. The method for enhancing UWB positioning based on a two-stage algorithm according to claim 6, wherein Performing precise positioning based on the rough positioning result (x coarse , y coarse ) according to the state space model to obtain the precise positioning result (x final , y final ), further comprising: Based on the time difference Δt i1 determine an observation variable, where the observation variable is: Z k represents an observed variable.

8. The method for enhancing UWB positioning based on a two-stage algorithm according to claim 7, wherein Performing precise positioning based on the rough positioning result (x coarse , y coarse ) according to the state space model to obtain the precise positioning result (x final , y final ), further comprising: Linearize the observation equation, then the observation function h i (X k ) is expressed as:

9. The method for enhancing UWB positioning based on a two-stage algorithm according to claim 8, wherein Performing precise positioning based on the rough positioning result (x coarse , y coarse ) according to the state space model to obtain the precise positioning result (x final , y final ), further comprising: For each observed component h i , the partial derivatives of its position components x k and y k are: The partial derivative of the velocity component is zero; The final Jacobian matrix is expressed as:

10. The method for enhancing UWB positioning based on a two-stage algorithm according to claim 9, characterized in that, Performing precise positioning based on the rough positioning result (x coarse , y coarse ) according to the state space model to obtain a precise positioning result (x final , y final ), further comprising: Determine the state at time k Covariance P k|k-1 = FP k-1|k-1 F T + Q; Calculate the Kalman gain as: Where R is the observation noise covariance matrix; Perform state update Obtain (x final , y final ) from the updated state.

Images