Session B2a: Advancements in Navigation Algorithms

Research on Bluetooth/MIMU Integrated Navigation Algorithm Based on Factor Graph Optimization
Sheng Menggang , Lu Jiajia , Cai Xiaowen , Chen Yangzhuo, Xiangtan University
Location: Beacon B

With the continuous improvement of human demand for indoor location services, various indoor navigation and positioning methods such as pedestrian dead reckoning and low-power Bluetooth are widely used in indoor positioning due to their advantages of low cost, low power consumption and easy portability. However, the positioning accuracy of the PDR algorithm is affected by the error of the micro inertial measurement unit, and the cumulative error of its positioning is large, and the Bluetooth positioning bounce is serious. Therefore, taking Bluetooth and micro-inertial navigation as the research object, the advantages of Bluetooth positioning technology and pedestrian dead reckoning technology are complementary, and the research on Bluetooth/micro-inertial navigation high-precision indoor fusion positioning technology based on factor graph optimization is carried out. By analyzing the error characteristics of the Bluetooth module and the inertial sensor in the indoor environment, an integrated navigation and positioning method based on the combination of AOA ranging and PDR was designed, and the experimental results show that compared with the single navigation mode, the positioning accuracy of the BLE/PDR information fusion algorithm based on factor graph optimization is higher, and the positioning stability of the model is better.
BLE/PDR factor diagram optimization model construction.
In the BLE/PDR integrated navigation system, the fusion algorithm is the key to reduce the positioning estimation error and improve the performance, and the fusion algorithm is the core problem of the integrated navigation. In the integrated navigation system, the carrier is equipped with various types of sensors, and the observation information of each sensor is fused and processed in the navigation center to estimate the system state. Therefore, the optimal estimation of the system state can be described as the Maximum a Posterior (MAP) estimation of the posterior probability of all states of the system with time. The maximum posterior estimation refers to finding the state estimation that maximizes the posterior probability by combining prior knowledge and measurement information under the condition of a given quantitative measurement. The mathematical explanation of the optimal solution of navigation is to seek the state estimation under the maximum posterior probability, and the maximum posterior estimation introduces prior knowledge on the basis of the maximum likelihood estimation, both of which are based on the optimization problem to obtain the estimation of the state of the system, and there is no essential difference between the two optimal estimations in the combined navigation.
In the Bluetooth/micro-inertial navigation integrated navigation system, the observation information of Bluetooth and inertial sensors is independent of each other, and the observation information of the same sensor is also independent at different times, so the global likelihood function can be expressed as the product of the likelihood function measured independently by each sensor. With factor graph composition, you can convert the state estimation problem into a factor graph relationship. Under the condition that the measurement error obeys the Gaussian distribution, the maximum likelihood estimation can be equivalent to the least squares problem, and through the factor graph optimization, the optimal estimation problem of the navigation state is transformed into the least squares optimization problem, which can be solved by the nonlinear optimization algorithm, when the observation information is enough, the dimension of the system equation is greater than the state dimension, and the optimal estimation of the state will be uniquely determined, and the optimal solution of the navigation state can be obtained by solving the optimization objective function.
Gaussian quasi-Newtonian algorithm model solving.
Gauss-Newton's algorithm is a common method to solve nonlinear least squares problems, firstly, the second-order Taylor expansion is carried out for the least squares problem to obtain the first-order approximation of the residual function, the Gaussian Newton algorithm uses the gradient of the objective function and the Hessian matrix to calculate the descending direction, in order to reduce the amount of computation, the Gaussian Newton method omits the second-order derivative information in the Hessian matrix and approximates the Hessian matrix.
The Gaussian Newton algorithm has a second-order convergence rate to solve the nonlinear least squares problem, and the calculation is relatively simple without calculating the higher-order derivative information of the Hessian matrix, but in the iterative solution process, due to the omission of the second-order derivative information, it is easy to appear that the approximate Hessian matrix is not positively definite, which leads to the failure of the algorithm. Therefore, the Gaussian Newtonian algorithm must meet the condition of positive definiteness of the Hessian matrix, and when the Hessian matrix of the Gaussian Newton algorithm loses positive definiteness, the quasi-Newtonian algorithm (BFGS) is used to supplement the equation and the Broyden-Fletcher-Goldfarb-Shanno algorithm is used to make supplementary corrections, and the Gaussian Newton algorithm based on the BFGS quasi-Newton matrix is proposed to ensure that the Hessian matrix always maintains positive definite in the iterative process.
The BFGS algorithm mainly solves the problem of complex inversion of Hessian matrices in Newton's algorithm and the failure of Hessian matrices to maintain positive definiteness and the failure of the algorithm.
The Gaussian Newton algorithm based on BFGS correction uses the approximate Hessian matrix to always maintain positive definiteness in iteration, which increases the robustness of the algorithm and reduces the sensitivity of the algorithm to outliers.
Experimental results and analysis.
In order to verify the effectiveness of the proposed factor-graph fusion model, the differences between the single localization results and the fusion localization results were compared by experiments, and the commonly used fusion method EKF was set up to compare and analyze the superiority of the factor-graph fusion model.
In order to verify the accuracy of the BLE/PDR integrated navigation model and Gaussian quasi-Newton algorithm based on factor diagram optimization, the Bluetooth/micro-inertial navigation integrated navigation system was experimentally tested, the inertial sensor in the Bluetooth/micro-inertial navigation integrated navigation device was HWT905-232 module, and the Bluetooth chip was Nordic nRF52833, which had Bluetooth direction finding function, which could calculate the arrival angle and departure angle in the direction finding and use the angle information for positioning. The Bluetooth base station is laid at a height of 3 meters, the micro inertial navigation module device is fixed on the pedestrian's feet, the Bluetooth tag is held, and the data is collected according to the set path normally, and the experimental data of the straight and rectangular routes are collected respectively.
In the experiment, the data for Bluetooth positioning and PDR positioning need to be collected respectively, wherein the Bluetooth module and the inertial navigation module are both collected at a frequency of 10Hz, wherein the PDR inertial sensor needs to collect acceleration data, angular velocity data and magnetometer data.
Pedestrians walked normally along the experimental route, collected Bluetooth AOA positioning data and inertial sensor data, carried out information fusion based on the proposed factor graph optimization model, used Gaussian quasi-Newton algorithm to solve the BLE/PDR factor graph model, and obtained the optimal solution of navigation state, and at the same time, the fusion results based on the factor graph model were compared with the fusion results based on Extended Kalman Filter (EKF) to verify the accuracy of the algorithm.
From the experimental results, it can be analyzed as follows: PDR positioning can continuously track the pedestrian position, but limited by the accumulation of errors, the positioning trajectory gradually deviates from the real trajectory, and the positioning error increases with the increase of the number of steps, and the final positioning root mean square error is 0.7167 meters; At the beginning and end of the walking trajectory, the BLE positioning diverges because the Bluetooth tag is far away from the base station. When the pedestrian moves to the vicinity of the Bluetooth base station, the BLE positioning trajectory coincides with the real trajectory very highly. In addition, the BLE/PDR fusion positioning trajectory under the two frameworks coincides with the real trajectory near the Bluetooth base station more than that of the single positioning method. The root mean square error of the single Bluetooth AOA algorithm is 0.8697 meters, the root mean square error of BLE/PDR fusion positioning based on EKF is 0.6823 meters, and the root mean square error of BLE/PDR fusion positioning based on factor graph optimization is 0.5797 meters. In general, the BLE/PDR fusion positioning error under the two frameworks is smaller than that of the single navigation mode, the fusion positioning trajectory is closer to the real trajectory, and the accuracy of the BLE/PDR fusion localization algorithm based on factor graph optimization is higher than that of the BLE/PDR fusion localization algorithm based on EKF, and the root mean square error of the positioning results is reduced by 15.04%.
Compared with the EKF algorithm, the positioning error of the factor graph optimization algorithm is smaller in most periods, the positioning is more stable, and the recovery ability of Bluetooth anomaly measurement values is stronger.