A filtering positioning method, system and filter based on maximum correlation entropy

Abstract

The present invention provides a positioning filtering method, system and filter based on maximum correlation entropy, the method comprising: obtaining the current state parameters of the carrier, obtaining the IMU state equation of the dynamic model and the UWB observation equation based on Kalman filtering; establishing an IMU-UWB hybrid positioning model based on the IMU state equation and the UWB observation equation; solving the IMU-UWB hybrid positioning model based on the maximum correlation entropy criterion, obtaining the analytical expression of the fixed point, and outputting the positioning result. The present invention can not only effectively weaken the influence of UWB observation anomalies, avoid the problem of matrix numerical instability caused by observation anomalies, and has good robustness; but also shows optimal performance in both positioning accuracy and positioning robustness.

Description

Technical Field

[0001] The present invention relates to the field of filtering technology, and in particular to a filtering positioning method, system and filter based on maximum correlation entropy. Background Art

[0002] Maximum Correntropy (MC) is a novel feature selection method based on information entropy. It selects the most relevant features by maximizing the direct correlation entropy between features and target variables. It plays a very important role in the field of numerical simulation verification.

[0003] At present, some MC filtering positioning methods mainly use fixed point iteration to obtain the posterior state of the state model for positioning, but this method is time-consuming, especially when large wild values ​​appear, the iteration may be difficult to converge, resulting in poor positioning robustness. Some MC filtering positioning methods also use empirical fixed Gaussian kernel bandwidth; but in practical applications, fixed Gaussian kernel bandwidth is difficult to adapt to real-time changing noise and observation anomalies, and cannot guarantee positioning accuracy, especially when large observation anomalies appear, which may cause matrix singularity, making the values ​​unclear or unsolvable, resulting in positioning failure. In addition, some scholars have proposed a method of adaptively calculating Gaussian kernel bandwidth for filtering positioning. Although this method has achieved better results than fixed Gaussian kernel bandwidth, it mainly searches for the optimal bandwidth parameters through iteration to meet the maximum correlation entropy of the cost function. This method is not only inefficient but also costly.

[0004] Therefore, there is an urgent need for a filtering positioning method that can not only identify observation anomalies and avoid numerical instability problems caused by matrix singularity caused by fixed Gaussian kernel bandwidth, but also ensure positioning accuracy, high computational efficiency, good timeliness and robustness. Summary of the invention

[0005] The present invention provides a positioning filtering method, system and filter based on maximum correlation entropy, which are used to simultaneously solve the problems that the existing filtering positioning method cannot identify observation anomalies, has low positioning accuracy, low calculation efficiency and high cost.

[0006] In a first aspect, the present invention provides a positioning filtering method based on maximum correlation entropy, the method comprising:

[0007] Obtain the current state parameters of the carrier, obtain the IMU state equation of the dynamic model and the UWB observation equation based on Kalman filtering;

[0008] Establishing an IMU-UWB hybrid positioning model based on the IMU state equation and the UWB observation equation;

[0009] The IMU-UWB hybrid positioning model is solved based on the maximum correlation entropy criterion, an analytical expression of the fixed point is obtained, and the positioning result is output.

[0010] Furthermore, the IMU-UWB hybrid positioning model is:

[0011]

[0012] in, Represents the IMU state equation at time k-1; represents the predicted UWB observation equation at time k; represents the predicted IMU state parameters at time k; represents the UWB observation equation at time k; represents the noise term, , Represents the IMU state noise vector; represents the UWB observation noise vector.

[0013] Furthermore, the noise term satisfy:

[0014] ;

[0015] in, Represents the IMU state equation The corresponding state covariance matrix; ], represents the measurement noise matrix; respectively and Perform Cholesky decomposition to obtain and ; , represents an augmented variable.

[0016] Furthermore, the solution includes:

[0017] Multiply each item of the IMU-UWB hybrid positioning model by ,make , , , we get: ;

[0018] The cost function is obtained based on the maximum relevant entropy criterion, and the predicted IMU state parameters are Substitute the cost function into the Gaussian kernel In, get ;in, is the key matrix, , represents the Gaussian kernel function, It represents the error between the predicted value and the true value. is the number of states, is the number of observations;

[0019] The predicted IMU state parameters obtained are Expressed as an analytical expression for a fixed point.

[0020] Furthermore, the analytical expression of the fixed point is: ;

[0021] in, represents the predicted IMU state parameters at time k; represents the design matrix, let , is the Jacobian matrix, is the identity matrix.

[0022] Furthermore, the method further comprises:

[0023] According to the key matrix Correct the measurement noise matrix to obtain the predicted measurement noise matrix ; And the measurement noise matrix in the covariance matrix corresponding to the predicted UWB observation equation Replaced by the predicted measurement noise matrix , get the predicted IMU state parameters and with The corresponding predicted state covariance matrix , output the positioning result;

[0024] Among them, the predicted measurement noise matrix for: ; The calculation method is: ; ; Represents the measurement information of the UWB observation equation.

[0025] Further, based on the maximum correlation entropy criterion, the Gaussian kernel bandwidth is obtained by adaptive calculation ; The Gaussian kernel bandwidth The calculation method is: ;in, represents a constant, represents the L2 norm.

[0026] Furthermore, the dynamic model is a four-state nine-axis MEMS inertial dynamic model.

[0027] In a second aspect, the present invention also provides a positioning filtering system based on maximum correlation entropy, which system includes at least a dynamic model, an observation model and a hybrid positioning model, and is used to execute the steps of any of the above methods.

[0028] In a third aspect, the present invention also provides a positioning filter based on maximum correlation entropy, which filter includes at least a dynamic model, an observation model and a hybrid positioning model, and is used to execute the steps of any of the above methods.

[0029] In general, the technical solution conceived by the present invention can achieve the following beneficial effects compared with the prior art:

[0030] (1) The present invention provides a positioning filtering method, system and filter based on maximum correlation entropy. By tightly combining the IMU state equation and the UWB observation equation, not only is the combination closer, but the environmental adaptation and recognition ability is better; and by using the maximum correlation entropy for solving, an IMU-UWB hybrid positioning model can be established in the filtering process, and the maximum similarity between the output and the expected response is achieved, so that the similarity of the two random processes is maximized, so that the positioning result has higher applicability and reliability.

[0031] (2) The present invention provides a positioning filtering method, system and filter based on maximum correlation entropy. Since the time resolution of the IMU state equation and the UWB observation equation is relatively high, the present invention uses the state prediction value obtained by the IMU mechanical arrangement Instead of traditional , no time-consuming fixed point iteration is required, so there is no need to correct the IMU state equation The corresponding state covariance matrix , only the corrected measurement noise matrix , while ensuring the filtering accuracy, it also improves the calculation efficiency and reduces the calculation cost.

[0032] (3) The present invention provides a positioning filtering method, system and filter based on maximum correlation entropy. By utilizing a simple and effective variational adaptive Gaussian kernel loan calculation method, it not only effectively weakens the negative impact of observation anomalies, but also avoids the matrix numerical instability problem caused by observation anomalies in the traditional MC fixed kernel bandwidth, and has good robustness; it also shows optimal performance in both positioning accuracy and positioning robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0034] Figure 1 It is a schematic diagram of a positioning filtering method, system and filter based on maximum correlation entropy provided by the present invention. DETAILED DESCRIPTION

[0035] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings and embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work belong to the scope of protection of the present invention.

[0036] It should be noted that, in the description of the embodiments of the present invention, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method or system including a series of elements includes not only those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, method or system. In the absence of further restrictions, an element defined by the sentence "includes a ..." does not exclude the existence of other identical elements in the process, method or system including the element.

[0037] In addition, the abbreviations mentioned in the present invention are specifically:

[0038] IMU: Inertial Measurement Unit, inertial measurement unit;

[0039] UWB: Ultra-Wide Band;

[0040] MC: Maximum Correntropy, maximum correlation entropy;

[0041] MEMS: Micro-Electro-Mechanical System.

[0042] In the first aspect, the present invention proposes a positioning filtering method based on maximum correlation entropy, such as Figure 1 As shown, the method includes:

[0043] Step 101: Obtain the current state parameters of the carrier, and obtain the IMU state equation of the dynamic model and the UWB observation equation based on Kalman filtering.

[0044] Among them, IMU is a device for measuring the three-axis attitude angle and acceleration of an object; the IMU state equation of the dynamic model in the present invention can be any IMU state equation such as a three-axis IMU, a six-axis IMU or a nine-axis IMU.

[0045] As an embodiment of the present invention, a four-state nine-axis MEMS inertial dynamics model is selected to obtain the current state parameters of the carrier and obtain the IMU state equation.

[0046] When positioning in a small-scale scenario, the gyroscope has low accuracy and cannot sense the earth's rotation information. At the same time, the carrier is in a low-speed motion state, so there is no need to consider the influence of the earth's rotation and its curvature. The nine-axis MEMS inertial dynamics model can form a heading reference system and output the carrier's attitude in real time.

[0047] Specifically, for the carrier plane positioning, the UWB coordinate system is used as the navigation coordinate system navigation reference system (n system) to obtain the current state parameters of the carrier for: .

[0048] in, , is the carrier plane position; , is the carrier speed.

[0049] Using the strapdown inertial navigation mechanical arrangement method, the motion transfer state equation group of the carrier at time k+1 is obtained for:

[0050] ;

[0051] in, and is the plane position of the carrier in the n system at time k; and is the carrier velocity in the n system at time k; is the sampling interval of IMU, ; is the acceleration term in the n system at time k.

[0052] It should be noted that the acceleration term ; It represents the attitude information output by the heading reference system, and the 2-D positioning is the heading angle; It represents the IMU acceleration output specific force term in the system (b system); represents the white noise of the accelerometer.

[0053] The IMU state equation is: ;in, Represents the IMU state equation at time k-1; Represents the motion transfer state equations of the carrier at time k-1; Represents the IMU state noise vector, which is Gaussian white noise and satisfies , is the process noise matrix.

[0054] Discretized state error linear model corresponding to the IMU state equation for: ;in, is the state error transfer matrix, according to The expression can be obtained as: .

[0055] The state covariance matrix corresponding to the IMU state equation for: ;in, , is the variance intensity matrix corresponding to Gaussian white noise, and its dimension unit is consistent with that of power spectral density.

[0056] The UWB observation equation based on Kalman filtering can be any one of the UWB observation equations such as extended Kalman filtering (EKF), unscented Kalman filtering (UKF) or cubature Kalman filtering (CKF).

[0057] Based on the IMU state equation, the UWB observation equation is obtained to obtain the predicted UWB observation equation for: ;in, represents the UWB observation equation at time k; Represents the UWB observation noise vector, which is Gaussian white noise and satisfies , is the measurement noise matrix.

[0058] Since IMU-UWB usually includes loose combination and tight combination, compared with the loose combination, the tight combination method is more thoroughly integrated, the sensor information is more closely combined, the environmental adaptability is better, and the reliability of the hybrid positioning model is better guaranteed. Therefore, the present invention preferably uses the UWB observation equation of the tight combination of nonlinear equations.

[0059] Preferably, due to the mechanical arrangement of the IMU state equations through motion transfer, the predicted UWB observation equation is obtained for , the corresponding information error equation for ;in, , virtual observation value satisfy , The locations of the UWB base stations are ; It is the actual distance measurement of UWB.

[0060] When based on EKF, the error equation needs to be linearized and expanded at the IMU recursive state, which is ;in, , , is the corresponding Jacobian matrix, specifically:

[0061] ;

[0062] in, and They are and The corresponding coordinates. Usually through Sigma points and corresponding weights are used to approximately describe the random distribution characteristics of the state vector. is the number of states, while ensuring the distribution characteristics of the input variables, the cost function of approximating the output performance index is minimized. As a preferred embodiment of the present invention, .

[0063] The state prediction value and the corresponding state covariance matrix obtained by the IMU mechanical arrangement are generated based on the symmetric sampling strategy of Cholesky decomposition Sigma Points ,

[0064] ;

[0065] ;

[0066] for The dimension of for The Cholesky decomposition of a lower triangular matrix has positive diagonal elements. is the scale factor, ; and is a positive number, usually , Set to 0 or , which is set during initialization.

[0067] The weights of each Sigma point are:

[0068] ;

[0069] ;

[0070] ;

[0071] in, and are the weights of the mean and variance, respectively. For merging Prior knowledge of the distribution, when When is normally distributed, is the optimal value.

[0072] The UWB observation equation is filtered and updated according to the predicted UWB observation equation, then:

[0073] ;

[0074] ;

[0075] ;

[0076] in, Weighted predicted observations, is the corresponding covariance matrix.

[0077] The UWB observation equation is filtered and updated according to the actual UWB observation equation, then:

[0078] ;

[0079] ;

[0080] ;

[0081] ;

[0082] in, is the gain matrix, is the estimated error state, Predicted IMU state parameters that can be updated through error closed loop , is the corresponding state covariance matrix.

[0083] Step 102: Establish an IMU-UWB hybrid positioning model based on the IMU state equation and the UWB observation equation.

[0084] It should be noted that the IMU-UWB hybrid positioning model is:

[0085]

[0086] in, Represents the IMU state equation at time k-1; represents the predicted UWB observation equation at time k; represents the predicted IMU state parameters at time k; represents the UWB observation equation at time k; represents the noise term, , Represents the IMU state noise vector; represents the UWB observation noise vector.

[0087] Furthermore, the noise term satisfy:

[0088] ;

[0089] in, Represents the IMU state equation The corresponding state covariance matrix; ], represents the measurement noise matrix; respectively and Perform Cholesky decomposition to obtain and ; , represents an augmented variable.

[0090] Step 103: Solve the IMU-UWB hybrid positioning model based on the maximum correlation entropy criterion, obtain the analytical expression of the fixed point, and output the positioning result.

[0091] It should be noted that the maximum correlation entropy criterion is: given two random variables , let their joint distribution function be , the correlation entropy of two random variables is defined as:

[0092] ;

[0093] in, represents the expectation operator, is a shift-invariant Gaussian kernel. The conditions for selecting the Gaussian kernel function need to satisfy continuous positive definiteness. The Gaussian kernel used is for:

[0094] ;

[0095] in, , is the Gaussian kernel width parameter and . Perform Taylor series expansion on the Gaussian kernel function, then:

[0096] ;

[0097] in, for The number of samples, from the above formula, we can see that the correlation entropy is non-negative, which is The weighted sum of all even-order moments of . Therefore, if and only if hour, Take the maximum value 1.

[0098] Importantly, the solution includes:

[0099] Multiply each item of the IMU-UWB hybrid positioning model by ,make , , , we get: .

[0100] In the prior art, after obtaining the cost function based on the maximum relevant entropy criterion, the solution is directly iterated. , obtain the posterior state parameters and kernel bandwidth; that is, ;in, . Among them, the cost function is: ; is the number of states, is the number of observations; is the correlation entropy of the Gaussian kernel function.

[0101] However, the present invention is based on the maximum relevant entropy criterion to obtain the cost function, and the predicted IMU state parameters Substitute the cost function into the Gaussian kernel In, get ;Finally, the predicted IMU state parameters are solved It is expressed as an analytical expression of a fixed point. is the key matrix, , represents the Gaussian kernel function, It represents the error between the predicted value and the true value. is the number of states, is the number of observations;

[0102] Specifically, the analytical expression of the fixed point is: ;in, represents the predicted IMU state parameters at time k; represents the design matrix, let , is the Jacobian matrix, is the identity matrix.

[0103] In addition, the prior art usually uses the key matrix At the same time, the state covariance matrix and the measurement noise matrix are corrected to obtain the predicted state covariance matrix and the predicted measurement noise matrix Specifically, by adjusting the formula and To modify the state covariance matrix and the measurement noise matrix.

[0104] However, due to the key matrix Included item, As the unknown parameter, the traditional solution Iteration is required, which is time-consuming and may not converge when large UWB observation anomalies occur.

[0105] Therefore, the present invention is based on the low-speed motion scenario of the carrier, and the IMU recursive value is more reliable in a short time. replace , so that the state covariance matrix No modification is required. As a preferred embodiment of the present invention, , let the state number is 4.

[0106] For the measurement noise matrix, according to the key matrix Correct the measurement noise matrix to obtain the predicted measurement noise matrix ; And predict the measurement noise matrix in the covariance matrix corresponding to the UWB observation equation Replaced by the predicted measurement noise matrix , get the predicted IMU state parameters and with The corresponding predicted state covariance matrix , output the positioning result.

[0107] Among them, the predicted measurement noise matrix for: ; The calculation method is: ; ; Represents the measurement information of the UWB observation equation.

[0108] In the prior art, the bandwidth of the Gaussian kernel is usually determined based on experience. Considering that a fixed bandwidth is difficult to adapt to time-varying noise in a complex environment, if a large UWB observation anomaly occurs, it will cause singularity.

[0109] And because It reflects the variance or error of the Gaussian kernel. Therefore, the present invention obtains the Gaussian kernel bandwidth through adaptive calculation based on the maximum correlation entropy criterion. .

[0110] Among them, the Gaussian kernel bandwidth The calculation method is: ; represents a constant, represents the L2 norm.

[0111] In a second aspect, the present invention also provides a positioning filtering system based on maximum correlation entropy, which system includes at least a dynamic model, an observation model and a hybrid positioning model, and is used to execute the steps of any of the above methods.

[0112] In a third aspect, the present invention also provides a positioning filter based on maximum correlation entropy, which filter includes at least a dynamic model, an observation model and a hybrid positioning model, and is used to execute the steps of any of the above methods.

[0113] The technical features of the system and the positioning filter are consistent with those of the above-mentioned method, and will not be described in detail here.

[0114] In summary, in non-line-of-sight and non-Gaussian complex environments for low-cost hardware, the present invention is based on the UWB observation model based on Kalman filtering and the MC criterion, and proposes an IMU-UWB robust filtering positioning method with high positioning accuracy, good robustness, good numerical stability, high computational efficiency and low cost. In addition, considering that the time resolution of IMU and UWB is relatively high, the mechanical arrangement results of IMU directly replace the posterior state that needs to be iterated in the prior art, and the positioning result is obtained by using fixed-point iteration steps that do not require time-consuming. In addition, according to the UWB observation model information, the variational Gaussian kernel bandwidth parameter calculation method is used to effectively identify UWB observation anomalies, such as the influence of non-line-of-sight errors, which not only avoids the numerical instability problem caused by the singularity of the matrix based on the traditional fixed kernel bandwidth, but also ensures the filtering accuracy, and has good timeliness and robustness.

[0115] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods of each embodiment or some parts of the embodiment.

[0116] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A positioning filtering method based on maximum correlation entropy, characterized in that: The method includes: Obtain the current state parameters of the carrier, obtain the IMU state equation of the dynamic model and the UWB observation equation based on Kalman filtering; The IMU-UWB hybrid positioning model is established based on the IMU state equation and the UWB observation equation: ; in, Represents the IMU state equation at time k-1; represents the predicted UWB observation equation at time k; represents the predicted IMU state parameters at time k; represents the UWB observation equation at time k; represents the noise term, , Represents the IMU state noise vector; represents the UWB observation noise vector; The noise term satisfy: ； in, Represents the IMU state equation The corresponding state covariance matrix; ], represents the measurement noise matrix; respectively and Perform Cholesky decomposition to obtain and ; , represents the augmented variable; The IMU-UWB hybrid positioning model is solved based on the maximum correlation entropy criterion to obtain the analytical expression of the fixed point and output the positioning result; wherein the Gaussian kernel bandwidth is obtained by adaptive calculation ; The Gaussian kernel bandwidth The calculation method is: ;in, represents a constant, represents the L2 norm; Represents the measurement information of the UWB observation equation; The solution includes: Multiply each item of the IMU-UWB hybrid positioning model by ,make , , , we get: ; The cost function is obtained based on the maximum relevant entropy criterion, and the predicted IMU state parameters are Substitute the cost function into the Gaussian kernel In, get ;in, is the key matrix, , represents the Gaussian kernel function, It represents the error between the predicted value and the true value. is the number of states, is the number of observations; The predicted IMU state parameters obtained are Expressed as an analytical expression for a fixed point; The method further comprises: according to a key matrix Correct the measurement noise matrix to obtain the predicted measurement noise matrix ; And the measurement noise matrix in the covariance matrix corresponding to the predicted UWB observation equation Replaced by the predicted measurement noise matrix , get the predicted IMU state parameters and with The corresponding predicted state covariance matrix , output positioning results; among them, the predicted measurement noise matrix for: ; The calculation method is: ; ; Represents the measurement information of the UWB observation equation.

2. A positioning filtering method based on maximum correlation entropy as claimed in claim 1, characterized in that: The analytical expression of the fixed point is: ; in, represents the predicted IMU state parameters at time k; represents the design matrix, let , is the Jacobian matrix, is the identity matrix.

3. A positioning filtering method based on maximum correlation entropy as claimed in claim 1, characterized in that: The dynamic model is a four-state nine-axis MEMS inertial dynamic model.

4. A positioning filtering system based on maximum correlation entropy, characterized in that: The system comprises at least a dynamic model, an observation model and a hybrid positioning model, and is used to execute the steps of the method described in any one of claims 1 to 3.

5. A positioning filter based on maximum correlation entropy, characterized in that: The filter comprises at least a dynamic model, an observation model and a hybrid positioning model, and is used to execute the steps of the method described in any one of claims 1 to 3.

Images