Indoor and outdoor navigation positioning method and system based on improved unscented particle filter algorithm

Abstract

The invention relates to the technical field of indoor navigation, in particular to an indoor and outdoor navigation positioning method and system based on an improved unscented particle filter algorithm, and the positioning method comprises the following steps: estimating the initial position of a positioning target through an extended fingerprint positioning algorithm based on LightGBM; processing the initial position of the positioning target through an improved unscented particle filtering algorithm to obtain a filtered initial position; calculating a final weight value according to the measurement weight and the map weight; and weighting the particles in each step by using the final weight value to obtain a fusion positioning result, and obtaining the real-time position of the measurement target. According to the invention, an outdoor navigation positioning part is fused with a BDS / INS / magnetometer / barometer through a particle filter optimization algorithm so as to realize accurate positioning under an outdoor condition; and a high-precision positioning result of an indoor environment can be provided based on an opportunity signal / geomagnetic module under the condition that an indoor GNSS signal is weak.

Description

Technical Field

[0001] The present invention relates to the technical field of indoor positioning, and particularly relates to an indoor and outdoor navigation and positioning method and system based on an improved unscented particle filter algorithm. Background Art

[0002] With the development of information technology, high-precision and real-time position information plays a crucial role in the LBS (Location-Based Service) industry. The GNSS positioning system is a widely used method today, which can provide a relatively high-standard LBS service in outdoor open environments. However, when a moving carrier enters an indoor environment such as a city building, satellite signals are often attenuated or blocked by the building, which affects the accuracy of indoor positioning and causes GNSS to be unable to meet the demand for stable position services. Summary of the Invention

[0003] The purpose of the present invention is to provide an indoor and outdoor navigation and positioning method and system based on an improved unscented particle filter algorithm to solve the technical problem of low indoor positioning accuracy of the existing GNSS positioning system. The specific technical solutions are as follows:

[0004] The present invention provides an indoor and outdoor navigation and positioning method based on an improved unscented particle filter algorithm, including the following steps:

[0005] S1. Obtain information of a reference point; estimate the initial position of a positioning target through an extended fingerprint positioning algorithm based on LightGBM based on the information of the reference point;

[0006] S2. Process the initial position of the positioning target through an improved unscented particle filter algorithm to obtain a filtered initial position, and use the estimated value of the sampling point calculated based on the multi-source fusion positioning system state model, system state equation, and multi-source fusion observation model as the importance probability density function for the filtered initial position, and obtain the sampling particle distribution through the importance probability density function;

[0007] S3. Calculate the measurement weight and map weight of each particle, and calculate the final weight value from the measurement weight and map weight;

[0008] S4. Resample to generate a new particle set, and perform the calculation of S3 to obtain the final weight value, and continuously iterate to calculate the optimal value of the final weight value;

[0009] S5. Use the optimal value of the final weight value to weight each step of the particles to obtain a fusion positioning result, and obtain the real-time position of the measurement target.

[0010] The further improvement of the indoor and outdoor navigation and positioning method based on the improved unscented particle filter algorithm lies in that when estimating the initial position of the positioning target through the extended fingerprint positioning algorithm based on LightGBM, the following steps are included:

[0011] In the offline stage, fingerprints are collected at each reference point. The position and fingerprint information of each reference point together constitute the fingerprint database, and the fingerprint database Z is expressed as:

[0012]

[0013] where FP i is the fingerprint of the i-th reference point, and P i is the position coordinate of the i-th reference point, and i takes integers from 1 to N RP , N RP is the total number of all reference points, and RP is the reference point;

[0014] The fingerprint FP of the reference point i is expressed as a 1×N AP vector, as shown in the following formula:

[0015]

[0016] where is the signal strength received by the i-th reference point from the n-th access point;

[0017] The real-time fingerprint information FP of the user user is expressed as:

[0018]

[0019] where RSS n is the signal strength received by the user from the n-th access point, and n takes integers from 1 to N AP , N AP is the total number of all access points, and AP is the access point.

[0020] The further improvement of the indoor and outdoor navigation and positioning method based on the improved unscented particle filter algorithm lies in that when processing the initial position of the positioning target through the improved unscented particle filter algorithm to obtain the filtered initial position, the following steps are included:

[0021] The Cholesky decomposition is defined as: assuming a matrix P is an m-order symmetric positive definite matrix, then there exists a matrix L with positive diagonal elements such that when the diagonal elements of matrix L are positive, the unique solution is obtained: P = LL T ; and when P is not positive definite, subsequent filter updates cannot be performed, so the SVD decomposition is used to replace the Cholesky decomposition of the state estimation covariance, and the formula is expressed as:

[0022] P = LL T = UΣD T DΣ T U T = UΣΣ T U T ;

[0023] where L represents the lower triangular matrix obtained by Cholesky decomposition, L T represents the transpose of matrix L, U and D are unitary matrices, U T and D T represent the transposes of matrices U and D, Σ is an m×n matrix, i.e., only the elements on the main diagonal are non-zero values, and the eigenvalues on the diagonal are singular values, Σ T represents the transpose of matrix Σ;

[0024] L m×n is decomposed by SVD into U m×m Σ m×n D n×n T , and the formula is as follows:

[0025] svd[L m×n = [U m×m , Σ m×n , D n×n T ;

[0026] where L m×n is an m×n matrix, svd[L m×n represents performing SVD decomposition on matrix L m×n , U and D matrices are unitary matrices, i.e., satisfying U T U = D T D = I, the subscript m×n indicates that the matrix is an m×n square matrix, T represents the transpose of the matrix, Σ m×n is an m×n matrix, i.e., only the elements on the main diagonal are non-zero values, and the eigenvalues on the diagonal are singular values, and the formula is expressed as:

[0027]

[0028] where diag(.) represents arranging elements along the diagonal, λ1,...,λ r are r eigenvalues, H is a diagonal matrix, and its elements are eigenvalues;

[0029] When the covariance matrix P is symmetric and positive definite, U and D are equal, i.e., L = UΣU T . Substituting the SVD decomposition values gives:

[0030]

[0031] where S i is the particle point set, λ is the scale factor, is the mean vector, N is the number of the particle point set, and T is the transpose of the matrix;

[0032] The covariance matrix of the observed quantity is expressed as:

[0033]

[0034] where, P ytyt is the covariance matrix of the observed quantity, is the weight of the i-th particle, y i,t|t-1 is the predicted value of the observation of the i-th particle at time t, is the predicted mean value of the observed quantity, is the weighted average of the predicted value of the i-th particle's observation and the predicted value, N is the number of the particle point set, and T is the transpose of the matrix;

[0035] Similarly, the unscented particle filter decomposition using the SVD algorithm is performed on the above formula to obtain the following formula, and the initial position after filtering is calculated through the following formula:

[0036]

[0037] where, represents a representation form of the observation covariance matrix.

[0038] The further improvement of the indoor and outdoor navigation and positioning method based on the improved unscented particle filter algorithm of the present invention lies in that the unscented particle filter based on the SVD algorithm is as follows:

[0039] Step 1: Initialize the particles at t = 0, generate n particle sets obeying the p(x0) distribution, expressed as and the corresponding mean value of the particles and the covariance

[0040] where p(x0) represents the probability distribution of the initial state, is the initial particle set containing n particles

[0041] Step 2: Perform importance sampling at t > 0, through the following formula:

[0042]

[0043] Calculate the sigma point set and weights of the n-dimensional state variable, and predict the next moment's sigma point set using the state transition function in the time update stage;

[0044] Step 3: In the observation update stage, through the formula The inverse matrix of the covariance of the observed quantity is replaced by the SVD decomposition value to obtain the filtering estimation at time t and the sampling particle distribution That is the initial position after filtering;

[0045] Among them, represents the state estimation value obtained by weighted average at time t, represents the covariance matrix of the i-th particle at time t, and N(.) represents the Gaussian distribution.

[0046] The further improvement of the indoor and outdoor navigation and positioning method based on the improved unscented particle filter algorithm in the present invention lies in that S4 is specifically: resampling from the sampling particle distribution to obtain a new particle set Among them is the weight corresponding to each particle, and the weights of each particle are equal after resampling to be

[0047] To obtain the state estimation value at each moment and the corresponding covariance Then steps two, three and S3 are repeated. When the change amount of the state variable is less than the threshold, the iteration ends, as shown in the following formula Among them represents the state variable of the i-th particle at time t + 1, represents the state variable of the i-th particle at time t, and ε represents a positive number, usually set to 10 -9 ~10 -4 .

[0048] The further improvement of the indoor and outdoor navigation and positioning method based on the improved unscented particle filter algorithm in the present invention lies in that the state model of the multi-source fusion positioning system is expressed as:

[0049] l k = f(l k -1) + v k-1 ;

[0050] Among them, l k = [x k , y k T is the system state at the k-th step, f(.) is the state equation, v k-1 is the system noise, x k is the system state value at time k, y k is the measurement value at time k, and T is the transpose of the matrix. This model establishes a spatial dynamic model of the system state by fusing the state information of multi-source sensors, outputs the joint estimation value of the system state, and the output result is used as the input of the importance probability density function to generate the sampling particle distribution, which directly affects the initial state sampling of the particles. ​

[0051] The further improvement of the indoor and outdoor navigation and positioning method based on the improved unscented particle filter algorithm of the present invention lies in that the system state equation is expressed as:

[0052]

[0053] where Lstep k-1 is the step length at the (k - 1)-th step, θ k-1 is the heading angle estimated at the (k - 1)-th moment, x k-1 is the system state value at the (t - 1)-th moment, and y k-1 is the measurement value at the (t - 1)-th moment. The input data of this model are the system state and the motion model parameters at the current moment. Based on the physical kinematic model, the dynamic evolution of the state variables is described by a mathematical equation, and the predicted value of the system state at the next moment is output. The predicted value is used for comparing the observed values of the multi-source fusion observation model and, at the same time, serves as the basis for state transition in the particle filter to update the state distribution of the particles.

[0054] The further improvement of the indoor and outdoor navigation and positioning method based on the improved unscented particle filter algorithm of the present invention lies in that the multi-source fusion observation model is expressed as:

[0055] Y k = h(l k ) + u k ;

[0056] where Y k is the observed quantity, u k is the measurement noise, and h(.) is the state transition equation. The observation model is specifically expressed as:

[0057]

[0058] where Z k is the extended fingerprint positioning result, α is the positioning result of the extended fingerprint in the observation model, β is the weight of the state value of the extended fingerprint in the observation model, and α + β = 1. is the state component, is the predicted value. The input data of this model are the real-time observation data and the predicted value of the system state. The multi-source observation data are fused to establish a relationship model between the observed values and the state variables. The output result is the predicted residual of the observed value. The residual information is used to calculate the measurement weight of each particle and generate the final weight value in combination with the map weight, which directly affects the resampling process and the accuracy of the positioning result.

[0059] The present invention also provides a system adopting the indoor and outdoor navigation and positioning method based on the improved unscented particle filter algorithm as described above, including:

[0060] Indoor navigation module, the indoor navigation module includes a microcomputer, and BDS, INS, magnetometer and barometer are integrated in the microcomputer. The microcomputer outputs the position and speed information of the moving carrier through an improved Kalman filtering algorithm;

[0061] Outdoor navigation module, the outdoor navigation module includes an opportunistic signal acquisition unit. The outdoor navigation module fuses different signal sources through an improved unscented particle filtering algorithm and outputs the position and speed information of the moving carrier.

[0062] Applying the technical solution of the present invention has the following beneficial effects:

[0063] The indoor and outdoor navigation and positioning system of the present invention based on the improved unscented particle filtering algorithm realizes accurate positioning under outdoor conditions by fusing BDS / INS / magnetometer / barometer through the particle filtering optimization algorithm in the outdoor navigation and positioning part; in the case of weak GNSS signals indoors, it can provide high-precision positioning results for the indoor environment based on opportunistic signals / geomagnetic modules. The multi-source fusion positioning algorithm using the improved unscented particle filtering can make up for the defects of single-source positioning and improve the positioning accuracy of the entire system. The system has the characteristics of high precision, low energy consumption and multi-platform compatibility, and is suitable for various application scenarios such as indoor and outdoor navigation, motion tracking and map services. It solves the technical problem of low indoor positioning accuracy in the existing GNSS positioning system. The present invention utilizes the rich indoor opportunistic signal resources and can select one or more of them for navigation and positioning according to the application scenario; since the opportunistic signals already exist, there is no need to deploy additional equipment, which greatly avoids the increase in cost; compared with the ultra-long-distance GNSS signals, the opportunistic signals have strong penetration and high power, and higher signal strength.

[0064] In addition to the purposes, features and advantages described above, the present invention has other purposes, features and advantages. The following will refer to the drawings to make a further detailed description of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] The drawings constituting a part of this application are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation to the present invention. In the drawings:

[0066] Figure 1 is a flowchart of the indoor and outdoor navigation and positioning method of the present invention based on the improved unscented particle filtering algorithm;

[0067] Figure 2 is a flowchart of the opportunistic signal processing of the indoor and outdoor navigation and positioning method of the present invention based on the improved unscented particle filtering algorithm.

[0068] Among them, 1. BDS; 2. INS; 3. Magnetometer; 4. Barometer; 5. Microcomputer; 6. Opportunistic signal acquisition unit; 7. Power supply module; 8. Outdoor navigation module; 9. Indoor navigation module. Specific embodiments

[0069] The embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0070] Opportunistic signal navigation is a navigation position information technology that uses potential signals in the indoor environment as opportunistic signals and extracts key information through opportunistic signal features. Most of these signals are part of commercial communication signals, which can be obtained at any time in the indoor environment and are not signals specifically designed for location services, including WiFi signals, Bluetooth signals, geomagnetic signals, satellite signals, etc. In areas where GNSS signals are blocked, opportunistic signals can replace GNSS to complete the positioning function.

[0071] As Figure 2 shown, WiFi signals, Bluetooth signals, geomagnetic signals, and satellite signals are respectively received by receiving module one, receiving module two, receiving module three, and receiving module four and preprocessed, then feature extraction or demodulation and decoding are performed, the estimated value of the sampling point is obtained through position prediction and estimation, and finally the position distribution is obtained through position calculation.

[0072] See Figure 1 shown, an indoor and outdoor navigation and positioning method based on an improved unscented particle filter algorithm includes the following steps:

[0073] S1. First, perform particle initialization to obtain information about the reference point; estimate the initial position of the positioning target through an extended fingerprint positioning algorithm based on LightGBM (Light Gradient Boosting Machine) using the information about the reference point; initialize a particle set composed of n particles where represents the initial position of the i-th particle, and i = 1,..., n represents the number of sampled particles;

[0074] S2. In the importance sampling stage, the initial position of the positioning target is processed through an improved unscented particle filter algorithm to obtain the filtered initial position, and the estimated value of the sampling point calculated based on the multi-source fusion positioning system state model, system state equation, and multi-source fusion observation model of the filtered initial position is used as the importance probability density function, and the sampling particle distribution is obtained through the importance probability density function where represents the state estimation of the i-th sampling particle at the current sampling step t; N(.) represents the Gaussian distribution; represents the mean of the i-th sampling particle at time t; Denote the covariance of the i-th sampled particle at time t;

[0075] The detailed calculation logic and correlation relationships of the above three models are as follows: In the multi-source fusion positioning system state model, the input data is the filtered initial position and multi-source sensor data. The sensor data of different frequencies / accuracies such as BDS, INS, and magnetometer are synchronized in time and calibrated in space. The joint estimated value of the system state is generated through the weighted fusion algorithm. The joint estimated value is used as the input of the system state equation to predict the state at the next moment; in the system state equation, the state prediction value at the next moment is calculated based on the kinematic model and the state covariance matrix is updated according to the system noise model. The prediction value is input into the multi-source fusion observation model; in the multi-source fusion observation model, the input data is the state prediction value and real-time observation data. The theoretical observation value is generated according to the state prediction value, and then the difference between the theoretical observation value and the actual observation value is compared to generate the residual. The residual is used to calculate the measurement weight of the particle. After combining with the map weight, the final weight value is generated to drive the resampling process.

[0076] S3. Calculate the measurement weight and map weight of each particle, and calculate the final weight value from the measurement weight and map weight. The weight is calculated. Among them, Denote the measurement weight obtained by the i-th particle; Denote the map weight of the i-th particle; w i Denote the final weight value;

[0077] Then perform normalization: Among them, Denote the normalized weight of the i-th particle at time t, Denote the final weight value obtained by the i-th particle at time t through the formula of S3;

[0078] S4. Resample to generate a new particle set, and perform the calculation of S3 to obtain the final weight value. Continuously iterate to calculate the optimal value of the final weight value;

[0079] S5. Use the optimal value of the final weight value to weight each step of the particles to obtain the fusion positioning result. Among them Denote the system state value at the i-th step at time t, Denote the measurement value at the i-th step at time t, X t Denote the system state value at time t, Y t Denote the system measurement value at time t;

[0080] Among them, Can represent the position and velocity information of the system, x t Denote the position of the system at time t, v tDenote the speed of the system at time t. Thus, the real-time position of the measurement target at time t can be obtained.

[0081] Figure 1 The specific process in [reference] is as follows: The power supply module of the outdoor navigation module powers BDS1, INS2, magnetometer 3, and barometer 4. The data of BDS1, INS2, magnetometer 3, and barometer 4 are input into the microcomputer for processing to obtain the position and speed and output them. At the same time, the opportunistic signal of the indoor navigation module passes through the improved unscented particle filter to obtain the position and speed and output them.

[0082] Preferably, when estimating the initial position of the positioning target by the extended fingerprint positioning algorithm based on LightGBM, the following steps are included:

[0083] In the offline stage, fingerprints are collected at each reference point. The position and fingerprint information of each reference point together constitute the fingerprint database, and the fingerprint database Z is expressed as:

[0084]

[0085] where FP i is the fingerprint of the i-th reference point, P i is the position coordinate of the i-th reference point, i takes integers from 1 to N RP and N RP is the total number of all reference points, and RP is the reference point;

[0086] The fingerprint FP of the reference point i is expressed as a 1×N AP vector, as shown in the following formula:

[0087]

[0088] where is the signal strength received by the i-th reference point from the n-th access point;

[0089] The real-time fingerprint information FP of the user user is expressed as:

[0090]

[0091] where RSS n is the signal strength received by the user from the n-th access point, n takes integers from 1 to N AP and N AP is the total number of all access points, and AP is the access point.

[0092] Preferably, the singular value decomposition (SVD) algorithm is introduced to replace the Cholesky decomposition of the sampling particle state covariance matrix and the solution of the inverse matrix of the measurement covariance matrix, reducing the non - positive definiteness of the sampling particle covariance matrix caused by cyclic updates in the unscented particle filter algorithm. Thus, the impact of filter update failures is reduced, and the stability and robustness of the unscented particle filter algorithm are improved. Specifically, when processing the initial position of the positioning target through the improved unscented particle filter algorithm to obtain the filtered initial position, the following steps are included:

[0093] The Cholesky decomposition (square - root method) is defined as: Suppose a matrix P is an m - order symmetric positive - definite matrix, then there exists a matrix L with positive diagonal elements such that when the diagonal elements of matrix L are positive, the unique solution is obtained: P = LL T ; and when P is not positive - definite, subsequent filter updates cannot be performed, so the Cholesky decomposition of the state - estimation covariance is replaced by SVD decomposition, and the formula is expressed as:

[0094] P = LL T = UΣD T DΣ T U T = UΣΣ T U T ;

[0095] Among them, L represents the lower - triangular matrix obtained by Cholesky decomposition, L T represents the transpose of matrix L, U and D are unitary matrices, U T and D T represent the transposes of matrices U and D, Σ is an m×n matrix, that is, only the elements on the main diagonal are non - zero values, and the eigenvalues on the diagonal are singular values, Σ T represents the transpose of matrix Σ;

[0096] L m×n is decomposed by SVD into U m×m Σ m×n D n×n T , and the formula is as follows:

[0097] svd[L m×n = [U m×m ,Σ m×n ,D n×n T ;

[0098] Among them, L m×n is an m×n - order matrix, svd[L m×n represents performing SVD decomposition on matrix L m×n , and matrices U and D are unitary matrices, that is, they satisfy U T U = DT D = I, where the subscript m×n indicates that the matrix is an m×n square matrix, T represents the transpose of the matrix, and Σ m×n is an m×n matrix, i.e., only the elements on the main diagonal are non-zero values, and the eigenvalues on the diagonal are singular values. The formula is expressed as:

[0099]

[0100] where diag(.) represents the elements arranged diagonally, λ1,..., λ r are r eigenvalues, and H is a diagonal matrix, where the elements are eigenvalues;

[0101] When the covariance matrix P is symmetric and positive definite, U is equal to D, i.e., L = UΣU T . Substituting the SVD decomposition values gives:

[0102]

[0103] where S i represents the particle point set, λ represents the scale factor, represents the mean vector, N represents the number of particle point sets, and T represents the transpose of the matrix;

[0104] The covariance matrix of the observed quantity is expressed as:

[0105]

[0106] where, P ytyt is the covariance matrix of the observed quantity, is the weight of the i-th particle, y i,t|t-1 is the predicted value of the i-th particle's observation at time t, is the predicted mean of the observed quantity, is the weighted average of the predicted value of the i-th particle's observation and the predicted value. N is the number of particle point sets, and T is the transpose of the matrix;

[0107] Similarly, the unscented particle filter decomposition using the SVD algorithm for the above formula gives the following formula, and the initial position after filtering is calculated through the following formula:

[0108]

[0109] where, represents a representation form of the observation covariance matrix.

[0110] Preferably, the steps of the unscented particle filter based on the SVD algorithm are as follows:

[0111] Step 1: At t = 0, initialize the particles to generate n particle sets that follow the p(x0) distribution, expressed as and the mean value corresponding to the particles and covariance

[0112] where p(x0) represents the probability distribution of the initial state, and the initial particle set contains n particles

[0113] Step 2: At time t>0, importance sampling is performed through the following formula:

[0114]

[0115] Calculate the sigma point set and weights of the n-dimensional state variable. In the time update stage, use the state transition function to predict the sigma point set at the next moment;

[0116] Step 3: In the observation update stage, through the formula Use the SVD decomposition value to replace the inverse matrix of the observation covariance to obtain the filtering estimate and the sampled particle distribution at time t which is the initial position after filtering;

[0117] where, represents the state estimate value obtained by weighted average at time t, represents the covariance matrix of the i-th particle at time t, and N(.) represents the Gaussian distribution.

[0118] Preferably, S4 is specifically: resample from the sampled particle distribution to obtain a new particle set where is the weight corresponding to each particle, and the weight of each particle is equal to

[0119] To obtain the state estimate value at each moment and the corresponding covariance Then repeat Step 2, Step 3 and S3. When the change amount of the state variable is less than the threshold, the iteration ends, as shown in the following formula where represents the state variable of the i-th particle at time t+1, represents the state variable of the i-th particle at time t, and ε represents a positive number, usually set to 10 -9 ~10 -4 .

[0120] Preferably, the state model of the multi-source fusion positioning system is expressed as:

[0121] l k = f(l k -1) + v k-1 ;

[0122] where \(l\) k = \([x\) k , y\) k \) T is the system state at the \(k\)-th step, \(f(.)\) is the state equation, \(v\) k-1 is the system noise, \(x\) k is the system state value at time \(k\), \(y\) k is the measurement value at time \(k\), and \(T\) is the transpose of the matrix. This model establishes a spatial dynamic model of the system state by fusing the state information of multi-source sensors, outputs the joint estimated value of the system state, and the output result is used as the input of the importance probability density function to generate the sampling particle distribution, which directly affects the initial state sampling of the particles.

[0123] Preferably, the system state equation is expressed as:

[0124]

[0125] where \(L_{step}\) k-1 represents the step size at the \((k - 1)\)-th step, \(\theta\) k-1 represents the heading angle estimated at the \((k - 1)\)-th moment, \(x\) k-1 is the system state value at time \(t - 1\), \(y\) k-1 is the measurement value at time \(t - 1\). The input data of this model are the system state and motion model parameters at the current moment. Based on the physical kinematic model, it describes the dynamic evolution of the state variables through mathematical equations, outputs the predicted value of the system state at the next moment, and the predicted value is used for the comparison of the observed values of the multi-source fusion observation model and, at the same time, as the basis for state transition in the particle filter to update the state distribution of the particles.

[0126] Preferably, the multi-source fusion observation model is expressed as:

[0127] \(Y\) k = h(l\) k ) + u\) k ;

[0128] where \(Y\) k is the observed quantity, \(u\) k is the measurement noise, \(h(.)\) is the state transition equation, and the observation model is specifically expressed as:

[0129]

[0130] where \(Z\) k is the extended fingerprint positioning result, \(\alpha\) is the positioning result of the extended fingerprint in the observation model, \(\beta\) is the weight of the state value of the extended fingerprint in the observation model, and \(\alpha+\beta = 1\), is the state component, is the predicted value. The input data of this model are real-time observation data and the predicted values of the system state. By fusing multi-source observation data, a relationship model between the observed value and the state variable is established. The output result is the prediction residual of the observed value. The residual information is used to calculate the measurement weight of each particle, and combined with the map weight to generate the final weight value, which directly affects the resampling process and the accuracy of the positioning result.

[0131] As Figure 1 shown, the present invention also provides a system adopting the indoor and outdoor navigation and positioning method based on the improved unscented particle filter algorithm as described above, including:

[0132] An indoor navigation module 9, the indoor navigation module 9 includes a microcomputer 5, and a BDS (Beidou Satellite Navigation System) 1, an INS (Inertial Orientation and Positioning Navigation System) 2, a magnetometer 3 and a barometer 4 are integrated in the microcomputer 5. The microcomputer 5 outputs the position and speed information of the moving carrier through the improved Kalman filter algorithm;

[0133] An outdoor navigation module 8, the outdoor navigation module 8 includes an opportunistic signal acquisition unit 6, and the outdoor navigation module 8 fuses different signal sources through the improved unscented particle filter algorithm to output the position and speed information of the moving carrier.

[0134] Furthermore, the outdoor navigation module 8 further includes a power supply module 7 for supplying power to the BDS 1, INS 2, magnetometer 3 and barometer 4.

[0135] The present invention combines the advantages of indoor and outdoor navigation and positioning technologies and makes up for their deficiencies. The fusion of the indoor navigation module and the outdoor navigation module is fused through the improved unscented particle filter in the present invention to obtain the only accurate and available position and speed information. The overall navigation accuracy in the three-dimensional space is significantly improved by fusing the INS 2, magnetometer 3 and barometer 4 through the improved Kalman filter method.

[0136] The above are only the preferred embodiments of the present invention and are not used to limit the present invention. For those skilled in the art, the present invention can have various changes and modifications. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. An indoor and outdoor navigation and positioning method based on an improved unscented particle filter algorithm, characterized in that It includes the following steps: S1. Obtain the information of the reference point; estimate the initial position of the positioning target through the extended fingerprint positioning algorithm based on LightGBM based on the information of the reference point; S2. Process the initial position of the positioning target through the improved unscented particle filter algorithm to obtain the filtered initial position, and use the estimated value of the sampling point calculated based on the multi-source fusion positioning system state model, system state equation and multi-source fusion observation model as the importance probability density function, and obtain the sampling particle distribution through the importance probability density function; S3. Calculate the measurement weight and map weight of each particle, and calculate the final weight value from the measurement weight and map weight; S4. Resample to generate a new particle set, and perform the calculation in S3 to obtain the final weight value, and continuously iterate to calculate the optimal value of the final weight value; S5. Use the optimal value of the final weight value to weight each step of the particles to obtain the fusion positioning result and obtain the real-time position of the measurement target.

2. The indoor and outdoor navigation and positioning system based on the improved unscented particle filter algorithm according to claim 1, wherein When estimating the initial position of the positioning target through the extended fingerprint positioning algorithm based on LightGBM, it includes the following steps: In the offline stage, fingerprints are collected at each reference point, and the position and fingerprint information of each reference point together constitute the fingerprint database, and the fingerprint database Z is expressed as: Among them, FP i is the fingerprint of the i-th reference point, P i is the position coordinate of the i-th reference point, where i takes integers from 1 to N RP and N RP is the total number of all reference points, and RP is the reference point; Fingerprint FP of the reference point i Expressed as a 1×N AP vector, as shown in the following equation: Among them, is the signal strength received by the i-th reference point from the n-th access point; The user's real-time fingerprint information FP user Expressed as: Among them, RSS n is the signal strength received by the user from the nth access point, where n takes integers from 1 to N AP , N AP is the total number of all access points, and AP is the access point.

3. The indoor and outdoor navigation and positioning system based on the improved unscented particle filter algorithm according to claim 1, characterized in that, When processing the initial position of the positioning target through the improved unscented particle filter algorithm to obtain the filtered initial position, it includes the following steps: The Cholesky decomposition is defined as follows: Suppose a matrix P is an m-order symmetric positive definite matrix, then there exists a matrix L with positive diagonal elements such that when the diagonal elements of matrix L are positive, a unique solution is obtained: P = LL T ; when P is not positive definite, subsequent filtering updates cannot be performed, so the SVD decomposition is used to replace the Cholesky decomposition of the state estimation covariance, and the formula is expressed as: P = LL T = UΣD T DΣ T U T = UΣΣ T U T ; where, L represents the lower triangular matrix obtained by Cholesky decomposition, L T represents the transpose of matrix L, U and D are unitary matrices, U T and D T represent the transposes of matrices U and D, Σ is an m×n matrix, that is, only the elements on the main diagonal are non-zero values, and the eigenvalues on the diagonal are singular values, Σ T represents the transpose of matrix Σ; L m×n is decomposed into U by SVD m×m Σ m×n D n×n T , and the formula is as follows: svd[L m×n = [U m×m , Σ m×n , D n×n T ; Among them, L m×n is an m×n matrix, and svd[L m×n represents performing SVD decomposition on matrix L m×n . The U and D matrices are unitary matrices, that is, they satisfy U T U = D T D = I. The subscript m×n indicates that this matrix is an m×n square matrix, T represents the transpose of the matrix, and Σ m×n is an m×n matrix, that is, only the elements on the main diagonal are non-zero values, and the eigenvalues on the diagonal are singular values. The formula is expressed as: where diag(.) represents the diagonal arrangement of elements, λ1,..., λ r are r eigenvalues, and H is an angular matrix; When the covariance matrix P is symmetric and positive definite, U is equal to D, that is, L = UΣU T , substituting the SVD decomposition values gives: Among which S i is the particle point set, λ is the scale factor, is the mean vector, and N is the number of the particle point set; The observation covariance matrix is expressed as: where P ytyt is the covariance matrix of the observed quantities, W i P is the weight of the i-th particle, y i,t|t-1 is the predicted value of the observation of the i-th particle at time t, is the predicted mean of the observed quantities, is the weighted average of the predicted value of the observation of the i-th particle and the predicted value; Similarly, the unscented particle filter decomposition using the SVD algorithm for the above formula obtains the following formula, and the filtered initial position is calculated through the following formula: Among them, represents a form of the observation covariance matrix.

4. The indoor and outdoor navigation and positioning system based on the improved unscented particle filter algorithm according to claim 3, characterized in that, The specific steps of the unscented particle filter based on the SVD algorithm are as follows: Step 1. At t = 0, initialize the particles, generating a set of n particles that follow the p(x0) distribution, denoted as and the mean value corresponding to the particles and covariance where p(x0) represents the probability distribution of the initial state, and the initial particle set contains n particles Step 2. Perform importance sampling at time t>0 through the following formula: Calculate the sigma point set and weight of the n-dimensional state variable, and predict the sigma point set at the next moment using the state transition function in the time update stage; Step 3: Observation update stage. Through the formula Replace the inverse matrix of the observation covariance with the SVD decomposition value to obtain the filtering estimation and the sampling particle distribution at time t which is the initial position after filtering; Among them, represents the state estimate value obtained by weighted average at time t, and P t i represents the covariance matrix of the i-th particle at time t, and N(.) represents the Gaussian distribution.

5. The indoor and outdoor navigation and positioning system based on the improved unscented particle filter algorithm according to claim 4, wherein Specifically, S4 is: resampling from the sampled particle distribution to obtain a new particle set where is the weight corresponding to each particle, and the weights of each particle are equal after resampling to be To obtain the state estimation value at each moment and the corresponding covariance Then repeat Step 2, Step 3, and S3. When the change amount of the state variable is less than the threshold, the iteration ends, as shown in the following formula where represents the state variable of the i-th particle at the (t + 1)-th moment represents the state variable of the i-th particle at the t-th moment, and ε represents a positive number 6. The indoor and outdoor navigation and positioning system based on the improved unscented particle filter algorithm according to claim 1, wherein The multi-source fusion positioning system state model is expressed as: l k = f(l k - 1) + v k-1 ; where l k = [x k , y k T is the system state at the k-th step, f(.) is the state equation, v k-1 is the system noise, x k is the system state value at time k, y k is the measurement value at time k; this model establishes a spatial dynamic model of the system state by fusing the state information of multi-source sensors, outputs the joint estimated value of the system state, and uses the output result as the input of the importance probability density function to generate the sampling particle distribution.​ 7. The indoor and outdoor navigation and positioning system based on the improved unscented particle filter algorithm according to claim 1, characterized in that, The system state equation is expressed as: where Lstep k-1 is the step size at the (k - 1)-th step, θ k-1 is the estimated heading angle at the (k - 1)-th moment, x k-1 is the system state value at the (t - 1)-th moment, y k-1 is the measurement value at the (t - 1)-th moment; the input data of this model are the system state and motion model parameters at the current moment. Based on the physical kinematic model, the dynamic evolution of state variables is described by mathematical equations, and the predicted value of the system state at the next moment is output. The predicted value is used to compare with the observed value of the multi-source fusion observation model and, at the same time, as the basis for state transition in the particle filter to update the state distribution of particles.

8. The indoor and outdoor navigation and positioning system based on the improved unscented particle filter algorithm according to claim 1, wherein The multi-source fusion observation model is expressed as: Y k = h(l k ) + u k ; where Y k is the observable quantity, u k is the measurement noise, h(.) is the state transition equation, and the observation model is specifically expressed as: Among them, Z k is the extended fingerprint positioning result, α is the positioning result of the extended fingerprint in the observation model, β is the state value weight of the extended fingerprint in the observation model, and α + β = 1. is the state component, is the predicted value; the input data of this model are the real-time observation data and the predicted values of the system state. By fusing multi-source observation data, a relationship model between the observed value and the state variable is established, and the output result is the predicted residual of the observed value. The residual information is used to calculate the measurement weight of each particle and generate the final weight value in combination with the map weight.

9. A system adopting the indoor and outdoor navigation and positioning method based on the improved unscented particle filter algorithm as described in claim 1, characterized in that, It includes: The indoor navigation module (9), the indoor navigation module (9) includes a microcomputer (5), and the BDS (1), INS (2), magnetometer (3) and barometer (4) are integrated in the microcomputer (5), and the microcomputer (5) outputs the position and speed information of the moving carrier through the improved Kalman filter algorithm; The outdoor navigation module (8), the outdoor navigation module (8) includes an opportunistic signal acquisition unit (6), and the outdoor navigation module (8) fuses different signal sources through the improved unscented particle filter algorithm to output the position and speed information of the moving carrier.

Images