A Novel Simultaneous Calibration and Localization Algorithm Framework for Indoor Scenarios

Under the navigational process of simultaneous beacon-calibrating and target-positioning, the real-time positioning and calibrating accuracy in indoor scenarios is significantly limited. To solve it, a novel Simultaneous Calibration and Localization (SCAL) algorithm framework for indoor scenarios is proposed. The proposed framework is mainly divided into the Target Localization and Beacon Calibration (TLBC) and the Global Optimization (GO) section: in the TLBC section, under the typical distributed beacon layout, a processing mechanism for synchronously locating the target and calibrating beacons is proposed; in GO section, parallel to TLBC section, a global optimization method based on Geometric Dilution of Precision based on Error-propagation (EGDOP) model is further proposed, and it can utilize the geometric trajectory of target and positioning error of target and beacons to efficiently and simultaneously optimize the positioning of target trajectory and beacons from TLBC section. The numerical simulation results show that the improvement of the GO section on the positioning performance is verified. Compared with the SCAL algorithm based on backward processing with inverse trajectory, the proposed framework has improved the accuracy of target and beacon positioning by 39.06% and 58.01%, respectively, and accuracy of positioning reaches 0.2557m and 0.2437m, respectively, in an actual indoor positioning scene.


I. INTRODUCTION
Nowadays, with the coming of the Internet of Everything era people's demand for high-precision indoor position increases significantly [1], [4]. Due to the high reliability, precision, portability and low cost of positioning technologies based on wireless signals (e.g. Wi-Fi [5], Ultra-Wide Band (UWB) [6], Bluetooth [7], etc.), it is widely used in indoor.
High-precision of positioning technology based on wireless signals is premised on the accurate calibration of beacons. Therefore, in high-precision positioning research based on wireless signals, accurate calibration of beacons is also an important research hotspot. At present, there are two main ways to obtain the position of beacon: offline and online calibration. The offline calibration method must obtain the position of the beacon using specific instruments such as Total The associate editor coordinating the review of this manuscript and approving it for publication was Giambattista Gruosso .
Station [8] before the positioning system runs. In general, the offline calibration method has high accuracy in estimating the position of the beacon, but generally requires considerable time and acquisition cost, which makes it not suitable for large positioning scenarios with a large number of beacons. Additionally, the position acquisition of a beacon needs to be performed in advance, so it is difficult to quickly configure the positioning system in an unfamiliar environment.
The online calibration method mainly includes the distributed positioning method and Simultaneous Calibration and Localization (SCAL) method. In the distributed positioning method each node in the distributed network realizes its localization by interacting with the observation information of neighboring nodes In [9] a distributed positioning algorithm based on particle filtering and a bi-directional communication network for UWB nodes was proposed, which can reach 1m-level positioning accuracy with several targets' interaction. The distributed positioning method can locate each dynamical node in indoor. However In the typical indoor scenarios with fixed beacons, highly complex calculations and negligence to the stationed trait of beacons limit the real-time positioning performance of the distributed positioning method. Besides, with the premise of several nodes' interaction, there are other engineering obstacles, such as rigid time synchronization, applicable information exchange protocol among nodes etc. Therefore, the distributed positioning method is not suitable for simultaneous positioning and calibration in a typical indoor environment with a large number of fixed beacons.
The SCAL method draws on traditional Simultaneous Localization and Mapping (SLAM) [10], [11] based on indoor visual positioning [12] or laser ranging [13]. In other words, the SCAL refers that, while positioning the target, the map of beacons is synchronously online calibrated to simultaneously obtain the localization of the target and beacons. In [14] a semi-automatic beacon calibration online procedure using the target's direction and Bluetooth RSSI is proposed, where the positioning accuracy of the target can reach 1.5m under linear motion. In [15] a method of SCAL based on multiple Ultrasonic Local Positioning Systems (ULPSs) is proposed, which calibrates and coincides coordinate systems of different sub-beacon groups by moving target trajectory to simultaneously perform calibration of the entire beacon system and positioning of a target, and global optimization of backward processing on inverse trajectory only perform well on the calibration of the final part of beacon system.
Aimed at the limitation of the positioning accuracy of realtime simultaneous target localization and beacon calibration in indoor, a novel SCAL algorithm framework is proposed to improve the precision of beacons and target and decline computational complex of SCAL The proposed algorithm is divided into the Target Localization and Beacon Calibration (TLBC) section and Global Optimization (GO) section, and the main contributions are as follows: 1) In the TLBC section, a processing mechanism to initially locating the target and beacon simultaneously is proposed, where synchronous positioning of target and beacons with unknown locations is performed. 2) In the GO Section, we proposed a Geometric Dilution of Precision based on Error-propagation (EGDOP) model to demonstrate the mechanism of positioning error propagation between target and beacons, and then a global optimization algorithm based on EGDOP is further performed on target positioning and beacon calibrating The paper is organized as follows: Section II gives the system model of this article. Section III demonstrates an overview of the proposed algorithm framework. The TLBC section and GO section are illustrated in Section IV and Section V. Then simulations in Section VI and experiments in Section VII are designed to evaluate the performance of the proposed SCAL algorithm framework. Section VIII gives the conclusions of this article.

II. SYSTEM MODEL
To ensure the applicability of the following discussion, the basic principle of beacon layout is: when the target with a signal receptor is moving, not less than 4 beacons are observed, which includes not less than 3 observable beacons with initially globallyknown positions at the start and end of target trajectory For the convenience of the following discussion, one of the suitable deployment of beacons is selected for the following discussion: there is an indoor scenario that has the entrance and exit; there are 6 Beacons with Globally-known Position (GP Beacon) with 3 of them placed at entrance and exit, respectively; there are several Beacons with Unknown Position (UP Beacon); target moves within the cubic space composed by beacon system, and the whole three-dimensional diagram is as Fig.1: The proposed SCAL algorithm framework is based on the ranging method (e.g. Wi-Fi, UWB, Bluetooth, etc.) between the target and beacons. The typical ranging observation equation is: where d represents the ranging from beacon to target; [x B , y B , z B ] and [x r , y r , z r ] represents the position of the beacon and target respectively; ε represents ranging noise For the feasibility of the proposed algorithm, at least 4 beacons need to be observed at any time within the target observing range, and its two-dimensional diagram is as in Fig.2:  As the geometric relationship of Fig.2, the relative distance distribution between any adjacent two beacons should fit: where R UWB is the semi-diameter of beacon signal coverage; d x , d y , d z are distances between any adjacent beacons respectively on the X, Y, and Zaxis.

III. OVERVIEW
The proposed algorithm framework is divided into two parts: the Target Localization and Beacon Calibration (TLBC) section (inside the dashed box of Fig.3) and the Global Optimization (GO) section (outside the dashed box of Fig.3).
In the TLBC section, as in Fig.3, target positioning based on GP beacon observations and UP beacon calibration based on target trajectories is synchronously performed. And UP beacons can be converted into GP beacons to provide beacon locations for subsequent target positioning.
In the GO section parallel to the TLBC section as in Fig.3, the target trajectory is screened based on the EGDOP model, and then the position of the target and beacon is simultaneously optimized by the GO algorithm.

IV. TLBC SECTION
As the front end of the proposed SCAL framework, the TLBC section proposes a processing mechanism on the simultaneous execution of target positioning and beacon calibrating In it, the target is located through the observation of GP beacon, and the UP beacon is calibrated by target trajectory as in Fig.4. The whole TLBC section can be illustrated with 3 subsections.

A. LOCATING TARGET
This subsection of TLBC corresponds to the blue boxes of the flowchart in Fig.4, where the target observes 3 or more GP beacons, and its trajectory is located through ranging observation of GP beacons, as in Fig.5.
To ensure the real-time performance of the system and utilize the relevance of target trajectory points, the filtering method is selected to locate the target. When the number of observable GP beacons is 3 or more, the basic form of filtering used for target positioning is: where prediction array, with I 3 for 3 × 3 Identity matrix and T s for time interval; Z k ∈ R N obs,k ×1 is the distance observation at time k, where N obs,k is the number of observed GP beacons at time k; W k ∈ R 6×1 and V k ∈ R N obs,k ×1 are respectively process noise and measurement noise, and two of them are mutually independent Gaussian processes; h (•) is the observation update equation: is the position of i th observed GP beacon. Considering the high nonlinearity of (4) and the requirement for real-time performing, Cubature Kalman Filter (CKF) is adopted to locate the target [16], andX k represents estimated state of target by CKF at time k.

B. CALIBRATING UP BEACON
This subsection of TLBC corresponds to the orange boxes of the flowchart in Fig.4, where the position of UP beacon is calibrated by target trajectory and the ranging observation relative to UP beacon, as in Fig.6. In the blue area of Fig.6, the target can observe at least 3 GP beacons and the beacon UP i . In this range, the beacon UP i can be calibrated according to the estimated target trajectory and the observation of UP i , as in Fig In Fig.7, based on estimated target trajectory set {X k } in time 1 − k, a sub-set of target trajectory points relative to i th observed UP beacon X s,UP i , s = 1, 2, · · · , S,, is selected, whereX s,UP i is the estimated target trajectory point with UP i beacon and at least 3 GP beacons observed. According to X s,UP i and corresponding observations on UP i beacon {Z s,UP i }, the calculation of P UP i the position of UP i beacon is modeled by the square error model: An optimization based on Least Squares Error is: To ensure real-time performance, the Gauss-Newton optimization algorithm [17] is utilized to solve (6) Meanwhile as in Fig.4, P UP i is stored for subsequent conversion into the location of GP beacon.

C. GP BEACON GROUP EXPANSION
This subsection of TLBC corresponds to the green boxes of the flowchart in Fig.4: the calibrated UP beacon is converted into a GP beacon, as in Fig.8.
To calculate the location of UP i beacon with enough observation, the converting mechanism follows: only if the number of GP beacons observed by the target is less than 3, UP beacon with the currently most observation is converted into a GP beacon (as in the lower right corner of Fig.8), as UP → GP and the stored P UP transforms accordingly, as P UP → P GP .

V. GO SECTION
After estimating the positions of the target and UP beacons in the TLBC section, the GO Section is to further optimize the estimated positions to improve the positioning accuracy of the target trajectory and calibrated beacon. However, a large number of positioning information (mainly on target trajectory) are generated during the process of the TLBC Section. It is unrealistic and unnecessary to optimize all of these data for realtime performing.
To solve it, a Geometric Dilution of Precision based on Error-propagation (EGDOP) model is proposed to demonstrate the mechanism of positioning error propagation between the target trajectory point and UP beacon at the TLBC section. On this basis, a global optimization algorithm based on the EGDOP is further proposed to globally optimize the position of target and beacon calibration in realtime.

A. EGDOP MODEL
According to [18], the magnification relationship from the ranging error to the target positioning error under the specific distribution of reference anchors can be illustrated by Geometric Dilution of Precision (GDOP). In other words, the influence of different anchor distributions on the target positioning accuracy can be reflected by GDOP.
However, when it comes to the localization of UP beacons, the traditional GDOP obtained by the estimated trajectory points is biased, due to the estimated position error of the target (anchors) in the TLBC section.
To solve this problem, the Error Propagation (EP) model [19] is utilized to derive the position error of the target trajectory and correct the traditional GDOP through error information. Hence, a Geometric Dilution of Precision based on Error-propagation (EGDOP) model is proposed to evaluate the calibrating quality of beacons given different sub-sets of target trajectory.

1) POSITION ERROR OF ESTIMATED TARGET TRAJECTORY
During positioning the target trajectory in the TLBC section, the basic observation equation is: where GP obs is the observed GP beacon; d k,GP obs is the ranging observation between the target and the GP obs beacon at time k; [x k , y k , z k ] is the target position; [x GP obs , y GP obs , z GP obs ] is the GP obs beacon position. According to (7), the error equation can be obtained by the total differential form of (7) and the EP law [19]: where [m x GP obs , m y GP obs , m z GP obs ] is the position error of the GP obs beacon; m d k,GP obs is the ranging error at time k; [m x k , m y k , m z k ] is the position error of target at time k; GP obs ] is the direction cosine of the target relative to GP obs beacon.
The position error noise of GP obs beacon (the error of GP beacons converted from UP beacons is derived from the subsequent discussion) and the ranging error noise can be acquired in advance. Hence for 3 or more GP obs beacons, an error equation system based on (8) can be obtained: where for the ℵ th GP obs beacon, that is the GP obs ℵ beacon, (ℵ = 1, 2, · · · ,N obs,k ): By performing the Least Squares Solution on (9), the positioning errors on X, Y, and Z-axis of the target [m x k , m y k , m z k ] can be obtained: Therefore, the overall position error of the target is: For X s,UP i ∈ {X k }, the same also exists: 2) EGDOP MODEL Due to the position error of the trajectory set of X s,UP i , the traditional GDOP of the estimated trajectory points relative to UP beacon has a deviation. To solve this problem, EGDOP with correction of the trajectory error m X s,UP i is proposed.
According to m X s,UP i , the GDOP array of target trajectory relative to UP i beacon weighted by error information: where is the direction cosine matrix of target trajectory relative to UP i beacon [18], and W UP i is the weighting matrix based on m X s,UP i : And the value of EGDOP by A EGDOP is: Among all of GP beacons, the position error of the initial GP beacons can be acquired in advance And the position of the rest GP beacons converted from UP beacons is obtained by the trajectory point X s,UP i . According to [18], the positioning error of those GP beacons can be calculated by A EGDOP of the corresponding UP beacons and the trajectory positioning error:

where [m x S,UP im y S,UP im z S,UP i
] is the average position error of target trajectory on the X-axis, Y-axis, and Z-axis; (A EGDOP ) i represents the i th diagonal element of the matrix A EGDOP

B. TRAJECTORY SCREENING METHOD BASED ON EGDOP
In GO Section, to ensure the real-time performance of the system, it is necessary to select a specific set of points from all estimated target trajectory points for global optimization. Therefore, a screening mechanism for trajectory points based on EGDOP is proposed At time k, for i-th UP beacon and its corresponding trajectory point set X s,UP i , EGDOP can reflect the influence of different trajectory points on the positioning accuracy of the beacon. To be specific, the smaller the EGDOP is, the smaller the beacon positioning error caused by target positioning error is. EGDOP of X s,UP i relative to UP i beacon is as in Fig.9   FIGURE 9. Log 10 (EGDOP) of the set X s,UP i relative to the UP i beacon. Fig.9, with the number of trajectory points increasing over time, the EGDOP gets smaller, and correspondingly, the beacon position calibration error gets smaller and according to (18), the positioning error of the beacon gets smaller However, when epoch =781 (as in Fig.9), the EGDOP of several trajectory points drops relatively slowly. It is because trajectory points temporally at adjacent times are also spatially adjacent. When the value of EGDOP relatively is small the improvement of EGDOP is relatively limited under spatially adjacent conditions [18]. Correspondingly, it causes relatively limited improvement in the positioning accuracy of UP beacon, but a sharp increase in the computational complexity of global optimization

As in
To solve it, the number of trajectory points is effectively reduced by the screening method based on EGDOP. Considering the real-time requirement for the overall algorithm framework, the method of differential EGDOP is adopted to filter out trajectory points X s,UP i . And in the new set X l,UP i , the difference W EGDOP between EGDOPs of any two adjacent points meet: where n represents the epoch; η is the screening threshold; η 0 is the initial value of the screening threshold; ϑ is the attenuation factor, whose value ranges from to 1 At the beginning of the trajectory where EGDOP drops faster, according to (20), the η is relatively larger, which helps to select as few trajectory points as possible, to speed up the screening process. In the subsequent part where the decline of EGDOP is slowed down, according to (20), η is effectively reduced In this case, it ensures that the overall EGDOP is small enough to accurately locate the position of VOLUME 8, 2020 the UP beacon at a stable screening rate. For UP i beacon, the screening process for trajectory points based on EGDOP is specifically displayed in Fig.10.  Table.1. For the Gauss-Newton optimization algorithm [20], the relationship between computational complexity and the number of tracepoints N trace is: As in Table.1, compared with the unfiltered X s,UP i , the positioning performance of X l,UP i screened by differential EGDOP is slightly reduced (EGDOP increases from 0.289 to 0.308), but the N trace of X l,UP i is significantly reduced. In other words, according to (21), the computational complexity of Gauss-Newton optimization has been greatly reduced.

C. GLOBAL OPTIMIZATION ALGORITHM BASED ON EGDOP
By screening method based on EGDOP, the point set X l,UP i related to the UP i beacon is selected in original time 1 − k, and then the overall point set {X ζ }, ζ = 1, 2, · · · , L including all X l,UP i is obtained. At time k, construct ξ = [X 0 , X 1 ,· · · ,X L , P UP 1 ,· · · ,P UP i ] as the optimized variable, and the weighted least square error model based on error information is: where, W GP l and W UP l are weighting factors based on observation error: The above optimization is solved by the Gauss-Newton optimization algorithm [17].

VI. SIMULATION
Through the UWB ranging simulation in which the target performs three-dimensional motion and planar motion, the positioning performance of the GO section of the proposed SCAL algorithm framework is evaluated. And the distribution of 6 GP beacons and 8 UP beacons is as in Fig.12:   FIGURE 12. Distribution diagram of beacons (the blue triangle for UP beacon initially, the red rice-shaped for GP beacon).
In Fig.12, the distance between beacons on the Y-axis is 4.44m (40/9 m) within a range of 40m; the distributing gaps between adjacent beacons on X-axis and Z-axis are 3.2m and 2.1m, respectively. The configuration parameters of UWB are: the max range of signal covering (semi-diameter) is 12m, and the ranging error noise is a Gaussian process, with a standard deviation of 0.1m.

A. SIMULATION OF THREE-DIMENSIONAL MOTION
The target performs the random three-dimensional movement, and its movement diagram is as in Fig.13:   FIGURE 13. Schematic diagram of the three-dimensional movement of the target (the blue triangle for UP beacon initially, the red rice-shaped for GP beacon).
The simulations of three-dimensional motion are repeated 10 times (Fig.13 is one). They compare the SCAL algorithm based on the TLBC+GO section with the SCAL algorithm based on only the TLBC section as in Fig.14 and Table2-3. According to Fig.14 and Table.2: compared with the SCAL algorithm without the GO section, the SCAL algorithm with GO+TLBC section has improved the positioning accuracy of the target on the X-axis by 13.32% (from 0.2064m to 0.1789m); the positioning accuracy on Y-axis has been improved 12.45% (0.0787m to 0.0689m); the positioning accuracy on Z-axis has increased by 17.54% (0.2702m to 0.2228m); the overall positioning accuracy has been improved by 15.78% (0.3491m to 0.2940m).
According to Fig.14 and Table.3: compared with the SCAL algorithm without the GO section, the SCAL algorithm with GO+TLBC section has improved the calibrating accuracy of UP beacons on average by 72.35% (0.5279m to 0.1459m).
After adding the GO section, the system's positioning accuracy of the target and the beacons has been significantly improved, and the improvement of the beacon calibrating accuracy is particularly significant. Therefore, the result demonstrates the positioning improvement of the proposed GO algorithm based on EGDOP on the SCAL framework under three-dimensional target motion.

B. SIMULATION OF PLANAR MOTION
The target performs the random planar movement, and its movement diagram is: The simulations of planar motion are repeated 10 times ( Fig.13   According to Fig.16 and Table.4: compared with the SCAL algorithm without the GO section, the SCAL algorithm with GO+TLBC section has improved the positioning accuracy of the target on the X-axis by 4.66% (0.2148m to 0.2048m); the positioning accuracy on Y-axis has been improved 5.23% (0.0688m to 0.0652m); its positioning accuracy on Z-axis has increased by 3.65% (0.2955m to 0.2847m); the overall positioning accuracy has been improved by 4.04% (0. 3717m to 0.3567m).
According to Fig.14 and Table.3: compared with the SCAL algorithm without the GO section, the SCAL algorithm with GO+TLBC section has improved the calibrating accuracy of UP beacons on average by 57.31% (0.6990m to 0.2983m); After adding the proposed GO section, the target positioning accuracy is slightly improved by 4.04%, and the overall accuracy reached 0.3567m. Besides, on Z-axis, the maximum positioning error of the target is reduced from 1.19m to 0.55m. Also, the average calibration accuracy of UP beacons is significantly improved by 57.31% and reaches 0.2983m. Therefore, the result demonstrates the positioning improvement of the proposed GO algorithm based on EGDOP on the SCAL framework under the planar motion.

VII. EXPERIMENT
To further evaluate the performance of the proposed SCAL algorithm, by utilizing the supermarket passageway as an actual indoor scenario, a positioning experiment is constructed. The two-dimensional diagram of the beacon system is as in Fig.17: The reference location of beacons is accurately calibrated by laser rangefinder (accuracy reaches 1cm level). The experimental environment and equipment areas in Fig.18-20: The LinkTrack P in Anchor Mode is selected as the UWB beacon, and the LinkTrack P in Receiver Mode bound to the cart in Fig.20 is selected as a UWB receiver with a sampling frequency of 25 Hz and a fixed height of 0.93 m (as a reference position on Z-axis). Bound to the car, a SLAMTEC M1M module (LIDARSLAM [21]) provides a trajectory with an accuracy of 0.02m, which is treated as the reference position on the X-axis and Y-axis.
After GO processing, positioning results output at the frequency of 5Hz, and the movement trajectory of the car is shown in Fig.21. The experiment compares the proposed SCAL algorithm framework based on EGDOP-GO with the SCAL algorithm based on the Backward Processing with Inverse Trajectory [15], as in Fig.21 and Table.6-7.
According to Table.6-7, compared with the SCAL algorithm based on Backward Processing, the proposed SCAL algorithm based on EGDOP-GO improves the positioning accuracy of the target trajectory on the X-axis by 24.41% (0.1901m to 0.1437m); the positioning accuracy FIGURE 17. The two-dimensional diagram of beacon distribution (the red pentagon for the initial GP beacon, and the blue hexagon for the UP beacon).   on the Y-axis increases by 55.18% (0.0908m to 0.0407m); the positioning accuracy on Z-axis increases by 42.82% (0.3629m to 0.2075m); overall positioning accuracy has been improved by 39.06% (0.4196m to 0.2557m); the average calibration accuracy for 8 beacons has increased by 47.97%    (0.4684 to 0.2437). Besides, according to Fig.22, compared with the backward process, the proposed SCAL algorithm has more efficient parallel computation on the GO section and TLBC section and can reach real-time standard with the outputting frequency of 5Hz.
According to Fig.23, the SCAL algorithm based on Backward Processing has limited positioning performance, especially within the middle part of the beacon system, but the proposed SCAL algorithm illustrates more accurate and comprehensive positioning performance on the whole beacon system: the positioning accuracy of the target is improved by 39.06% and reaches 0.2557m, with maximum positioning error on Z-axis reduced from 1.46m to 0.57m; and the calibration accuracy of UP beacons has been improved by 47.97% and reaches 0.2437m.

VIII. CONCLUSION
Under simultaneous beacon-calibrating and targetpositioning, the real-time positioning and calibrating accuracy in indoor scenarios are significantly reduced. To solve it, a novel SCAL algorithm framework is proposed, which is divided into the TLBC section and GO section. In a typical distributed beacon layout, by a processing mechanism of synchronous acquisition of target position and beacon position, the initial positioning of target and beacon is achieved in the TLBC section. In the GO section (parallel to TLBC section), a global optimization algorithm based on EGDOP is proposed, which utilizes the EGDOP model to filter out the set of trajectory points for optimization, and then performs global optimization on the positioning of target trajectory and beacons in realtime. By simulation, we verify that the GO algorithm based on EGDOP significantly improves the positioning performance under the target's planar motion and three-dimensional motion. By experiments, we verify that compared with the SCAL algorithm based on Backward Processing with Inverse Trajectory, the proposed SCAL algorithm illustrates more comprehensive and more efficient positioning performance and its positioning accuracy of target trajectory and beacons are improved respectively by 39.06% and 47.97%, and reach 0.2557m and 0.2437m, respectively SHUANGZHI LI received the B.S. degree in electronic science and technology from the Beijing University of Posts and Telecommunications, Beijing, China, in 2018, where he is currently pursuing the M.S. degree with the School of Electronic Engineering. His current research interests include computer vision, multisource data fusion, high-precision indoor positioning, and high-precision global navigation satellite systems.