Bluetooth inertial navigation fusion positioning method based on Kalman filtering

Abstract

The invention provides a Bluetooth inertial navigation fusion positioning method based on Kalman filtering, and relates to the technical field of fusion positioning, and the method comprises the following steps: caching a Bluetooth signal at each step of inertial navigation positioning; converting the signal intensity into a distance through a free space path loss model; calculating the coordinates of the point to be positioned by using a trilateral positioning method; step detection, step length estimation and track plotting are carried out through inertial navigation positioning; using an extended Kalman filtering algorithm to fuse inertial navigation and Bluetooth positioning results to obtain a final positioning coordinate; and emptying the Bluetooth signal strength data cached at the (k-2) th step after the kth new step of inertial navigation positioning arrives. According to the method, the Kalman filtering algorithm is used for smoothing, and then the extended Kalman filtering algorithm is used for fusing inertial navigation and Bluetooth positioning results, so that the positioning precision is further improved, and more accurate and more reliable position estimation is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of integrated positioning, and particularly to a Bluetooth inertial navigation integrated positioning method based on Kalman filtering. Background Art

[0002] Currently, the demand for indoor positioning is increasing day by day, such as in aspects like indoor shopping mall shopping guides, indoor parking guidance, and hospital patient guides. Indoor positioning mostly relies on several sensors for positioning, such as WIFI, Bluetooth, infrared, geomagnetism, etc. However, due to the complex indoor environment and numerous obstacles, the positioning accuracy of such wireless sensor-based positioning is greatly affected by the environment, and the positioning accuracy is unstable.

[0003] In the prior art, there are currently three types of Bluetooth positioning algorithms: ranging-based positioning algorithms, fingerprint database positioning algorithms based on RSSI values, and Bluetooth AoA positioning algorithms based on direction finding. Their accuracies can all reach the meter level and can basically meet the requirements of indoor positioning. However, the complex indoor electromagnetic environment and obstructions will cause problems such as uneven Bluetooth positioning and backtracking. Inertial navigation has advantages such as high short-term positioning accuracy and stable positioning results, but there are problems such as inability to determine the initial position and error accumulation, making it difficult to be used alone in the field of indoor positioning.

[0004] In harsh environments such as indoor multi-obstructions, strong interference, and non-line-of-sight, relying on a single sensor for positioning will inevitably result in phenomena such as information loss and errors, which will lead to the positioning algorithm relying on a single sensor being difficult to meet the indoor high-precision and high-stability positioning requirements. Summary of the Invention

[0005] In view of the above problems, the present invention aims to provide a Bluetooth inertial navigation integrated positioning method based on Kalman filtering. By caching the scanned Bluetooth signal strength and using the Kalman filtering algorithm for smoothing processing, the fluctuations and noise of the signal strength are effectively reduced. Then, the extended Kalman filtering algorithm is used to fuse the inertial navigation and Bluetooth positioning results to further improve the positioning accuracy and achieve a more accurate and reliable position estimation.

[0006] Specifically, the first aspect of the present invention provides a Bluetooth inertial navigation integrated positioning method based on Kalman filtering, including the following steps: Step 1: At each step of inertial navigation positioning, cache the scanned Bluetooth signal strength, and use the Kalman filtering algorithm to smooth the cached Bluetooth signal strength to obtain a stable signal value; Step 2: Convert the filtered Bluetooth signal strength into the distance from the Bluetooth base station to the point to be located through the free space path loss model; Step 3: Use the trilateration method to solve the coordinates of the point to be located, and this coordinate is the Bluetooth positioning result; Step 4: Conduct step detection, step length estimation, and track inference through inertial navigation positioning; Step 5: Use the extended Kalman filter algorithm to fuse the inertial navigation and Bluetooth positioning results to obtain the final positioning coordinates; Step 6: After the arrival of the k-th new step in inertial navigation positioning, clear the Bluetooth signal strength data cached at the (k - 2)-th step, and repeat the above steps to obtain new fused positioning coordinates.

[0007] Further, in the above Step 1, the stochastic system state space model of the Kalman filter used in the Kalman filter algorithm is: ; ; ; Where: is the predicted value at time; is the state gain from time to time; is the predicted value at time; is the noise driving matrix at time; is the noise at time; is the observed value at time; are known system structure parameters; is the measurement noise; is the predicted value of the signal strength of the first Bluetooth base station; is the predicted value of the signal strength of the n-th Bluetooth base station; is the measured value of the signal strength of the first Bluetooth base station; is the measured value of the signal strength of the n-th Bluetooth base station; is the total number of Bluetooth base stations.

[0008] The implementation of the Kalman filter mainly consists of five recursive equations, including: Predict the estimated value of the current state variable based on the estimated value of the previous step: ; Error covariance matrix for predicting the current state: ; Calculate the Kalman filter gain: ; Use the Kalman filter gain and the measurement value to correct the state estimate: ; Update the state estimate covariance matrix: ; Where: is the state prediction at time based on at time; is the covariance matrix at time based on at time and the noise covariance matrix and the covariance prediction value; is the covariance matrix at for time; is the transpose matrix of ; is the noise covariance matrix at time; is the transpose matrix of the noise driving matrix at time; is the Kalman filter gain at time; is the transpose matrix of the gain of the measurement value; is the gain of the measurement value; is the measurement noise; is the covariance matrix at time; is the identity matrix.

[0009] Furthermore, in the second step, the basic formula of the free space path loss model is: ; Where: is the received signal strength of base station i, in dB; is the distance between base station i and the receiver; is the transmission signal frequency of the base station, with the unit of MHz, and the value in the present invention is 2400 MHz; is the speed of light, which is 299792458 m / s; is the pi.

[0010] The free space path loss model is a basic theoretical model that describes the power attenuation when electromagnetic waves propagate in an ideal isotropic medium. This model is based on the Friis transmission equation. By deriving the spherical wave diffusion characteristics of electromagnetic waves in free space, a quantitative relationship between path loss and propagation parameters is established.

[0011] Further, in the third step, the basic formula of the trilateration method is: ; The trilateration method simplifies to the matrix form as: ; where: ; ; ; ( , ) is the position of the first positioning base station; ( , ) is the position of the second positioning base station; ( , ) is the position of the third positioning base station; ( , ) is the position of the point to be located; is the square of the distance from the point to be located to the first positioning base station; is the square of the distance from the point to be located to the second positioning base station; is the square of the distance from the point to be located to the third positioning base station.

[0012] When the distances from the point to be located to at least three positioning base stations are known and the three positioning base stations are not on the same straight line, the coordinates of the point to be located can be obtained. The solution with the minimum mean square error obtained by using optimization methods such as the least squares method is: ; Therefore, the positioning result is: ; where: is The value of the first element in; is the value of the second element in; the transpose of matrix P; .

[0013] Furthermore, in the fourth step, the inertial navigation positioning includes three steps: step detection, step length estimation, and dead reckoning.

[0014] Furthermore, in the inertial navigation positioning, the step detection algorithm determines the steps by the peak value of the combined acceleration in three axes. The formula for the combined acceleration is: ; where: is the combined acceleration; is the acceleration value on the x-axis; is the acceleration value on the y-axis; is the acceleration value on the z-axis.

[0015] The step length estimation algorithm is: ; where: is the step length; is a constant parameter, and its value range is 0.4 to 0.8; is the peak value of the acceleration wave; is the trough value of the acceleration wave.

[0016] The basic formula for pedestrian dead reckoning is: ; where: is the coordinate of the kth step; is the coordinate of the (k - 1)th step; is the heading of the (k - 1)th step; is the step length.

[0017] Furthermore, the fifth step is specifically that after obtaining the state vector and the observation vector through the Bluetooth positioning algorithm and the inertial navigation algorithm respectively, an extended Kalman filter is used to update the state parameters and the observation parameters over time.

[0018] Furthermore, the state equation of the extended Kalman filter is mainly a state vector composed of the position coordinates and the heading, and the formula is as follows: ; where: is the state vector; is the coordinate at the k-th step; is the heading at the k-th step; is the coordinate at the (k - 1)-th step; is the heading at the (k - 1)-th step; is the step length parameter at the (k - 1)-th step; is the heading at the k-th step; is the process noise of the state vector.

[0019] From the state equation of the extended Kalman filter and the basic formula of pedestrian dead reckoning, the state transition matrix is: ; The Jacobian matrix of the state equation is: ; The measurement equation is mainly a measurement vector composed of the position coordinates, step length, and heading of Bluetooth positioning, and the formula is as follows: ; From the Jacobian matrix of the state equation, the Jacobian matrix of the measurement equation is: ; where: is the state transition matrix; is the Jacobian matrix of the state equation; is the heading at the (k - 1)-th step; is the step length parameter at the (k - 1)-th step; is the measurement equation; is the coordinate at the k-th step; is the coordinate at the (k - 1)-th step; and are the final X-axis and Y-axis positioning coordinates of the Bluetooth and inertial navigation fusion respectively; and are the X-axis coordinate and Y-axis coordinate of the Bluetooth sensor positioning respectively; is the step length after the Bluetooth and inertial navigation sensors are fused; is the heading angle after the fusion of Bluetooth and inertial navigation sensors; is the process noise vector; is the Jacobian matrix of the measurement equation.

[0020] After obtaining the state vector and the observation vector through the Bluetooth positioning algorithm and the inertial navigation algorithm respectively, an extended Kalman filter is used to update the state parameters and the observation parameters over time.

[0021] In a second aspect, the present invention further provides a computing device, which has the function of implementing the method described in the first aspect above. The beneficial effects can be seen in the description of the first aspect and will not be elaborated here. The function can be implemented by hardware or by hardware executing corresponding software. The hardware or software includes one or more modules corresponding to the above functions. In a possible design, the structure of the device includes an acquisition module, a training module, and may also include a construction module. These modules can implement the functions of the training nodes in the method example of the first aspect above. For specific reference, see the detailed description in the method example and will not be elaborated here.

[0022] In a third aspect, the present invention further provides a computing device, which is used to implement the function of the method described in the first aspect above. The beneficial effects can be seen in the description of the first aspect and will not be elaborated here. The structure of the computing device includes a processor and a memory. The memory is used to store instructions or data. The memory is coupled to the processor. When the processor executes the program instructions stored in the memory, it can implement the functions of the training nodes in the example of the first aspect above. The structure of the computing device also includes a communication interface for communicating with other devices.

[0023] In a fourth aspect, the present invention further provides a computer-readable storage medium, in which instructions are stored. When it runs on a computer, it causes the computer to execute the methods in the first aspect and all possible designs of the first aspect.

[0024] In a fifth aspect, the present invention further provides a computer program product containing instructions. When it runs on a computer, it causes the computer to execute the methods in the first aspect and all possible designs of the first aspect.

[0025] In a sixth aspect, the present invention further provides a computing chip. The chip is connected to the memory. The chip is used to read and execute the software program stored in the memory and execute the methods in the first aspect and all possible implementation manners of the first aspect. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] To more clearly illustrate the technical solutions in the embodiments of the present drawings or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are only some embodiments of the present drawings. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on the structures shown in these drawings.

[0027] Figure 1 It is the flowchart of the steps of the present invention; Figure 2 It is the comparison chart of the positioning error and the inertial navigation positioning error of the embodiments of the present invention; Figure 3 It is the comparison chart of the navigation trajectories of the embodiments of the present invention.

[0028] The realization of the purpose of the present drawings, the functional features and advantages will be further described in combination with the embodiments with reference to the drawings. Detailed implementation manners

[0029] In order to make the purpose, technical solutions and advantages of the present invention more clear and understandable, the following will describe and explain the present invention in combination with the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention. Based on the embodiments provided by the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0030] Obviously, the drawings in the following description are only some examples or embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, the present invention can also be applied to other similar scenarios based on these drawings. In addition, it can also be understood that although the efforts made in this development process may be complex and lengthy, for those of ordinary skill in the art related to the content disclosed by the present invention, some designs, manufacturing or production changes based on the technical content disclosed by the present invention are only conventional technical means and should not be understood that the content disclosed by the present invention is insufficient.

[0031] If there is no special instruction, all the implementation manners and optional implementation manners of the present invention can be combined with each other to form a new technical solution.

[0032] If there is no special instruction, all the technical features and optional technical features of the present invention can be combined with each other to form a new technical solution.

[0033] Unless otherwise specified, all steps of the present invention can be carried out sequentially or randomly, preferably sequentially. For example, the method includes steps (a) and (b), which means that the method may include steps (a) and (b) carried out sequentially, or may also include steps (b) and (a) carried out sequentially. For example, it is mentioned that the method may further include step (c), which means that step (c) can be added to the method in any order. For example, the method may include steps (a), (b) and (c), or may also include steps (a), (c) and (b), or may also include steps (c), (a) and (b), etc.

[0034] Unless otherwise specified, the terms "comprising" and "including" mentioned in the present invention mean open-ended, and can also be closed-ended. For example, the "comprising" and "including" may mean that other components not listed can also be included or comprised, or may only include or comprise the listed components.

[0035] To better understand the solutions of the embodiments of the present invention, some related terms and concepts that may be involved in the embodiments of the present invention will be introduced below.

[0036] (1) Kalman filter (KF) is a recursive filter that dynamically estimates the system state from noisy measurements by fusing redundant and complementary data from multiple sensors to achieve the optimal estimate of the minimum mean square error. In the prediction and update steps of the Kalman filter, the covariance matrix is repeatedly calculated and updated to describe the uncertainty of the state estimate. The covariance matrix is the object explicitly calculated in the algorithm and is used to quantify the error distribution of the state estimate (including the correlation between state variables). The mean square error is the implicit optimization objective and is indirectly reflected by the trace of the covariance matrix.

[0037] (2) Extended Kalman filter (EKF). EKF is an extension of KF in a non-linear scenario. It applies KF after locally linearizing the non-linear function through Taylor expansion. Typical applications include: for example, the pseudorange and phase range observation equations of RTK (Real-Time Kinematic) are non-linear, and EKF achieves high-precision positioning through linearization. EKF depends on accurate linearization assumptions. If the system is highly non-linear or there are abnormal observations (such as multipath effects), its performance may degrade, and robust filtering methods or machine learning need to be combined for improvement.

[0038] (3) Inertial navigation: Inertial Navigation System. Inertial navigation measures the motion acceleration and angular velocity of a device through sensors such as accelerometers, gyroscopes, and magnetometers, and calculates the position and attitude through integral operations. Its core is autonomous calculation and does not rely on external signals. It can still perform short-term autonomous navigation when the GPS signal is lost. However, its error accumulates over time. It does not require external infrastructure and is suitable for indoor or complex environments without GPS signals (such as tunnels, chemical plants); it has high short-term accuracy and fast data update.

[0039] (4) Bluetooth positioning, based on RSSI (Received Signal Strength Indication), determines the location by measuring the difference in Bluetooth signal strength. Its implementation methods are divided into two categories. Terminal-side positioning: Terminal devices such as mobile phones have built-in algorithms to calculate the location according to the difference in the signal strength of surrounding Bluetooth Beacons, which is often used for indoor navigation (such as shopping malls, hospitals); Network-side positioning: It relies on a network composed of Bluetooth gateways, Beacons, and servers to comprehensively solve the location through the signal strength of multiple nodes, and is suitable for asset tracking or personnel positioning.

[0040] In this embodiment, as Figure 1 shown, a Bluetooth inertial navigation fusion positioning method based on Kalman filtering includes the following steps: Step 1: At each step of inertial navigation positioning, cache the scanned Bluetooth signal strength, and use the Kalman filtering algorithm to smooth the cached Bluetooth signal strength to obtain a stable signal value; Step 2: Convert the filtered Bluetooth signal strength into the distance from the Bluetooth base station to the point to be located through the free space path loss model; Step 3: Use the trilateration method to solve the coordinates of the point to be located, and this coordinate is the Bluetooth positioning result; Step 4: Perform step detection, step length estimation, and track calculation through inertial navigation positioning; Step 5: Use the extended Kalman filtering algorithm to fuse the inertial navigation and Bluetooth positioning results to obtain the final positioning coordinates; Step 6: After the arrival of the k-th new step in inertial navigation positioning, clear the cached Bluetooth signal strength data at the (k - 2)-th step, and repeat the above steps to obtain new fused positioning coordinates.

[0041] Furthermore, in Step 1, the stochastic system state space model of the Kalman filter used in the Kalman filtering algorithm is: ; ; ; The implementation of the Kalman filter mainly consists of five recursive equations, including: Predict the estimated value of the current state variable based on the estimated value of the previous step: ; Predict the error covariance matrix of the current state: ; Calculate the Kalman filter gain: ; Use the Kalman filter gain and the measured value to correct the state estimate: ; Update the state estimation covariance matrix: ; Furthermore, in step two, the basic formula of the free space path loss model is: ; The free space path loss model is a basic theoretical model that describes the power attenuation when electromagnetic waves propagate in an ideal isotropic medium. This model is based on the Friis transmission equation. By deriving the spherical wave diffusion characteristics of electromagnetic waves in free space, a quantitative relationship between path loss and propagation parameters is established.

[0042] Furthermore, in step three, the basic formula of the trilateration method is: ; The trilateration method simplifies to matrix form as: ; ; ; ; When the distances from the point to be located to at least three positioning base stations are known and the three positioning base stations are not on the same straight line, the coordinates of the point to be located can be obtained. The solution with the minimum mean square error obtained by using optimization methods such as the least squares method is: ; Therefore, the positioning result is: ; Furthermore, in step four, inertial navigation positioning includes three steps: step detection, step length estimation, and dead reckoning.

[0043] Furthermore, in inertial navigation positioning, the step detection algorithm determines steps through the peak value of the combined acceleration of three axes. The calculation formula of the combined acceleration is: ; The step length estimation algorithm is: ; The basic formula for pedestrian dead reckoning is: ; Furthermore, step five is specifically that after obtaining the state vector and the observation vector through the Bluetooth positioning algorithm and the inertial navigation algorithm respectively, an extended Kalman filter is used to update the state parameters and the observation parameters over time.

[0044] Furthermore, the state equation of the extended Kalman filter mainly consists of a state vector composed of position coordinates and heading, and the formula is as follows: ; From the state equation of the extended Kalman filter and the basic formula of pedestrian dead reckoning, the state transition matrix is: ; The Jacobian matrix of the state equation is: ; The measurement equation mainly consists of a measurement vector composed of the position coordinates, step length, and heading of Bluetooth positioning, and the formula is as follows: ; From the Jacobian matrix of the state equation, the Jacobian matrix of the measurement equation is: ; After obtaining the state vector and the observation vector through the Bluetooth positioning algorithm and the inertial navigation algorithm respectively, the extended Kalman filter is used to update the state parameters and the observation parameters over time.

[0045] In this embodiment, the test method is as follows: A number of Bluetooth beacons are arranged in the test area, and the distance between the beacons is 5 - 10 meters to ensure that the signal covers the entire test area, and the positions of the beacons and the test path are marked in the system. During the test, the tester walks along the preset path with a smartphone, receives the RSSI values of the beacons through the Bluetooth module of the smartphone, and uses the signal strength values to calculate the Bluetooth positioning result in real time. At the same time, the step count, step length, and direction data are calculated using the data of the inertial sensor. The system uses the extended Kalman filter for data fusion: the Bluetooth positioning result is used as the observation input to provide absolute position information, and the inertial navigation dead reckoning result is used as the basis for updating the system state to estimate the continuous position information during the movement process.

[0046] The comparison chart of the positioning error between this embodiment and inertial navigation is as shown in Figure 2 , and the comparison chart of the positioning trajectory is as shown in Figure 3 . In the case of normal walking, the average error of the traditional positioning method is 2.5 meters, and the positioning error of this method is 1.29 meters. Analyzing from the numerical values, this method has significant advantages in the pedestrian navigation scenario.

[0047] It should be noted that the present invention is not limited to the above-described embodiments. The above-described embodiments are merely examples, and embodiments having the same constitution as the technical idea and exhibiting the same effects within the scope of the technical solution of the present invention are all included in the technical scope of the present invention. In addition, within the scope not departing from the gist of the present invention, various modifications that can be conceived by those skilled in the art to the embodiments, and other forms constructed by combining some of the constituent elements in the embodiments are also included in the scope of the present invention.

Claims

1. A Bluetooth inertial navigation fusion positioning method based on Kalman filtering, characterized in that: The following steps are involved: Step 1: At each step of inertial navigation positioning, cache the scanned Bluetooth signal strength, and use the Kalman filter algorithm to smooth the cached Bluetooth signal strength to obtain a stable signal value; Step 2: Convert the filtered Bluetooth signal strength into the distance from the Bluetooth base station to the point to be located through the free space path loss model; Step 3: Use the trilateral positioning method to solve the coordinates of the point to be positioned, which is the Bluetooth positioning result; Step 4: Perform step detection, step length estimation and track calculation through inertial navigation positioning; Step 5: Use the extended Kalman filter algorithm to fuse the inertial navigation and Bluetooth positioning results to obtain the final positioning coordinates; Step 6: After the kth new step of inertial navigation positioning arrives, clear the Bluetooth signal strength data cached at the k-2th step, repeat the above steps to obtain new fusion positioning coordinates.

2. The method for Bluetooth inertial navigation fusion positioning based on Kalman filtering according to claim 1 is characterized in that: In step 1, the random system state space model of the Kalman filter used in the Kalman filter algorithm is: ； ； ； in: for The predicted value at the moment; for Time has come Status gain at the moment; for The predicted value at the moment; for The noise driving matrix at the moment; for The noise of the moment; for The observed value at time; are known system structure parameters; To measure noise; is the predicted value of the signal strength of the first Bluetooth base station; is the predicted value of the signal strength of the nth Bluetooth base station; is the measured value of the signal strength of the first Bluetooth base station; is the measured value of the signal strength of the nth Bluetooth base station; is the total number of Bluetooth base stations.

3. The method for Bluetooth inertial navigation fusion positioning based on Kalman filtering according to claim 1 is characterized in that: In step 2, the basic formula of the free space path loss model is: ； in: is the received signal strength of base station i, in dB; is the distance between base station i and the receiver; is the transmission signal frequency of the base station, in MHz; is the speed of light; is the ratio of pi.

4. The method for Bluetooth inertial navigation fusion positioning based on Kalman filtering according to claim 1 is characterized in that: In step 4, inertial navigation positioning includes three steps: step detection, step length estimation and track calculation.

5. The method for Bluetooth inertial navigation fusion positioning based on Kalman filtering according to claim 4 is characterized in that: In the inertial navigation positioning, the step detection algorithm is to determine the step by the peak value of the three-axis combined acceleration, and the calculation formula of the combined acceleration is: ； in: is the combined acceleration; is the acceleration value of the x-axis; is the acceleration value of the y-axis; is the acceleration value of the z-axis; The step length estimation algorithm is: ; in: is the step length; is a constant parameter, ranging from 0.4 to 0.8; is the peak value of acceleration wave; is the acceleration trough value; The basic formula for dead reckoning is: ; in: is the k-th step coordinate; is the k-1th step coordinate; is the heading of the k-1th step; is the step length.

6. The method for Bluetooth inertial navigation fusion positioning based on Kalman filtering according to claim 1 is characterized in that: The step five specifically involves using an extended Kalman filter to update the state parameters and the observation parameters over time after the state vector and the observation vector are obtained through the Bluetooth positioning algorithm and the inertial navigation algorithm respectively.

7. The method for Bluetooth inertial navigation fusion positioning based on Kalman filtering according to claim 6 is characterized in that: The state equation of the extended Kalman filter is mainly a state vector composed of position coordinates and heading, and the formula is as follows: ； in: is the state vector; is the k-th step coordinate; is the heading of the kth step; is the k-1th step coordinate; is the heading of the k-1th step; is the step size parameter of the k-1th step; is the heading of the kth step; is the process noise of the state vector.

Images