Introduction

Several indoor localization systems use angle of arrival (AOA) or angle difference of arrival (ADOA) measurements, using light [1], [2], [3], [4], acoustic [5], or radio [6], [7] sources. In these systems the beacons transmit signals, which are detected by the sensors; and the AOA values of the beacons, or ADOA values of beacon pairs, are measured. These angle measurements are used to estimate either the unknown location of the sensor (using multiple beacons, deployed at known positions), or the unknown location of the beacon (using multiple sensors, deployed at known positions).

The angles can be measured in 3-D, where both the azimuth and elevation of the beacon are measured [4], or in 2-D, using only the azimuth values [3]. The azimuth-only estimation requires much less computation [8], and is sufficient for applications where only the 2-D location is to be determined and the elevation is irrelevant (e.g. when a fork-lift truck is tracked, see Fig. 1). The sensor's direction, however, must be known or measured in the azimuth-only method; the simplest solution being when the sensor is facing upwards [3].

The estimation of the sensor position can be made by solving an equation system [9]; or in case of redundant measurements, least squares methods [4], consensus-based approaches [3], or exhaustive search methods were proposed [4]. The various estimation schemes provide good estimates when enough measurements are available, and the measurements have reasonably small error. Measurements with large error (i.e. outliers), usually resulting from incorrect detections or reflections, may cause large error in the position estimate. Such outliers must be removed from the data used in the estimation process, to avoid incorrect estimates. Popular methods include model-based approaches (using e.g. Kalman-filters) [10], statistical methods [11], consistency-based approaches [3], and Random Sample Consensus (RANSAC) [12], [13]. RANSAC is popular because it iteratively adds inliers to and excludes outliers from a small starting set of measurements, thus provides robust estimates with reasonable amount of computation.

In this paper we propose a robust RANSAC-based solution for azimuth-only AOA/ADOA systems. The proposed solution requires angle measurements of at least 3 beacons. With minimal number of beacons an algebraic solution is provided, which is naturally sensitive to outliers. When redundant (i.e. four or more) measurements are present, RANSAC is used to select the larges possible consistent beacon set to calculate a robust position estimate. A practical error model for cameras is proposed to determine the position-dependent inlier angle tolerance. The performance of the proposed solution is illustrated by simulation examples and practical measurements as well.

Proposed Localization Method

2.1.

System architecture

In the proposed system the position of a camera is estimated; the camera may be deployed e.g. on top of a fork-lift truck, as shown in Fig 1 . The camera, equipped with a fisheye lens, is facing upwards, thus the sensor's normal vector is in line with the coordinate system's Z axis. The beacons (e.g. blinking LEDs) are deployed at known positions (e.g. along the walls). The beacons continuously transmit their unique IDs, using visible light communication techniques (in the test system the RUPSOOK protocol was used [14]). The beacons are detected on the camera's image stream.

Figure 1:

The architecture of the localization system. Blinking beacons are detected on the image of the fish-eye camera. The camera location is determined using the known beacon positions. The world coordinate system is 𝐾 with axes 𝑋 , 𝑌 , 𝑍 , while the camera coordinate system is 𝐾 with axes 𝑋 , 𝑌 , 𝑍 , where 𝑍 and 𝑍 are parallel. The origin of 𝐾 is the unknown camera position 𝐶 * = (𝑥 , 𝑦 , 𝑧 ), and its orientation is the unknown angle 𝜑 , as shown in Fig. 2. Since 𝑍 is parallel with 𝑍 (the camera is facing upwards), the 2-D beacon directions can be used, as shown in Fig. 2, which greatly simplifies the computation need [15]. Notice that we assumed that the axis of the camera is parallel with the Z axis. It may easily be satisfied in certain applications (see e.g. Fig. 1), or the sensor inclination can be measured and compensated for, e.g. [3].

In coordinate system 𝐾 let us denote the known coordinates of beacon 𝐵 by 𝑃 * = (𝑥 , 𝑦 , 𝑧 ), and the orthogonal projections of 𝑃 * and 𝐶 * to plane X-Y by 𝑃 = (𝑥 , 𝑦 ) and 𝐶 = (𝑥 , 𝑦 ), respectively. The measured direction of beacon 𝐵 on the X-Y plane in coordinate system 𝐾 is denoted by 𝛼 , as shown in Fig. 2. Thus the inputs of the estimator process are measurements 𝛼 , and beacon positions 𝑃 , 𝑖 = 1, 2, … , 𝑁 , where 𝑁 is the number of beacons, while the output is position 𝐶 and orientation 𝜑 (in world coordinate system 𝐾 ).

In real scenarios bad measurements are not uncommon (e.g. due to reflections), resulting in outlier measurement values. Also, beacons may be unintentionally relocated, thus certain values 𝑃 may be wrong and the measured angles are inconsistent with them, practically behaving like outliers. To achieve a fault tolerant system, it is essential that the estimation process be resilient to outliers in the set of input values. In the proposed solution RANSAC is used to provide robust estimates.

Error Model

The beacons are detected on the camera's image stream. On the left-hand side of Fig. 3 a bird's-eye view of the area is shown, with the camera 𝐶, beacons 𝐵 , 𝐵 , 𝐵 , 𝐵 , and an obstacle partly covering beacon 𝐵 . On the right-hand side the fish-eye camera image is shown, with two possible sources of detection error.

First, the center estimate of a beacon image may be inaccurate, as illustrated on 𝐵 . This center detection error Δ𝐶 is small (around one pixel), and if all beacons are approximately on the horizon, the angle error is independent of the distance between the beacon and the camera. We model this error as a random additive error 𝛼 ( ) with uniform distribution between −Δ𝛼 and +Δ𝛼 , where Δ𝛼 is constant. If the radius of the horizon on the camera image is 𝑅 pixels, then the value of Δ𝛼 can be approximated as follows:

Δ𝛼 = atan2 Δ𝐶 , 𝑅 . (1)

where atan2 is the two-argument arctangent function.

The second type of detection error may happen when a beacon is partially obscured (see 𝐵 in Fig. 3). In this case the camera image contains a misshapen beacon, the center of which may have an offset with regard to the true center. The error is the largest when almost the whole beacon is covered; in this case the center of the observed beacon may appear away from the true position, where 𝑊 is the horizontal size of the beacon. The resulting angle error may be higher if the beacon is closer to the camera (i.e. the beacon image is larger). We model this angle error as a random additive error 𝛼 ( ) , with uniform distribution between −Δ𝛼 , and +Δ𝛼 , , where Δ𝛼 , is the maximal angle error as follows:

Δ𝛼 , = atan2 𝑊, 𝐷 ,(2)

where 𝑊 is the horizontal size of beacon 𝐵 , and 𝐷 is the distance between 𝐵 and the camera.

The measured angle of beacon 𝐵 is modelled as follows:

𝛼 = 𝛼 ( ) + 𝛼 ( ) + 𝛼 ( ) ,(3)

Overview of the RANSAC-based localization process

In this section we briefly summarize the operation of the proposed localization algorithm. The technical details will be discussed later; the summary contains references to sections with the detailed discussion. The pseudo-code of the proposed algorithm is the following:

Repeat Steps 1-4 𝑁 times. The number 𝑁 of iterations is discussed in Section 2.4.

Step 1: Select 3 beacons randomly. Let the selected beacon triplet be 𝐵𝑇 .

Step 2: Calculate initial position estimate 𝐶 and orientation estimate 𝜑 using 𝐵𝑇 . The estimates are calculated from the positions of beacons 𝐵𝑇 and the measured angles of these beacons. The details of the estimation are presented in Section 2.5.

Step 3: For all the beacons, check whether their measurements are consistent with 𝐶 and 𝜑 .

The set of supporter beacons of 𝐵𝑇 (i.e. the inliers) is denoted by 𝑆𝑈𝑃 . The proposed inlier-outlier classification method uses a position-dependent threshold; thus the effect of the measurement errors will be location-independent. The method is discussed in Section 2.6.

Step 4: Calculate the number of supporter beacons 𝑁𝑆 = 𝑆𝑈𝑃 and their mean squared angle measurement error 𝐸 , as described in Section 2.6.

End Repeat

Step 5: From the 𝑁 experiments select the triplet with the highest value 𝑁𝑆 . If multiple triplets exist with the same 𝑁𝑆 , then select the one with the smallest value 𝐸 .

Step 6: Using all of the supporter beacons in 𝑆𝑈𝑃 , calculate refined estimates 𝐶 and 𝜑 , starting from 𝐶 , as described in Section 2.7.

Number of iterations

The estimation process uses random beacon triplets to find an initial position estimate. In order the estimator algorithm to provide a good estimate the measurement must fulfill the following two conditions: a) All three initial beacons must be detected correctly, i.e. with small measurement error. If some of the initial measurements are outliers, the initial estimate will be wrong. b) According to the phenomenon called geometrical dilution of precision (GDOP), some setups are more sensitive to measurement errors than others [16]. Thus the geometry of the initial beacons and the camera must allow a good estimate when the measurements are correct (i.e. the measurement error is small, according to condition (a)). In order the estimator algorithm to provide reliable estimate, we need at least one good initial triplet, fulfilling both requirements, among the 𝑁 experiments. Since beacon triplets 𝐵𝑇 are chosen randomly, it is possible to determine the probability 𝑃𝑟 of having at least one good beacon triplet among the randomly chosen 𝑁 triplets. On the reverse direction, we can use this design parameter 𝑃𝑟 to determine the necessary number of iterations 𝑁 [17].

Let us denote the number of beacons and the number of total experiments by 𝑁 and 𝑁 , respectively. Let us suppose that out of 𝑁 measurements 𝑁 are inliers. The probability of choosing a reliable triplet is

𝑃𝑟 𝐵𝑇 is reliable = 𝑃 * ,(4)

where 𝑃 * is the conditional probability of providing a good estimate if all of the initial measurements are inliers [17].

The probability 𝑃𝑟 of having at least one reliable triplet out of 𝑁 trials can be expressed as follows:

𝑃𝑟 = 1 − 1 − 𝑃 * (5)

Thus the number of necessary trials, given probability 𝑃𝑟 and 𝑃 * , is the following:

𝑁 = ( ) *(6)

In practice the number of inlier/outlier measurements is usually unknown. However, it is usually possible to give a safe upper bound for the outliers (e.g. if there are rarely more than one outliers then a safe assumption is that 𝑁 ≥ 𝑁 − 2, and an even safer assumption is that 𝑁 ≥ 𝑁 − 3). If the number of inliers is at least 𝑁 then the calculated 𝑁 , according to (6), provides an upper bound for the required number of trials.

The probability 𝑃𝑟 is a design parameter, set close to 1 (e.g. 0.999). Probability 𝑃 * can be estimated using simulations, using the inlier angle tolerance value, as input parameter [17].

For small number of beacons, it is feasible to use all the possible beacon triplets. Even for 15 visible beacons, the number of trials is only 455 (see the speed measurements in Section 3.2). However, for higher number of beacons (6) may really be helpful: e.g. for 50 visible beacons 19600 trials would be necessary, which is not feasible for real-time applications. In the tests, we had small number of beacons and thus used all possible triplets.

Calculation of the initial estimate

For sake of simplicity, and without loss of generality, in iteration step 𝑗 let us denote the selected beacons by 𝐵𝑇 = {𝐵 , 𝐵 , 𝐵 }. Our goal is to determine camera position 𝐶 and orientation 𝜑 . For simpler notation for now we will omit iteration index 𝑗 and denote the estimates by 𝐶 and 𝜑 . Let us use the following notations: 𝑏 = 𝑃 − 𝐶, (7) 𝜃 , = ∠𝑃 𝐶𝑃 = 𝛼 − 𝛼 (8) where 𝑖, 𝑗 = 1,2,3. Let us choose two beacons (e.g. 𝐵 and 𝐵 ), and suppose that 𝜃 , ≠ 0 and 𝜃 , ≠ 𝜋.

Let us express the square of their distance 𝑑 , , using the law of cosines, as follows:

𝑑 , = 𝑏 + 𝑏 − 2 𝑏 𝑏 cos 𝜃 ,(9)

Using the fact that 𝑏 and 𝑏 are on the same plane X-Y, the equality 𝑏 × 𝑏 = 𝑛 𝑏 𝑏 sin 𝜃 , , holds, where 𝑛 is the unity vector in direction of axis Z. In this case (9) takes the following form:

𝑑 , = 𝑏 + 𝑏 − 2 ( ) ( ) , ,(10)

Substituting 𝑑 , = (𝑥 − 𝑥 ) + (𝑦 − 𝑦 ) and 𝑏 = (𝑥 − 𝑥 ) + (𝑦 − 𝑦 ) , and using notation 𝑐 , = cot 𝜃 , , we get the following equation:

𝑥 𝑥 + 𝑦 𝑦 + 𝑥 𝑦 − 𝑥 𝑦 𝑐 , = 𝑥 𝑥 + 𝑥 + 𝑦 − 𝑦 𝑐 , + 𝑦 𝑦 + 𝑦 + 𝑥 − 𝑥 𝑐 , − 𝑥 − 𝑦 (11)

Similar result is obtained if beacon pair 𝐵 and 𝐵 is used:

𝑥 𝑥 + 𝑦 𝑦 + 𝑥 𝑦 − 𝑥 𝑦 𝑐 , = 𝑥 𝑥 + 𝑥 + 𝑦 − 𝑦 𝑐 , + 𝑦 𝑦 + 𝑦 + 𝑥 − 𝑥 𝑐 , − 𝑥 − 𝑦(12)

By subtracting ( 12) from ( 11) the squared terms vanish and we get the following linear equation: −𝑥 𝜉 , − 𝑦 𝜐 , − ω , 𝑐 , + ω , 𝑐 , = 𝑥 −𝜉 , + 𝜐 , 𝑐 , − 𝜐 , 𝑐 , + 𝑦 −𝜐 , − 𝜉 , 𝑐 , + 𝜉 , 𝑐 , (13) where 𝜉 , = 𝑥 − 𝑥 , 𝜐 , = 𝑦 − 𝑦 , and ω , = 𝑥 𝑦 − 𝑥 𝑦 Now let us calculate (13), by substituting (𝑖, 𝑗, 𝑘) = (1,2,3), (2,3,1), and (3,1,2). We get the following over/determined linear equation system:

𝐴 ⋅ 𝑥 𝑦 = 𝑏(14)

where

𝐴 = −𝜉 , +

If none two of the three beacons are on the same line with C (i.e. sin 𝜃 , ≠ 0, sin 𝜃 , ≠ 0, and sin 𝜃 , ≠ 0) then 𝑐 , , 𝑐 , , and 𝑐 , are finite and any two lines can be selected from 𝐴 and 𝑏 to construct 𝐴 and 𝑏 , resulting an equation system with two equations and two unknowns. The initial location estimate is the following:

𝐶 = 𝑥 𝑦 = (𝐴 ) 𝑏 . (17)

If one pair of beacons is on the same line with C (e.g. 𝐵 and 𝐵 ), then sin 𝜃 , = 0. In this case from 𝐴 and 𝑏 one row is selected only, the one which does not include the infinite term 𝑐 , . (E.g. if 𝐵 , 𝐵 , and C are on the same line then 𝑐 , would be infinite, thus only the second row is used from 𝐴 and 𝑏). In this case another equation must be constructed for 𝐴 and 𝑏 ; from (13) using limit sin 𝜃 , → 0 the following equation can be obtained:

𝑥 𝜐 , − 𝑦 𝜉 , = −ω , ,(18)

and again (17) can be used to calculate the position estimate. If all three beacons and C are on the same line (i.e. sin 𝜃 , = sin 𝜃 , = sin 𝜃 , = 0) then the position cannot be determined from this set of measurements. Also, there is no solution if all three beacons and C are on the same circle (det(𝐴 ) ≅ 0).

In practice the angle differences 𝜃 , are checked before calculating 𝐴 and 𝑏: if any of them is closer to 0 or 𝜋 than a small threshold then (18) is applied. If all measurements are close to 0 or 𝜋 then the triplet is ignored (since the position cannot be reliable determined), and the iteration is continued by selecting a different beacon triplet.

Once the initial position is estimated, the initial orientation estimate can be calculated as follows: Let us choose any of the three beacons, e.g. 𝐵 . From point 𝐶 beacon 𝐵 is observed in 𝐾 under angle 𝛽 = atan2(𝑦 − 𝑦 , 𝑥 − 𝑥 ).

(19) The observed angle in 𝐾 was 𝛼 , thus the estimated orientation, according to Fig. 2, is the following:

𝜑 = 𝛽 − 𝛼 .(20)

Location-dependent inlier-outlier classification

In the previous step the initial position 𝐶 = (𝑥 , 𝑦 ), and the initial orientation 𝜑 were estimated. If the initial estimates are correct then any beacon 𝐵 should be seen from ideal direction 𝛽 , in the world coordinate system: 𝛽 = atan2(𝑦 − 𝑦 , 𝑥 − 𝑥 ), (25) Using ( 23) and ( 25), the location-dependent inlier/outlier classification is the following:

𝐵 ∈ 𝑆𝑈𝑃 (inlier): if |𝜀 | ≤ Δ𝛼 𝐵 ∉ 𝑆𝑈𝑃 (outlier): if |𝜀 | > Δ𝛼 , (26)

where 𝑆𝑈𝑃 is the set of supporter beacons for triplet 𝐵𝑇 . Notice that beacons in 𝐵𝑇 are naturally elements of 𝑆𝑈𝑃 . The mean square angle measurement error 𝐸 [4], using the consistent measurements only, is the following:

𝐸 = ∑ 𝜀 ∈ . (27)

Refined location estimate

Let us construct error function 𝐸(𝑥, 𝑦) as follows: if the camera position were in (𝑥, 𝑦) then beacon 𝐵 would be observed in direction 𝛽 , in coordinate system 𝐾 : 𝛽 (𝑥, 𝑦) = atan2(𝑦 − 𝑦, 𝑥 − 𝑥), (28) From measurements 𝛼 of the consistent beacons, the orientation estimate is refined as follows:

𝜑 (𝑥, 𝑦) = ∑ (𝛽 (𝑥, 𝑦) − 𝛼 ) ∈ . (29)

Thus the observation error of beacon 𝐵 , provided the camera position is (𝑥, 𝑦), is 𝜀 (𝑥, 𝑦) = 𝛼 − 𝑎𝑡𝑎𝑛2(𝑦 − 𝑦, 𝑥 − 𝑥) + 𝜑 (𝑥, 𝑦), (30) and the mean square error function [4], similarly to (27), is the following:

𝐸(𝑥, 𝑦) = ∑ 𝜀 ∈ (𝑥, 𝑦). (31)

The refined position estimate 𝐶 is at the minimum of 𝐸(𝑥, 𝑦):

𝐶 = (𝑥 , 𝑦 ) = argmin ( , ) 𝐸(𝑥, 𝑦),(32)

and the refined orientation estimate 𝜑 is the following:

𝜑 = 𝜑 (𝑥 , 𝑦 ). (33)

The location estimate is calculated by a gradient search on the error function 𝐸(𝑥, 𝑦), starting from initial position estimate (𝑥 , 𝑦 ). The search finds a (possibly local) minimum (𝑥 , 𝑦 ) near the initial estimate. It is not guaranteed that the global minimum of (31) is found, but since the selected measurements of beacons in 𝑆𝑈𝑃 are consistent and the search is started from a consistent initial estimate (𝑥 , 𝑦 ), the iterative search in practice quickly and accurately finds the correct estimate, usually close to the initial value (𝑥 , 𝑦 ).

Simulation results

Test environment

The tests were performed in the simulated environment, shown in Fig. 4. The size of the simulated room was 22m × 22m, where 12 beacons were deployed in the positions shown in Fig. 4. Performance tests were made on a test grid containing 23x23 points, the grid coordinates being integer numbers in meters, between −11m and +11m.

The measurements were generated as follows. In each test point the ideal angles were calculated, then a random error between −0.4° and +0.4° was added to simulate the center detection error, which is equivalent to Δ𝛼 = 0.4°. This value corresponds to the case when the distance between the beacon's image and the center of the camera is ~150 pixels and the center detection error is Δ𝐶 = 1 pixel. Also, a potential coverage error was simulated with beacon width value 𝑊 = 3𝑐𝑚, according to (2). In each test points 20 independent noise realizations (i.e. measurement errors 𝛼 ( ) + 𝛼 ( ) , according to (3)), were generated. The outliers were generated as follows: When one outlier was present, a random beacon was selected and a random angle measurement error in the range of ±[10°, 170°] was added. When 𝐾 ≥ 2 outliers were present then 𝐾 beacons were randomly selected and their generated measurements were shuffled. This error simulates multiple incorrect beacon identifications and thus resulting false measurements.

The proposed method was compared with the method of exhaustion (MEX), proposed in [4], applied for the 2D case. The MEX method utilizes error function 𝐸 , which is similar to the error function of the proposed method:

𝐸 (𝑥, 𝑦) = ∑ 𝜀 (𝑥, 𝑦). (34)

MEX uses an exhaustive (brute-force) search on a grid and is guaranteed to find the optimal position on the search grid. In the tests we used a search grid of size 1cm for MEX to find the smallest error function value. On the other hand, the proposed algorithm is not restricted to a search grid, but finds the solution anywhere on the search plane, without any guarantee for global optimum. Since the error functions are similar (see ( 31) and (34)), the expected behavior of the two algorithms is the same, when there are no outliers.

Since the number of beacons is low (𝑁 = 12), in the tests all possible beacon triplets were used, i.e. 𝑁 = 𝑁 3 = 220.

Simulation results

In the first test measurement noise was added for the ideal angle measurements, but no outliers were present. Fig. 4 shows the position errors for the proposed and the reference methods. The performances of the algorithms are very similar, as it was expected. The error is small, mainly below 10 cm. At certain points of the search space slightly higher errors can be observed: this is because of the poor GDOP; here 8 of the beacons (𝐵 , 𝐵 , 𝐵 , 𝐵 , 𝐵 , 𝐵 , 𝐵 , 𝐵 ) are on the same circle, while the other 4 beacons are also close to this circle. To allow detailed analysis of the errors, the error distributions are shown in Fig. 5, using all of the experiments (23x23x20=10580 test runs). The behavior of the two algorithms is apparently quite similar, according to the expectations. The average error was 2.65cm and 2.67cm for the proposed and reference methods, respectively. The slight difference is probably due to the fact that the reference method was evaluated on a 1cm grid, while the proposed method provides results on the continuous plane.

Density

The computational costs between the proposed and the reference method were compared. Both methods were implemented in Matlab and run on a conventional laptop with Intel Core i7-9750H processor. The proposed method (with 12 beacons, using all possible beacon triplets) required 56 microseconds in average to compute an estimate, while for the reference MEX method the average runtime was 278 milliseconds.

Measurement results

The test measurements were made inside of an industrial warehouse and in front of the warehouse. The map of the test area is shown in Fig. 8 (a). On the right-hand side a part of the warehouse is visible, where nine beacons were deployed along the walls and on the shelves. There were another six beacons deployed outside the building on the walls and poles. The camera was deployed on top of the vehicle, as shown in Fig. 8 (b). An estimated path of the vehicle is shown in Fig. 8 During the measurements the number of visible beacons varied from 3 to 13. Apart from the bumpy pavement, where the camera swayed significantly on the top of the vehicle, the trajectory estimate is smooth and contains variation of a few centimeters only. During the test, the algorithm detected outliers (due to reflecting surfaces in the warehouse) at three different path segments, in total of ~550 image frames, as marked in Fig. 8(a). The outliers had no significant effect on the position estimates which clearly illustrates the fault-tolerance of the proposed system.

Summary

A robust RANSAC-based solution, for 2-D position estimation using azimuth-only angle measurements, was proposed. The proposed method uses a practical error model and a corresponding position-dependent inlier angle tolerance to detect outliers. The initial estimate is calculated by a linearized equation system, using measurements from 3 beacons. The number of supporter beacons is increased incrementally using RANSAC. The best initial estimate (with the largest set of supporters and the smallest error function value) is used as an initial point for a gradient search to find the final position estimate; in the search those measurements are used which are consistent with the winner initial estimate. The performance of the proposed method was illustrated by simulation examples. It was shown that the proposed method and the reference MEX method has essentially the same accuracy when no outliers are present. This is due to the similar cost functions used by the two methods. However, in the presence of outliers, the reference method provided no usable results, since it used the bad measurements as well for the estimation, generating large errors. The proposed method was not sensitive to outliers: its performance degradation was minor, even when as much as 5 simultaneous outliers were present.

The computational cost of the proposed method is low: in the realistic test cases the computational time of one position estimate was as low as 56 microseconds, on a conventional laptop. The speed of the reference method, due to its exhaustive (brute-force) nature, was more than three orders of magnitude lower.

The performance of the proposed method was tested using real measurements as well, in an industrial warehouse. During the tests, the outliers had no effect on the position estimates, allowing positioning with a few centimeters of error only.