Algorithm:

SimpleCorrection( )

Input:	$\tilde{P}{(i)}_{i=1}^n$, the serial measured positions;
	lmax the radium of the largest moving area in a sampling period;
	lrx the width of main position area;
Output:	$\widehat{P}{(i)}_{i=1}^n$ the serial estimate positions
1. work out i* = argi min{||P̃(i) − P(i)||, i = 1, ⋯, n}
2. for each i ∈ (1,2, ċ, i* − 1)
3. θ = ∠(P̃(i* −i), P̂(i* −i+1))
4. l = ||(P̃(i* −i) − P̂(i* −i+1))||
5. if lmax<=lrx/2 // there is no intersection point
6.	{if l<=lmax
7.	Δx=lcos(θ); Δy=lsin(θ)
8.	else if l>lmax
9.	Δx=lmaxcos(θ); Δy=lmaxsin(θ)
10.	end if
11.	P̂(i* −i) = P̂(i* −i+1) + [Δx, Δy]T
12.	}
13. if lmax>lrx/2 // there is an intersection point
14.	{if $\overrightarrow{(\tilde{P}(i^{*}-i),\widehat{P}(i^{*}-i+1))}$ and the auxiliary positioning area intersect at P0(i*−i+1)
15.	l = ||(P̃(i* −i) − P0(i* − i+1))||
16.	goto (line7.)
17.	else
18.	goto (line7.)
19.	}
20. for each i ∈ (i* + 1, i* + 2, ċ, n)
21. θ = ∠(P̃(i), P̂(i − 1))
22. l = ||(P̃(i) − P̂(i − 1))||
23. goto (line5.)