# A Three-Photo-Detector Optical Sensor Accurately Localizes a Mobile Robot Indoors by Using Two Infrared Light-Emitting Diodes

Evandro Bernardes, Stéphane Viollet, Thibaut Raharijaona

► To cite this version:

Evandro Bernardes, Stéphane Viollet, Thibaut Raharijaona. A Three-Photo-Detector Optical Sensor Accurately Localizes a Mobile Robot Indoors by Using Two Infrared Light-Emitting Diodes. IEEE Access, 2020, 8, pp.87490-87503. 10.1109/ACCESS.2020.2992996. hal-02865963

HAL Id: hal-02865963

https://amu.hal.science/hal-02865963v1

Submitted on 12 Jun 2020

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons CC BY 4.0 - Attribution - International License
Received April 8, 2020, accepted May 1, 2020, date of publication May 7, 2020, date of current version May 21, 2020.

Digital Object Identifier 10.1109/ACCESS.2020.2992996

# A Three-Photo-Detector Optical Sensor Accurately Localizes a Mobile Robot Indoors by Using Two Infrared Light-Emitting Diodes

EVANDRO BERNARDES, STÉPHANE VIOLET, AND THIBAUT RAHARIJAONA*

Aix Marseille univ, CNRS, ISM, Marseille, France

Corresponding author: Thibaut Raharijaona (thibaut.raharijaona@univ-amu.fr)

This work was supported in part by the Société Accélérateur du Transfert de Technologies (SATT Sud-Est), National Center for Scientific Research (CNRS), Aix-Marseille University, and in part by the French National Research Agency (ANR) through the Equipex/Robotex Project.

**ABSTRACT** Indoor positioning systems are facing to the demand of large-scale industrial applications in mobile robotics. It is still challenging to create an indoor positioning system that is easily embeddable, accurate, robust and power efficient. We constructed an easily embeddable, low-power optical sensor named InLock without lens to localize a mobile robot indoors moving at 0.20 m/s with an accuracy inferior to 10cm for the position and 0.1rad for the heading by using only three photo-detectors (PDs) and two infrared Light-Emitting Diodes (LEDs). (i) We modelled the optical sensor based on only three photo-detectors and two infrared LEDs by taking into account radiometric properties. (ii) We constructed the optical sensor by optimizing the geometry of the beacon and the receiver. (iii) We implemented and validated online estimation algorithms for an operating range at a height up to 3m by using an extended Kalman filter and a complementary filter. Our results showed that modelling the optical sensor so that it takes into account radiometric properties and it optimizes the geometry of the beacon can enhance the accuracy of the indoor positioning system.

**INDEX TERMS** Indoor positioning system, infrared light communication, LED, complementary filter, Kalman filter, robot's localization.

## I. INTRODUCTION

Global Positioning System (GPS) makes outdoors tracking and navigation reliable and easily embeddable for real-time applications. However, in confined environments, GPS positioning and navigation are inaccurate due to the strong degradation of the satellite signals which are attenuated by clouds, walls and obstructions [1]. The attenuated signal provides an unsatisfactory accuracy of localization that led to the development of Indoor Positioning Systems (IPS). There are two classes of localization scheme using PD or image sensor [2]. Accurate IPS presents multiple challenges such as risk of collision, variations on lighting conditions, congestion of the building infrastructure and limitations on embedded computer resources. How can we model and construct an

accurate IPS for a mobile robot taking into account these constraints?

Literature survey shows that the most popular IPS are based on (i) Simultaneous Localization And Mapping (SLAM) [3], (ii) inertial sensors [4], [5] and (iii) signal communication by using emitters and receivers. The communication signal changes in values as the receiver moves inside the indoor environment. For instance, the communication link can be based on WiFi [6], Bluetooth Low Energy (BLE) [7], Radio Frequency Identification (RFID) [8] and Ultra Wide Band (UWB) [9].

The studies reported in [10] and [11] have explored the subject of Indoor Visible Light Communication (VLC) technologies since visible light spectrum (380nm-780nm) is freely available. In VLC systems, the transmitter is usually a Light-Emitting Diode (LED), which performs some additional functions besides its primary use as a lighting source. Visible light LED-based IPS can estimate indoor position with the

The associate editor coordinating the review of this manuscript and approving it for publication was Ting Yang*.
help of ceiling LED lamps acting as beacons [12]. How can we reach the standard level of accuracy of 20cm for the indoor localization of the industrial Internet of Things by using a LED-based IPS?

The main challenge for the LED-based IPS is to estimate its own position from the optical signals received from the beacon. Any visible LED-based IPS aims at exploiting the received signal characteristics. Received Signal Strength (RSS), Time of Flight (TOF) or Angle of Arrival (AOA) by using photodiodes (PD) or cameras are combined with a positioning algorithm [13], [14]. In [15] and [16], trilateration and triangulation algorithms compute the position estimation.

For optical communications in free space under fog and smoke conditions, Ijaz et al. showed that near infrared light sources are the most robust wavelengths to link failure [17]. In [18], we proposed a minimalistic optical sensor without lens that estimates the relative position between the sensor and active markers using amplitude modulated infrared light. We showed that the sensor was able to estimate the position *x* and *y* at a distance of *2m* with an accuracy as small as *2cm* at a sampling rate of 100Hz. We implemented the sensor in a position feedback loop for indoor robotic applications in GPS-denied environment.

We aim at constructing a robust sensor for indoor position and heading estimations of a mobile robot for an operating range at a height up to 3m. We also aim at reaching a positioning accuracy inferior to 10cm limiting infrastructures modifications. We aim at using off-the-shelf PDs without lens instead of specific PDs as proposed in [19]. The IPS aims at accurately estimating the pose (position and orientation) of a robot in a set of critical areas such as near edges, near entrance and exit of corridors using a fixed beacon and a embedded receiver while the repeated unit cells of LEDs define the visual light positioning system in [20], [21].

In this paper, we modelled an optical sensor without lens based on only three PDs and two infrared LEDs by taking into account radiometric properties. We constructed the optical sensor by optimizing the geometry of the beacon and the receiver. Finally, we implemented and validated online estimation algorithms for an operating range at a height up to 3m by using an extended Kalman filter and a complementary filter.

## II. SYSTEM OVERVIEW

We constructed an optical sensor without lens that we called InLock by optimizing the geometry of the beacon and the receiver.

The system overview is presented in Fig. 1. Figure 1a) describes the system configuration with one beacon. The beacon is composed of two infrared LEDs: LED 1 and LED 2. Lens can introduce strong distortions on the LED emission patterns that would add complexity to the overall system, as seen in [22]. To simplify the system, instead of using a lens, each LED was placed behind an optical diffuser. Optical diffuser is cheaper than lens, simpler to use, and homogenize the emission pattern to a smooth Gaussian emission pattern

<figure><img src="image_2.png" /><figcaption>FIGURE 1. a) Perspective view of the sensing device. It is composed of 3 photodiodes (3 pixels) and a custom made analog demodulation board. It is embedded on the TurtleBot3 Burger for the experiments. b) Model of the mobile robot. The vehicle's body frame is shown in red and the earth frame of reference is shown in black. The velocity in the x-direction is $v$ and the vehicle's heading is $\phi$.</figcaption></figure>

<figure><img src="image_3.png" /><figcaption>FIGURE 2. Picture of the infrared light beacon. It is composed of two infrared LEDs oriented in a specific direction of emission.</figcaption></figure>

with useful mathematical properties. The latter allowed us to model the system as detailed in appendix B. The LEDs emit a modulated infrared signal at two distinct frequencies $f_1 = 5$kHz and $f_2 = 17$kHz. The receiver is composed of three photodiodes PD$_A$, PD$_B$ and PD$_C$ organized in a plane right-angle triangle and mounted on the commercial mobile robot TurtleBot3. Figure 1b) presents the mobile robot in top view. The vehicle's body frame $\{B\}$ is shown in red and the earth frame of reference $\{E\}$ is shown in black. The velocity in the *x*-direction is $v$ and the vehicle's heading is $\phi$. The vehicle's velocity in $\{E\}$ is $(v \cos(\phi), v \sin(\phi))$.

### A. INFRARED LIGHT BEACON

The beacon is composed of two infrared LEDs as presented in Fig. 2. The LEDs flicker at the frequencies $f_1 = 5$kHz and $f_2 = 17$kHz. Each LED is oriented in a specific direction of emission. The angle of emission of each LED provides mathematical properties for the algorithm of position and heading estimations.

### B. INFRARED LIGHT RECEIVER

The infrared light receiver is composed of only three pixels without optics as shown on Fig. 3. It measures the demodulated infrared signals. It provides an analog signal which is the input for the digital processing.

## III. SYSTEM MODEL

We modelled an optical sensor based on only three PDs and two infrared LEDs by taking into account radiometric properties.
<figure><img src="image_4.png" /><figcaption>FIGURE 3. Picture of the infrared light receiver. It is composed of only three pixels without optics. It is embedded on the commercial mobile robot TurtleBot3.</figcaption></figure>

<figure><img src="image_5.png" /><figcaption>FIGURE 4. Side view of the optical wireless channel model. The model gives a nonlinear expression of the voltage amplitude $V_{PD,i}$ for each frequency $f_i$. It depends on the angle of emission $\theta_{PD,i}^E$, the angle of reception $\theta_{PD,i}^R$ and the distance $D_{PD,i}$ from the beacon to the photodiode.</figcaption></figure>

Figure 4 gives an illustration in side view of the system for indoor positioning. The system model is composed of an infrared light beacon and a photodiode sensitive to the infrared light. It aims at modelling the voltage amplitude $V_{PD,i}$ taking to account radiometric properties.

The infrared LED i flickers at frequency $f_i$ just as in [20], [21], [23]. Each photodiode receives the infrared signal at each frequency. Consequently, the optical sensor composed of 3 photodiodes receives 6 signals (2 LEDs × 3 photodiodes). The signal is demodulated with an analog circuit and converted for digital processing. The voltage amplitude $V_{PD,i}$ is modelled by:

$$V_{PD,i} = \frac{\alpha_i}{D_{PD,i}^2} \exp\left(-\left(\frac{\theta_{PD,i}^E}{\beta_i}\right)^2\right) \cos(\theta_{PD,i}^R) \quad (1)$$

We explain the expression of (1) as the product of three different gains:

i. $\frac{\alpha_i}{D_{PD,i}^2}$, models the variation over the distance $D_{PD,i}$ between the LED and the photodiode on the received signal strength,

ii. $\exp\left(-\left(\frac{\theta_{PD,i}^E}{\beta_i}\right)^2\right)$, models the gain of the LED's radiation over the angle of emission $\theta_{PD,i}^E$,

iii. $\cos(\theta_{PD,i}^R)$, models the gain of the photodiode with respect to $\theta_{PD,i}^R$, the angle of reception. We used the photodiode OSRAM BPW 34 FAS whose the radiant sensitive area is $7.02mm^2$. The angular sensitivity of the photodiode is

<figure><img src="image_6.png" /><figcaption>FIGURE 5. Computer Aided Design of the beacon. a) The distance between the LEDs is 4cm. b) Since the distance between the beacon and the receiver is greater than 3m, we assume that $D_{PD,1} \approx D_{PD,2}$.</figcaption></figure>

<figure><img src="image_7.png" /><figcaption>FIGURE 6. Illustration of the direction of emission of each LED with respect arbitrary angles $\gamma_1$ for LED 1 (a) and $\gamma_2$ for LED 2 (b).</figcaption></figure>

modelled by a cosine-like angular sensitivity by taking to account the datasheet. The use of photo-detectors (PDs) is advantageous in terms of speed, sensitivity, energy consumption and system complexity [24].

The constant $\alpha_i$ is defined by the input voltage and electronic gains of both the emitter and the receiver circuits, and we found $\alpha_1 = 15$, $\alpha_2 = 13$ in our experiments. The constant $\beta_i$ is defined by the optical diffuser's characteristic curve. $\beta_1 = \beta_2 = 0.3$ according to both the diffuser's datasheet and our experiments.

As shown in Fig. 5, the distance that separates the LEDs mounted on the beacon is 4cm. Therefore the distances from each LED to the photodiode PD are practically the same:

$$D_{PD} = D_{PD,1} \approx D_{PD,2} \quad (2)$$

Since the angles of reception depend on the distances, we assume they are the same:

$$\theta_{PD}^R = \theta_{PD,1}^R \approx \theta_{PD,2}^R \quad (3)$$

Taking these two approximations into account, we find the following relationship (see Appendix A):

$$(\theta_{PD,1}^E)^2 - (\theta_{PD,2}^E)^2 = \beta^2 \ln\left(\frac{\alpha_1 V_{PD,2}}{\alpha_2 V_{PD,1}}\right) \quad (4)$$

Assuming the beacon is mounted horizontally on the ceiling of a room, we define the direction of emission of each LED with the angles $\gamma_1, \gamma_2$ and the vectors $\vec{N}_1^{(E)}$ and $\vec{N}_2^{(E)}$ as described in Fig. 6. The vector components of $\vec{N}_1^{(E)}$ and $\vec{N}_2^{(E)}$
are written in the earth frame of reference $O_E - \{x_E, y_E, z_E\}$:

$$ 
\begin{aligned}
\vec{N}_1^{[E]} &= \begin{pmatrix} +\sin \gamma_1 \\ 0 \\ -\cos \gamma_1 \end{pmatrix} \\
\vec{N}_2^{[E]} &= \begin{pmatrix} +\sin \gamma_2 \\ 0 \\ -\cos \gamma_2 \end{pmatrix}
\end{aligned}
\qquad (5) $$

We define $s_1$ and $c_1$ as the sine and cosine of $\gamma_1$. $s_2$ and $c_2$ are the sine and cosine of $\gamma_2$.

We also define in the body fixed frame attached to the robot $\mathbf{L}^{[B]} = (x_L \ y_L \ z_L)^T$ which is the position vector of the beacon and $\mathbf{PD}^{[B]} = (x_{PD} \ y_{PD} \ z_{PD})^T$ which is the position vector of the photodiode PD. Considering the voltage ratio provided by (4) and the system's configuration, we introduce the variable $r_{PD}$:

$$ r_{\text{PD}} := \frac{c_{\phi}(x_L - x_{\text{PD}}) - s_{\phi}(y_L - y_{\text{PD}})}{z_L - z_{\text{PD}}} \qquad (6) $$

The angle $\phi$ is the heading of the robot introduced in Fig. 1b). Taking into account the geometry of the beacon, the expression of $r_{PD}$ becomes (see Appendix B for the mathematical development):

$$ r_{\text{PD}} = \frac{(c_1 - c_2) - (c_1 + c_2)\lambda_{\text{PD}}}{(s_1 - s_2) - (s_1 + s_2)\lambda_{\text{PD}}} \qquad (7) $$

where

$$ 
\begin{aligned}
\lambda_{\text{PD}} &:= \frac{\beta^2}{4} \ln \left( \frac{\alpha_1 V_{\text{PD},2}}{\alpha_2 V_{\text{PD},1}} \right) \\
&:= \frac{\beta^2}{4} \left( \ln \left( \frac{V_{\text{PD},2}}{V_{\text{PD},1}} \right) - \ln \left( \frac{\alpha_2}{\alpha_1} \right) \right)
\end{aligned}
\qquad (8) $$

The expression of $r_{PD}$ gives:

(i) a mathematical expression of the robot's position with $x_{PD}$, $y_{PD}$, $z_{PD}$ and the heading with $c_{\phi}$, $s_{\phi}$ in (6),

(ii) a mathematical model of the measurements. This model takes into account the geometry of the beacon ($c_1$, $s_1$, $c_2$ and $s_2$) and the radiometric properties with $V_{PD,1}$ and $V_{PD,2}$ in (7).

$\lambda_{\text{PD}}$ stands for a variable of measurement which takes into account the optical properties of the beacon and the voltage amplitudes provided by the sensor.

## IV. SENSOR CALIBRATION

We calibrated the sensor in order to estimate the robot's heading and the robot's position along $x$ and $y$.

From (7) and the expression of $\lambda_{\text{PD}}$ in (8), we defined the variable $\lambda_{\text{PD}}^* = \ln \left( \frac{V_{\text{PD},2}}{V_{\text{PD},1}} \right)$. Substituting the expression of $\lambda_{\text{PD}}$ given by (8) in (7), we write $r_{PD}$ as follows:

$$ r_{\text{PD}} = \frac{a_0 + a_1 \lambda_{\text{PD}}^*}{a_2 + a_3 \lambda_{\text{PD}}^*} \qquad (9) $$

where:

$$ 
\begin{aligned}
a_0 &= (c_1 - c_2) + (c_1 + c_2) \frac{\beta^2}{4} \ln \left( \frac{\alpha_2}{\alpha_1} \right) \\
a_1 &= -(c_1 + c_2) \frac{\beta^2}{4}
\end{aligned}
$$

<figure><img src="image_8.png" /><figcaption>FIGURE 7. Calibration result. We found the optimized gains $k_0$, $k_1$ and $k_2$ such that the mathematical expression of $r_{PD}$ in (11) fits the measurements.</figcaption></figure>

$$ 
\begin{aligned}
a_2 &= (s_1 - s_2) + (s_1 + s_2) \frac{\beta^2}{4} \ln \left( \frac{\alpha_2}{\alpha_1} \right) \\
a_3 &= -(s_1 + s_2) \frac{\beta^2}{4}
\end{aligned}
\qquad (10) $$

and $\alpha_1 = 15$, $\alpha_1 = 13$, $\beta = 0.3$. The robot is controlled using the motion capture system VICON. The position and the heading are recorded. When we plotted $r_{PD}$ from (6) versus $\lambda_{\text{PD}}^*$ obtained from the measurements, we observed from Fig. 7, a linear shape. We decided to approximate $r_{PD}$ with a second order polynomial function of $\lambda_{\text{PD}}^*$:

$$ r_{\text{PD}} \approx f_{\text{PD}} (\lambda_{\text{PD}}^*) = k_0 + k_1 \lambda_{\text{PD}}^* + k_2 (\lambda_{\text{PD}}^*)^2 \qquad (11) $$

The calibration step aims at determining the values of the gains $k_0$, $k_1$ and $k_2$ by using a least squares regression. The aim is to fit the expression of $r_{PD}$ in (11) to the expression of $r_{PD}$ in (6) given by the measured position and heading.

### A. HEADING ESTIMATION

The use of LED1 and LED2 with chosen orientations allowed us to estimate the heading of the robot.

Performing a calibration step for each photodiode A, B and C embedded in the receiver, we compute $r_A$, $r_B$ and $r_C$ from (9). Moreover, the position coordinates are given in the robot's frame of reference. The positions of the photodiodes are known and correspond to their fixed position on the receiver. From (6), we can write the following equations for each photodiode:

$$ 
\begin{aligned}
r_A \ z_L - z_A \ r_A &= c_{\phi}(x_L - x_A) - s_{\phi}(y_L - y_A) \\
r_B \ z_L - z_B \ r_B &= c_{\phi}(x_L - x_B) - s_{\phi}(y_L - y_B) \\
r_C \ z_L - z_C \ r_C &= c_{\phi}(x_L - x_C) - s_{\phi}(y_L - y_C)
\end{aligned}
\qquad (12) $$

Computing the differences $r_A - r_B$ and $r_A - r_C$, we write the following matrix equation:

$$ \Delta \mathbf{PD} \cdot \begin{pmatrix} c_{\phi}/z_L \\ -s_{\phi}/z_L \end{pmatrix} = \begin{pmatrix} r_A - r_B \\ r_A - r_C \end{pmatrix} - \frac{1}{z_L} \begin{pmatrix} r_A z_A - r_B z_B \\ r_A z_A - r_C z_C \end{pmatrix} \qquad (13) $$

where:

$$ \Delta \mathbf{PD} = \begin{pmatrix} x_A - x_B & y_A - y_B \\ x_A - x_C & y_A - y_C \end{pmatrix} \qquad (14) $$
Since in our case of $z_A = z_B = z_C = 0$, it gives:

$$ \left( \begin{array}{c} c_{\phi}/z_{L} \\ -s_{\phi}/z_{L} \end{array} \right) = \Delta \mathbf{PD}^{-1} \cdot \left( \begin{array}{c} r_{A} - r_{B} \\ r_{A} - r_{C} \end{array} \right) \quad (15) $$

Equation (15) allows us to find both $\frac{c_{\phi}}{z_{L}}$ and $\frac{s_{\phi}}{z_{L}}$. Since the height $z_L$ is a positive and constant value, we estimated the heading $\phi$ as follows:

$$ \phi = a \tan 2(s_{\phi}, c_{\phi}) = a \tan 2\left(\frac{s_{\phi}}{z_{L}}, \frac{c_{\phi}}{z_{L}}\right) \quad (16) $$

We can remark from (15) that we can estimate the heading even if the height is unknown. Once the heading calculated, one can use $\phi$ in (12) to estimate $z_L$. In our case, we assumed that the height $z_L$ was known and constant. We found experimentally that this method gives a very good estimate of the heading.

## B. ESTIMATION OF THE ROBOT'S POSITION ALONG THE X-AXIS

The use of LED1 and LED2 with chosen orientations allowed us to estimate the robot's position along the x-axis.

We want to find the robot's position in the earth frame of reference {E} along the x-axis. We know that the relationship between the robot's position $\mathbf{X} = \mathbf{X}^{[E]} = (x \ y \ z)^T$ in {E} and the beacon's position $\mathbf{L} = \mathbf{L}^{[B]}$ in the body frame of reference {B} is given by:

$$ \mathbf{X} = -R \cdot \mathbf{L} \quad (17) $$

We can also write:

$$ \mathbf{X} + R \cdot \mathbf{PD} = -R \cdot (\mathbf{L} - \mathbf{PD}) \quad (18) $$

Developing the left side of the equation for photodiode A, we write:

$$ \begin{aligned} \mathbf{X} + R \cdot \mathbf{PD} &= \begin{pmatrix} x \\ y \\ z \end{pmatrix} + \begin{pmatrix} c_{\phi} & -s_{\phi} & 0 \\ s_{\phi} & c_{\phi} & 0 \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} x_A \\ y_A \\ z_A \end{pmatrix} \\ &= \begin{pmatrix} x + c_{\phi}x_A - s_{\phi}y_A \\ y + s_{\phi}x_A + c_{\phi}y_A \\ z + z_A \end{pmatrix} \end{aligned} \quad (19) $$

And developing the right side of (18):

$$ \begin{aligned} -R \cdot (\mathbf{L} - \mathbf{PD}) &= - \begin{pmatrix} c_{\phi} & -s_{\phi} & 0 \\ s_{\phi} & c_{\phi} & 0 \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} x_L - x_A \\ y_L - y_A \\ z_L - z_A \end{pmatrix} \\ &= - \begin{pmatrix} c_{\phi}(x_L - x_A) - s_{\phi}(y_L - y_A) \\ s_{\phi}(x_L - x_A) + c_{\phi}(y_L - y_A) \\ z_L - z_A \end{pmatrix} \end{aligned} \quad (20) $$

We can note from (20) that $z = -z_L$. We can also note that:

$$ c_{\phi}(x_L - x_A) - s_{\phi}(y_L - y_A) = -(x + c_{\phi}x_A - s_{\phi}y_A) \quad (21) $$

Using (21) and the definition of $r_{PD}$ in (6), we can write $r_A$ as follows:

$$ \begin{aligned} r_A &= \frac{x + c_{\phi}x_A - s_{\phi}y_A}{z + z_A} \\ x &= (z + z_A) \cdot r_A - (c_{\phi}x_A - s_{\phi}y_A) \end{aligned} \quad (22) $$

<figure><img src="image_9.png" /><figcaption>FIGURE 8. Illustration of the direction of emission of each LED mounted on the beacon. The angle $\gamma_1$ is equal to zero and $\gamma_2 < 0$.</figcaption></figure>

<figure><img src="image_10.png" /><figcaption>FIGURE 9. Illustration of LED1 and the photodiode PD for the estimation of the robot's position along the y-axis.</figcaption></figure>

Equation (22) gives an expression of $x$ with respect $r_A$. As described in (10), $r_A$ depends on the direction of emission of LED1 and LED2. In Fig. 8, we set $\gamma_1 = 0$ and $\gamma_2 < 0$.

The use of LED1 and LED2 installed with two tilted angles $\gamma_1$ and $\gamma_2$ allowed us to estimate the robot's heading and the robot's position along the x-axis.

We can remark that $y$ does not appear in (22) making impossible the estimation of the position along the y-axis. The reason is the use of only two LEDs instead of three. In future works, we will construct a beacon endowed with three infrared LEDs. The use of three infrared LEDs implies the construction of two new circuits boards for both the beacon and the receiver.

## C. ESTIMATION OF THE ROBOT'S POSITION ALONG THE Y-AXIS

LED1 allowed us to estimate the robot's position along the y-axis. We assume that the height $z_L$ is known and constant and the robot only rotates in yaw. We have $\theta_{PD,1}^E = \theta_{PD,1}^R = \theta_1$ as presented in Fig. 9.

Since $D_{PD,1} = \frac{z_L}{\cos(\theta_1)}$, substituting in (1), we write $V_{PD,1}$ as follows:

$$ \begin{aligned} V_{PD,1} &= \frac{\alpha_i}{D_{PD,1}^2} \exp\left(-\left(\frac{\theta_1}{\beta}\right)^2\right) \cos(\theta_1) \\ &= \frac{\alpha_i}{z_L^2} \exp\left(-\left(\frac{\theta_1}{\beta}\right)^2\right) \cos(\theta_1)^3 \end{aligned} $$
Assuming $z_L$ is known and constant, the voltage amplitude $V_{PD,1}$ is a bijective function of $\theta_1$. We defined the function $u_{PD}(\cdot)$ such that:

$$V_{PD,1} = u_{PD}(\theta_1) \qquad (23)$$

Or, equivalently:

$$\theta_1 = u_{PD}^{-1}(V_{PD,1}) \qquad (24)$$

since the mathematical expression of $\tan \theta_1$ is:

$$\tan \theta_1 = \frac{\sqrt{(x_L - x_{PD})^2 + (y_L - y_{PD})^2}}{z_L} \qquad (25)$$

To simplify the calculation, we defined the function $g_{PD}(\cdot)$ such that:

$$(\tan \theta_1)^2 \approx g_{PD}(V_{PD,1}) \qquad (26)$$

And we approximated the function $g_{PD}(\cdot)$ by a second-order polynomial function. The aim of the calibration is to find by using a least squares regression, the optimal values of the gains $b_0, b_1$ and $b_2$ as the following:

$$g_{PD}(V_{PD,1}) = b_0 + b_1 V_{PD,1} + b_2 (V_{PD,1})^2 \qquad (27)$$

For the sensor calibration, the distance between each photodiode and the beacon is accurately measured with the motion capture system. We used a trilateration-based algorithm to compute the robot's full position given by the following equation:

$$z_L^2 (\tan \theta_1)^2 = (x_L - x_{PD})^2 + (y_L - y_{PD})^2 \qquad (28)$$

And then, from (28) and (26):

$$z_L^2 g_{PD}(V_{PD,1}) = (x_L - x_{PD})^2 + (y_L - y_{PD})^2 \qquad (29)$$

We programmed the robot to follow a reference trajectory. In Fig. 10, for each frequency $f_1 = 5$kHz and $f_2 = 17$kHz, we plotted the voltage amplitude measured for each PD used for the sensor calibration.

In Fig. 11, we plotted the results of the sensor calibration for the estimations of the heading and the robot's position along the x-axis. The plot in Fig. 11 a), b) and c) define the calibration functions useful for the estimations. For each photodiode A, B and C, the fitting curves obtained by using a least squares regression are linear.

The plot in Fig. 12 a), b) and c) allowed us to define the calibration functions for the estimation of the robot's position along the y-axis. For each photodiode A, B and C, the fitting curves obtained by using a least squares regression are second-order polynomial functions.

Figure 13 presents the algorithm flowchart that allows us to calculate the heading and the positions in x and y of the mobile robot.

<figure><img src="image_11.png" /><figcaption>FIGURE 10. Plots of voltage measurements of the demodulated infrared signal for each frequency $f_1 = 5$kHz and $f_2 = 17$kHz and for each PD.</figcaption></figure>

<figure><img src="image_12.png" /><figcaption>FIGURE 11. Plots of the sensor calibration results. The plots a), b) and c) define the calibration functions in black for the estimations of the heading and the robot's position along the x-axis.</figcaption></figure>

## V. ALGORITHMS FOR HEADING AND POSITION ESTIMATIONS

We implemented online estimation algorithms for an operating range at a height up to 3m by using a complementary filter for the heading and an extended Kalman filter for the position.

Instead of using Angle of Arrival (AOA) based methods, we developed an algorithm based on (22) takes into account the radiometric properties of the optical diffuser and the geometry of the emitter. We present (i) the algorithm that
<figure><img src="image_13.png" /><figcaption>FIGURE 12. Plots of the sensor calibration results. The plots a), b) and c) define the calibration functions for the estimation of the robot's position along the y-axis.</figcaption></figure>

<figure><img src="image_14.png" /><figcaption>FIGURE 13. The algorithm flowchart describes the calculation of the heading and the positions in x and y of the mobile robot.</figcaption></figure>

gives the heading estimation of the robot. A complementary filter optimizes the accuracy fusing the sensor measurements and the angular velocity provided by a gyroscopic sensor, (ii) the algorithm that estimates the positions in x and y using an Extended Kalman Filter (EKF).

## A. INITIALIZATION

We initialized the algorithms by using the IMU and the optical sensor.

### 1) TILTING

First, we use the IMU's accelerometer to calculate a first estimate of the robot's tilt. We define the orientation quaternion as $q = q_z \otimes q_{xy}$ where $\otimes$ is the Hamilton product. Here we note quaternions as 4x1 matrixes, where the fourth element is the quaternion's real part:

$$q = q_w + iq_x + jq_y + kq_z = \begin{pmatrix} q_x \\ q_y \\ q_z \\ q_w \end{pmatrix} \quad (30)$$

Using the normalized accelerometer vector $\vec{a} = (a_x \ a_y \ a_z)^T$, we can use the methods defined in [25] to determine that:

$$q_{xy} = \begin{pmatrix} +a_y \sqrt{2(a_z+1)} \\ -a_x \sqrt{2(a_z+1)} \\ 0 \\ \sqrt{\frac{a_z+1}{2}} \end{pmatrix} \quad (31)$$

### 2) HEADING

We use Equation (11) to get the values of $r_A$, $r_B$ and $r_C$. With the method described in Sec. IV, we initialize the value of the heading $\phi$. We can define the heading quaternion as:

$$q_z = \begin{pmatrix} 0 \\ 0 \\ \sin(\phi/2) \\ \cos(\phi/2) \end{pmatrix} \quad (32)$$

The Hamilton product $q = q_z \otimes q_{xy}$ gives the initial orientation of the robot.

### 3) x ESTIMATE

Assuming that we know the (constant) height $z_L$ and that we have already calculated the orientation quaternion, we compute the yaw angle $\phi$ and use (22) to get three expressions for $x$:

$$\begin{aligned} x &= z_L r_A - c_\phi x_A - s_\phi y_A \\ x &= z_L r_B - c_\phi x_B - s_\phi y_B \\ x &= z_L r_C - c_\phi x_C - s_\phi y_C \end{aligned} \quad (33)$$

We initialize the first value of $x$ taking the mean of these three values.

### 4) y ESTIMATE

From 18, we write the following equations:

$$\begin{aligned} (x_L - x_{PD})^2 &= (x + c_\phi x_{PD} - s_\phi y_{PD})^2 \\ (y_L - y_{PD})^2 &= (y + s_\phi x_{PD} + c_\phi y_{PD})^2 \\ z_L^2 &= z^2 \end{aligned} \quad (34)$$

From 29, we can write the following equations for each photodiode:

$$\begin{aligned} z_L^2 g_A(V_{A,1}) &= (x + c_\phi x_A - s_\phi y_A)^2 + (y + s_\phi x_A + c_\phi y_A)^2 \\ z_L^2 g_B(V_{B,1}) &= (x + c_\phi x_B - s_\phi y_B)^2 + (y + s_\phi x_B + c_\phi y_B)^2 \\ z_L^2 g_C(V_{C,1}) &= (x + c_\phi x_C - s_\phi y_C)^2 + (y + s_\phi x_C + c_\phi y_C)^2 \end{aligned} \quad (35)$$
We find the unknown y in each equation and we compute the mean value.

## B. HEADING ESTIMATION WITH A COMPLEMENTARY FILTER

We filtered the orientation quaternion q by using a complementary filter to prevent the robot's heading estimation from noise. The complementary filter that we implemented is taken from [25]. Instead of using the magnetometer to find the heading, we used the optical sensor InLock.

### 1) GYROSCOPE PREDICTION

The gyroscope gives a very precise measurement of the robot's angular velocities in every axis, defined here as $ω = (ω_x, ω_y, ω_z)$. We defined the quaternion $q_ω$ as:

$$q_ω = \begin{pmatrix} ω_x \\ ω_y \\ ω_z \\ 0 \end{pmatrix}$$

Its time derivative $\dot{q}_k$ is given by:

$$\dot{q}_k = \frac{q_k \otimes q_ω}{2} \qquad (36)$$

We used it to calculate the prediction quaternion:

$$q_{pred,k} = q_{k-1} + \Delta t \dot{q}_{k-1} \qquad (37)$$

Then we normalized this prediction to make sure it continues to be a rotation quaternion. The pure integration of the gyroscope can lead to errors after some time, so after each prediction step we must correct it with the accelerometer and InLock sensor.

### 2) ACCELEROMETER CORRECTION

Assuming that the quaternion $\Delta q_{acc}$ corrects the prediction $q_{pred}$ to the real quaternion, we can write:

$$q'_{k} = \Delta q_{acc} \otimes q_{pred,k} \qquad (38)$$

In order to find $\Delta q_{acc}$, we compute the modified accelerometer vector $\vec{g}$:

$$\vec{g} = R(q_{pred,k}) \vec{a} = \begin{pmatrix} g_x \\ g_y \\ g_z \end{pmatrix} \qquad (39)$$

Then we define:

$$\Delta q_{acc} = \begin{pmatrix} +g_y \sqrt{2(g_z + 1)} \\ -g_x \sqrt{2(g_z + 1)} \\ 0 \\ \sqrt{\frac{g_z+1}{2}} \end{pmatrix} \qquad (40)$$

We combine the data from both the accelerometer and the gyroscope using linear interpolation by (i) defining the gain $\alpha$ between [0, 1], (ii) updating the correction:

$$\overline{\Delta q_{acc}} = \alpha \cdot \Delta q_{acc} + (1 - \alpha) \cdot \begin{pmatrix} 0 \\ 0 \\ 0 \\ 1 \end{pmatrix} \qquad (41)$$

We normalized $\Delta q_{acc}$ and defined the corrected quaternion as:

$$q'_{k} = \overline{\Delta q_{acc}} \otimes q_{pred,k} \qquad (42)$$

In our experiments, $\alpha = 0.1$ produced the best results.

### 3) InLock CORRECTION

We implemented the heading correction with the InLock sensor by using a correction quaternion $\Delta q_z$.

We applied first the accelerometer corrected prediction rotation to the positions of the photodiodes:

$$\begin{aligned} A' &= R(q'_{k}) A \\ B' &= R(q'_{k}) B \\ C' &= R(q'_{k}) C \end{aligned} \qquad (43)$$

The rotated positions $A', B'$ and $C'$ of the photodiodes result in a non-zero value for their z-component ($z'_A \neq 0, z'_B \neq 0$ and $z'_C \neq 0$). We write (13) as follows:

$$\Delta \mathbf{PD}' \cdot \begin{pmatrix} c \Delta \phi / z_L \\ -s \Delta \phi / z_L \end{pmatrix} = \begin{pmatrix} r_A - r_B \\ r_A - r_C \end{pmatrix} - \frac{1}{z_L} \begin{pmatrix} r_A z'_A - r_B z'_B \\ r_A z'_A - r_C z'_C \end{pmatrix} \qquad (44)$$

where:

$$\Delta \mathbf{PD}' = \begin{pmatrix} x'_{A} - x'_{B} & y'_{A} - y'_{B} \\ x'_{A} - x'_{C} & y'_{A} - y'_{C} \end{pmatrix} \qquad (45)$$

Then we can calculate $\Delta \phi$ by first finding both $c \Delta \phi / z_L$ and $s \Delta \phi / z_L$ from Equation 45. Then, the heading correction quaternion is defined as:

$$\Delta q_z = \begin{pmatrix} 0 \\ 0 \\ \sin(\Delta \phi / 2) \\ \cos(\Delta \phi / 2) \end{pmatrix} \qquad (46)$$

We defined the gain $\beta$ between [0, 1] and defined the quaternion:

$$\overline{\Delta q_z} = \beta \cdot \Delta q_z + (1 - \beta) \cdot \begin{pmatrix} 0 \\ 0 \\ 0 \\ 1 \end{pmatrix} \qquad (47)$$

The quaternion $\overline{\Delta q_z}$ is normalized. The fully corrected quaternion is given by:

$$q_k = \overline{\Delta q_z} \otimes q'_{k} \qquad (48)$$

In our experiments, $\beta = 0.1$ produced the best results.

## C. POSITION ESTIMATION WITH AN EKF

We implemented an EKF to estimate the robot's position in x and y.
## 1) MEASUREMENT VECTOR

In this case, the measurement vector $Y$ is defined as:

$$ Y = \begin{pmatrix} f_A \left( \ln \left( \frac{V_{A,2}}{V_{A,1}} \right) \right) \\ f_B \left( \ln \left( \frac{V_{B,2}}{V_{B,1}} \right) \right) \\ f_C \left( \ln \left( \frac{V_{C,2}}{V_{C,1}} \right) \right) \\ g_A(V_{A,1}) \\ g_B(V_{B,1}) \\ g_C(V_{C,1}) \end{pmatrix} \quad (49) $$

With $r_{PD} = f(\ln(V_{PD,1}/V_{PD,2})$ and $\tan^2(\theta_{PD,1}) = g_{PD}(V_{PD,1})$, where the functions $f$ and $g$ are defined as polynomial functions.

## 2) EKF STATE VECTOR AND MEASUREMENT VECTOR

We defined the state vector $X$ as:

$$ X = \begin{pmatrix} x \\ v_x \\ y \\ v_y \end{pmatrix} \quad (50) $$

## 3) EKF STATE PREDICTION

$v_x$ and $v_y$ are defined as the velocities of the robot in its own frame. We wrote a 1st order approximation for the state transitions as following:

$$ \begin{aligned} x &\leftarrow x + \Delta t (c_{\phi} v_x - s_{\phi} v_y) \\ y &\leftarrow y + \Delta t (s_{\phi} v_x + c_{\phi} v_y) \end{aligned} \quad (51) $$

where $s_{\phi}$ and $c_{\phi}$ were calculated from $q$ as:

$$ \begin{aligned} s_{\phi} &= 2(q_w q_z + q_x q_y) \\ c_{\phi} &= 1 - 2(q_x^2 + q_y^2) \end{aligned} \quad (52) $$

Studying the kinematics of the commercial TurtleBot3, the state transition matrix is written as:

$$ F(\Delta T) = \begin{pmatrix} 1 & +c_{\phi}\Delta t & 0 & -s_{\phi}\Delta t \\ 0 & 1 & 0 & 0 \\ 0 & +s_{\phi}\Delta t & 1 & +c_{\phi}\Delta t \\ 0 & 0 & 0 & 1 \end{pmatrix} \quad (53) $$

## 4) EKF OBSERVATION MODEL

We defined the observation model as:

$$ h(X) = \begin{pmatrix} r_A(X) \\ r_B(X) \\ r_C(X) \\ h_A(X) \\ h_B(X) \\ h_C(X) \end{pmatrix} \quad (54) $$

where the functions $r_{PD}$ and $h_{PD}$ are:

$$ \begin{aligned} r_{PD}(X) &= \frac{x + c_{\phi} x_{PD} - s_{\phi} y_{PD}}{z + z_{PD}} \\ h_{PD}(X) &= \frac{(x + c_{\phi} x_{PD} - s_{\phi} y_{PD})^2 + (y + s_{\phi} x_{PD} + c_{\phi} y_{PD})^2}{(z + z_{PD})^2} \end{aligned} \quad (55) $$

## 5) EKF OBSERVATION MATRIX

The observation matrix is defined as the Jacobian matrix of the observation vector in the state vector:

$$ H^k = \frac{\partial h(X)}{\partial X} = \begin{pmatrix} \frac{\partial r_A(X)}{\partial x} & \frac{\partial r_A(X)}{\partial \dot{x}} & \frac{\partial r_A(X)}{\partial y} & \frac{\partial r_A(X)}{\partial \dot{y}} \\ \frac{\partial r_B(X)}{\partial x} & \frac{\partial r_B(X)}{\partial \dot{x}} & \frac{\partial r_B(X)}{\partial y} & \frac{\partial r_B(X)}{\partial \dot{y}} \\ \frac{\partial r_C(X)}{\partial x} & \frac{\partial r_C(X)}{\partial \dot{x}} & \frac{\partial r_C(X)}{\partial y} & \frac{\partial r_C(X)}{\partial \dot{y}} \\ \frac{\partial h_A(X)}{\partial x} & \frac{\partial h_A(X)}{\partial \dot{x}} & \frac{\partial h_A(X)}{\partial y} & \frac{\partial h_A(X)}{\partial \dot{y}} \\ \frac{\partial h_B(X)}{\partial x} & \frac{\partial h_B(X)}{\partial \dot{x}} & \frac{\partial h_B(X)}{\partial y} & \frac{\partial h_B(X)}{\partial \dot{y}} \\ \frac{\partial h_C(X)}{\partial x} & \frac{\partial h_C(X)}{\partial \dot{x}} & \frac{\partial h_C(X)}{\partial y} & \frac{\partial h_C(X)}{\partial \dot{y}} \end{pmatrix} \quad (56) $$

Computing all the partial derivatives:

$$ H^k = \begin{pmatrix} \frac{1}{z+z_A^*} & 0 & 0 & 0 \\ \frac{1}{z+z_B^*} & 0 & 0 & 0 \\ \frac{1}{z+z_C^*} & 0 & 0 & 0 \\ 2 \frac{(x+c_{\phi}x_A^*-s_{\phi}y_A^*)}{(z+z_A^*)^2} & 0 & 2 \frac{(y+s_{\phi}x_A^*-c_{\phi}y_A^*)}{(z+z_A^*)^2} & 0 \\ 2 \frac{(x+c_{\phi}x_B^*-s_{\phi}y_B^*)}{(z+z_B^*)^2} & 0 & 2 \frac{(y+s_{\phi}x_B^*-c_{\phi}y_B^*)}{(z+z_B^*)^2} & 0 \\ 2 \frac{(x+c_{\phi}x_C^*-s_{\phi}y_C^*)}{(z+z_C^*)^2} & 0 & 2 \frac{(y+s_{\phi}x_C^*-c_{\phi}y_C^*)}{(z+z_C^*)^2} & 0 \end{pmatrix} \quad (57) $$

We observed experimentally that the last three states in the measurement vector are very sensitive to small variations of height, pitch and roll. We decided to replace all the derivatives of the last three measurements in $x$, forcing the $x$ estimate to only take into account the first three states in the measurement vector:

$$ H^k = \begin{pmatrix} \frac{1}{z+z_A^*} & 0 & 0 & 0 \\ \frac{1}{z+z_B^*} & 0 & 0 & 0 \\ \frac{1}{z+z_C^*} & 0 & 0 & 0 \\ 0 & 0 & 2 \frac{(y+s_{\phi}x_A^*+c_{\phi}y_A^*)}{(z+z_A^*)^2} & 0 \\ 0 & 0 & 2 \frac{(y+s_{\phi}x_B^*+c_{\phi}y_B^*)}{(z+z_B^*)^2} & 0 \\ 0 & 0 & 2 \frac{(y+s_{\phi}x_C^*+c_{\phi}y_C^*)}{(z+z_C^*)^2} & 0 \end{pmatrix} \quad (58) $$
# VI. RESULTS

We validated the online estimation algorithms for an operating range at a height up to 3m by using a complementary filter and an extended Kalman filter.

## A. THE OPTICAL SENSOR IS EASILY EMBEDDABLE ON MOBILE ROBOTS AND CONSUMES LITTLE ENERGY

We previously proposed in [18] a minimalistic optical sensor in terms of small size and low power consumption (0.4W for the sensor and the analog demodulation board). The sensor enabled us to localize a mobile robot indoors (x and y positions) in operating range at a height of 2 meters. We asked whether we could construct an easily embeddable optical sensor for indoor localization of mobile robots (x, y and heading φ).

The mobile robot is a commercial TurtleBot3 Burger. It is equipped with the optical sensor as presented in Fig. 3. The position and heading estimations are compared to the reference using the motion capture system Vicon. We used ROS (Robot Operating System) to combined both the software (the algorithms for motion control) and the hardware components (the optical sensor equipped with the analog demodulation board, an IMU BNO055, an arduino Teensy 3.2, the TurtleBot3 motherboard).

The robot's velocity was about 0.20 m/s (the TurtleBot3 Burger's maximum velocity is around 0.22 m/s, according to the specifications). This value is coherent with the average velocity of the automatic guided vehicles used for industrial applications in warehouses (1 m/s) depending on their shape and their weight. Moreover, the InLock system is currently limited at a refresh rate of 33 Hz which is not an issue as regards of the robot's velocity. Since each LED emits alternatively during 15ms, the receiver has to wait for 30 ms to process the signal.

ROS is embedded in a raspberry pi 3 board which was mounted on the robot. We implemented using ROS the algorithms which regulated the robot's motion along a reference trajectory. We implemented the online algorithms (i) for the heading estimation by using a complementary filter, (ii) for the position estimation by using an Extended Kalman Filter detailed in Sec. V in an arduino Teensy 3.2 board (CPU 32 bit ARM Cortex-M4 at 72MHz).

## B. THE OPTICAL SENSOR REACHED AN ACCURACY INFERIOR TO 0.1RAD FOR THE HEADING ESTIMATION OF THE MOBILE ROBOT

In [18], we succeeded in estimating the positions x and y of the mobile robot, but the heading was missing. We asked whether it was necessary to revise the geometry of the beacon and of the sensor to combine two flickering infrared LEDs. Two flickering infrared LEDs are placed in the beacon as presented in Fig. 2 and only three PDs are placed in the receiver.

Figure 14 a) gives a comparison of the heading estimation (in red) to the actual heading of the robot (in black).

<figure><img src="image_15.png" /><figcaption>**FIGURE 14.** Estimation of the heading. a) Comparison of the heading estimation (in red) to the actual heading of the robot (in black) versus time. b) Histogram of the localization error in 2D using InLock sensor.</figcaption></figure>

Figure 14 b) presents the histogram of the heading error. The mean error is $\mu_{\varphi} = -0.03$ rad and the standard deviation is $\sigma_{\varphi} = 0.07$ rad. These results show the great accuracy of the heading estimation achieved using the optical sensor.

## C. THE OPTICAL SENSOR REACHED AN ACCURACY INFERIOR TO 10cm FOR THE INDOOR LOCALIZATION OF THE MOBILE ROBOT

The sensor provided an accurate estimation of the position by using an Extended Kalman Filter.

Figure 15 a) presents a comparison of the position estimation along x-axis (in red) to the actual position of the robot versus time (in black). The estimate is closed to the actual position of the robot. Figure 15 b) gives the mean error $\mu_x = -0.036$cm and the standard deviation is $\sigma_x = 1.1$cm. This result enables us to state that the optical sensor can reach an accuracy very much lower than 10cm as targeted.

Figure 16 a) presents a comparison of the position estimation along y-axis (in red) to the actual position of the robot versus time (in black). The estimate is closed to the actual position of the robot. Nevertheless, one can remark that the estimation differs from the reference at the lowest values. The estimation position along the y-axis is less accurate than the one along the x-axis because it is sensitive to the variations of height, pitch and roll. The reason is the use of only LED1 for the estimation of position. Figure 15 b) gives the mean error $\mu_y = -2.1$cm and the standard deviation is $\sigma_y = 3.2$cm. This result enables us to state that the optical sensor can reach an accuracy inferior to 10cm as targeted. Table 1
<figure><img src="image_16.png" /><figcaption>**FIGURE 15.** Estimation of position along x-axis. a) Comparison of the position estimation (in red) to the actual position of the robot (in black) versus time. b) Histogram of the localization error in 2D using In-Lock sensor.</figcaption></figure>

<figure><img src="image_17.png" /><figcaption>**FIGURE 16.** Estimation of position along y-axis. a) Comparison of the position estimation (in red) to the actual position of the robot (in black) versus time. b) Histogram of the localization error in 2D using In-Lock sensor.</figcaption></figure>

gives a comparison of the position and angular performances with a non-exhaustive list of indoor localization schemes.

<table><caption>**TABLE 1.** Non-exhaustive list of indoor localization schemes.</caption><thead><tr><th>Ref.</th><th>Method</th><th>Signal</th><th>Material</th><th>Pos. acc.</th><th>Ang. acc.</th></tr></thead><tbody><tr><td>[26]</td><td>Trilateration</td><td>RSSI-based</td><td>WiFi, Bluetooth, Zigbee, LoRaWAN</td><td>66cm (WiFi), 75cm (BLE), 90cm (Zigbee), 120cm (LoRaWAN)</td><td>none</td></tr><tr><td>[21]</td><td>Trilateration</td><td>optical, visible</td><td>visible LED, solar panel</td><td>10cm at 2m height</td><td>none</td></tr><tr><td>[23]</td><td>TDOA (Time Difference Of Arrival)</td><td>optical, visible</td><td>visible LED, avalanche PD</td><td>10cm at 2m height</td><td>none</td></tr><tr><td>[19]</td><td>AoA (Angle of Arrival)</td><td>optical, visible</td><td>visible LED, quadrant PD</td><td>none</td><td>≤0.11°</td></tr><tr><td>[20]</td><td>Trilateration & Machine Learning</td><td>optical, visible</td><td>visible LED, PD</td><td>2cm at 4m height</td><td>none</td></tr><tr><td>[18]</td><td>Trilateration</td><td>optical, infrared</td><td>Infrared LED, PD</td><td>2cm at 2m height</td><td>none</td></tr><tr><td><strong>InLock</strong></td><td>Radiometry & Trilateration</td><td>optical, infrared</td><td>Infrared LED, PD, optical diffuser</td><td>2cm at 3m height</td><td>5°</td></tr></tbody></table>

With an accuracy inferior to 10cm for the position and 5° for the heading in experiments, InLock can be considered of great interest for robotic applications.

## VII. DISCUSSION

The main conclusion of this work is that we constructed an easily embeddable, low-power optical sensor without lens that localizes a mobile robot indoors with an accuracy inferior to 10cm by using only three PDs and two infrared LEDs. Our results showed that modelling the optical sensor so that it takes into account radiometric properties and it optimizes the geometry of the beacon can enhance the accuracy of the indoor positioning system.

In order to prove the robustness of the positioning system, we programmed the robot to successively repeat the same trajectory ten times.

Figure 17 presents in 2D the actual trajectory X versus Y of the robot (in black). The estimated trajectory is plotted in red. The optical InLock sensor estimated the robot's position without drift.

As limitations, we noted in the results that the position estimation along the y-axis is less accurate than the one along the x-axis because of the sensor's sensitivity to the variations of height, pitch and roll. The sensor's sensitivity is due to the use of only one infrared LED for the estimation of the robot's position along the y-axis. In future work, we will construct a beacon composed of three infrared LED's in order to improve the accuracy of the position estimation. It will be necessary to construct analog modulation and demodulation boards.

One further limitation of InLock is the current refresh rate. Since each LED emits alternatively during 15ms, the receiver has to wait for 30 ms to process the signal. Since InLock aims at localizing faster robots in the future, the refresh rate will be
<figure><img src="image_18.png" /><figcaption>FIGURE 17. Comparison in 2D of the actual robot's trajectory (in black) to the position estimation (in red). The trajectory is repeated ten times. The robot's position is robustly estimated without drift by using the optical InLock sensor.</figcaption></figure>

increased with an improved design of the modulation board driving the LEDs.

As benefit of our work, we provided a model of the optical sensor taking into account radiometric properties.

# APPENDIX A
## DERIVATION OF VOLTAGE RATIO RELATIONSHIP

Taking $V_{PD,1}$ and $V_{PD,2}$ the voltage values perceived by photodiode PD from LEDs 1 and 2, respectively, and taking the approximations of (2) and (3) into account:

$$ \frac{V_{\text{PD},2}}{V_{\text{PD},1}} = \frac{\frac{\alpha_2}{D^2} \exp\left(-\left(\frac{\theta_{\text{PD},2}^E}{\beta}\right)^2\right) \cos(\theta_A^R)}{\frac{\alpha_1}{D^2} \exp\left(-\left(\frac{\theta_{\text{PD},1}^E}{\beta}\right)^2\right) \cos(\theta_A^R)} \\ \Rightarrow \frac{V_{\text{PD},2}}{V_{\text{PD},1}} = \frac{\alpha_2 \exp\left(-\left(\frac{\theta_{\text{PD},2}^E}{\beta}\right)^2\right)}{\alpha_1 \exp\left(-\left(\frac{\theta_{\text{PD},1}^E}{\beta}\right)^2\right)} \\ \Rightarrow \frac{V_{\text{PD},2}}{V_{\text{PD},1}} = \frac{\alpha_2}{\alpha_1} \exp\left(-\frac{(\theta_{\text{PD},2}^E)^2 - (\theta_{\text{PD},1}^E)^2}{\beta^2}\right) $$

The relation between the angles of emission and the voltage ratio for the photodiode A is given as:

$$ (\theta_{\text{PD},1}^E)^2 - (\theta_{\text{PD},2}^E)^2 = \beta^2 \ln \left( \frac{\alpha_1 V_{\text{PD},2}}{\alpha_2 V_{\text{PD},1}} \right) \quad (59) $$

The same deduction is used to find similar expressions for the other photodiodes.

# APPENDIX B
## DERIVATION OF THE SYSTEM MODEL

We develop the mathematical equations that give the position and heading of the sensor InLock. We assume that the beacon is mounted horizontally on the ceiling.

<figure><img src="image_19.png" /><figcaption>FIGURE 18. Illustration of the vector $\vec{D}_{\text{PD}}$ with respect the coordinates of the LED$_i$ and the photodiode PD in (B).</figcaption></figure>

We model the equations in {B} which is the robot's frame of reference. We also assume that the robot can only rotate around the z axis by an angle $\phi$ from the beacon's frame of reference. Defining $c_{\phi} = \cos(\phi)$ and $s_{\phi} = \sin(\phi)$, we use the heading $\phi$ to write the following rotation matrix:

$$ R_{\{B\}}^{[E]} = R = \begin{pmatrix} c_{\phi} & -s_{\phi} & 0 \\ s_{\phi} & c_{\phi} & 0 \\ 0 & 0 & 1 \end{pmatrix} \quad (60) $$

And the inverse rotation matrix is given by:

$$ R_{\{E\}}^{[B]} = R^T = \begin{pmatrix} c_{\phi} & s_{\phi} & 0 \\ -s_{\phi} & c_{\phi} & 0 \\ 0 & 0 & 1 \end{pmatrix} \quad (61) $$

Defining the distance vector $\vec{D}_{\text{PD}}$ as shown in Fig. 18, we write:

$$ \vec{D}_{\text{PD}} = -(\mathbf{L} - \mathbf{PD}) = - \begin{pmatrix} x_L - x_{\text{PD}} \\ y_L - y_{\text{PD}} \\ z_L - z_{\text{PD}} \end{pmatrix} \quad (62) $$

Noting that $R^T \vec{N}_i^{[E]}$ gives the normal vector $\vec{N}_i$ in the local frame of reference {B}, we find the angle of emission $\theta_{\text{PD},i}^E$ by calculating the dot product between the normal and the distance vectors:

$$ \cos(\theta_{\text{PD},i}^E) = - \frac{R^T \vec{N}_i^{[E]} \cdot (\mathbf{L} - \mathbf{PD})}{|\vec{D}_{\text{PD}}|} \quad (63) $$

Moreover, the sum and difference of the cosines of both angles are given by:

$$ \cos(\theta_{\text{PD},1}^E) \pm \cos(\theta_{\text{PD},2}^E) = - \frac{\left( R^T \left( \vec{N}_1^{[E]} \pm \vec{N}_2^{[E]} \right) \right) \cdot (\mathbf{L} - \mathbf{PD})}{|\vec{D}_{\text{PD}}|} $$

and, dividing the difference by the sum of the cosines, we get:

$$ \frac{\cos(\theta_{\text{PD},1}^E) - \cos(\theta_{\text{PD},2}^E)}{\cos(\theta_{\text{PD},1}^E) + \cos(\theta_{\text{PD},2}^E)} = \frac{\left( R^T \left( \vec{N}_1^{[E]} - \vec{N}_2^{[E]} \right) \right) \cdot (\mathbf{L} - \mathbf{PD})}{\left( R^T \left( \vec{N}_1^{[E]} + \vec{N}_2^{[E]} \right) \right) \cdot (\mathbf{L} - \mathbf{PD})} \quad (64) $$
$$ \frac{(R^T (\vec{N}_1^{[E]} - \vec{N}_2^{[E]})) \cdot (\mathbf{L} - \mathbf{PD})}{(R^T (\vec{N}_1^{[E]} + \vec{N}_2^{[E]})) \cdot (\mathbf{L} - \mathbf{PD})} = \frac{c_{\phi}(s_1 - s_2)(x_L - x_{PD}) - s_{\phi}(s_1 - s_2)(y_L - y_{PD}) - (c_1 - c_2)(z_L - z_{PD})}{c_{\phi}(s_1 + s_2)(x_L - x_{PD}) - s_{\phi}(s_1 + s_2)(y_L - y_{PD}) - (c_1 + c_2)(z_L - z_{PD})} \quad (68) $$

$$ \frac{\beta^2}{4} \ln \left( \frac{\alpha_1 V_{\text{PD},2}}{\alpha_2 V_{\text{PD},1}} \right) = \frac{c_{\phi}(s_1 - s_2)(x_L - x_{\text{PD}}) - s_{\phi}(s_1 - s_2)(y_L - y_{\text{PD}}) - (c_1 - c_2)(z_L - z_{\text{PD}})}{c_{\phi}(s_1 + s_2)(x_L - x_{\text{PD}}) - s_{\phi}(s_1 + s_2)(y_L - y_{\text{PD}}) - (c_1 + c_2)(z_L - z_{\text{PD}})} \quad (69) $$

Using the properties of the trigonometric functions, the left side of (64) can be written as follows:

$$ \frac{\cos(\theta_{\text{PD},1}^E) - \cos(\theta_{\text{PD},2}^E)}{\cos(\theta_{\text{PD},1}^E) + \cos(\theta_{\text{PD},2}^E)} = \tan\left(\frac{\theta_{\text{PD},1}^E + \theta_{\text{PD},2}^E}{2}\right) \tan\left(\frac{\theta_{\text{PD},1}^E - \theta_{\text{PD},2}^E}{2}\right) \quad (65) $$

For small angles, the approximation $\tan(x) \approx x$ gives:

$$ \tan\left(\frac{\theta_{\text{PD},1}^E + \theta_{\text{PD},2}^E}{2}\right) \tan\left(\frac{\theta_{\text{PD},1}^E - \theta_{\text{PD},2}^E}{2}\right) \\ \approx \left(\frac{\theta_{\text{PD},1}^E + \theta_{\text{PD},2}^E}{2}\right) \left(\frac{\theta_{\text{PD},1}^E - \theta_{\text{PD},2}^E}{2}\right) \\ = \frac{(\theta_{\text{PD},1}^E)^2 - (\theta_{\text{PD},2}^E)^2}{4} \quad (66) $$

Using the relationship found in (59) in (66), we have:

$$ \frac{\cos(\theta_{\text{PD},1}^E) - \cos(\theta_{\text{PD},2}^E)}{\cos(\theta_{\text{PD},1}^E) + \cos(\theta_{\text{PD},2}^E)} \approx \frac{\beta^2}{4} \ln \left( \frac{\alpha_1 V_{\text{PD},2}}{\alpha_2 V_{\text{PD},1}} \right) \quad (67) $$

Calculating the dot products on the right side of (64) and defining $\sin(\gamma_1) = s_1$, $\sin(\gamma_2) = s_2$, $\cos(\gamma_1) = c_1$ and $\cos(\gamma_2) = c_2$ (68), as shown at the top of this page.

Using the left side of (64) and the right side (67), we replace in (68). It leads to (69), as shown at the top of this page.

Dividing the numerator and the denominator of the right side of (69) by $z_L - z_{\text{PD}}$ and regrouping the elements, we write:

$$ \frac{\beta^2}{4} \ln \left( \frac{\alpha_1 V_{\text{PD},2}}{\alpha_2 V_{\text{PD},1}} \right) = \frac{(s_1 - s_2) \left( \frac{c_{\phi}(x_L - x_{\text{PD}}) - s_{\phi}(y_L - y_{\text{PD}})}{z_L - z_{\text{PD}}} \right) - (c_1 - c_2)}{(s_1 + s_2) \left( \frac{c_{\phi}(x_L - x_{\text{PD}}) - s_{\phi}(y_L - y_{\text{PD}})}{z_L - z_{\text{PD}}} \right) - (c_1 + c_2)} \quad (70) $$

Using the definitions of $r_{\text{PD}}$ and $\lambda_{\text{PD}}$ in (6) and (8) leads to:

$$ \lambda_{\text{PD}} = \frac{(s_1 - s_2)r_{\text{PD}} - (c_1 - c_2)}{(s_1 + s_2)r_{\text{PD}} - (c_1 + c_2)} \quad (71) $$

Isolating $r_{\text{PD}}$ shows that (71) is equivalent to (7).

## ACKNOWLEDGMENT

The authors would like to thank J. Diperi and M. Boyron for their technical support.

## REFERENCES

[1] M. B. Kjærgaard, H. Blunck, T. Godsk, T. Toftkjær, D. L. Christensen, and K. Grønbæk, "Indoor positioning using GPS revisited," in Proc. Int. Conf. Pervas. Comput. Berlin, Germany: Springer, 2010, pp. 38–56.

[2] M. Shahjalal, M. T. Hossan, M. K. Hasan, M. Z. Chowdhury, N. T. Le, and Y. M. Jang, "An implementation approach and performance analysis of image sensor based multilateral indoor localization and navigation system," Wireless Commun. Mobile Comput., vol. 2018, Oct. 2018, Art. no. 7680780.

[3] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid, and J. J. Leonard, "Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age," IEEE Trans. Robot., vol. 32, no. 6, pp. 1309–1332, Dec. 2016.

[4] G. Dudek and M. Jenkin, "Inertial sensors, GPS, and Odometry," in Springer Handbook Robotics. Berlin, Germany: Springer, 2008, pp. 477–490.

[5] L.-H. Chen, E. H.-K. Wu, M.-H. Jin, and G.-H. Chen, "Intelligent fusion of Wi-Fi and inertial sensor-based positioning systems for indoor pedestrian navigation," IEEE Sensors J., vol. 14, no. 11, pp. 4034–4042, Nov. 2014.

[6] C. Yang and H.-R. Shao, "WiFi-based indoor positioning," IEEE Commun. Mag., vol. 53, no. 3, pp. 150–157, Mar. 2015.

[7] L. Kanaris, A. Kokkinis, A. Liotta, and S. Stavrou, "Fusing Bluetooth beacon data with Wi-Fi radiomaps for improved indoor localization," Sensors, vol. 17, no. 4, p. 812, 2017.

[8] B. Shen, Q. Zheng, X. Li, and L. Xu, "A framework for mining actionable navigation patterns from in-store RFID datasets via indoor mapping," Sensors, vol. 15, no. 3, pp. 5344–5375, 2015.

[9] V. Moghtadaiee and A. G. Dempster, "Determining the best vector distance measure for use in location fingerprinting," Pervas. Mobile Comput., vol. 23, pp. 59–79, Oct. 2015.

[10] J. Luo, L. Fan, and H. Li, "Indoor positioning systems based on visible light communication: State of the art," IEEE Commun. Surveys Tuts., vol. 19, no. 4, pp. 2871–2893, 4th Quart., 2017.

[11] L. Mainetti, L. Patrono, and I. Sergi, "A survey on indoor positioning systems," in Proc. 22nd Int. Conf. Softw., Telecommun. Comput. Netw. (SoftCOM), Sep. 2014, pp. 111–120.

[12] H.-S. Kim, D.-R. Kim, S.-H. Yang, Y.-H. Son, and S.-K. Han, "An indoor visible light communication positioning system using a RF carrier allocation technique," J. Lightw. Technol., vol. 31, no. 1, pp. 134–144, Jan. 2013.

[13] Z. Zhou, "Indoor positioning algorithm using light-emitting diode visible light communications," Opt. Eng., vol. 51, no. 8, Aug. 2012, Art. no. 085009.

[14] S.-H. Yang, S.-K. Han, E.-M. Jeong, D.-R. Kim, Y.-H. Son, and H.-S. Kim, "Indoor three-dimensional location estimation based on LED visible light communication," Electron. Lett., vol. 49, no. 1, pp. 54–56, Jan. 2013.

[15] E.-M. Jeong, S.-K. Han, S.-H. Yang, and H.-S. Kim, "Tilted receiver angle error compensated indoor positioning system based on visible light communication," Electron. Lett., vol. 49, no. 14, pp. 890–892, Jul. 2013.

[16] G. B. Prince and T. D. C. Little, "A two phase hybrid RSS/AoA algorithm for indoor device localization using visible light," in Proc. IEEE Global Commun. Conf. (GLOBECOM), Dec. 2012, pp. 3347–3352.

[17] F. Ijaz, H. K. Yang, A. W. Ahmad, and C. Lee, "Indoor positioning: A review of indoor ultrasonic positioning systems," in Proc. 15th Int. Conf. Adv. Commun. Technol. (ICACT), Jan. 2013, pp. 1146–1150.

[18] T. Raharijaona, R. Mawonou, T. Nguyen, F. Colonnier, M. Boyron, J. Diperi, and S. Viollet, "Local positioning system using flickering infrared LEDs," Sensors, vol. 17, no. 11, p. 2518, 2017.

[19] S. Cincotta, C. He, A. Neild, and J. Armstrong, "High angular resolution visible light positioning using a quadrant photodiode angular diversity aperture receiver (QADA)," Opt. Express, vol. 26, no. 7, p. 9230, Apr. 2018.
[20] Y.-C. Chuang, Z.-Q. Li, C.-W. Hsu, Y. Liu, and C.-W. Chow, "Visible light communication and positioning using positioning cells and machine learning algorithms," *Opt. Express*, vol. 27, no. 11, p. 16377, May 2019.

[21] C.-W. Hsu, J.-T. Wu, H.-Y. Wang, C.-W. Chow, C.-H. Lee, M.-T. Chu, and C.-H. Yeh, "Visible light positioning and lighting based on identity positioning and RF carrier allocation technique using a solar cell receiver," *IEEE Photon. J.*, vol. 8, no. 4, pp. 1-7, Aug. 2016.

[22] D. Rodríguez-Navarro, J. L. Lázaro-Galilea, A. Gardel-Vicente, I. Bravo-Muñoz, and A. De-La-Llana-Calvo, "Indoor positioning system based on PSD sensor," in *Geographical and Fingerprinting Data to Create Systems for Indoor Positioning and Indoor/Outdoor Navigation*. New York, NY, USA: Academic, 2019, pp. 353-370.

[23] P. Du, S. Zhang, C. Chen, A. Alphones, and W.-D. Zhong, "Demonstration of a low-complexity indoor visible light positioning system using an enhanced TDOA scheme," *IEEE Photon. J.*, vol. 10, no. 4, pp. 1-10, Aug. 2018.

[24] S. Arnon, *Visible Light Communication*. Cambridge, U.K.: Cambridge Univ. Press, 2015.

[25] R. Valenti, I. Dryanovski, and J. Xiao, "Keeping a good attitude: A quaternion-based orientation filter for IMUs and MARGs," *Sensors*, vol. 15, no. 8, pp. 19302-19330, 2015. [Online]. Available: http://www.mdpi.com/1424-8220/15/8/19302

[26] S. Sadowski and P. Spachos, "RSSI-based indoor localization with the Internet of Things," *IEEE Access*, vol. 6, pp. 30149-30161, 2018.

<figure><img src="image_20.png" /></figure>

**EVANDRO BERNARDES** was born in Brazil. He received the M.Eng. degree in robotics engineering from ENSTA Bretagne, Brest, France, and the M.Sc. degree from Angers University, Angers, France, both in 2017. He is currently pursuing the Ph.D. degree in robotics with Aix-Marseille University, Marseille, France. His current research interests include robotics and origami-based structures.

<figure><img src="image_21.png" /></figure>

**STÉPHANE VIOLET** was born in Limoges, France. He received the M.Sc. degree in control engineering from the University of Bordeaux 1 and the Ph.D. degree from the National Polytechnic Institute, Grenoble, in September 2001. In 1997, he joined the Biorobotics Research Group, Marseille, France, as the Ph.D. degree Student. He is Research Director at the National Center for Scientific Research (CNRS) and currently running the Biorobotics Laboratory, CNRS, Institute of Movement Sciences (ISM), Aix-Marseille University, Marseille. His current research interests include biorobotics, oculomotor reflexes, sensorimotor control, and retinal micromovements, as well as the development of novel bioinspired visual sensors and control laws for implementing onboard autonomous flying robots.

<figure><img src="image_22.png" /></figure>

**THIBAUT RAHARIJAONA** was born in Besançon, France. He received the Engineering degree in automatic control, electronics, and computer science from the National School of Higher Education in Physics of Strasbourg, in 2001, and the Ph.D. degree from Orsay-Paris XI University, in 2004. In 2001, he joined the Department of Automatics, Ecole Supérieure d'Electricité (Supélec), Paris, France. He spent one year as an Assistant Professor with the Department of Control and Industrial Engineering, Ecole des Mines de Nantes (EMN). He is currently a Lecturer with the Biorobotics Laboratory, National Center for Scientific Research (CNRS), Institute of Movement Sciences (ISM), Aix-Marseille University. His current research interests include biomimetics, robotics, event-based sensors, robust control, and sensors for indoor localization.