Bluetooth Phased-array Aided Inertial Navigation Using Factor Graphs: Experimental Verification

Phased-array Bluetooth systems have emerged as a low-cost alternative for performing aided inertial navigation in GNSS-denied use cases such as warehouse logistics, drone landings, and autonomous docking. Basing a navigation system off of commercial-…

Authors: Glen Hjelmerud Mørkbak Sørensen, Torleiv H. Bryne, Kristoffer Gryte

Bluetooth Phased-array Aided Inertial Navigation Using Factor Graphs: Experimental Verification
Blueto oth Phased-arra y Aided Inertial Na vigation Using F actor Graphs: Exp erimen tal V erification ⋆ Glen Hjelmerud Mørkbak Sørensen ∗ T orleiv H. Bryne ∗ Kristoffer Gryte ∗ T or Arne Johansen ∗ ∗ Dep artment of Engine ering Cyb ernetics, Norwe gian University of Scienc e and T e chnolo gy (NTNU) NO-7491 T r ondheim, Norway (c orr esp onding author e-mail: glen.h.m.sor ensen@ntnu.no). Abstract: Phased-arra y Blueto oth systems ha ve emerged as a lo w-cost alternative for per- forming aided inertial navigation in GNSS-denied use cases such as w arehouse logistics, drone landings, and autonomous do c king. Basing a navigation system off of commercial-off-the-shelf comp onen ts ma y reduce the barrier of en try for phased-arra y radio na vigation systems, alb eit at the cost of significantly noisier measuremen ts and relatively short feasible range. In this paper, w e compare robust estimation strategies for a factor graph optimisation-based estimator using exp erimen tal data collected from multirotor drone flight. W e ev aluate p erformance in loss-of- GNSS scenarios when aided by Bluetooth angular measuremen ts, as well as range or barometric pressure. Keywor ds: Robust estimation, Angle of arriv al measuremen ts, F actor graph optimisation, GNSS-denied na vigation, Blueto oth low energy (BLE) 1. INTR ODUCTION Global na vigation satellite systems (GNSS) are ubiquitous in mo dern-da y aided inertial na vigation systems (INS). Ho wev er, dep ending on the use case, GNSS ma y not b e a v ailable at all (e.g., when indo ors) or the av ailability of GNSS ma y b e sev erely degraded due to natural or in tentional interference (e.g., m ultipath in a dense urban area or signal jamming or spo ofing). Consequen tly , GNSS- denied INS are necessary in these domains with examples including systems based on LiD AR (Brossard et al., 2022), vision (Lupton and Sukk arieh, 2012), and phased-array radio systems (P ARS) (Okuhara et al., 2023). Although most P ARS-based navigation systems can b e categorised as industry- or military-grade, Blueto oth L ow Ener gy (BLE) P ARS has emerged as a lo w-cost alternativ e based on commercial-off-the-shelf (COTS) comp onen ts for aiding of e.g., fixed-wing UA V fligh t (Sollie et al., 2024). By basing a navigation system on COTS BLE comp onen ts, the barrier of entry for employing such phased-arra y radio systems can b e greatly reduced, alb eit at the cost of a relativ ely short op erating range and considerably noisier measuremen ts. F urthermore, additional time sync hronisa- tion of measuremen ts is necessary as a result of an tenna switc hing and less stable oscillators (Sollie et al., 2022). Consequen tly , it is essential to employ robust estimation tec hniques to handle outliers. ⋆ The work is supported by the Researc h Council of Norw ay through the pro ject Phased-array radio systems for resilient lo calization and navigation of autonomous systems in GNSS-denied environmen ts P ARN A V (no. 338789). This work has been submitted to IF AC for p ossible publication. In Sørensen et al. (2025), we presented an estimation sc heme fusing P ARS and inertial measurements based on factor graph optimisation (FGO), comparing p erformance against the industry-standard error-state Kalman filter (ESKF) in a simulation study . Outliers w ere handled with the natural test (NT), the Hub er M-estimator, or the T ukey M-estimator (Gustafsson, 2010; Zhang, 1997) in the presence of simulated sensor faults. In this pap er, w e present the follo wing contributions building on our previous publication: • W e apply our estimation framework on experimental data from m ultirotor drone fligh t, show casing feasi- bilit y of BLE P ARS-aided INS. • W e compare the p erformance of our FGO-based es- timator on the S E (3) matrix Lie group against a b enc hmark ESKF in handov er scenarios where the drone go es from using Real-time kinematics (R TK)- GNSS aiding with p osition and compass measure- men ts to (1) BLE P ARS and R TK-GNSS range and (2) BLE P ARS and barometric pressure. R TK-based range measurements are used as an inter- mediary step since it allows us to experimentally v erify the metho ds in the face of e.g., multipath. Integration of Blueto oth LE range from c hannel sounding (Nordic Semiconductor, 2025) is planned. 2. PRELIMINARIES 2.1 Notation and c o or dinate fr ames V ectors and matrices are giv en in b old face, cursive low er- case v and uppercase letters A , respectively . R b a represen ts the rotation matrix betw een tw o co ordinate frames, i.e., from frame { a } to frame { b } . In this pap er, four frames are considered: lo cal North-East-Down navigation frame { n } , the BOD Y-frame { b } , the inertial measuremen t unit (IMU) sensor frame { s } , and the Blueto oth P ARS radio frame { r } . E.g., p n rb denotes the p osition measured in { b } relativ e to { r } , decomp osed in { n } . Estimates are expressed with a hat, e.g., ˆ x is an estimate of x . 2.2 SE(3) matrix Lie gr oup the ory The S E (3) matrix Lie group is defined as the set of p oses T ∈ R 4 × 4 S E (3) ≜  T =  R p 0 1 × 3 1      R ∈ S O (3) , p ∈ R 3  , (1) with the corresp onding Lie algebr a se ( 3 ) giv en by the set of matrices se ( 3 ) ≜  ξ ∧ =  [ ξ o ] × ξ p 0 1 × 3 0      [ ξ o ] × ∈ so ( 3 ) , ξ p ∈ R 3  , (2) where ξ • is a small, lo cal perturbation mapped to the Lie algebra using the hat op erator ∧ . The exp onential map of the Lie group is subsequen tly defined using the matrix exp onen tial: Exp ( ξ ) ≜ exp ( ξ ∧ ), and maps a matrix in the Lie algebra onto the Lie group itself. The reader is referred to Barfoot and F urgale (2014) for details. 3. BLUETOOTH LE DIRECTION FINDING The angle-of-arriv al (AoA) measurements from the Blue- to oth P ARS receiver are obtained using direction finding. This inv olves app ending a c onstant tone extension (CTE) to each advertisemen t pack et sen t from the transmitter, whic h is subsequen tly sampled by the antennae at the re- ceiv er. The raw samples are then transformed in to angular measuremen ts (azimuth Ψ and elev ation α ), similar to the ones found in Gryte et al. (2019): z r P ARS =  Ψ r α r  =   arctan2  p r rb,y , p r rb,x  arctan2  − p r rb,z , ¯ ρ    + ε , (3) where ε is zero-mean Gaussian noise and ¯ ρ is the horizontal range of the measured p osition in { r } , i.e., the Euclidean distance betw een the horizon tal comp onen ts and the origin of { r } . This is the same mo del we used in Sørensen et al. (2025), with the notable exclusion of the range, whic h is not av ailable with direction finding. The rotation matrix R n r relating { r } to { n } needs to be estimated via rough calibration based on mounting or through some form of calibration algorithm using GNSS, as done in e.g., Okuhara et al. (2023). If the origin of { r } differs from that of { n } , the lever arm l n P ARS m ust also b e accounted for, i.e, p n rb = R n r p r rb + l n P ARS . The reader is referred to Nordic Semiconductor (2020) for details on the underlying direction finding technology and to Sollie et al. (2022) for details on the algorithm used for obtaining the angular measuremen ts, as b oth are outside the scope of this pap er. 4. INER TIAL NA VIGA TION SYSTEM This section presents the comp onents of the F GO-based aided inertial na vigation system. At its core, it is the same na vigation system presented in Sørensen et al. (2025), whic h is based on the iSAM2 fixed-lag smoother from the GTSAM C++ library developed by Dellaert et al. (2022). The estimator maintains separate state- and co v ariance estimates of the p ose, v elo cit y , and IMU biases, i.e., ˆ x ≜ ( ˆ T n b , ˆ v n nb , ˆ b b ) ∈ S E (3) × R 3 × R 6 , where ˆ b b ≜ [ ˆ b b, ⊤ acc ˆ b b, ⊤ gyro ] ⊤ , The estimation itself is based on minimising ˆ T = arg min T ∥ h ( T ) − z ∥ 2 P , (4) in the p ose optimisation giv en in Ch. 6.2 in Dellaert and Kaess (2017) (and in equiv alent expressions for the velocity and bias states). T o solve the problem with respect to the lo cal p erturbation ξ , the problem is reform ulated to ˆ ξ ≈ arg min ξ ∥ h ( ˆ T ) + H ξ − z ∥ 2 P , (5) where H is the measuremen t Jacobian with resp ect to se ( 3 ) (or v elo cit y or bias) and P is the giv en estimation er- ror cov ariance. W e first present the different measurement factors, before presen ting the core na vigation system. 4.1 On-manifold IMU pr einte gr ation factor F or on-manifold IMU preintegration, w e use the mo del presen ted in F orster et al. (2015) to encode IMU mea- suremen ts betw een tw o time steps i and j in an efficien t manner: ∆ ˜ R ij = R ⊤ i R j Exp ( w o ) ∆ ˜ v ij = R ⊤ i ( v j − v i − g ∆ t ij ) + w v ∆ ˜ p ij = R ⊤ i ( p j − p i − v i ∆ t ij − 1 2 g ∆ t ij 2 ) + w p . (6) Here, ∆ ˜ • ij denotes the pr einte gr ate d me asur ement of the giv en state b et ween the t wo time steps, ∆ t ij is the time b et w een the t wo given time steps, g is the gravit y v ector, and w • is zero-mean Gaussian noise for angular velocity , acceleration, and the prein tegration itself (from top to b ottom). The implemen tation we use is GTSAM’s in ternal on-manifold IMU prein tegration factor, whic h is developed based on the ab ov e reference, as well as Lupton and Sukk arieh (2012); Carlone et al. (2014). The pose and velocity estimates are propagated in betw een measuremen t up dates using (6), whilst the bias estimates are updated whenever an optimisation is performed on the graph. 4.2 GNSS p osition and attitude factors As a reference we emplo y the GNSS F actor (GPSF actor in ternally in GTSAM) z n R TK = p n nb + ε , (7a) h R TK ( ˆ T ) = ˆ p n nb , (7b) H R TK = h 0 3 × 3 ˆ R n b i , (7c) and a factor for GNSS compass z n comp = R n b l b R TK + ε , (8a) h comp ( ˆ T ) = ˆ R n b l b R TK , (8b) H comp = h − ˆ R n b [ l b R TK ] × 0 3 × 3 i , (8c) where l b R TK is the R TK baseline b et ween the tw o GNSS an tennas and ε is zero-mean Gaussian noise. These t w o factors are used b efore the hando ver to Blueto oth P ARS- based aiding measurements to the na vigation system. 4.3 Blueto oth factors The Jacobians of the Blueto oth factors on S E (3) are based on (3). Since BLE range is not presen t in the experimental data, we utilise range derived from the relative NED p osition from R TK in the in terim z r R TK range = ρ r = ∥  ˆ R n r  ⊤ ( p n nb − l n P ARS ) + ε ∥ 2 , (9) and treat this as BLE P ARS range as presen ted in our previous w ork. Thus, the Jacobians are giv en b y H ρ = 1 ∥ ˆ p r rb ∥ 2 ( ˆ p r rb ) ⊤ H p (10a) H Ψ = 1 ˆ p 2 x + ˆ p 2 y [ − ˆ p y ˆ p x 0 ] H p (10b) H α = 1 ∥ ˆ p r rb ∥ 2 2  p x p z √ ˆ p 2 x + ˆ p 2 y p y p z √ ˆ p 2 x + ˆ p 2 y − q ˆ p 2 x + ˆ p 2 y  H p , (10c) where H p =  0 3 × 3  ˆ R n r  ⊤ ˆ R n b  relates the p osition measured by the lo cator to se ( 3 ). The Jacobians and corresp onding factors w ere derived in Sørensen et al. (2025) and we refer to that pap er for details. The difference b et w een the tw o setups is that p n rb is replaced with ˆ R n r p r rb in the deriv ations to get the Jacobians on { r } . 4.4 Bar ometric factor W e also use a barometer to pro vide vertical corrections with GTSAM internal barometric factor whic h is imple- men ted based on the Earth atmosphere model found online (NASA Glenn Research Center, 2013): z baro = p baro 101 . 29 1 / 5 . 256 · 288 . 08 − 288 . 14 − 0 . 00649 + ε (11a) ˆ z = [ 0 0 − 1 ] ˆ p n nb + ˆ b baro (11b) H baro , SE(3) =  0 1 × 3 [ 0 0 1 ] ˆ R n b  (11c) H baro , baro = 1 , (11d) where ε is zero-mean Gaussian noise and p baro is the pressure measuremen t from the barometer given in kPa. Similarly to the IMU biases, ˆ b baro m ust b e mo delled and estimated, e.g., as a random walk or a constant. 4.5 Pr op agating the estimate and de aling with outliers The INS uses the predicted state from the preintegrator b et w een measuremen t updates. Whenever an aiding mea- suremen t is av ailable, a factor represen ting the preinte- grated IMU measurement since the last update is added to the factor graph alongside the aiding measurement fac- tor(s). The fixed-lag smoother then calculates the up dated estimate and resets the prein tegrator with the updated IMU bias estimate. Eac h factor will ha ve an uncertain ty asso ciated with it, typically mo delled as zero-mean Gaussian noise. In the implementation, the noise mo del is “wrapp ed” in a robust mo del representing one of tw o M-estimators in order to mitigate outliers. Initially prop osed in Hub er (1964), M-estimators are a group of estimators suitable for mitigating the effect of outliers without necessarily rejecting them outrigh t. Here, they differ from the natural test (Gustafsson, 2010, Ch. 7.6), which is a technique that rejects a measurement outrigh t if the test statistic exceeds the giv en threshold. In this study , w e apply the T ukey and Geman-McClur e M-estimators on the exp erimen tal multirotor data. They are described b y their ob jective/cost functions Q • ( ˜ z ) Q T ( ˜ z ) = ( c 2 6  1 − [1 − ( ˜ z − ( ˜ z /c ) 2 ] 3  , for | ˜ z | ≤ c c 2 6 , otherwise, (12) Q G ( ˜ z ) = 1 2 c 2 ˜ z 2 c 2 + ˜ z 2 , (13) for T ukey (TK) and Geman-McClure (GM), respectively . c is the giv en estimator bound and ˜ z denotes the residual b et w een the measuremen t and its predicted v alue ( innova- tion ). W e note that the tw o estimators behav e differen tly – T ukey caps large outliers at a constan t max v alue if the residual exceeds the threshold, whereas Geman-McClure smo oths the curv e in a contin uous fashion, suitable for real-time use. In our previous pap er, w e considered the Hub er M-estimator alongside T ukey and found that the former’s inclusion of outliers no matter how large resulted in p oorer performancey whic h is why we omit it from this study . The reader is referred to Zhang (1997); Y ang et al. (2026) for further details on the M-estimators. 5. EXPERIMENT SETUP The m ultirotor drone flight experiment data w ere obtained from an experimental campaign at one of NTNU’s test sites in a field w est of T rondheim. The drone and ground station are both equipp ed with a Rasp erry Pi 4B, Nordic Semicondcutor nRF52833 BLE devkit, and Sen tisystems Sen tib oard v1.3. The ground station BLE devkit is con- nected to a 12-element an tenna arra y , whilst the drone is equipp ed with a STIM300 IMU, a barometric pressure sen- sor, and a uBlo x dual R TK GNSS receiv er. The drone runs a modified v ersion of the DUNE robotic middlew are (Pin to et al., 2013) and uses the Pixha wk Cub e Orange autopilot with ArduCopter v4.6.0. The drone and ground station are shown in Fig. 1. After takeoff, the woo den frame was mo ved as to not obstruct line-of-sight betw een the drone- moun ted transmitter and ground station receiv er. During the experiment, the drone follow ed a preconfigured path giv en b y the autopilot soft ware. The relativ e NED p osition computed from the measuremen ts is shown in Fig. 2. The empirical up date rates of the sensors used are approxi- mately: 2000Hz for the IMU, 1Hz for R TK, 2Hz for barom- eter, and 16.6Hz for BLE P ARS. In order to obtain range measuremen ts for P ARS, the R TK p osition measuremen ts are interpolated at the IMU frequency and measuremen ts that match BLE P ARS timestamps are used to compute range for that giv en time. The follo wing pre-pro cessing is p erformed: • IMU sp ecific force and angular v elo cit y measuremen ts are transformed from { s } to { b } using R b s , which is roughly kno wn. • The raw IQ samples from Bluetooth are conv erted in to angular measuremen ts. W e compare the FGO-based estimation scheme with smo other lag set to t lag = 2s against a benchmark ESKF in Fig. 1. Multirotor drone and ground station in the field. The Blueto oth antenna arra y (green) is mounted flat. Fig. 2. NED position computed from measurements. a hando ver scenario, where the drone switc hes from na vi- gating aided by R TK p osition and compass measurem en ts to aiding b y Blueto oth angular measuremen ts and either R TK-based range or barometric pressure meas uremen ts. In the latter case, the barometer m easuremen ts are used b efore handov er to allow the barometer bias estimate to con verge. Outside of the nominal comparison (i.e., using BLE P ARS measuremen ts without any outlier rejection or mitigation) the follo wing configurations are tested: • ESKF + Outlier rejection with the NT with k = 3 . 841 • F GO + T uk ey M-estimator with c = 3 . 6851 • F GO + Geman-McClure M-estimator with c = 1 The tuning of the different metho ds are k ept as close to one another as p ossible. The in terp olated position from R TK in { n } at the frequency of the IMU is chosen as ground truth for p osition. W e compare the roll and pitch estimates with the autopilot’s o wn estimates, whilst the y aw computed from the dual GNSS receiver is chosen as y aw reference. W e ev aluate p erformance using the root mean square error (RMSE) of eac h substate, as well as the estimation error and corresp onding 3 σ -b ounds. W e use the QUEST algorithm to obtain our initial esti- mate of the drone attitude whilst stationary , see Shuster and Oh (1981) for details. The GNSS compass is used for further stabilisation of the attitude b efore the handov er, and is not used afterwards. Fig. 3. Estimator p erformance without mitigation of BLE P ARS outliers. Note the shaded sections roughly segmen ting the data based on R TK/P ARS and the mano euvre p erformed during the giv en time interv al. 6. RESUL TS AND DISCUSSION This section presents the p erformance of the ESKF and F GO-based estimators and discusses the findings. F or comparison, the position and attitude estimates when the BLE measurements are used without any outlier mitiga- tion/rejection are shown in Fig. 3. The data is roughly segmen ted based on the manoeuvre and whether w e ha ve switc hed from R TK, and we will refer to these segmen ts as se gments throughout this section. W e highligh t tw o problematic areas – one shortly after the set hando ver p oin t in section 1 and one shortly after the 500s mark un til the end of the run (section 4). In addition, we observ e some spikes in the North-East p osition estimate in section 2, though these mostly affect the ESKF. 6.1 BLE angular me asur ements and R TK r ange The estimation error for NED p osition and attitude in the handov er case to BLE angular measurements and R TK range are shown in T ab. 1 and Fig. 4. W e observ e comparable p erformance across the b oard for p osition. The Down-position is consistently well-estimated, with the F GO configurations achieving equiv alent or marginally b etter performance in all segments. W e generally observe go od estimation of the NE-error, alb eit with a higher uncertain ty for FGO with T uk ey than the other config- urations shortly after the handov er. Given that this is one of the problematic areas, it is reasonable to assume that T ukey has set the influence of some of the measurements to T able 1. RMSE BLE angular + R TK range. N [m] E [m] D [m] ϕ [ ◦ ] θ [ ◦ ] ψ [ ◦ ] R TK NT 0.391 0.454 0.416 1.168 1.197 23.132 TK 0.344 0.312 0.238 1.251 1.193 23.076 GM 0.344 0.312 0.238 1.251 1.193 23.076 SEG. 1 NT 1.548 1.478 0.512 0.850 2.981 41.853 TK 0.917 0.696 0.287 0.808 3.130 41.791 GM 0.844 0.574 0.288 0.835 3.117 41.786 SEG. 2 NT 0.921 1.639 0.409 0.688 2.909 19.840 TK 0.821 1.641 0.411 0.705 3.040 19.235 GM 0.842 1.670 0.410 0.701 3.012 19.797 SEG. 3 NT 0.819 0.347 0.717 0.590 3.114 2.307 TK 0.883 0.401 0.652 0.492 3.038 2.356 GM 0.883 0.583 0.665 0.541 3.044 1.584 T able 2. RMSE BLE angular + Barometer. N [m] E [m] D [m] ϕ [ ◦ ] θ [ ◦ ] ψ [ ◦ ] R TK NT 0.404 0.461 0.345 1.155 1.199 23.128 TK 0.345 0.312 0.386 1.250 1.196 23.076 GM 0.345 0.312 0.386 1.250 1.196 23.076 SEG. 1 NT 2.866 1.664 0.424 0.873 3.049 41.852 TK 1.150 0.834 0.443 0.799 3.147 41.796 GM 0.880 0.587 0.436 0.831 3.118 41.787 SEG. 2 NT 1.120 1.742 1.145 0.732 2.948 19.797 TK 0.978 1.598 1.303 0.715 3.053 19.194 GM 1.000 1.630 1.270 0.708 3.020 19.797 SEG. 3 NT 1.402 2.959 0.485 0.573 3.172 2.575 TK 1.536 0.403 0.680 0.451 3.002 2.688 GM 1.476 0.572 0.610 0.524 3.031 1.400 (near)-zero, so an increased uncertain ty is to b e exp ected. Similarly , when considering the b enchmark ESKF, w e ob- serv e some spikes in the NE-estimate in segmen t 1, lik ely due to measurements b eing rejected b y the natural test, resulting in less av ailable information during filter up dates. The rapid ya w changes in segment 1 are challenging for all configurations, resulting in several spik es in estimation error wrapping around ± 180 ◦ , resulting in a large RMSE. Ho wev er, w e note that ev en before R TK is lost, the estima- tor is struggling with this mano euvre, which may indicate that e.g., the gyroscop e bias is not prop erly estimated. By incorp orating information from, e.g., magnetometer aidin g or using m ultiple P ARS antennas on the drone, this issue ma y b e mitigated. The roll angle is well-estimated, but for pitc h, we observe a bias b et ween the autopilot’s and our estimate after the hando ver. A possible explanation may b e misalignmen t in sp ecified moun ting angles of the P ARS an tenna array or a dela y/time synchronisation issue in the AoA measuremen ts or artefacts from the AoA estimation. 6.2 BLE angular me asur ements and b ar ometric pr essur e The estimation results for p osition and attitude in the hando ver case to BLE angular measurements and baro- metric pressure are shown in T ab. 2 and Fig. 5. Perfor- mance is generally comparable to the first case, but w e observ e sligh tly greater p osition uncertain ty o verall, with the ESKF affected the most. These differences can also b e seen in the relev ant RMSE v alues. Giv en that the range measuremen ts are received at the same rate as the BLE Fig. 4. Estimation error in handov er from R TK to BLE angular measurements and R TK range. 3 σ -b ounds are giv en b y dashed lines of the same colour. measuremen ts, whilst the barometer measurements are receiv ed at a rate of 2Hz, there will consequen tly b e fewer measuremen ts av ailable. F urthermore, the updates will in general not b e synchronised like those in the previous case. T ogether, this may explain some of the more notable fluctuations in the estimation error in this case. 7. CONCLUSION The results demonstrate the feasibility of our Bluetooth P ARS-aided FGO-based estimator for GNSS-denied nav- igation at short ranges. Position is w ell-estimated and erroneous data are handled b etter by the FGO-based na vigation system configurations compared to equiv alent based on the ESKF. Rapid ya w changes of the drone are c hallenging, but ma y b e mitigated b y , e.g., INS aiding from a magnetome ter or b y moun ting m ultiple P ARS an ten- nas on the drone. The tw o M-estimators tested achiev e comparable p erformance, with T ukey resulting in higher estimation uncertain ty in the first segment. F uture w ork includes incorp orating BLE range via c hannel sounding in order to make a fully-fledged Bluetooth- aided INS. Making use of a more sophisticated calibration metho d to determine the antenna orien tation is also of particular in terest, as this can make the system more suitable for use in real-time exp erimen ts do wn the line. A CKNOWLEDGEMENTS The authors would like to thank colleagues at NTNU: Johan Nicolas Suarez at the Department of Electronic Fig. 5. Estimation error in handov er from R TK to BLE angular measurements and barometric pressure. 3 σ - b ounds are giv en b y dashed lines of the same colour. Systems and P ˚ al Kv aløy and Morten Einarsve at the De- partmen t of Engineering Cyb ernetics for their help in con- ducting field exp erimen ts with the multirotor drone. Johan Nicolas Suarez also participated in the post-pro cessing and analysis w ork inv olved in conv erting the raw BLE P ARS measuremen ts in to angular measuremen ts. REFERENCES Barfo ot, T.D. and F urgale, P .T. (2014). Asso ciating Uncertain ty With Three-Dimensional Poses for Use in Estimation Problems. IEEE T r ans. R ob ot. , 30(3), 679– 693. Brossard, M., Barrau, A., Chauc hat, P ., and Bonnab el, S. (2022). Asso ciating Uncertaint y to Extended P oses for on Lie Group IMU Prein tegration With Rotating Earth. IEEE T r ans. R ob ot. , 38(2), 998–1015. Carlone, L., Kira, Z., Beall, C., Indelman, V., and Dellaert, F. (2014). Eliminating conditionally independent sets in factor graphs: A unifying p ersp ectiv e based on smart factors. In 2014 IEEE Int. Conf. on R ob otics and A utomation (ICRA) , 4290–4297. ISSN: 1050-4729. Dellaert, F. and Kaess, M. (2017). F actor Graphs for Rob ot P erception. F oundations and T r ends in R ob otics , 6(1-2), 1–139. Dellaert, F. et al. (2022). borglab/gtsam. URL https:// github.com/borglab/gtsam) . F orster, C., Carlone, L., Dellaert, F., and Scaramuzza, D. (2015). IMU Prein tegration on Manifold for Efficien t Visual-Inertial Maxim um-a-Posteriori Estimation. In R ob otics: Scienc e and Systems XI . Robotics: Science and Systems F oundation. Gryte, K., Bryne, T.H., Albrektsen, S.M., and Johansen, T.A. (2019). Field T est Results of GNSS-denied Inertial Na vigation aided by Phased-array Radio Systems for UA Vs. In 2019 Int. Conf. on Unmanne d Air cr aft Sys- tems (ICUAS) , 1398–1406. IEEE, A tlanta, GA, USA. Gustafsson, F. (2010). Statistic al sensor fusion . Studentlit- teratur, 3rd edition. Hub er, P .J. (1964). Robust Estimation of a Lo cation P arameter. The Annals of Mathematic al Statistics , 35(1), 73–101. Publisher: Institute of Mathematical Statistics. Lupton, T. and Sukk arieh, S. (2012). Visual-Inertial- Aided Navigation for High-Dynamic Motion in Built En vironments Without Initial Conditions. IEEE T r ans. R ob ot. , 28(1), 61–76. NASA Glenn Research Cen ter (2013). Earth A tmosphere Mo del - Metric Units. URL https://www.grc.nasa. gov/www/k- 12/airplane/atmosmet.html . Nordic Semiconductor (2020). Blueto oth Direction Find- ing Whitepap er. URL https://docs.nordicsemi. com/bundle/nwp_036/page/WP/nwp_036/intro.html . Nordic Semiconductor (2025). Blueto oth ® Channel Sounding. URL https://www.bluetooth.com/ learn- about- bluetooth/feature- enhancements/ channel- sounding/ . Okuhara, M., Bryne, T.H., Gryte, K., and Johansen, T.A. (2023). Phased Array Radio Navigation System on UA Vs: Real-Time Implementation of In-fligh t Calibra- tion. IF AC-Pap ersOnLine , 56(2), 1152–1159. Pin to, J., Dias, P .S., Martins, R., F ortuna, J., Marques, E., and Sousa, J. (2013). The LSTS to olc hain for net- w orked v ehicle systems. In 2013 MTS/IEEE OCEANS - Ber gen , 1–9. IEEE, Bergen. Sh uster, M.D. and Oh, S.D. (1981). Three-axis attitude determination from vector observ ations. Journal of Guidanc e and Contr ol , 4(1), 70–77. Publisher: American Institute of Aeronautics and Astronautics. Sollie, M.L., Gryte, K., Bryne, T.H., and Johansen, T.A. (2024). Automatic Reco very of Fixed-Wing Unmanned Aerial V ehicle Using Blueto oth Angle-of-Arriv al Na viga- tion. In 2024 Int. Conf. on Unmanne d Air cr aft Systems (ICUAS) , 390–397. ISSN: 2575-7296. Sollie, M.L., Gryte, K., Bryne, T.H., and Johansen, T.A. (2022). Outdo or Na vigation Using Bluetooth Angle-of- Arriv al Measuremen ts. IEEE A c c ess , 10, 88012–88033. Sørensen, G.H.M., Bryne, T.H., Gryte, K., Synnev ˚ ag, T., and Johansen, T.A. (2025). Robust phased-array radio system aided inertial na vigation using factor graph optimisation. In 2025 Eur op e an Contr ol Conf. (ECC) , 1787–1794. ISSN: 2996-8895. Y ang, H., Mangelson, J., Chang, Y., Shi, J., and Carlone, L. (2026). Robustness to incorrect data asso ciation and outliers. In L. Carlone, A. Kim, T. Barfoot, D. Cremers, and F. Dellaert (eds.), SLAM Handb o ok. F r om L o c alization and Mapping to Sp atial Intel ligenc e . Cam bridge Univ ersity Press. Zhang, Z. (1997). Parameter Estimation T echniques: A T utorial with Application to Conic Fitting. Image and Vision Computing , 15(1), 59–76.

Original Paper

Loading high-quality paper...

Comments & Academic Discussion

Loading comments...

Leave a Comment