Inertial navigation attitude compensation method based on adaptive unscented Kalman filter

Abstract

This invention discloses an inertial navigation attitude compensation method based on adaptive unscented Kalman filtering, mainly addressing the inaccuracy problem of existing unscented Kalman filtering technologies. The implementation scheme involves: determining the state vector of the adaptive unscented Kalman filter at each time step; determining the measurement vector of the adaptive unscented Kalman filter at each time step; establishing a maximum likelihood function with respect to the adaptive coefficients using the measurement vectors; obtaining the optimal adaptive coefficients by comparing the likelihood functions at each time step under different states; calculating the state covariance matrix and the measurement covariance matrix based on the selected adaptive coefficients; calculating the gain coefficients of the adaptive unscented Kalman filter based on the state covariance matrix and the measurement covariance matrix; and updating the state of the inertial navigation attitude information at each time step based on the gain coefficients to obtain the compensated state. This invention can accurately measure inertial navigation attitude data, improving indoor positioning accuracy, and can be used in indoor positioning systems.

Description

Technical Field

[0001] This invention belongs to the field of navigation technology, and in particular relates to an inertial navigation positioning method that can be used in indoor positioning systems. Background Technology

[0002] Indoor positioning technology is indispensable in modern life. The commonly used outdoor positioning and navigation global positioning system has weak signal and cannot guarantee the validity of information in complex indoor environments. In order to meet the needs of various indoor positioning, researchers have developed many positioning systems suitable for indoor environments, from single data source positioning to multiple methods to assist each other in positioning. Some technologies can achieve satisfactory positioning results.

[0003] Since the 1960s, Kalman filtering theory has been used to eliminate random disturbances in a system based on its measured values. Kalman filtering achieves the filtering process by updating the mean and covariance in real time. It is an estimation method suitable for linear systems, but in reality, systems whose states need to be estimated are often nonlinear. Therefore, in the following decades, a series of nonlinear estimation algorithms based on Kalman filtering have been developed, such as extended Kalman filtering, unscented Kalman filtering, and particle filtering. Among them:

[0004] Extended Kalman filtering first linearizes the nonlinear system and then uses Kalman filtering to estimate it. This filtering method performs well for systems with low nonlinearity, but it is difficult to provide reliable estimates in cases of high nonlinearity.

[0005] Unscented Kalman filtering achieves filtering through unscented transformation. This type of filtering can provide higher-order estimation accuracy while requiring less computation, and it can also provide good estimation results for systems with high nonlinearity.

[0006] Particle filtering is a completely nonlinear estimator that performs better for nonlinear systems, but it increases computational cost significantly and is used in specific high-precision applications.

[0007] The Kalman filter and its extended forms mentioned above are all based on the known model. Although they can achieve good estimation for stable systems, the filtering effect will deteriorate and the estimation results will become more biased when the model is uncertain. This leads to poorer positioning results when multiple motions are combined.

[0008] Beijing Institute of Technology disclosed a "Combined Positioning Method Based on Unscented Kalman Filtering" in its patent application CN201710365995.5. This method first obtains inertial navigation motion data using an inertial navigation module, then calculates the visible light positioning coordinates using a visible light positioning unit. Finally, it fuses the two data using an unscented Kalman filter algorithm to improve positioning accuracy. However, the unscented Kalman filter used in this method has a scale parameter that is selected before the filtering experiment and remains unchanged throughout the process. This scale parameter limitation severely affects the quality of state estimation, resulting in poor positioning accuracy. Summary of the Invention

[0009] The purpose of this invention is to address the shortcomings of the existing technology by proposing an indoor positioning method based on adaptive unscented Kalman filtering, which improves positioning accuracy by selecting the highest value of the scale parameter in the unscented Kalman filter through the maximum likelihood function method.

[0010] To achieve the above objectives, the technical solution adopted by the present invention includes the following steps:

[0011] (1) Determine the state vector X of the adaptive unscented Kalman filter at each time step. k :

[0012] (1a) Use the BWT901BLECL inertial navigation module to measure the attitude at each moment and form the state vector X;

[0013] (1b) Perform an unscented transformation on the attitude at each time step to obtain the state vector X of the adaptive unscented Kalman filter. k ;

[0014] (2) Determine the measurement vector Z of the adaptive unscented Kalman filter at each time step. k :

[0015] (2a) Use the inertial navigation module to measure the velocity error at each moment and form the measurement vector Z;

[0016] (2b) Perform an unscented transformation on the velocity error at each time step to obtain the measurement vector Z of the adaptive unscented Kalman filter. k ;

[0017] (3) Determine the adaptive coefficients of the adaptive unscented Kalman filter at each time step using the maximum likelihood function method:

[0018] (3a) Based on the measurement vector Z k Establish the following likelihood function:

[0019]

[0020] in, For measurement vector Z k The average value, Let α be the covariance and α be the adaptive coefficient.

[0021] (3b) Based on the likelihood function established in (3a), determine the most suitable adaptive coefficients:

[0022] (3b1) Specify the initial state i = 0 as the feasible region of the adaptive coefficient α at time k: k min ≤α≤k max ;

[0023] (3b2) Set the step value m, and calculate the adaptive coefficient α in state i, which takes the value i*m+k. min probability density at time

[0024]

[0025] (3b3) Compare the current state i with the probability density of the highest value of the adaptive coefficient α in the previous state i-1, and select the optimal adaptive coefficient α:

[0026] If the probability density of the current state i is greater than the probability density of the previous state i-1, then take α = i*m + k. min ,

[0027] Otherwise, α = (i-1)*m + k min ;

[0028] (4) The state covariance matrix is ​​obtained based on the selected adaptive coefficient α. and measurement covariance matrix Substituting these two matrices into the Kalman filter gain formula, we can determine the gain coefficient E of the adaptive unscented Kalman filter at each time step. k ;

[0029] (5) Based on the selected gain coefficient E k Update the state of the inertial navigation attitude information at each time step to obtain the compensated state.

[0030]

[0031] This invention addresses the issue that the scale parameter of the unscented Kalman filter can significantly impact the quality of system state estimation. By adding an adaptive adjustment function to the filter and adjusting the scale parameter of the unscented Kalman filter by adjusting the covariance of the state vector and the observation vector, the robustness and average positioning accuracy of indoor positioning are further improved. Attached Figure Description

[0032] Figure 1This is a flowchart illustrating the implementation of the present invention;

[0033] Figure 2 This is a diagram showing the movement trajectory of the inertial navigation module in this invention.

[0034] Figure 3 This is a simulation comparison diagram of heading angles acquired using the present invention and existing methods. Detailed Implementation

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

[0036] The measurement device in this example is an indoor unmanned vehicle equipped with a BWT901BLECL inertial navigation module. The main control unit of the unmanned vehicle is an STM32 microcontroller, and the BWT901BLECL inertial navigation module is connected to the STM32 microcontroller on the vehicle via a serial port.

[0037] Reference Figure 1 The implementation steps for this example are as follows.

[0038] Step 1: Determine the state vector X of the adaptive unscented Kalman filter at each time step. k .

[0039] 1.1) Obtain the attitude values ​​measured by the BWT901BLECL inertial navigation module at each time step, and form the state vector X:

[0040] The microcontroller mounted on the autonomous vehicle uses interrupt control to control its BWT901BLECL inertial navigation module to collect attitude data every second, forming a state vector X.

[0041]

[0042] Where, δv x ,δv y These are the velocity errors in the x-axis and y-axis directions, respectively. ε represents the azimuth misalignment angles along the x, y, and z axes, respectively. x , ε y The gyroscope drift along the x-axis and y-axis are respectively; Δ x Δ y The zero bias of the accelerometers is shown for the x-axis and y-axis directions, respectively.

[0043] 1.2) Perform an unscented transformation on the attitude at each time step to obtain the state vector X of the adaptive unscented Kalman filter. k :

[0044] 1.2.1) Calculate the state vector at the k-th positioning time where state i takes different values.

[0045]

[0046]

[0047]

[0048] In the formula, i is the state number, i∈[0, 2n]; α is the adaptive coefficient that affects accuracy; n is the expansion coefficient of the unscented transformation, n∈[1, ∞);

[0049] 1.2.2) Based on the 2n+1 adaptive unscented Kalman filter state vectors at the k-th positioning time, calculate the final adaptive unscented Kalman filter state vector X at the k-th positioning time. k :

[0050]

[0051] Step 2: Determine the measurement vector Z of the adaptive unscented Kalman filter at each time step. k .

[0052] 2.1) The microcontroller mounted on the unmanned vehicle uses interrupt control to control its BWT901BLECL inertial navigation module to collect velocity error data every second, forming the measurement vector Z:

[0053] Z = [δv] x ,δv y ]

[0054] Where, δv x ,δv y These are the velocity errors in the x-axis and y-axis directions, respectively.

[0055] 2.2) Perform an unscented transformation on the velocity error at each time step to obtain the attitude vector Z of the adaptive unscented Kalman filter. k :

[0056] 2.2.1) Calculate the attitude vector for state i taking different values ​​at the k-th positioning time.

[0057]

[0058]

[0059]

[0060] In the formula, i is the state number, i∈[0, 2n]; α is the adaptive coefficient that affects accuracy; n is the expansion coefficient of the unscented transformation, n∈[1, ∞);

[0061] 2.2.2) Based on the 2n+1 adaptive unscented Kalman filter attitude vectors at the k-th positioning time, calculate the final adaptive unscented Kalman filter attitude vector Z at the k-th positioning time. k :

[0062]

[0063] Step 3: Determine the adaptive coefficients of the adaptive unscented Kalman filter at each time step.

[0064] 3.1) Based on the measurement vector Z k Establish the following likelihood function:

[0065]

[0066] in, For measurement vector Z k The average value, Let α be the covariance and α be the adaptive coefficient.

[0067] 3.2) Based on the likelihood function established in (3.1), determine the most suitable adaptive coefficients:

[0068] 3.2.1) The initial state i = 0 is designated as the feasible region of the adaptive coefficient α at time k: k min ≤α≤k max ,

[0069] Where k min For the BWT901BLECL inertial navigation module, measure the start time, k max The measurement end time for the BWT901BLECL inertial navigation module;

[0070] 3.2.2) Set the step value m, and calculate the adaptive coefficient α in state i, which takes the value i*m+k. min probability density at time

[0071]

[0072] 3.2.3) Compare the current state i with the probability density of the highest value of the adaptive coefficient α in the previous state i-1, and select the optimal adaptive coefficient α:

[0073] If the probability density of the current state i is greater than the probability density of the previous state i-1, then choose α = i*m + k. min ,

[0074] Otherwise, choose α = (i-1)*m + k min ;

[0075] Step 4: Determine the gain coefficient E of the adaptive unscented Kalman filter at each time step. k .

[0076] 4.1) Calculate the state covariance matrix of the adaptive unscented Kalman filter at time k.

[0077]

[0078] in, X is a state vector that takes different values ​​for state i at the k-th positioning time. k Let be the adaptive unscented Kalman filter state vector at the k-th positioning time.

[0079] 4.2) Calculate the measurement covariance matrix of the adaptive unscented Kalman filter at time k.

[0080]

[0081] in, Z is the state vector that takes different values ​​for state i at the k-th positioning time. k Let be the adaptive unscented Kalman filter state vector at the k-th positioning time.

[0082] 4.3) Based on the state covariance matrix of the adaptive unscented Kalman filter and measurement covariance matrix The gain coefficient E of the adaptive unscented Kalman filter at the k-th positioning time is obtained. k :

[0083]

[0084] in for Inverse operation.

[0085] Step 5, based on the selected gain coefficient E k Update the state of the inertial navigation attitude information at each time step to obtain the compensated state.

[0086]

[0087] The effects of this invention will be further illustrated below with simulation experiments:

[0088] 1. Simulation conditions.

[0089] The hardware platform for the simulation experiment of this invention is: a laptop computer with an Intel i7-6700 processor and an indoor unmanned vehicle equipped with a BWT901BLECL inertial navigation module.

[0090] Simulation hardware and software environment: Windows 10, MATLAB R2017a.

[0091] The data used in the simulation experiment of this invention were collected from the laboratory of the West Building of Xi'an University of Electronic Science and Technology in October 2022. The sample set consists of heading angle output data from 300 sampling points collected by the BWT901BLECL inertial navigation module.

[0092] 2. Simulation content and result analysis.

[0093] To verify the compensating effect of the adaptively tuned unscented Kalman filter algorithm on attitude information, the mobile platform followed the procedure as follows: Figure 2 The trajectory shown starts from the initial position [0, 0], moves at a constant speed along a straight line for a certain distance, then turns 90 degrees to the right and continues moving. The accuracy of the heading angle data from 300 sampling points collected by the inertial navigation module is compared with that of the present invention and the existing "a combined positioning method based on unscented Kalman filtering". The results are as follows: Figure 3 As shown.

[0094] from Figure 3 It can be clearly observed that the heading angle information obtained by the adaptively tuned unscented Kalman filter of this invention is closer to the true value than the heading angle information obtained by existing unscented Kalman filters.

Claims

1. An inertial navigation attitude compensation method based on adaptive unscented Kalman filtering, characterized in that, Including the following: (1) Determine the state vector of the adaptive unscented Kalman filter at each time step. : (1a) Use the BWT901BLECL inertial navigation module to measure the attitude at each moment and form the state vector X; (1b) Perform an unscented transformation on the attitude at each time step to obtain the state vector of the adaptive unscented Kalman filter. ; (2) Determine the measurement vector of the adaptive unscented Kalman filter at each time step. : (2a) Measure the velocity error at each moment using the inertial navigation module to form the measurement vector Z; (2b) making an unscented transformation on the velocity error of each time instant to obtain a measurement vector of the adaptive unscented Kalman filter ; (3) Determine the adaptive coefficients of the adaptive unscented Kalman filter at each time step using the maximum likelihood function method: (3a) According to the measurement vector The following likelihood function is established: ； in, Measurement vector The average value, Let α be the covariance and α be the adaptive coefficient. (3b) Based on the likelihood function established in (3a), determine the most suitable adaptive coefficients: (3b1) Initial state The feasible region of the adaptive coefficient α at time k is specified as follows: ; (3b2) Set step value Calculate in state The adaptive coefficient α takes the value of probability density at time : ； (3b3) compare the current state with the previous state and select the optimal adaptive coefficient : If the probability density at the current state i is greater than the probability density at the previous state , then take , otherwise ; (4) Based on the selected adaptive coefficient The obtained state covariance matrix and measurement covariance matrix Substituting these two matrices into the Kalman filter gain formula, the gain coefficient of the adaptive unscented Kalman filter at each time step is determined. ; (5) Based on the selected gain coefficient Update the state of the inertial navigation attitude information at each time step to obtain the compensated state. : .

2. The method of claim 1, wherein, The state vector X formed in step (1a) is represented as follows: ； in , These are the velocity errors in the x-axis and y-axis directions, respectively. , , These are the azimuth misalignment angles along the x-axis, y-axis, and z-axis, respectively. , These represent the gyroscope drift along the x-axis and y-axis, respectively. , These are the zero bias of the accelerometers in the x-axis and y-axis directions, respectively.

3. The method according to claim 1, characterized in that, In step (1b), the pose at each time step is traceless. transformations, resulting in a state vector of the adaptive unscented Kalman filter This is achieved as follows: (1b1) Calculate the state at the k-th positioning time. State vectors with different values : ； ； ； In the formula, is a state sequence number, ; is an adaptive coefficient affecting precision; is an expansion coefficient of the unscented transformation ; (1b2) calculating the final adaptive unscented Kalman filter state vector at the kth positioning time instant according to the 2n+1 adaptive unscented Kalman filter state vectors at the kth positioning time instant :

4. The method of claim 1, wherein, In step (2a), the inertial navigation module is used to measure each time. The measurement vector Z, composed of the velocity error at each mark, is represented as follows: ； wherein , are the velocity errors in the x-axis, y-axis directions, respectively.

5. The method according to claim 1, characterized in that, In step (2b), the speed error at each time is transformed by unscented transformation to obtain the measurement vector of the adaptive unscented Kalman filter This is implemented as follows: (2b1) Calculate the state at the k-th positioning time. Take measurement vectors with different values : ； ； ； In the formula, For the status sequence number, ; It is an adaptive coefficient that affects accuracy; The expansion coefficients of the unscented transformation ; (2b2) ​​Based on the 2n+1 adaptive unscented Kalman filter measurement vectors at the k-th positioning time, calculate the final adaptive unscented Kalman filter measurement vector at the k-th positioning time. :

6. The method of claim 5, wherein, In step (4), the gain coefficient of the adaptive unscented Kalman filter at each positioning time is determined using the Kalman filter gain formula. The implementation is as follows: (4a) calculating a state covariance matrix of the first adaptive unscented Kalman filter at the second time instant ； in, The state at the k-th positioning time Take state vectors with different values. For the first Adaptive unscented Kalman filter state vector at each positioning time; (4b) calculating the measurement covariance matrix of the adaptive unscented Kalman filter at the :​ ； wherein, is the state at the kth positioning instant takes different values of the state vector; (4c) Based on the state covariance matrix of the adaptive unscented Kalman filter and measurement covariance matrix , obtained the Gain coefficient of adaptive unscented Kalman filter at each positioning time : ； in for Inverse operation.

Images