WO2023221655A1

WO2023221655A1 - Joint estimation method for target position and environmental propagation parameter of underwater wireless sensor network

Info

Publication number: WO2023221655A1
Application number: PCT/CN2023/084119
Authority: WO
Inventors: 梅骁峻; 韩德志; 吴中岱; 王骏翔; 郭磊; 胡蓉; 韩冰; 徐一言; 杨珉; 朱宇
Original assignee: 上海船舶运输科学研究所有限公司; 上海海事大学; 中远海运科技股份有限公司
Priority date: 2022-05-17
Filing date: 2023-03-27
Publication date: 2023-11-23
Also published as: CN115038165A; CN115038165B

Abstract

Provided in the present invention is a joint estimation method for a target position and an environmental propagation parameter of an underwater wireless sensor network. The method comprises: S1, according to a layered propagation effect of a sound signal under the water, constructing a ranging model of a received signal strength based on Snell's Law and a ray tracing theorem; S2, by means of a plurality of first-order Taylor series expansions, constructing an objective function using an environmental propagation parameter and a target position as variables; S3, performing coarse-grained estimation on the variables by using a dichotomy; and S4, performing linear expansion according to the value of the coarse-grained estimation, and performing iterative re-optimization to further improve the precision of a solution. The method has the advantages of solving the problem of an increased positioning error resulting from an unknown environmental propagation parameter and a layering effect in an underwater signal propagation process.

Description

A joint estimation method for underwater wireless sensor network target position and environmental propagation parameters

Technical field

The invention relates to the technical field of underwater wireless sensor network node positioning, and specifically relates to a joint estimation method of target position and environmental propagation parameters.

Background technique

As an important part of the marine three-dimensional monitoring system, Underwater Wireless Sensor Networks (UWSNs) is its important foundation and support. It can provide better information for maritime security, marine environmental protection, safe navigation of smart ships and other activities. Good technical means and information platform. In UWSNs, if the sensed data does not carry location information, the data will become meaningless. Therefore, how to obtain more accurate positioning is not only a requirement for ocean monitoring applications, but also the basis for studying other theoretical issues of UWSNs.

However, how to obtain more accurate position information in a complex and changeable underwater environment is still a problem that needs to be solved. On the one hand, the layered effect of signal propagation underwater makes the ranging and positioning accuracy low; on the other hand, the model parameters of signal propagation underwater, especially the environmental propagation factors, often change with the underwater temperature, humidity and salinity. It changes with the change, further increasing the positioning error. Existing positioning technology does not have a good solution to the above problems and cannot obtain high positioning accuracy in an underwater environment where signals have layered effects and the environmental propagation parameters are unknown.

Chapter 5 of "Research on Key Technologies for Target Node Positioning in Ocean Sensor Networks" (Mei Xiaojun, Shanghai Maritime University) proposes to consider radio signal propagation on the water surface. The scenario is 2D. This paper does not consider the simultaneous propagation of underwater acoustic signals. There are layering effects and absorption effects to build the ranging model, and the underwater acoustic signal propagation scene is 3D. The technical complexity and accuracy requirements of the underwater scene are also very different. Therefore, this article does not solve the problem of underwater wireless sensing in the environment. The problem of accurate positioning when propagation parameters are unknown.

Contents of the invention

The purpose of this invention is to provide a joint estimation method (TLESE) for underwater sensor network target positions and environmental propagation parameters to solve the problem of reduced positioning accuracy caused by signal layering effects and unknown environmental parameters in highly dynamic marine environments. . Through the implementation of the present invention, higher stability can be obtained under the above harsh conditions. bit precision.

In order to achieve the above objects, the present invention is achieved through the following technical solutions:

A joint estimation method for underwater sensor network target position and environmental propagation parameters, which is characterized by including the following steps:

S1. According to the layered propagation effect of acoustic signals underwater, construct a ranging model of received signal strength based on Snell's law and ray tracing theorem; the Snell's law is combined with the ray tracing theorem to obtain the i-th anchor The distance from the node to the target node

Among them, a represents the gradient parameter, b represents the propagation speed of sound waves on the water surface, θ ⁱ and θ ^x represent the received signal angle of the i-th anchor node and the target node respectively, and their value range is [-π/2, π/2 ]; According to the distance of the target node Construct a ranging model of received signal strength;

S2. Construct an objective function with environmental propagation parameters and target position as variables through multiple first-order Taylor series expansions;

S3. Use the dichotomy method to conduct coarse-grained estimation of variables;

S4. Perform linear expansion based on the coarse-grained estimated value and re-optimize iteratively to further improve the accuracy of the solution and obtain the estimated value of the target position:

in, is the κth target position estimate, is the position of the i-th anchor node at time t, is the distance between the i-th anchor node and the target node, and N is the number of buoy sensor nodes deployed underwater that contain position information.

As described in step S1, based on the layered propagation effect of acoustic signals underwater, a ranging model of received signal strength based on Snell's law is constructed, specifically including:

S11. If N buoy sensor nodes containing position information are deployed underwater, that is, anchor nodes and a target node. Assume that the position of the i-th anchor node at time t is Target node at time t The location is Assume that all nodes are equipped with pressure sensors and can accurately know their own depth information; according to the propagation speed model of acoustic signals underwater, we can get:

C(z)＝az+b (3)

Among them, a represents the gradient parameter; z represents the water depth; b represents the acoustic wave propagation speed on the water surface; C(z) represents the acoustic signal propagation velocity solution function when the water depth is z.

S12. According to Snell’s law, it can be obtained:

Among them, θ ⁱ and θ ^x represent the received signal angle of the i-th anchor node and the target node respectively, and their value range is [-π/2, π/2]; k represents a constant.

S13. Obtain the distance information between the anchor node and the target based on the formula (1), and obtain the ranging model based on signal strength in the underwater environment through the path loss model, which is:

in, Indicates the power of the target node received by the i-th anchor node at time t; Represents the transmit power of the target node at time t; PL(d ₀ ) represents the loss value when the reference distance is d ₀ , d ₀ is usually 1m; α ^t represents the environmental propagation parameter of the signal; is the distance between the i-th anchor node and the target node; It means that for the i-th anchor node at time t, the signal attenuates the noise. Assume that the noise variance at each moment is equal. If it obeys the mean value of zero, the variance is The Gaussian distribution can be expressed as α ^f represents the absorption factor of the signal, which can be obtained according to the emission frequency according to Thorpe's theorem, that is:

As described in step S2, the objective function with environmental propagation parameters and target position as variables is constructed through multiple first-order Taylor series expansions, specifically including:

S21. Hypothesis in is the maximum distance value between the anchor node and the target. According to Euler's geometric theorem and signal processing principles, when the area of the underwater monitoring area is determined, the maximum error c _max caused by the absorption effect of the signal can be determined.

S22. Perform the term shift transformation according to the expression (5) obtained in S13, and square the distance between each anchor node and the target node at time t to obtain:

in, is the distance between the i-th anchor node and the target node, Indicates the signal attenuation noise of the i-th anchor node at time t, d ₀ is the reference distance, usually 1m, represents the power of the target node received by the i-th anchor node at time t, α ^t represents the environmental propagation parameter of the signal;

S23. Using Taylor’s first-order expansion of the exponential function a ^x = 1 + xlna, for Performing Taylor's first-order expansion, when the reference distance d ₀ =1m, equation (7) in S22 can be expressed as:

in, is the distance between the i-th anchor node and the target node, Indicates the signal attenuation noise of the i-th anchor node at time t, represents the power of the target node received by the i-th anchor node at time t, α ^t represents the environmental propagation parameter of the signal;

S24. Introduce parameters make and in when When it is smaller, approaches α ^t . Then S23 Chinese formula (8) can be transformed into:

S25. Assumption smaller, i.e. right Perform Taylor's first-order expansion, then:

in, is the distance between the i-th anchor node and the target node, represents the signal attenuation noise of the i-th anchor node at time t, α ^t represents the environmental propagation parameter of the signal;

S26. However, equation (10) in S25 is still nonlinear and difficult to solve, so it is assumed is smaller, using Taylor's first-order expansion of the exponential function, for Perform Taylor's first-order expansion, which can be further transformed into:

in, is the distance between the i-th anchor node and the target node, represents the signal attenuation noise of the i-th anchor node at time t, α ^t represents the environmental propagation parameter of the signal, As shown in S24;

S27. After obtaining the square of the distance, construct a least squares framework based on weights:

in, ||·|| is the second-order norm, is the distance between the i-th anchor node and the target node, is the position of the i-th anchor node at time t;

S28, order Expand S27 Chinese formula (12), we can get:

in,

I and 0 represent the identity matrix and zero matrix respectively.

As described in step S3, the dichotomy method is used to conduct coarse-grained estimation of variables, specifically including:

S31. Introduce the multiplier λ, and solve its value at each moment according to equation (14), that is:
λ=((A ^T ωA+λD) ^-1 (A ^T ωB-λf)) ^T D((A ^T ωA+λD) ^-1 (A ^T ωB-λf)) (15);

S32. Solve the optimal multiplier λ ^* at each moment according to equation (15), that is:
λ ^* =max[-diag(A ^T ωA), λ] (16);

S33. According to the obtained optimal multiplier λ ^* , the variable value of coarse-grained estimation can be calculated, that is:

θ ^t = (A ^T ωA+λ ^* D) ^-1 (A ^T ωB-λ ^* f) (17);

S34. After obtaining the variable θ ^r , the estimated value of the environmental propagation parameter is Among them, θ ^t | _5,1 is the 5th row and 1st column of the variable θ ^r to be determined; in order to further optimize the estimated value of the path loss factor, the obtained position information, that is, θ ^t | _2:4,1 is used to solve the path The average value of the loss factor, that is:

in, Estimated position information for the unknown node; θ ^t | _2:4,1 The estimated distance to the anchor node.

As described in step S4, linear expansion is performed based on the coarse-grained estimated value and iterative re-optimization is performed to further improve the accuracy of the solution and obtain the estimated value of the target position, which specifically includes:

S41. Coarse-grained target estimate obtained according to S34 As the initial value, that is According to the first-order expansion of Taylor series, the estimated value is linearized at the κth time, and we can get:

S42. Derive and simplify S41 equation (19) with respect to x ^t , and the κ+1th target position estimate can be obtained as the above formula (2).

Compared with the existing technology, the present invention has the following advantages: it establishes a UWSNs ranging model with a layered effect and unknown environmental propagation parameters, and can jointly estimate the environmental propagation parameters and target position at each moment, thereby improving the accuracy of positioning. Compared with the existing paper "Research on Key Technologies for Target Node Positioning in Ocean Sensor Networks", this invention considers the propagation of underwater acoustic signals, the scene is 3D, and it also takes into account the layered effect of signal propagation underwater. And the ranging model constructed by absorption effect. In addition, during the solution process of the algorithm, the solution method proposed by the present invention combines the dichotomy method and the linear re-optimization method based on Taylor series expansion, which is obviously different from the related methods in Chapter 5 of the paper. More importantly, the method proposed by the present invention is targeted at underwater scenes, and has significant improvements in computational complexity and accuracy. Therefore, the present invention is suitable for positioning field The scene, model establishment and algorithm solution process are all essentially different from Chapter 5 of this paper, and the positioning performance of this invention is better than the method proposed in Chapter 5 of this paper.

Description of the drawings

Figure 1 is a flow chart of a joint estimation method of underwater sensor network target position and environmental propagation parameters according to the present invention.

Figure 2(a) and Figure 2(b) show the estimation errors corresponding to different numbers of anchor nodes in the present invention.

Figure 3(a) and Figure 3(b) show the estimation errors of the target position and environmental propagation parameters (path loss factor) of different anchor nodes in the present invention.

Figure 4(a) and Figure 4(b) show the estimation errors corresponding to the target position and environmental propagation parameters (path loss factor) of different absorption factors according to the present invention.

Detailed ways

The present invention will be further elaborated below by describing in detail a preferred specific implementation case in conjunction with the accompanying drawings.

Figure 1 shows a flow chart of a joint estimation method of underwater sensor network target position and environmental propagation parameters of the present invention, which specifically includes:

S2. Construct an objective function with environmental propagation parameters and target position as variables through multiple first-order Taylor series expansions:

S3. Use the dichotomy method to conduct coarse-grained estimation of variables;

In this implementation case, the step S1 specifically includes:

S11. If N buoy sensor nodes containing position information are deployed underwater, that is, anchor nodes and a target node. Assume that the position of the i-th anchor node at time t is The position of the target node at time t is Assume that all nodes are equipped with pressure sensors and can accurately know their own depth information; according to the propagation speed model of acoustic signals underwater, we can get:

C(z)＝ax+b (3)

S12. According to Snell’s law, it can be obtained:

S13. After obtaining the distance information between the anchor node and the target (ie the formula (1)), obtain the ranging model based on signal strength in the underwater environment through the path loss model, which is:

in, Indicates the power of the target node received by the i-th anchor node at time t; Represents the transmit power of the target node at time t; PL(d ₀ ) represents the loss value when the reference distance is d ₀ , d ₀ is usually 1m; α ^t represents the environmental propagation parameter of the signal; is the distance between the i-th anchor node and the target node; It means that the signal attenuation noise of the i-th anchor node at time t, assuming that the noise variance at each time is equal, if it obeys the uniform The value is zero and the variance is The Gaussian distribution can be expressed as α ^f represents the absorption factor of the signal, which can be obtained according to the emission frequency according to Thorpe's theorem, that is:

The step S2 specifically includes:

in, is the distance between the i-th anchor node and the target node, Indicates the signal attenuation noise of the i-th anchor node at time t, d ₀ is the reference distance, usually 1m, represents the power of the target node received by the i-th anchor node at time t, and α ^t represents the environmental propagation parameter of the signal.

in, is the distance between the i-th anchor node and the target node, Indicates the signal attenuation noise of the i-th anchor node at time t, represents the power of the target node received by the i-th anchor node at time t, and α ^t represents the environmental propagation parameter of the signal.

in, is the distance between the i-th anchor node and the target node, represents the signal attenuation noise of the i-th anchor node at time t, and α ^t represents the environmental propagation parameter of the signal.

in, is the distance between the i-th anchor node and the target node, represents the signal attenuation noise of the i-th anchor node at time t, α ^t represents the environmental propagation parameter of the signal, As shown in S24.

S27, order Expand S27 Chinese formula (12), we can get:

in,

I and 0 represent the identity matrix and zero matrix respectively.

The step S3 specifically includes:

S31. Introduce the multiplier λ, and solve its value at each moment according to equation (14), that is:
λ＝((A ^T ωA+λD) ^-1 (A ^T ωB-λf)) ^T D((A ^T ωA+λD) ^-1 (A ^T ωB-λf)) (15)

S32. Solve the optimal multiplier λ ^* at each moment according to equation (15), that is:
λ ^* =max[-diag(A ^T ωA), λ](16)

S33. According to the obtained optimal multiplier λ ^* , the variable value of coarse-grained estimation can be calculated, that is:
θ ^t = (A ^T ωA+λ ^* D) ^-1 (A ^T ωB-λ ^* f) (17)

The step S4 specifically includes:

S42. Derive and simplify S41 equation (18) with respect to x ^t to obtain the κ+1th target position estimate. is the formula (2).

In order to verify the effectiveness of the method of the present invention (referred to as TLESE algorithm), simulation experiments were conducted in Matlab R2021b. For different scenarios, compare the existing algorithms USR, PLSE, and TSE, and use the minimum root mean square error (RSME) as the evaluation index to evaluate the performance of the algorithm, namely:

Among them, MC represents the total number of Monte Carlo simulations; nu represents the number of Monte Carlo simulations.

In addition, in order to simulate the dynamic scene in the ocean, the anchor node and target node positions of each Monte Carlo simulation are set to change in the simulation. The settings of other fixed parameters are as follows: a=0.1, b=1473m/s, P ₀ =-55dBm, MC=1000, κ=1000, the side length of the simulation area is 50m; in addition, the empirical value of α ^t is usually 2 to 6 ,like If the value range is too small or too large, the corresponding δ will be too small or too large, so the equation (9) expanded by Taylor series in step S25 will be invalid, so the median 4.75 is selected as The value of

Figure 2(a) and Figure 2(b) show the estimation errors of target position and environmental propagation parameters of different noises in the case of N=8 and α ^f =0.06. It can be seen from the figure that as the noise increases, the corresponding positioning error also increases. For the estimation of target position, the method TLESE proposed by the present invention incorporates the linear iterative re-optimization method, and its positioning accuracy is significantly better than the other three methods. The same result can be obtained from the environmental parameter estimation error in Figure 2(b). Although the environmental parameter estimation error of the method proposed in this invention increases as the noise increases, the overall estimation performance is better than the other three methods.

Figure 3(a) and Figure 3(b) show that at α ^f =0.06, In this case, the estimation errors of different anchor nodes for the target position and environmental propagation parameters. As the number of anchor nodes increases, the received signal strength (RSS) information that can be used to estimate underwater acoustic signal propagation also increases, so the estimation error of each algorithm decreases as the number of anchor nodes increases. As can be seen from Figure 3(a), the positioning accuracy of each algorithm is relatively close when N=6, especially PLSE and TSE. As the number of anchor nodes continues to increase, the advantages of TLESE's positioning performance gradually become apparent, and its position estimation accuracy is better than the other three methods. This superiority is further illustrated in the environmental propagation parameter estimation error in Figure 3(b).

Figure 4(a) and Figure 4(b) show that at N=8, In this case, the estimation error of target position and environmental propagation parameters due to different absorption factors. It can be seen from Figure 4(a) that the estimation performance of USR, PLSE and TLESE is relatively robust to changes in absorption factors, while the position estimation error of TSE increases with the absorption factor. Increases as the income factor increases. Among the four methods, the TLESE proposed by the present invention has better position estimation performance. As can be seen in Figure 4(b), the estimation errors of USR, PLSE and TSE for environmental parameters increase as the absorption factor increases. On the other hand, for TLESE, the estimation error of environmental propagation parameters increases with the increase of absorption factor. And decrease. Therefore, the TLESE proposed by the present invention has better estimation performance of environmental parameters under different absorption factors.

Although the content of the present invention has been described in detail through the above preferred embodiments, it should be understood that the above description should not be considered as limiting the present invention. Various modifications and substitutions to the present invention will be apparent to those skilled in the art after reading the above. Therefore, the protection scope of the present invention should be defined by the appended claims.

Claims

A joint estimation method for underwater wireless sensor network target position and environmental propagation parameters, which is characterized by including the following steps:

S1. According to the layered propagation effect of acoustic signals underwater, construct a ranging model of received signal strength based on Snell's law and ray tracing theorem; the Snell's law is combined with the ray tracing theorem to obtain the i-th anchor The distance from the node to the target node

Among them, a represents the gradient parameter, b represents the propagation speed of sound waves on the water surface, θ i and θ x represent the received signal angle of the i-th anchor node and the target node respectively, and their value range is [-π/2, π/2 ]; According to the distance of the target node Construct a ranging model of received signal strength;

S2. Construct an objective function with environmental propagation parameters and target position as variables through multiple first-order Taylor series expansions;

S3. Use the dichotomy method to conduct coarse-grained estimation of variables;

S4. Perform linear expansion based on the coarse-grained estimated value and re-optimize iteratively to further improve the accuracy of the solution and obtain the estimated value of the target position:

in, is the κth target position estimate, is the position of the i-th anchor node at time t, is the distance between the i-th anchor node and the target node, and N is the number of buoy sensor nodes deployed underwater that contain position information.
A joint estimation method of underwater wireless sensor network target position and environmental propagation parameters as claimed in claim 1, characterized in that step S1 is based on the layered propagation effect of acoustic signals underwater to construct a Sneak-based The ranging model of received signal strength based on Er's law specifically includes:

S11. If N buoy sensor nodes containing position information are deployed underwater, that is, anchor nodes and a target node; assuming that the position of the i-th anchor node at time t is Target node at time t The location is Assume that all nodes are equipped with pressure sensors and can accurately know their own depth information; according to the propagation speed model of acoustic signals underwater, we can get:
C(z)＝az+b (3)

Among them, a represents the gradient parameter; z represents the water depth; b represents the sound wave propagation speed on the water surface; C(z) represents the acoustic signal propagation speed solution function when the water depth is z;

S12. According to Snell’s law, it can be obtained:

Among them, θ i and θ x represent the received signal angle of the i-th anchor node and the target node respectively, and their value range is [-π/2, π/2]; k represents a constant;

S13. Obtain the distance information between the anchor node and the target based on the formula (1), and obtain the ranging model based on signal strength in the underwater environment through the path loss model, which is:

in, Indicates the power of the target node received by the i-th anchor node at time t; Represents the transmit power of the target node at time t; PL(d 0 ) represents the loss value when the reference distance is d 0 , d 0 is usually 1m; α t represents the environmental propagation parameter of the signal; is the distance between the i-th anchor node and the target node; It means that for the i-th anchor node at time t, the signal attenuates the noise. Assume that the noise variance at each moment is equal. If it obeys the mean value of zero, the variance is The Gaussian distribution can be expressed as α f represents the absorption factor of the signal, which can be obtained according to the emission frequency f according to Thorpe’s theorem, that is:
A joint estimation method of underwater wireless sensor network target position and environmental propagation parameters as claimed in claim 2, characterized in that in step S2, the environmental propagation parameters and The target position is the objective function of the variable, specifically including:

S21. Hypothesis in is the maximum distance value between the anchor node and the target. According to Euler's geometric theorem and signal processing principle, when the area of the underwater monitoring area is determined, the absorption effect of the signal causes The maximum error c max can be determined;

S22. Perform the term shift transformation according to the expression (5) obtained in S13, and square the distance between each anchor node and the target node at time t to obtain:

in, is the distance between the i-th anchor node and the target node, Indicates the signal attenuation noise of the i-th anchor node at time t, d 0 is the reference distance, usually 1m, represents the power of the target node received by the i-th anchor node at time t, α t represents the environmental propagation parameter of the signal;

S23. Using Taylor’s first-order expansion of the exponential function a x = 1 + xlna, for Performing Taylor's first-order expansion, when the reference distance d 0 =1m, equation (7) in S22 can be expressed as:

in, is the distance between the i-th anchor node and the target node, Indicates the signal attenuation noise of the i-th anchor node at time t, represents the power of the target node received by the i-th anchor node at time t, α t represents the environmental propagation parameter of the signal;

S24. Introduce parameters make and in when When it is smaller, Approaching α t ; then the formula (8) in S23 can be transformed into:

in, is the distance between the i-th anchor node and the target node, Indicates the signal attenuation noise of the i-th anchor node at time t, represents the power of the target node received by the i-th anchor node at time t, α t represents the environmental propagation parameter of the signal;

S25. Assumption smaller, i.e. right Perform Taylor's first-order expansion, then:

in, is the distance between the i-th anchor node and the target node, represents the signal attenuation noise of the i-th anchor node at time t, α t represents the environmental propagation parameter of the signal;

S26. However, equation (10) in S25 is still nonlinear and difficult to solve, so it is assumed is smaller, using Taylor's first-order expansion of the exponential function, for Perform Taylor's first-order expansion, which can be further transformed into:

in, is the distance between the i-th anchor node and the target node, represents the signal attenuation noise of the i-th anchor node at time t, α t represents the environmental propagation parameter of the signal, As shown in S24;

S27. After obtaining the square of the distance, construct a least squares framework based on weights:

in, ||·|| is the second-order norm, is the distance between the i-th anchor node and the target node, is the position of the i-th anchor node at time t;

S28, order Expand S27 Chinese formula (12), we can get:

in,

I and 0 represent the identity matrix and zero matrix respectively.
A joint estimation of underwater wireless sensor network target position and environmental propagation parameters as claimed in claim 3 The method is characterized in that, as described in step S3, the dichotomy method is used to conduct coarse-grained estimation of variables, specifically including:

S31. Introduce the multiplier λ, and solve its value at each moment according to equation (14), that is:
λ=((A T ωA+λD) -1 (A T ωB-λf)) T D((A T ωA+λD) -1 (A T ωB-λf)) (15);

S32. Solve the optimal multiplier λ * at each moment according to equation (15), that is:
λ * =max[-diag(A T ωA), λ] (16);

S33. According to the obtained optimal multiplier λ * , the variable value of coarse-grained estimation can be calculated, that is:
θ t =(A T ωA+λ * D) -1 (A T ωB-λ * f) (17);

S34. After obtaining the variable θ t , the estimated value of the environmental propagation parameter is Among them, θ t | 5,1 is the 5th row and 1st column of the variable θ t to be determined; in order to further optimize the estimated value of the path loss factor, the obtained position information, that is, θ t | 2:4,1 is used to solve the path The average value of the loss factor, that is:

in, Estimated position information for the unknown node; θ t | 2:4,1 The estimated distance to the anchor node.
A joint estimation method of underwater wireless sensor network target position and environmental propagation parameters according to claim 4, characterized in that step S4 performs linear expansion according to the coarse-grained estimated value and re-optimizes iteratively, and further Improve the accuracy of the solution, including:

S41. Coarse-grained target estimate obtained according to S34 As the initial value, that is According to the first-order expansion of Taylor series, the estimated value is linearized at the κth time, and we can get:

S42. Derive and simplify S41 equation (19) with respect to x t , and the κ+1th target position estimate can be obtained as the above formula (2).