6-DoF Optical Positioning System with RSS-AoA Measurements and Harmony Search Refinement

Abstract

Indoor localization is an ever-growing area of research, driven by applications such as autonomous robot navigation, cultural and commercial venue guidance, logistics, emergency response, and health monitoring in older adults. This paper presents a two-stage hybrid method for the full six degrees-of-freedom (6-DoF) pose estimation in optical indoor localization systems using a quadrant photodiode (QP) sensor. First, a geometric Angle-of-Arrival (AoA) algorithm estimates the receiver's 3D position and yaw rotation using normalized energy ratios. Then, a Harmony Search (HS) algorithm expands this partial estimation to full 6-DoF by also estimating the roll and pitch angles, while simultaneously refining all pose parameters through reprojection error minimization. The method is validated through simulations over a 2 × 2 × 1 m 3 volume. Results show that the proposed approach significantly outperforms the AoA-only baseline. In scenarios with diferent orientations, the 90-th percentile position error drops from 0.61 m to 0.38 m in the and coordinates, and from 0.24 m to 0.20 m in the one. Orientation errors in roll, pitch, and yaw are below 6.56∘ , 6.48∘ , and 1.03∘ , respectively.

Keywords

• eol>Optical Positioning System
• Harmony Search
• Angle of Arrival
• Quadrant Photodiode

1. Introduction

Indoor localization is an ever-growing area of research, driven by the need to locate mobile objects or people in a variety of contexts, such as autonomous robot navigation, localization in museums or shopping centres, logistics and emergency management, and physical activity monitoring in older adults to promote healthy ageing [ 1, 2 ]. To address these needs, several technologies, such as radio frequency, ultrasound, and optical signals, can be used. The choice of a specific approach usually depends on factors like accuracy, coverage, infrastructure deployment, or cost [ 3 ]. Among these technologies, optical systems stand out due to their ability to achieve centimetric accuracies at a reduced cost.

Optical positioning systems use CCD sensors or photodiodes (PD) as receivers, with quadrant photodiodes (QP) among the latter [ 4 ]. The most typically used positioning techniques in optical systems are triangulation, trilateration and multilateration, with measurements of received signal strength (RSS) [ 5 ] or angle of arrival (AoA) [ 6 ]. Among these, AoA-based techniques provide a compromise between complexity and accuracy, especially in systems with constrained infrastructure and computation. By analysing the distribution of received light energy across segments of a sensor such as a QP, it is possible to infer incident angles of incoming rays and estimate spatial location using geometric models [ 7 ].

However, recovering the full six degrees-of-freedom (6-DoF) pose remains a challenge. A common approach is Perspective-n-Point (PnP) algorithms, which estimate the 6-DoF pose of a camera by solving a geometric optimization problem based on a set of known 3D points and their corresponding 2D projections [ 8 ]. These methods ofer various trade-ofs between computational eficiency and robustness to noise. However, their performance tends to degrade when a minimal set of 2D observations is available, which is the case in optical systems using QP sensor outputs rather than a full image data. Recently, machine learning techniques have been explored for 6-DoF pose estimation [ 9 ]. These methods can deliver strong accuracy and generalization performance, but they typically require large labelled datasets for training. Acquiring such large datasets [ 10 ] covering a wide range of poses, sensor configurations and environmental conditions is time-consuming, labour-intensive and often impractical.

To enhance pose estimation under these limitations, this work proposes a hybrid approach for full pose estimation in optical positioning systems that combines geometric and optimization-based methods without requiring large datasets or dense image information. First, the AoA algorithm estimates the receiver’s 3D position (, , ) from normalized energy ratios (, ) measured by the QP sensor. Then, the full 6-DoF pose (, , , , , ) is obtained via Harmony Search (HS) by minimizing the error between modeled and observed projections on the sensor plane. The rest of the manuscript is structured as follows: Section II outlines the proposed system, including the sensing configuration and the main processing stages; Section III details the optimization algorithm used for pose estimation; Section IV presents the simulated results; and, finally, Section V summarizes the main conclusions of this work.

2. General Overview of the Proposal

The proposed optical positioning system is based on a set of four fixed LED emitters placed at known locations on the ceiling and a mobile QP receiver. Fig. 1 shows a general overview of the proposed architecture. The system can be modeled as a pinhole configuration, where the light emitted by the LEDs passes through the aperture of the QP receiver and impacts on the photodiode surface. We consider three independent coordinate systems: 1) the global coordinate system is defined by the cartesian axes (, , ), with its origin located at the corner of the room; 2) the camera coordinate system, defined by (, , ), has its origin at the center of the square aperture in the receiver; and 3) the local 2D coordinate system of the photoreceiver is given by (, ), with its origin placed at the center of the QP. The complete pose of the receiver in the global coordinate system is denoted as (, , , , , ).

Each LED emitter = {1, 2, 3, 4} transmits a distinct Binary Phase-Shift Keying (BPSK) modulated signal based on a Loosely Synchronous (LS) code, selected for its robustness against noise and multipath interference [ 11 ]. A Code-Division Multiple Access (CDMA) technique is employed, where each LED transmits a unique code . The receiver identifies these codes by applying the corresponding matched iflters and thereby determines its own location.

The receiver consists of a quadrant photodiode sensor (QP), specifically the QP50-6-TO8 model [ 12 ], with a mechanical aperture of side length positioned above the QP surface at a distance ℎ. This aperture restricts the angular field of view of the sensor and defines the projection geometry of the incoming light, ensuring that the incident signal passes through the aperture and irradiates part of the sensitive region of the QP. The illuminated area depends on the angle of incidence of the incoming light. The resulting distribution of light energy among the four quadrants produces a set of four electrical currents, one per quadrant. These currents are first conditioned using analog circuitry [ 13 ] and then digitized by a System-on-Chip (SoC) platform using the ZC706 evaluation board [ 14 ], which integrates a Zynq-7000 XC7Z045 Field Programmable Gate Array (FPGA) device. The results from the hardware processing are later sent via a serial link to a computer, where they are further processed [ 15 ].

During the conditioning stage, the signals from the four quadrants are combined to compute three key signals: the total energy across all quadrants, ; the left-right diferential signal, ; and the bottom-top diferential signal, . These signals are then processed through digital correlation blocks, allowing the system to isolate the contribution of each emitter . The resulting correlation signals are used to determine their peak values ,, , and , for signals , and , respectively. These peak values , and , are then normalized with respect to ,, yielding the ratios , = ( ,, ) and , = ,

, for every LED emitter . These ratios represent the relative energy between the bottom and top quadrants (,) and the left and right quadrants (,) of the QP sensor, with respect to the total received energy. The ratios , and , are sensitive to the geometry of the light path, the relative orientation of the receiver, and the position of each LED emitter. In a first approximation, considering that the image coordinate system is aligned with the global coordinate system, they are used to estimate the image point (, ) for each transmitter (1). ︂[ ]︂ = − 2 ·

︂[ + · ]︂ · − · + + ︂[ ]︂ where is the aperture misalignment, (, ) is the central point of the aperture projected on the QP sensor, ideally (0,0), is the aperture side length, and the ratio between the expected focal length ℎ, and the actual focal length ℎ′ is defined as = ℎℎ′ [ 16 ].

The image points in (1) are also geometrically related with the transmitter’s 3D coordinates as follows: where (, , ) are the transmitter’s position in the camera coordinates, obtained as: = ℎ ·

= ℎ · ⎡⎤ ⎡⎤ ⎣⎦ = [︀ R|t]︀ · ⎣⎦

where [︀ R|t]︀ concatenates the rotation matrix R = R(, , ), defined as R = Rz( ) · Ry( ) · Rx( ), and the translation matrix t = (x, y, z)⊤, defined by the receiver’s coordinates (, , ), and (, , ) is the transmitter’s position in the global coordinate system.

3. Pose Estimation

This section describes the two-stage methodology developed to estimate the 6-DoF pose of the receiver, defined by its position (, , ) and orientation angles (, , ). First, an initial position is computed using an analytical method based on angles of arrival; then, this estimate is refined and extended to all orientation angles by using a global optimization strategy based on the Harmony Search algorithm. (1) (2) (3)

3.1. Stage 1: AoA-based Initial Position Estimation

Firstly, we suppose that the plane of the sensor is parallel to the plane containing the four emitters. In this case, the pose is determined with (0,0,0, 0). After the coordinates of the image points (, ) for each emitter are estimated (1), the algorithm continues to determine the rotation 0 of the receiver around the axis by means of trigonometric equations [ 17 ]. The rotation angle 0 is used to rotate the image points − 0 degrees to obtain the non-rotated image points (′, ′). This is a necessary step since the positioning algorithm requires the receiver to be aligned with the reference frame. Then, the positioning method proceeds to estimate the receiver’s coordinates (0, 0, 0) by using a Least Squares Estimator (LSE) and some trigonometric considerations, detailed in (4) and (5) [ 7 ]. Note that coordinate 0 is obtained as the weighted average considering the distances between the estimated receiver’s position (0, 0) and the projection of each transmitter in the plane where the receiver is placed. where

(0, 0) = (A · A)− 1 · A · b ⎡− ′,1 A = ⎢⎢− ′,2 ⎣− ′,3 − ′,4 ′,1⎤ ′,2⎥⎥ , ′,3⎦ ′,4 = , − ℎ · 1 +

⎡,1 · ′,1 − b = ⎢⎢⎣,,32 ·· ′′,,32 −− ,1 · ′,1⎤ ,2 · ′,2⎥ ,3 · ′,3⎦

⎥ ,4 · ′,4 −

,4 · ′,4 √︃ (, − )2 + (, − )2 )︃

′2, + ′2, ︃( 4 0 =

1 ∑︀4=1 2 =1

∑︁ 2 ·

3.2. Stage 2: Harmony Search for Pose Optimization

In the second stage, the full pose of the receiver (, , , , , ) is estimated using a Harmony Search (HS) algorithm [ 18 ]. The HS begins by initializing the Harmony Memory (HM) with HMS (Harmony Memory Size) candidate solutions. One of them is set to be the initial estimate of the pose vector x0 = [0, 0, 0, 0, 0, 0], where t0 = [0, 0, 0] and 0 are obtained from the AoA method, while 0 and 0 are set to 0∘ . The rest of the HMS are randomly sampled within predefined upper and lower bounds. The objective function to be minimized is the median reprojection error of the image points: (x) = median=1,...,4 ⃦⃦ ⃦ ,

expected − ⃦⃦ [︃ex,pected]︃ [︂ m,e asured]︂ ⃦⃦ m,easured ⃦⃦ where the expected image points are computed by using the pinhole projection model described in (2). The HS algorithm adapts the pitch adjustment rate () and the bandwidth parameter () dynamically as defined in (7). ⃦ (4) (5) (6) (7) (8) generated according to (8) [ 19 ].

At each iteration , with ∈ {1, . . . , max}, a new candidate solution xnew = (1, 2, . . . , ) is () = min + ( max − min) · max () = 0 · ︂( 1

)︂ − max new, =

HM, ⎧⎪⎨HM + () · , if 1 < and 2 < ()

if 1 < and 2 ≥ () ⎪ ⎩ ( , ),

if 1 ≥ where HM is the -th component of a randomly selected vector coming from the current harmony memory, ∼ (0, 1) is a standard Gaussian noise, and ( , ) are the lower and upper bounds, respectively. The variables 1, 2 ∼ (0, 1) are random values used to decide whether the component is drawn from memory or the pitch adjustment is applied. The parameter Harmony Memory Considering Rate, ∈ [ 0, 1 ], controls the probability of choosing values from the harmony memory, instead of generating a new value randomly.

Once a new candidate xnew is generated, its cost is calculated using (6). If it improves the worst memory solution, it replaces it. The iterative process continues until either a maximum number of iterations max is reached, or the standard deviation of the cost values across the memory, HM, falls below a predefined threshold , (HM) < .

Finally, once the HS algorithm converges, a gradient-based local non-linear optimization algorithm is applied to refine all six pose parameters using the same projection-based cost function. This step uses HS to locate a promising region and then fine-tunes the solution to improve accuracy beyond the HS stopping threshold. A summary of the complete algorithm is presented in Algorithm 1. Algorithm 1 AoA + Harmony Search + Gradient-Based Refinement

4. Results

4.1. Experimental Setup

The simulated tests have been carried out in a 2 × 2 × 1 m3 volume, which was divided into a grid of points spaced every 20 cm along the plane and every 50 cm along the axis. Each point in the grid is evaluated over 10 iterations, and all points lie within the coverage area of at least three transmitters. To simulate a realistic scenario, zero-mean Gaussian white noise with a standard deviation = 0.001 was added to the ratios and [ 10 ].

The coordinates of the four transmitters are (4.55, 2.98, 3.39) m, (3.23, 2.98, 3.39) m, (3.23, 4.06, 3.39) m, and (4.54, 4.06, 3.39) m for transmitters 1 to 4, respectively. The system was calibrated using the experimentally obtained parameters = 1, = 0.05 mm, = 0.02 mm, = 0∘ , = 2.65 mm, and ℎap = 2.61 mm. The parameters used for the Harmony Search algorithm are: = 200, = 0.7, = 0.1, = 0.5, 0 = 0.01, = 2000, = 10− 4 and [lb, ub] = [[0, 0, 0] ± 1 , [ 0, 0, 0] ± 10∘ ].

1 0.9 0.8 0.7 0.6 ()x0.5 F 0.4 0.3 0.2 0.1 0 ) (° 0 10 8 6 4 2 -2 -4 -6 -8 -10 x AoA+HS y AoA+HS z AoA+HS x AoA y AoA z AoA

Error (m) 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0

4.2. Results

The system was first validated by only applying rotations around the axis ( ) while keeping =

= 0∘ . The angle was varied from 0∘ to 350∘ in steps of 10∘ . The Cumulative Distribution Function (CDF) of the positioning errors for coordinates , , and rotation angles , , is shown in Fig. 2. Results are presented both for the initial AoA-based estimation and after refinement with the pose estimation algorithm. For the initial AoA estimation, the 90-th percentile (90) of the absolute error is 3.6 mm in and , and 1.9 mm in . After applying the pose estimation algorithm, the error in was reduced to 0.05 mm, while the errors in and remained unchanged. Regarding the rotation angles, 90% of the cases yielded errors below 0.003∘ in , , and .

In practical applications such as mobile robots, drones, or wearable systems, the orientation of the receiver is rarely perfectly aligned with the reference frame. In particular, small tilts around the and axes are common and often unavoidable due to movement, mounting constraints, or vibrations. To evaluate the robustness of the proposed method under these more realistic conditions, the analysis was extended to include rotations in , ∈ [− 10∘ , 10∘ ] in steps of 2∘ , combined with the grid for both the initial AoA estimation and for the proposed pose estimation algorithm. ∈ {0∘ , 10∘ , . . . , 350∘ }. Figure 3 shows the heatmaps of the mean 3D Euclidean positioning error over ()x0.5 F 1 0.9 0.8 0.7 0.6 0.4 0.3 0.2 0.1 0 ) (° 0 10 8 6 4 2 -2 -4 -6 -8 -10

Error (m) 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 coordinates , , ; (b) rotation angles , , .

The results confirm that the use of the AoA algorithm alone is highly sensitive to orientation changes. In contrast, the proposed pose estimation algorithm significantly improves accuracy, as detailed in the CDFs of Fig. 4. Specifically, the 90 error for the initial AoA estimation is 0.61 m in and , and 0.24 m in . After the pose estimation algorithm, these errors are reduced to 0.38 m in and , and 0.20 m in . The 90 error for the rotation angles , and is below 6.56∘ , 6.48∘ , and 1.03∘ , respectively.

To further validate the proposal, a trajectory was simulated in Fig. 5a. The trajectory consists of 50 points distributed within a 2.5 × 2.5 × 1 m3 volume with orientations in = = ± 10∘ and = ± 80∘ . The ground-truth trajectory is represented with a black line, the estimated positions using the AoA algorithm are shown with a red line, and the results from the proposed pose estimation method (AoA+HS) are shown as a blue line. The obtained results for angles , , and are presented in Fig. 5b. 5 Error (º) 0.3 0.4 Error (m) x AoA+HS y AoA+HS z AoA+HS x AoA y AoA z AoA 0.5 0.6 0.7

Additionally, the absolute errors in position and orientation for the trajectory are presented in the CDFs of Fig. 6. In the 90% of cases, the absolute error for the AoA-only estimation are 0.34 m, 0.37 m, and 0.22 m in , and , respectively. After applying the AoA+HS approach, the 90 errors are reduced to 0.17 m, 0.15 m, and 0.14 m in , and , respectively. Similarly, the 90 is 6∘ in , and 0.6∘ in .

The algorithms are implemented in MATLAB® on an Intel i7-8750H CPU (2.20 GHz, 8 GB RAM). The average processing time per full pose estimation is 2.6 s for the Harmony Search refinement step with 0.86 ms for the initial AoA estimation.

5

Error (º) 1 2 3 4 6 7 8

4.3. Discussion

The proposed AoA+HS approach significantly improves pose estimation accuracy compared to the initial AoA estimation, particularly in scenarios involving rotations along multiple axes. Specifically, the hybrid method reduces the 90 positioning error by 38% on both the and coordinates, and by 16% on , while keeping the 90 below 6.6∘ for all rotation angles. The trajectory simulation further support these findings, with the 90 error dropped by 50%, 60% and 36% for , , and , respectively, demonstrating the algorithm’s robustness to changes in receiver rotations. This robustness is essential in real-world applications, where sensor alignment cannot be guaranteed, such as mobile robotics or wearable devices. Although the HS refinement step increases computational time compared to the initial AoA estimation, it remains feasible for applications tolerant to moderate latency or ofline processing. Future work will focus on reducing runtime through GPU acceleration and adaptive stopping criteria.

5. Conclusions

This work presents a hybrid approach for 6-DoF pose estimation in optical indoor localization systems based on a quadrant photodiode receiver. The proposal combines an initial geometric AoA-based estimation with a global Harmony Search optimization followed by local gradient-based refinement. While the AoA stage provides a partial estimation of the pose—specifically, the 3D position and the yaw angle—, the Harmony Search stage extends this to full 6-DoF by improving the initial estimate and also estimating the roll and pitch angles. Moreover, the optimization process significantly improves the overall accuracy. Simulation results demonstrate that the 90-th percentile position error is reduced from 0.61 m to 0.38 m after the HS optimization in and , and from 0.24 m to 0.20 m in in a volume of 2 × 2 × 1 m3. Orientation errors in , , and are below 6.6∘ in 90% of the test cases.

Acknowledgments

This work was supported by MCIN/AEI/10.13039/501100011033 (refs. PID2021-122642OB-C41, RED2022134355-T, PID2023-146254OB-C43), and the Community of Madrid (ref. CM/DEMG/2024-007).

Declaration on Generative AI

During the preparation of this work, the authors used OpenAI’s ChatGPT (GPT-4) in order to check grammar and spelling. After using this tool, the authors reviewed and edited the content as needed and take full responsibility for the publication’s content.

References

[1] Z. Zhu , Y. Yang , M. Chen , C. Guo , J. Cheng, S. Cui, A Survey on Indoor Visible Light Positioning Systems: Fundamentals , Applications, and Challenges, IEEE Communications Surveys & Tutorials 27 ( 2025 ) 1656 - 1686 . doi: 10 .1109/COMST. 2024 . 3471950 .

[2] A. Chan , J. Cai , L. Qian , B. Coutts , S. Phan , G. Gregson, M. Lipsett , A. M. Ríos Rincón, In-Home Positioning for Remote Home Health Monitoring in Older Adults: Systematic Review, JMIR Aging 7 ( 2024 ). doi: 10 .2196/57320.

[3] J. Kunhoth , A. Karkar , S. Al-Maadeed , A. Al-Ali , Indoor positioning and wayfinding systems: a survey, Human-centric Comp. and Inf . Sciences 10 ( 2020 ) 18 . doi: 10 .1186/s13673-020-00222-0.

[4] S. Cincotta , C. He , A. Neild , J. Armstrong , Indoor Visible Light Positioning: Overcoming the Practical Limitations of the Quadrant Angular Diversity Aperture Receiver (QADA) by Using the Two- Stage QADA-Plus Receiver , Sensors 19 ( 2019 ). doi: 10 .3390/s19040956.

[5] G. Lee , J. K. Park , J. T. Kim, Multitarget Passive Indoor Visible Light Positioning Method Based on Specular Reflection, IEEE Sensors Jo . 23 ( 2023 ) 31196 - 31208 . doi: 10 .1109/JSEN. 2023 . 3328646 .

[6] C.-Y. Hong , Y. -C. Wu , Y. Liu , C.-W. Chow , C.-H. Yeh , K.-L. Hsu , D.-C. Lin , X.-L. Liao , K.-H. Lin , Y.-Y. Chen , Angle-of-Arrival (AOA) Visible Light Positioning (VLP) System Using Solar Cells With Third-Order Regression and Ridge Regression Algorithms , IEEE Photonics Journal 12 ( 2020 ) 1 - 5 .

[7] E. Aparicio-Esteve , Á. Hernández, J. Ureña , J. M. Villadangos , Visible Light Positioning System Based on a Quadrant Photodiode and Encoding Techniques, IEEE Transactions on Instrumentation and Measurement 69 ( 2020 ) 5589 - 5603 . doi: 10 .1109/TIM. 2019 . 2962563 .

[8] J. Yang , L. Feng , P. He , W. Chen , A. Yang , J. Lu , A High-Precision Indoor Positioning Scheme Based on Feature Code and Lighting Device, IEEE Systems Journal 16 ( 2022 ) 6717 - 6726 . doi: 10 .1109/ JSYST. 2022 . 3176678 .

[9] D. Su , X. Wang , S. Liu , W. Ding, Four-dimensional indoor visible light positioning: A deep-learningbased perspective , Journal of the Franklin Institute 360 ( 2023 ) 4071 - 4090 .

[10] E. Aparicio-Esteve , J. Ureña , Á. Hernández, D. Moltó , A Dataset for Assessing and Optimizing Quadrant Photodiode-based Visible Light Positioning Systems , Scientific Data 12 ( 2025 ). doi: 10. 1038/s41597-025-04929-6.

[11] E. Aparicio-Esteve , Á. Hernández, J. Ureña , J. M. Villadangos , S. Lluva , M. C. Pérez-Rubio , Detecting Relative Amplitude of IR Signals with Active Sensors and Its Application to a Positioning System , Applied Sciences 10 ( 2020 ). doi: 10 .3390/app10186412.

[12] Sensor Quadrant Series Data Sheet , Pacific Silicon Sensor., 2010 . Product Spec., CA 91362 USA.

[13] J. Ureña , J. M. Villadangos , Á. Hernández, D. Moltó , E. Aparicio , J. C. García , Conditioning Stage of a QADA Receiver for a Large Range IR Positioning System , in: 2024 IEEE Int. Instrum. and Meas. Technology Conf. (I2MTC) , 2024 , pp. 1 - 6 . doi: 10 .1109/I2MTC60896. 2024 . 10561150 .

[14] AMD, ZC706 Evaluation Board User Guide , 1 .8 ed., AMD , 2019 . https://www.xilinx.com.

[15] D. Moltó , E. Aparicio-Esteve, Á. Hernández, J. Ureña , Architecture for implementing an infrared local positioning system , Measurement 250 ( 2025 ). doi: 10 .1016/j.measurement. 2025 . 117116 .

[16] E. Aparicio-Esteve , Á. Hernández, J. Ureña , Design, Calibration, and Evaluation of a LongRange 3-D Infrared Positioning System Based on Encoding Techniques , IEEE Transactions on Instrumentation and Measurement 70 ( 2021 ) 1 - 13 . doi: 10 .1109/TIM. 2021 . 3089223 .

[17] E. Aparicio-Esteve , Á. Hernández, J. Ureña , J. M. Villadangos , F. Ciudad , Estimation of the Polar Angle in a 3D Infrared Indoor Positioning System based on a QADA receiver , in: 2019 Inter. Conf. on Indoor Positioning and Indoor Navigat . (IPIN) , 2019 , pp. 1 - 8 . doi: 10 .1109/IPIN. 2019 . 8911775 .

[18] Z. W. Geem , J. H. Kim , G. Loganathan , A New Heuristic Optimization Algorithm: Harmony Search , SIMULATION 76 ( 2001 ) 60 - 68 . doi: 10 .1177/003754970107600201.

[19] J. Wang , H. Ouyang , C. Zhang , S. Li , J. Xiang , A novel intelligent global harmony search algorithm based on improved search stability strategy , Scientific Reports 13 ( 2023 ).