CN102170658A - Geometric positioning improvement method under NLOS (non-line-of-sight) environment - Google Patents

Abstract

The invention relates to a geometric positioning improvement method under an NLOS (non-line-of-sight) environment, which adopts the Gauss-Newton iteration method with fast convergence, and combines with a grid searching method to optimize the iteration initial coordinate value. Compared with the existing geometric positioning method, the geometric positioning improvement method has the advantages that the complexity is moderated, the fast and stable convergence is provided, and the higher mobile station positioning accuracy can be obtained.

Description

Geometry location under a kind of NLOS environment is improved one's methods

Technical field

The geometry location that the present invention relates under a kind of NLOS environment is improved one's methods, and is applied to the Wireless Location in Cellular Network technical field.

Background technology

The geometry location method is according to the geometric distributions relation of one or more measurement parameters and base station and travelling carriage travelling carriage to be carried out location estimation.

Be used to carry out the parameter TOA of position calculation and the measure error of AOA mainly is made up of two parts, promptly systematic measurement error and NLOS propagate the error that produces.The systematic measurement error Gaussian distributed, along with the continuous development of technology can reduce gradually, and the NLOS propagated error is existed by the meeting that influences of radio propagation environment all the time, and becomes the chief component of measure error.Propagating in numerous methods to the influence that mixes TOA/AOA method positioning accuracy at reducing NLOS, scattering model is wherein a kind of.Under the prerequisite of using the individual reflection model, the existing geometric localization method traditional TOA (Time Of Arrival)/AOA (Angle Of Arrival) method is arranged and improve one's methods, LOP (linear line of position) method and the HLOP that improves one's methods (hybrid LOP) method thereof etc., this several method utilizes the geometry site between travelling carriage, scattering object and the base station to estimate the position of travelling carriage, though method is simple, operand is little, positioning accuracy is but not high.Generally speaking, orientation problem to travelling carriage all is summed up as lsqnonlin, wherein the most basic a kind of method is Gauss-Newton iteration method, its feature is when having preferably the iteration initial value, has good convergence, if and selected relatively poor initial value, convergence will variation.

Summary of the invention

For avoiding above the deficiencies in the prior art, the geometry location that the present invention proposes under a kind of NLOS environment is improved one's methods, to solve the not high problem of positioning accuracy.The present invention uses the grid search method to optimize travelling carriage primary iteration coordinate figure, makes gauss-newton method obtain better convergence.

Purpose of the present invention is achieved through the following technical solutions:

Geometry location under a kind of NLOS environment is improved one's methods, and this method comprises parameter measurement and location of mobile station two stages of estimation, and concrete grammar is as follows:

1) the parameter measurement stage:

According to the base station signal parameter that moving table measuring obtains, time of arrival (toa)

Arrive angle with signal

And the geometry site of travelling carriage, scattering object and base station has:

${\widetilde{\mathrm{\α}}}_i-\arctan \left(\frac{y_i-y_b^i}{x_i-x_b^i}\right)=0---\left(1\right)$

${\tilde{L}}_i-r_b^i-r_m^i={\tilde{L}}_i-\sqrt{{(x_i-x_b^i)}^2+{(y_i-y_b^i)}^2}-\sqrt{{(x_i-x_m)}^2+{(y_i-y_m)}^2}=0---\left(2\right)$

Its neutralization

Represent the base station signal propagation distance respectively and arrive angle, (x _i, y _i) and (x _m, y _m) represent the position coordinates of base station, scattering object and travelling carriage respectively, the signal propagation distance can be multiplied each other by measurement parameter signal propagation time and light velocity C and obtain:

${\tilde{L}}_i={\widetilde{\mathrm{\τ}}}_i\×C---\left(3\right)$

2) location of mobile station estimating stage: according to the base station signal parameter and Adopt Gauss's Newton iteration method and optimize the iteration initial coordinate values, resolve the position that obtains travelling carriage, specifically may further comprise the steps in conjunction with the grid search method:

(1) dwindles feasible zone

Under the NLOS environment, because travelling carriage (x _m, y _m) and each base station

Between distance add that to the distance of travelling carriage scattering object constitutes to the distance the base station by scattering object, promptly

Then according to parameter and

The position of rough estimate travelling carriage:

$\left\{\begin{array}{c}{\hat{x}}_{\mathrm{ml}}^i=x_b^i+{\tilde{L}}_i\cos {\widetilde{\mathrm{\α}}}_i\\ {\hat{y}}_{m1}^i=y_b^i+{\tilde{L}}_i\sin {\widetilde{\mathrm{\α}}}_i\end{array}\right.---\left(4\right)$

$\left\{\begin{array}{c}{\hat{x}}_{m2}^i=x_b^i+({\tilde{L}}_i-R_d^i)\cos {\widetilde{\mathrm{\α}}}_i\\ {\hat{y}}_{m2}^i=y_b^i+({\tilde{L}}_i-R_d^i)\sin {\widetilde{\mathrm{\α}}}_i\end{array}\right.---\left(5\right)$

The constraints that is proposed according to the GBSBCM model

Obtain the feasible zone of algorithm:

$0\≤x_m\≤(\sqrt{3}/2)R$ (6)

$0\≤y_m\≤\min \{\sqrt{3}{x}_m,-(\sqrt{3}/3)x_m+R\}$

$0\≤r_m^i=|x_i+{\mathrm{jy}}_i-(x_m+{\mathrm{jy}}_m)|\≤R_d^i,i=\mathrm{1,2,3}---\left(7\right)$

Wherein, R is a radius of society,

Be the maximum of the scattering object distribution radius of each base station correspondence, i=1,2,3.

From the coordinate of above (4), (5), choose maximum, the min coordinates value of satisfied (6), (7), obtain a feasible zone [x who dwindles _Min, x _Max] * [y _Min, y _Max].

(2) grid search is determined the iteration initial coordinate values

In the feasible zone that this dwindles, carry out grid search, choose the grid point that satisfies constraints (7) and constitute candidate's point set CPS;

The coordinate figure of all candidate points among the CPS is asked on average the iteration initial value x that is optimized ₀

(3) adopt Gauss-Newton iteration method to estimate the position coordinates of travelling carriage

In theory, each base station signal arrives the propagation distance of travelling carriage and arrives angle and can be expressed as:

L(x)＝[L ₁(x)，L ₂(x)，L ₃(x)，α ₁(x)，α ₂(x)，α ₃(x)] (8)

X=[x wherein _m, y _m], $L_i\left(x\right)=\sqrt{{(x-x_b^i)}^2+{(y-y_b^i)}^2},$ ${\mathrm{\α}}_i\left(x\right)=\arctan \frac{y-y_b^i}{x-x_b^i}$

And the actual propagation distance of each base station signal with the arrival angle is:

$\tilde{L}=L\left(x\right)+n---\left(9\right)$

Wherein n is that NLOS propagates the error cause and obeys the systematic measurement error that average is zero Gaussian Profile;

Because the existence of error, (1), (2) always can not be met, and obtain target function thus:

$\mathrm{\ϵ}\left(x\right)={(\tilde{L}-L\left(x\right))}^T{\mathrm{\Σ}}_n^{-1}(\tilde{L}-L\left(x\right))---\left(10\right)$

∑ wherein _nCovariance matrix for noise n:

∑ _n＝E{nn ^T} (11)

The coordinate that then satisfies following formula promptly can be used as the location estimation value of travelling carriage:

$\hat{x}=\underset{x}{\arg \min }\mathrm{\ϵ}\left(x\right)---\left(12\right)$

To (9) formula at iteration initial value x ₀The place carries out linearisation:

$L\left(x\right)=L\left(x_0\right)+\mathrm{\φ}\left(x\right){|}_{x=x_0}(x-x_0)---\left(13\right)$

Wherein

$\mathrm{\φ}\left(x\right)={{\▿}_x}^T\⊗L\left(x\right)=\left[\begin{array}{c}(x-x_1)/r_1,\left({y-y}_1\right)/r_1\\ \left({x-x}_2\right)/r_2,\left({y-y}_2\right)/r_2\\ (x-x_3)/r_3,\left({y-y}_3\right)/r_3\\ (y-y_1)/{r_1}^2,(x-x_1)/{r_1}^2\\ \left({y-y}_2\right)/{r_2}^2,\left({x-x}_2\right)/{r_2}^2\\ \left({y-y}_3\right)/{r_3}^2,(x-x_3)/{r_3}^2\end{array}\right],---\left(14\right)$

According to (10), (13) formula, following formula is carried out iterative:

$x^{(k+1)}=x^{\left(k\right)}+{\left({\mathrm{\φ}}^T\left(x^{\left(k\right)}\right){\mathrm{\Σ}}_n^{-1}\mathrm{\φ}\left(x^{\left(k\right)}\right)\right)}^{-1}$

$..{\mathrm{\φ}}^T\left(x^{\left(k\right)}\right){\mathrm{\Σ}}_n^{-1}(\tilde{L}-L\left(x^{\left(k\right)}\right))---\left(15\right)$

$=x^{\left(k\right)}+A^{\left(k\right),-1}\·{\mathrm{\φ}}^T\left(x^{\left(k\right)}\right){\mathrm{\Σ}}_n^{-1}(\tilde{L}-L\left(x^{\left(k\right)}\right))$

When twice iteration result's difference during less than an arbitrarily small positive number, iteration termination obtains final travelling carriage estimated coordinates

The invention has the advantages that:

Feasible zone, two optimization orders of grid search have been dwindled in the employing primary iteration coordinate figure of gauss-newton method obtains stable more, convergence fast, and moderate complexity can obtain higher mobile station positioning accuracy.

Description of drawings

Fig. 1 is the flow chart of concrete grammar of the present invention;

Fig. 2 is the GBSBCM model;

Fig. 3 is the geometrical relationship figure of travelling carriage, base station and scattering object.

Fig. 4 is the cell layout schematic diagram;

Fig. 5 (a) is that the average position error under different scattering object distribution maximum radius situations compares;

Fig. 5 (b) has shown the cumulative distribution function curve of each algorithm.

Embodiment

As shown in Figure 1, be the particular flow sheet of implementation method of the present invention.The improved mixing of the present invention TOA/AOA geometry location method realizes as follows:

(1) parameter measurement

The wireless signal parameter of moving table measuring base station, promptly time of arrival (toa) (Time Of Arrival, TOA) and signal arrive angle (Angle Of Arrival, AOA)

And, can extrapolate the position of travelling carriage in conjunction with the geometry site of travelling carriage and base station.

In NLOS (non-line-of-sight, non line of sight) environment, owing to have obstacle or scattering object, TOA that moving table measuring arrives and AOA parameter value exist than mistake.As shown in Figure 2, (geometrically based single bounce macrocell circular model, GBSBCM) model is simulated the NLOS communication environments with individual reflection circle model.This model has utilized and an actual hypothesis that conforms to: in the macrocellular environment, antenna for base station is higher, and reverberation does not produce reflected signal near the base station.Scattering object is being center, R with the travelling carriage _dBe Gaussian Profile in the circle for radius.

As shown in Figure 3, the geometry site according to travelling carriage, scattering object and base station has:

${\widetilde{\mathrm{\α}}}_i-\arctan \left(\frac{y_i-y_b^i}{x_i-x_b^i}\right)=0---\left(1\right)$

${\tilde{L}}_i-r_b^i-r_m^i={\tilde{L}}_i-\sqrt{{(x_i-x_b^i)}^2+{(y_i-y_b^i)}^2}-\sqrt{{(x_i-x_m)}^2+{(y_i-y_m)}^2}=0---\left(2\right)$

Its neutralization

Represent the propagation distance of base station signal arrival travelling carriage respectively and arrive angle, (x _i, y _i) and (x _m, y _m) represent the position coordinates of base station, scattering object and travelling carriage respectively.The signal propagation distance can be multiplied each other by measurement parameter signal propagation time and light velocity C and obtain:

${\tilde{L}}_i={\widetilde{\mathrm{\τ}}}_i\×C---\left(3\right)$

(2) dwindle feasible zone

Under the NLOS environment, because travelling carriage (x _m, y _m) and each base station

Between distance add that to the distance of travelling carriage scattering object constitutes to the distance the base station by scattering object, promptly

Then according to parameter and

The position of rough estimate travelling carriage:

$\left\{\begin{array}{c}{\hat{x}}_{\mathrm{ml}}^i=x_b^i+{\tilde{L}}_i\cos {\widetilde{\mathrm{\α}}}_i\\ {\hat{y}}_{m1}^i=y_b^i+{\tilde{L}}_i\sin {\widetilde{\mathrm{\α}}}_i\end{array}\right.---\left(4\right)$

$\left\{\begin{array}{c}{\hat{x}}_{m2}^i=x_b^i+({\tilde{L}}_i-R_d^i)\cos {\widetilde{\mathrm{\α}}}_i\\ {\hat{y}}_{m2}^i=y_b^i+({\tilde{L}}_i-R_d^i)\sin {\widetilde{\mathrm{\α}}}_i\end{array}\right.---\left(5\right)$

And in cell layout schematic diagram as shown in Figure 4, the number of base stations that participates in the location is 3, and wherein base station 1 is a serving BS, and travelling carriage is in the gray area of OABC encirclement.The constraints that is proposed according to the GBSBCM model in addition

We obtain the feasible zone of algorithm:

$0\≤x_m\≤(\sqrt{3}/2)R$ (6)

$0\≤y_m\≤\min \{\sqrt{3}{x}_m,-(\sqrt{3}/3)x_m+R\}$

$0\≤r_m^i=|x_i+{\mathrm{jy}}_i-(x_m+{\mathrm{jy}}_m)|\≤R_d^i,i=\mathrm{1,2,3}---\left(7\right)$

Wherein, R is a radius of society,

Be the scattering object distribution radius maximum of each base station correspondence, i=1,2,3.

From the coordinate of above (4), (5), choose maximum, the min coordinates value of satisfied (6), (7), obtain a feasible zone [x who dwindles _Min, x _Max] * [y _Min, y _Max].

(3) grid search is determined the iteration initial coordinate values

In the feasible zone that this dwindles, carry out grid search, choose the grid point that satisfies constraints (7) and constitute candidate point set CPS (Candidate Point Set).

The coordinate figure of all candidate points among the CPS is asked on average the iteration initial value x that is optimized ₀

(4) adopt Gauss-Newton iteration method to estimate the position coordinates of travelling carriage

In theory, each base station signal arrives the propagation distance of travelling carriage and arrives angle and can be expressed as:

L(x)＝[L ₁(x)，L ₂(x)，L ₃(x)，α ₁(x)，α ₂(x)，α ₃(x)] (8)

X=[x wherein _m, y _m], $L_i\left(x\right)=\sqrt{{(x-x_b^i)}^2+{(y-y_b^i)}^2},$ ${\mathrm{\α}}_i\left(x\right)=\arctan \frac{y-y_b^i}{x-x_b^i}.$

And the actual propagation distance of each base station signal with the arrival angle is:

$\tilde{L}=L\left(x\right)+n---\left(9\right)$

Wherein n is that NLOS propagates the error cause and obeys the systematic measurement error that average is zero Gaussian Profile.

Because the existence of error, (1), (2) always can not be met, and obtain target function thus:

$\mathrm{\ϵ}\left(x\right)={(\tilde{L}-L\left(x\right))}^T{\mathrm{\Σ}}_n^{-1}(\tilde{L}-L\left(x\right))---\left(10\right)$

∑ wherein _nCovariance matrix for noise n:

∑ _n＝E{nn ^T} (11)

The coordinate that then satisfies following formula promptly can be used as the location estimation value of travelling carriage:

$\hat{x}=\underset{x}{\arg \min }\mathrm{\ϵ}\left(x\right)---\left(12\right)$

To (9) formula at iteration initial value x ₀The place carries out linearisation:

$L\left(x\right)=L\left(x_0\right)+\mathrm{\φ}\left(x\right){|}_{x=x_0}(x-x_0)---\left(13\right)$

Wherein

$\mathrm{\φ}\left(x\right)={{\▿}_x}^T\⊗L\left(x\right)=\left[\begin{array}{c}(x-x_1)/r_1,\left({y-y}_1\right)/r_1\\ \left({x-x}_2\right)/r_2,\left({y-y}_2\right)/r_2\\ (x-x_3)/r_3,\left({y-y}_3\right)/r_3\\ (y-y_1)/{r_1}^2,(x-x_1)/{r_1}^2\\ \left({y-y}_2\right)/{r_2}^2,\left({x-x}_2\right)/{r_2}^2\\ \left({y-y}_3\right)/{r_3}^2,(x-x_3)/{r_3}^2\end{array}\right],---\left(14\right)$

According to (10), (13) formula, following formula is carried out iterative:

$x^{(k+1)}=x^{\left(k\right)}+{\left({\mathrm{\φ}}^T\left(x^{\left(k\right)}\right){\mathrm{\Σ}}_n^{-1}\mathrm{\φ}\left(x^{\left(k\right)}\right)\right)}^{-1}$

$..{\mathrm{\φ}}^T\left(x^{\left(k\right)}\right){\mathrm{\Σ}}_n^{-1}(\tilde{L}-L\left(x^{\left(k\right)}\right))---\left(15\right)$

$=x^{\left(k\right)}+A^{\left(k\right),-1}\·{\mathrm{\φ}}^T\left(x^{\left(k\right)}\right){\mathrm{\Σ}}_n^{-1}(\tilde{L}-L\left(x^{\left(k\right)}\right))$

When twice iteration result's difference during less than an arbitrarily small positive number, iteration termination obtains final travelling carriage estimated coordinates

Improved method and existing method are carried out Computer Simulation relatively, shown in Fig. 5 (a) and 5 (b).

Fig. 5 (a) is that the average position error under different scattering object distribution maximum radius situations compares, and for serving BS 1, scattering object distribution maximum radius value is fixed as 0.15km.As can be seen, this paper method mean error all is lower than existing localization method, and its positioning accuracy improves.

Fig. 5 (b) has shown the cumulative distribution function curve of each algorithm.For 3 base stations, the scattering object distribution radius is respectively 0.15km, 0.25km and 0.25km around the travelling carriage.Wherein the position error of this paper method is 83.7% less than the probability of 0.1km, and improved traditional TOA/AOA algorithm, traditional TOA/AOA algorithm, LOP algorithm and improved HLOP algorithm in the existing localization method are respectively 73%, 47.1%, 31.3% and 62.7%.Hence one can see that, and the positioning performance that the geometry location that the present invention proposes is improved one's methods is better than above-mentioned existing localization method.

Claims (1)

1. the geometry location under the NLOS environment is improved one's methods, and it is characterized in that, this method comprises parameter measurement and location of mobile station two stages of estimation, and concrete grammar is as follows:

1) the parameter measurement stage:

According to the base station signal parameter that moving table measuring obtains, time of arrival (toa)

Arrive angle with signal

And the geometry site of travelling carriage, scattering object and base station has:

${\widetilde{\mathrm{\α}}}_i-\arctan \left(\frac{y_i-y_b^i}{x_i-x_b^i}\right)=0---\left(1\right)$

${\tilde{L}}_i-r_b^i-r_m^i={\tilde{L}}_i-\sqrt{{(x_i-x_b^i)}^2+{(y_i-y_b^i)}^2}-\sqrt{{(x_i-x_m)}^2+{(y_i-y_m)}^2}=0---\left(2\right)$

Its neutralization

Represent the base station signal propagation distance respectively and arrive angle, (x _i, y _i) and (x _m, y _m) represent the position coordinates of base station, scattering object and travelling carriage respectively, the signal propagation distance can be multiplied each other by measurement parameter signal propagation time and light velocity C and obtain:

${\tilde{L}}_i={\widetilde{\mathrm{\τ}}}_i\×C---\left(3\right)$

2) location of mobile station estimating stage: according to the base station signal parameter and

Adopt Gauss's Newton iteration method and optimize the iteration initial coordinate values, resolve the position that obtains travelling carriage, specifically may further comprise the steps in conjunction with the grid search method:

(1) dwindles feasible zone

Under the NLOS environment, because travelling carriage (x _m, y _m) and each base station Between distance add that to the distance of travelling carriage scattering object constitutes to the distance the base station by scattering object, promptly

Then according to parameter and

The position of rough estimate travelling carriage:

$\left\{\begin{array}{c}{\hat{x}}_{\mathrm{ml}}^i=x_b^i+{\tilde{L}}_i\cos {\widetilde{\mathrm{\α}}}_i\\ {\hat{y}}_{m1}^i=y_b^i+{\tilde{L}}_i\sin {\widetilde{\mathrm{\α}}}_i\end{array}\right.---\left(4\right)$

$\left\{\begin{array}{c}{\hat{x}}_{m2}^i=x_b^i+({\tilde{L}}_i-R_d^i)\cos {\widetilde{\mathrm{\α}}}_i\\ {\hat{y}}_{m2}^i=y_b^i+({\tilde{L}}_i-R_d^i)\sin {\widetilde{\mathrm{\α}}}_i\end{array}\right.---\left(5\right)$

The constraints that is proposed according to the GBSBCM model

Obtain the feasible zone of algorithm:

$0\≤x_m\≤(\sqrt{3}/2)R$ (6)

$0\≤y_m\≤\min \{\sqrt{3}{x}_m,-(\sqrt{3}/3)x_m+R\}$

$0\≤r_m^i=|x_i+{\mathrm{jy}}_i-(x_m+{\mathrm{jy}}_m)|\≤R_d^i,i=\mathrm{1,2,3}---\left(7\right)$

Wherein, R is a radius of society,

Be the maximum of the scattering object distribution radius of each base station correspondence, i=1,2,3;

From the coordinate of above (4), (5), choose maximum, the min coordinates value of satisfied (6), (7), obtain a feasible zone [x who dwindles _Min, x _Max] * [y _Min, y _Max];

(2) grid search is determined the iteration initial coordinate values

In the feasible zone that this dwindles, carry out grid search, choose the grid point that satisfies constraints (7) and constitute candidate's point set CPS;

The coordinate figure of all candidate points among the CPS is asked on average the iteration initial value x that is optimized ₀

(3) adopt Gauss-Newton iteration method to estimate the position coordinates of travelling carriage

In theory, each base station signal arrives the propagation distance of travelling carriage and arrives angle and can be expressed as:

L(x)＝[L ₁(x)，L ₂(x)，L ₃(x)，α ₁(x)，α ₂(x)，α ₃(x)] (8)

X=[x wherein _m, y _m], $L_i\left(x\right)=\sqrt{{(x-x_b^i)}^2+{(y-y_b^i)}^2},$ ${\mathrm{\α}}_i\left(x\right)=\arctan \frac{y-y_b^i}{x-x_b^i}$

And the actual propagation distance of each base station signal with the arrival angle is:

$\tilde{L}=L\left(x\right)+n---\left(9\right)$

Wherein n is that NLOS propagates the error cause and obeys the systematic measurement error that average is zero Gaussian Profile;

Because the existence of error, (1), (2) always can not be met, and obtain target function thus:

$\mathrm{\ϵ}\left(x\right)={(\tilde{L}-L\left(x\right))}^T{\mathrm{\Σ}}_n^{-1}(\tilde{L}-L\left(x\right))---\left(10\right)$

∑ wherein _nCovariance matrix for noise n:

∑ _n=E{nn ^T} (11)

The coordinate that then satisfies following formula promptly can be used as the location estimation value of travelling carriage:

$\hat{x}=\underset{x}{\arg \min }\mathrm{\ϵ}\left(x\right)---\left(12\right)$

To (9) formula at iteration initial value x ₀The place carries out linearisation:

$L\left(x\right)=L\left(x_0\right)+\mathrm{\φ}\left(x\right){|}_{x=x_0}(x-x_0)---\left(13\right)$

Wherein

$\mathrm{\φ}\left(x\right)={{\▿}_x}^T\⊗L\left(x\right)=\left[\begin{array}{c}(x-x_1)/r_1,\left({y-y}_1\right)/r_1\\ \left({x-x}_2\right)/r_2,\left({y-y}_2\right)/r_2\\ (x-x_3)/r_3,\left({y-y}_3\right)/r_3\\ (y-y_1)/{r_1}^2,(x-x_1)/{r_1}^2\\ \left({y-y}_2\right)/{r_2}^2,\left({x-x}_2\right)/{r_2}^2\\ \left({y-y}_3\right)/{r_3}^2,(x-x_3)/{r_3}^2\end{array}\right],---\left(14\right)$

According to (10), (13) formula, following formula is carried out iterative:

$x^{(k+1)}=x^{\left(k\right)}+{\left({\mathrm{\φ}}^T\left(x^{\left(k\right)}\right){\mathrm{\Σ}}_n^{-1}\mathrm{\φ}\left(x^{\left(k\right)}\right)\right)}^{-1}$

$..{\mathrm{\φ}}^T\left(x^{\left(k\right)}\right){\mathrm{\Σ}}_n^{-1}(\tilde{L}-L\left(x^{\left(k\right)}\right))---\left(15\right)$

$=x^{\left(k\right)}+A^{\left(k\right),-1}\·{\mathrm{\φ}}^T\left(x^{\left(k\right)}\right){\mathrm{\Σ}}_n^{-1}(\tilde{L}-L\left(x^{\left(k\right)}\right))$

When twice iteration result's difference during less than an arbitrarily small positive number, iteration termination obtains final travelling carriage estimated coordinates

Images