Kalman filtering navigation information fusion method based on intelligent optimization algorithm optimization

Abstract

The invention provides a Kalman filtering navigation information fusion method based on intelligent optimization algorithm optimization, and relates to the technical field of integrated navigation, and the method comprises the following steps: obtaining inertial navigation data and auxiliary measurement data of a navigation target at a current moment, obtaining a historical state prediction result and a historical state covariance matrix at a previous moment adjacent to the current moment; based on the inertial navigation data and the auxiliary measurement data, performing adaptive search estimation on the measurement noise matrix of the navigation target to obtain an optimal measurement noise matrix of the navigation target; calculating Kalman gain according to the initial state covariance matrix and the optimal measurement noise matrix; and correcting the initial state prediction result according to the auxiliary measurement data and the Kalman gain to obtain a state prediction result of the navigation target at the current moment and a state covariance matrix corresponding to the state prediction result. The precision of the integrated navigation system can be improved under the condition that no additional sensor is added.

Description

Technical Field

[0001] The present application relates to the field of combined navigation technology, and in particular to a Kalman filter navigation information fusion method based on intelligent optimization algorithm optimization. Background Art

[0002] The underwater environment is complex and changeable. During long-duration navigation, the Doppler odometer (DVL) measurement will inevitably be disturbed by the external environment, making the measurement noise susceptible to non-Gaussian noise contamination. The prior information of the statistical characteristics of the measurement noise is not always known, which will affect the performance of the Kalman filter algorithm. The measurement noise matrix R represents the credibility of the observation data. The selection of R has a great influence on the performance of the filtering algorithm based on the Kalman filter (KF) framework. When the observed quantity is contaminated by non-Gaussian noise such as wild values, treating R as a constant and assuming a Gaussian distribution for the measurement noise will reduce the filtering accuracy, or even filter divergence, which will lead to reduced navigation accuracy.

[0003] The prior art discloses adaptively adjusting filter parameters (measurement noise variance R, system noise variance Q, etc.) to solve the combined navigation problem when DVL measurement information is interfered by wild value interference; it also discloses improving positioning or navigation accuracy through a robust adaptive KF method, but the adaptive adjustment scheme designed based on real-time filter update information has a small amount of calculation, but poor accuracy, and there is a situation where the filter state cannot be effectively identified; the scheme of increasing external observation information (such as ultra-short baseline USBL positioning) can improve the system navigation positioning accuracy, but increases the economic cost, and cannot be applied to scenarios where accurate external observation information cannot be obtained; the adaptive adjustment scheme designed using machine learning does not require the addition of additional sensors, but is limited by the quality of training data, and generally has poor universality, resulting in low navigation accuracy. Summary of the invention

[0004] The purpose of the embodiments of the present application is to provide a Kalman filter navigation information fusion method based on intelligent optimization algorithm optimization, so as to solve the above-mentioned problems existing in the prior art, and to improve the accuracy of a strapdown inertial navigation system (SINS) / odometer (OD) or a SINS / DVL combined navigation system without adding additional sensors.

[0005] In a first aspect, a Kalman filter navigation information fusion method based on intelligent optimization algorithm optimization is provided, and the method may include:

[0006] Obtain the inertial navigation data and auxiliary measurement data of the navigation target at the current moment, as well as the historical state prediction results and historical state covariance matrix obtained at the previous moment adjacent to the current moment;

[0007] According to the historical state prediction result and the historical state covariance matrix, the state of the navigation target at the current moment is predicted to obtain the initial state prediction result and the initial state covariance matrix of the navigation target;

[0008] Based on the inertial navigation data and the auxiliary measurement data, adaptively search and estimate the measurement noise matrix of the navigation target to obtain the optimal measurement noise matrix of the navigation target;

[0009] Calculating a Kalman gain according to the initial state covariance matrix and the optimal measurement noise matrix;

[0010] According to the auxiliary measurement data and the Kalman gain, the initial state prediction result is corrected to obtain the state prediction result of the navigation target at the current moment and the state covariance matrix corresponding to the state prediction result; wherein the state prediction result is the fusion result of the inertial navigation data and the auxiliary measurement data.

[0011] In an optional implementation, based on the inertial navigation data and the auxiliary measurement data, adaptively searching and estimating the measurement noise matrix of the navigation target to obtain the optimal measurement noise matrix of the navigation target includes:

[0012] Obtain configuration initialization parameters and multiple measurement noise matrices;

[0013] Based on the pre-constructed cost function and fitness function, the fitness value of each measurement noise matrix is ​​calculated; wherein the cost function is configured based on the minimum mean square error of the speed error of the navigation target of the integrated navigation system as the optimization target; and the fitness function is the inverse of the cost function;

[0014] According to the fitness value of each measurement noise matrix, an optimal measurement noise matrix is ​​selected from each measurement noise matrix;

[0015] Based on the fitness value of each measurement noise matrix and the process optimal measurement noise matrix, adaptive update is performed until the configured termination condition is met, and the optimal measurement noise matrix is ​​output.

[0016] In an optional implementation, before calculating the fitness value of each measurement noise matrix based on the pre-constructed cost function and fitness function, the method further includes:

[0017] Calculate the evaluation index of the auxiliary measurement data at the current moment; if the evaluation index is not greater than the configured evaluation threshold, the auxiliary measurement data at the current moment is normal measurement data; if the evaluation index is greater than the configured evaluation threshold, the auxiliary measurement data at the current moment is abnormal measurement data.

[0018] In an optional implementation, before calculating the Kalman gain corresponding to the measurement noise matrix, the method further includes:

[0019] Matching a corresponding target cost function based on a determination result of whether the auxiliary measurement data is abnormal measurement data;

[0020] Based on the pre-constructed cost function and fitness function, the fitness value of each measurement noise matrix is ​​calculated, including: based on the target cost function and the corresponding fitness function, the fitness value of each measurement noise matrix is ​​calculated.

[0021] In an optional implementation, the fitness value of each measurement noise matrix is ​​calculated based on the target cost function and the corresponding fitness function, including:

[0022] For any measurement noise matrix, calculating the Kalman gain corresponding to the measurement noise matrix according to the initial state covariance matrix and the measurement noise matrix;

[0023] The initial state prediction result and the initial state covariance matrix are measured and updated according to the Kalman gain to obtain the state estimation result and the state estimation covariance matrix corresponding to the measurement noise matrix;

[0024] Using the state estimation result and the state estimation covariance matrix to correct the inertial navigation data at the current moment, to obtain the inertial navigation data corrected by the measurement noise matrix;

[0025] The inertial navigation data corrected by the measurement noise matrix and the auxiliary measurement data at the current moment are input into the target cost function and the corresponding fitness function to obtain the fitness value of the measurement noise matrix.

[0026] In an optional implementation, calculating the Kalman gain corresponding to the measurement noise matrix according to the initial state covariance matrix and the measurement noise matrix includes:

[0027] If the auxiliary measurement data at the current moment is normal measurement data, the Kalman gain corresponding to the measurement noise matrix is ​​calculated according to the initial state covariance matrix and the measurement noise matrix.

[0028] In an optional implementation, calculating the Kalman gain corresponding to the measurement noise matrix according to the initial state covariance matrix and the measurement noise matrix further includes:

[0029] If the auxiliary measurement data at the current moment is abnormal measurement data, the measurement noise matrix is ​​amplified by using the configured expansion factor to obtain an amplified measurement noise matrix;

[0030] The Kalman gain corresponding to the measurement noise matrix is ​​calculated according to the initial state covariance matrix and the amplified measurement noise matrix.

[0031] In a second aspect, a Kalman filter navigation information fusion device based on intelligent optimization algorithm optimization is provided, and the device may include:

[0032] An acquisition unit, used to acquire the inertial navigation data and auxiliary measurement data of the navigation target at the current moment, as well as the historical state prediction result and historical state covariance matrix obtained at the previous moment adjacent to the current moment;

[0033] A prediction unit, used to predict the state of the navigation target at the current moment according to the historical state prediction result and the historical state covariance matrix, and obtain the initial state prediction result and the initial state covariance matrix of the navigation target;

[0034] an estimating unit, configured to perform adaptive search and estimation on a measurement noise matrix of the navigation target based on the inertial navigation data and the auxiliary measurement data, so as to obtain an optimal measurement noise matrix of the navigation target;

[0035] A calculation unit, used for calculating the Kalman gain according to the initial state covariance matrix and the optimal measurement noise matrix;

[0036] A correction unit is used to correct the initial state prediction result according to the auxiliary measurement data and the Kalman gain to obtain the state prediction result of the navigation target at the current moment and the state covariance matrix corresponding to the state prediction result; wherein the state prediction result is a fusion result of the inertial navigation data and the auxiliary measurement data.

[0037] In a third aspect, an electronic device is provided, the electronic device comprising a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory communicate with each other via the communication bus;

[0038] Memory, used to store computer programs;

[0039] The processor is used to implement any method step described in the first aspect when executing the program stored in the memory.

[0040] In a fourth aspect, a computer-readable storage medium is provided, wherein a computer program is stored in the computer-readable storage medium, and when the computer program is executed by a processor, any method step described in the first aspect is implemented.

[0041] This application uses the strategy of linearly reducing the inertial weight with the number of iterations to improve the performance of the optimization algorithm, adaptively estimates the optimal solution of the measurement noise variance R, and improves the accuracy of combined navigation systems such as SINS / DVL or SINS / OD without adding additional sensors. This application uses the speed information in the auxiliary measurement data as the reference speed to correct the SINS inertial navigation measurement data, effectively integrating the SINS inertial navigation measurement data and the auxiliary navigation data of the carrier itself, and obtains high-precision positioning and navigation data of the navigation target at the current moment by integrating the two navigation data without relying on the reference information provided by the external navigation system.

[0042] The present application applies the optimization algorithm to the intelligent estimation of the statistical characteristics of the measurement noise, fully exploits the auxiliary measurement information, solves the problem of intelligent search and estimation of the measurement noise matrix under Gaussian conditions, and improves the filtering performance; at the same time, based on the Mahalanobis distance combined with the expansion factor, the Kalman filter optimized by the optimization algorithm is robustly improved, and the robust filtering problem under non-Gaussian conditions is solved; the fitness function optimized by the optimization algorithm is improved for robustness, and the problem of intelligent search and estimation and robust adaptive filtering of the measurement noise matrix under non-Gaussian conditions is solved, which can be applied to the combined navigation information fusion in complex underwater environments or on land environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments of the present application will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0044] Figure 1 A flowchart of a Kalman filter navigation information fusion method based on intelligent optimization algorithm optimization provided in an embodiment of the present application;

[0045] Figure 2 A flow chart of the particle swarm optimization algorithm provided in the embodiment of the present application;

[0046] Figure 3 A schematic diagram of an initial alignment structure provided in an embodiment of the present application;

[0047] Figure 4 A schematic diagram of the DVL output speed provided in an embodiment of the present application;

[0048] Figure 5 A global graph of the root mean square error of the pitch angle provided in an embodiment of the present application;

[0049] Figure 6A local enlarged diagram of the root mean square error of the pitch angle provided in an embodiment of the present application;

[0050] Figure 7 A global graph of the root mean square error of the roll angle provided in an embodiment of the present application;

[0051] Figure 8 A local enlarged view of the root mean square error of the roll angle provided in the embodiment of the present application;

[0052] Fig. 9 A global diagram of the root mean square error of the heading angle provided in the embodiment of the present application;

[0053] Fig.10 A partially enlarged diagram of the root mean square error of the heading angle provided in an embodiment of the present application;

[0054] Fig.11 A global graph of the eastward velocity root mean square error provided in an embodiment of the present application;

[0055] Fig.12 A local enlarged diagram of the root mean square error of the eastward velocity provided in an embodiment of the present application;

[0056] Fig.13 A global graph of the root mean square error of north velocity provided in an embodiment of the present application;

[0057] Fig.14 A local enlarged diagram of the root mean square error of the north velocity provided in an embodiment of the present application;

[0058] Fig.15 A schematic diagram of the position root mean square error provided in an embodiment of the present application;

[0059] Fig.16 A schematic diagram of b-system velocity error (PSO-RKF) obtained by different methods provided in the embodiments of the present application;

[0060] Fig.17 A schematic diagram of b-system velocity error (RPSO-RKF) obtained by different methods provided in the embodiments of the present application;

[0061] Fig.18 A schematic diagram of the structure of a Kalman filter navigation information fusion device based on intelligent optimization algorithm optimization provided in an embodiment of the present application;

[0062] Fig.19 A schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0063] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments of the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0064] In the process of navigation information fusion, the inertial coordinate system is usually called the i system, the earth coordinate system is called the e system, the navigation coordinate system is called the n system, the calculation coordinate system is called the n' system, the carrier coordinate system is called the b system, and the auxiliary measurement coordinate system (DVL measurement coordinate system or OD measurement coordinate system) is called the d system. Under ideal conditions, the carrier coordinate system and the auxiliary measurement coordinate system coincide, but due to the installation error of DVL or OD, there will be a deviation between the two. The SINS / DVL or SINS / OD combined navigation system uses the SINS solution speed and the DVL or OD output speed v b Obtain the observation residual, estimate the navigation error of the navigation target in real time through the optimal estimation algorithm such as KF, and then feed back the error value to correct the position, attitude, and speed calculated by SINS, so as to obtain accurate navigation information. SINS / DVL or SINS / OD combined navigation has two different combination modes: loose combination and tight combination. The tight combination mode is to combine the DVL or OD original beam measurement with SINS, and the loose combination is to combine the DVL or OD output b system velocity v b The velocity v calculated by SINS n′ Combined navigation is widely used due to its simple structure. The loose combination method uses SINS to solve the attitude matrix Convert DVL or OD output b-speed to n'speed Then use SINS to calculate the speed v n′ Find the residual, which is the difference in velocities in the n′ system.

[0065] The prior art (CN202310069689.2 SINS / DVL navigation method based on particle swarm optimization robust filtering) discloses a SINS / DVL combined navigation method. Although this method also utilizes an optimization algorithm, its optimization algorithm performs poorly when processing data containing outliers. In the process of constructing the fitness function, it does not consider how to suppress anomalies when the reference position is contaminated, resulting in its fitness function being sensitive to outliers and not robust to abnormal observations. At the same time, it uses the Huber function as a robustness measure in the particle swarm optimization algorithm, which affects the accuracy of the optimal solution in the optimization process. When there are abnormal observation data, its accuracy will be greatly reduced.

[0066] The Kalman filter navigation information fusion method based on intelligent optimization algorithm optimization provided in the embodiment of the present application can be applied in a server or in a terminal with strong computing power.

[0067] The preferred embodiments of the present application are described below in conjunction with the drawings in the specification. It should be understood that the preferred embodiments described herein are only used to illustrate and explain the present application, and are not used to limit the present application. In addition, the embodiments and features in the embodiments of the present application may be combined with each other if there is no conflict.

[0068] Figure 1 The flowchart of a Kalman filter navigation information fusion method based on intelligent optimization algorithm optimization provided in the embodiment of the present application is as follows. Figure 1 As shown, the method may include:

[0069] Step S110: Acquire the inertial navigation data and auxiliary measurement data of the navigation target at the current moment, as well as the historical state prediction result and the historical state covariance matrix obtained at the previous moment adjacent to the current moment.

[0070] The Kalman filter navigation information fusion method based on intelligent optimization algorithm optimization provided in the embodiment of the present application can be applied to land navigation scenarios and underwater navigation scenarios. When applied to underwater navigation scenarios, the navigation target is underwater detection equipment, and the auxiliary measurement data can be DVL measurement data; when applied to land navigation scenarios, the navigation target can be a vehicle, etc., and the auxiliary measurement data can be the OD measurement data of a land vehicle-mounted odometer.

[0071] In the embodiment of the present application, the inertial navigation data of the navigation target (i.e., the carrier) is the data detected by the SINS of the navigation target, including: the speed, position and attitude of the navigation target; specifically, the inertial measurement unit of the SINS is used to measure the angular velocity and linear acceleration of the b system relative to the inertial space; the attitude angle and heading angle parameters can be obtained by solving the computer attitude matrix based on the measured angular velocity, and the acceleration measured is converted into the acceleration of the navigation system by the computer, and the carrier velocity parameters are obtained after one integration, and the carrier position parameters can be obtained after two integrations. In practical applications, there is an error between the navigation coordinate system simulated by the SINS and the geographic coordinate system, so the attitude, velocity and position error equations can actually be obtained.

[0072] In the embodiment of the present application, after processing the inertial navigation data, an inertial navigation state vector is obtained, and its expression is as follows:

[0073]

[0074] Among them, x SINS represents the inertial navigation state vector; δL and δλ represent the latitude error and longitude error respectively; δvE and δv N Represents the eastward velocity error and the northward velocity error respectively; φ=[φ x ; φ y ; φ z ] represents the attitude angle error, φ x ,φ y and φ z They represent the pitch angle error, roll angle error and heading angle error respectively, and the subscripts E, N and U represent the east, north and sky directions respectively; ε b =[ε x ; ε y ; ε z ] represents the gyroscope drift error; Represents the accelerometer bias error. The subscripts x, y, and z represent the orthogonal x, y, and z axes, respectively.

[0075] In the embodiment of the present application, the state equation of the SINS system is: Among them, F SINS represents the state transfer matrix, ω SINS represents the system noise vector; the state transfer matrix F SINS It is a 13×13 matrix and can be expressed as:

[0076]

[0077] Among them, F represents a 7×7 matrix; G is a 7×6 matrix, and the specific form is as follows:

[0078]

[0079] in, Represents the posture matrix The first two lines of .

[0080] In the embodiment of the present application, the auxiliary measurement data, namely the measurement data output by the DVL device or the OD device, includes: the speed and direction of the navigation target, etc.

[0081] In an embodiment of the present application, the historical state prediction result and the historical state covariance matrix obtained at the previous moment adjacent to the current moment are the final results obtained by executing the Kalman filter navigation information fusion method optimized by the intelligent optimization algorithm at the previous moment.

[0082] In the embodiment of the present application, the velocity error in the inertial navigation data and the auxiliary measurement data at the current moment is selected as the observed value, that is, the velocity measured by SINS is selected. The velocity in the b system measured by DVL The difference of projection on the n system is taken as the observed quantity; the expression of the observed quantity is as follows:

[0083]

[0084] Among them, z k represents the observed value at time k; H k represents the measurement matrix, the measurement noise V k ~N(0,R k ), R k represents the measurement noise array; specifically, under the condition of linear small misalignment angle, Select the eastward velocity error δv E and north velocity error δv N As the observed quantity z at time k k , which is expressed as follows:

[0085]

[0086] in, Indicates the speed of the navigation target in the inertial navigation data, δv n Indicates speed error; Indicates auxiliary measurement data; v is the direction cosine matrix from b to n, i.e., the attitude matrix, which is determined based on the attitude angle detected by SINS; k represents the measurement noise vector, and v k It is usually assumed to follow a normal distribution, that is, v k ~N(0,R k ), R k represents the measurement noise covariance matrix; I represents the identity matrix.

[0087] In practical applications, replace The measurement matrix H can be obtained k for in, Representation Matrix The first two lines of .

[0088] Step S120: predict the state of the navigation target at the current moment according to the historical state prediction result and the historical state covariance matrix, and obtain the initial state prediction result and the initial state covariance matrix of the navigation target.

[0089] In the embodiment of the present application, the state of the navigation target at the current moment is predicted according to the historical state prediction result and the historical state covariance matrix, including:

[0090] First, based on the historical state prediction results of the previous moment adjacent to the current moment, the initial state prediction results of the current moment are obtained through the state transfer equation:

[0091]

[0092] in, represents the initial state prediction result at the current moment (i.e., moment k); F k-1 represents the state transfer matrix, Indicates the historical state prediction result of the previous moment (i.e., moment k-1) adjacent to the current moment;

[0093] Secondly, according to the historical state covariance matrix of the previous moment adjacent to the current moment and the process noise covariance matrix, the uncertainty of the state estimation is predicted, that is, the initial state covariance matrix corresponding to the initial state prediction result at the current moment;

[0094]

[0095] Among them, P k|k-1 represents the initial state covariance matrix at the current moment, P k-1|k-1 represents the historical state covariance matrix of the previous moment adjacent to the current moment, Q k-1 represents the process noise covariance matrix.

[0096] Step S130: Based on the inertial navigation data and the auxiliary measurement data, the measurement noise matrix of the navigation target is adaptively searched and estimated to obtain the optimal measurement noise matrix of the navigation target; and the Kalman gain is calculated according to the initial state covariance matrix and the optimal measurement noise matrix.

[0097] In practical applications, the speed measurement error of DVL in underwater navigation environment will not accumulate over time. When AUV is sailing in large depths and wide waters, DVL can provide reliable external speed auxiliary information for SINS. The complexity of the underwater environment means that the prior information of the statistical characteristics of the measurement noise is not always known in the actual dynamic alignment process. Figure 3 As shown, in underwater dynamic alignment, the external environment is complex and changeable, such as the complex changes in the underwater terrain and the changes in the water flow rate, which will affect the DVL's measurement of the velocity, and then affect the measurement noise covariance matrix (measurement noise matrix) R. The measurement noise matrix R represents the credibility of the observed data. The larger R is, the less credible the observed data is. Therefore, the selection of R has a great influence on the filtering performance. When the observed quantity is contaminated by non-Gaussian noise such as wild values, treating R as a constant and assuming a Gaussian distribution for the measurement noise will result in a reduction in filtering accuracy, or even filter divergence. Based on this, the present application establishes a cost function or a fitness function to adaptively search for the optimal measurement noise matrix.

[0098] In the embodiment of the present application, the search for the optimal measurement noise matrix may utilize a particle optimization algorithm, a genetic algorithm, a simulated annealing algorithm, a fish swarm algorithm, an ant colony algorithm, and the like.

[0099] In an embodiment of the present application, based on inertial navigation data and auxiliary measurement data, an adaptive search and estimation is performed on the measurement noise matrix of the navigation target to obtain the optimal measurement noise matrix of the navigation target, including:

[0100] Acquire the configured initialization parameters and multiple measurement noise matrices; calculate the fitness value of each measurement noise matrix based on the pre-constructed cost function and fitness function; select the process optimal measurement noise matrix from each measurement noise matrix according to the fitness value of each measurement noise matrix; perform adaptive update based on the fitness value of each measurement noise matrix and the process optimal measurement noise matrix until the configured termination condition is met, and output the optimal measurement noise matrix.

[0101] In an embodiment of the present application, before calculating the fitness value of each measurement noise matrix, the method also needs to determine whether the current moment is abnormal measurement data, specifically including: calculating the evaluation index of the acquired auxiliary measurement data, and judging whether the auxiliary measurement data at the current moment is abnormal measurement data based on the calculated evaluation index: if the calculated evaluation index of the auxiliary measurement data is not greater than the configured evaluation threshold, the auxiliary measurement data at the current moment is normal measurement data; if the calculated evaluation index of the auxiliary measurement data is greater than the configured evaluation threshold, the auxiliary measurement data at the current moment is abnormal measurement data.

[0102] In the embodiment of the present application, the Mahalanobis distance between the observation quantity at the current moment (i.e., the velocity error in the inertial navigation data and the auxiliary measurement data at the current moment) and the configured observation quantity prior estimate is used as the evaluation index of the auxiliary measurement data at the current moment; by calculating the real observation quantity The relationship between the evaluation index (i.e. auxiliary measurement data) and the configured evaluation threshold value determines the true observation value Is it an abnormal observation? If the real observation The evaluation index θ k satisfy The real observed quantity is a normal observation; on the contrary, if the real observation The evaluation index θ k satisfy The real observed quantity is the abnormal observation.

[0103] In the embodiment of the present application, the real observation amount The evaluation index θ k The real observed quantity The corresponding observation z at time k k A priori estimate of the observed quantity The Mahalanobis distance between the real observations Judging index θ at time k kis defined as follows:

[0104]

[0105] in, is the Mahalanobis distance; R i,k represents the i-th measurement noise matrix at time k.

[0106] In the embodiment of the present application, if the auxiliary measurement data is abnormal measurement data, it is necessary to use the expansion factor λ k The measurement noise covariance matrix R i,k To zoom in, that is:

[0107] In the embodiment of the present application, the expansion factor λ k The calculation method includes: Substituting formula (10) into formula (9) yields:

[0108]

[0109] in, and It follows a chi-square distribution with n1 degrees of freedom. Formula (11) can be transformed into solving λ k The nonlinear problem is as follows:

[0110]

[0111] In formula (12), λ k It can be solved by Newton's iteration method; therefore, λ k (i+1) and λ k The relationship (i) can be expressed as:

[0112]

[0113] in, And k (i) The initial value is λ k (0)=1. When the evaluation index satisfies When , the iteration terminates.

[0114] In the embodiment of the present application, after solving λ k Then, the measurement noise matrix R is calculated by formula (10) i,k Expand and obtain the new measurement noise matrix λ k R i,k . Use λ k R i,k Replace R i,k The robust Kalman filter algorithm (ie, RKF algorithm) can be obtained by performing standard Kalman filtering.

[0115] In the embodiment of the present application, the calculation of the fitness value of each measurement noise matrix includes:

[0116] For any measurement noise matrix, the Kalman gain corresponding to the measurement noise matrix is ​​calculated according to the initial state covariance matrix and the measurement noise matrix; the initial state prediction result and the initial state covariance matrix are measured and updated according to the Kalman gain to obtain the state estimation result and the state estimation covariance matrix corresponding to the measurement noise matrix; the inertial navigation data at the current moment is corrected by using the state estimation result and the state estimation covariance matrix to obtain the inertial navigation data corrected by the measurement noise matrix; the inertial navigation data corrected by the measurement noise matrix and the auxiliary measurement data at the current moment are input into a pre-constructed fitness function to obtain the fitness value of the measurement noise matrix.

[0117] In the embodiment of the present application, when the auxiliary measurement data is normal measurement data, the Kalman gain K is calculated using the following formula: k :

[0118] When the auxiliary measurement data is abnormal measurement data, the Kalman gain is calculated using the following formula:

[0119]

[0120] In an embodiment of the present application, the initial state prediction result and the initial state covariance matrix are measured and updated according to the Kalman gain, including: calculating the measurement residual according to the initial state prediction result, the measurement matrix and the observed value at the current moment; updating the initial state prediction result using the Kalman gain and the measurement residual to obtain the state estimation result corresponding to the measurement noise matrix; updating the initial state covariance matrix using the Kalman gain to obtain the state estimation covariance matrix corresponding to the measurement noise matrix.

[0121] In the embodiment of the present application, the formula involved in the measurement update process is as follows:

[0122]

[0123] P k|k =(IK k H k ) k|k-1 (19)

[0124] Among them, μ k Represents the measurement residual; z k represents the observed quantity; represents the state estimation result; P k|j represents the state estimation covariance matrix; I represents the identity matrix.

[0125] In an embodiment of the present application, the fitness function is obtained by solving the auxiliary measurement data at time k and the inertial navigation data corrected by each measurement noise matrix, and the cost function is configured based on the minimum mean square error of the velocity error of the navigation target of the combined navigation system; therefore, when calculating the fitness value of each measurement noise matrix, it is necessary to bring the measurement noise matrix into the measurement update equation for a complete measurement update.

[0126] In the embodiment of the present application, before calculating the fitness value of each measurement noise matrix, the method further includes: matching the target cost function based on the judgment result of whether the auxiliary measurement data is abnormal measurement data.

[0127] In the embodiment of the present application, when the method is applied in an underwater navigation scenario, considering that the external auxiliary information obtained by the AUV in the actual underwater environment mainly depends on the b-system velocity output by the DVL, the initial alignment performance of the SINS / DVL depends on the velocity accuracy. Therefore, the velocity error output by the filter is used as the fitness function, that is, the smaller the velocity error, the larger the corresponding fitness value, which means that the searched particles are better. In order to find the optimal measurement noise covariance matrix at time k, the cost function is constructed as follows:

[0128]

[0129] Wherein, time j represents the current time, j = 1…k; R i,k represents the i-th measurement noise matrix at time k; J(R i,k ) represents R i,k When is the parameter, the mean square error of the velocity error of the navigation target of the integrated navigation system; is the velocity value in the inertial navigation data corrected by the i-th measurement noise matrix at time k; is the velocity in the auxiliary measurement data at time k; θ k Indicates the evaluation index of the auxiliary measurement data at the current moment, represents the judgment threshold of the configuration, n1 represents the dimension of the observation, α represents the probability parameter, It follows a chi-square distribution with n1 degrees of freedom.

[0130] In the embodiment of the present application, the fitness function is:

[0131] Among them, f(R i,k ) represents the fitness value of the i-th measurement noise matrix at time k. The measurement noise matrix that satisfies the maximum fitness value and the minimum mean square error of b-series velocity error is the optimal measurement noise matrix.

[0132] In one embodiment of the present application, n1 can be set to 2, α can be set to 0.99, that is, the efficiency of the filtering method is 99%, and the 2-degree-of-freedom chi-square distribution value is

[0133] In the embodiment of the present application, according to the above-mentioned cost function, when the auxiliary measurement data is normal, the cost function value is the sum of the mean square errors of the right, forward and upward speeds of the b system; when the auxiliary measurement data is abnormal, the cost function value is the sum of the mean square errors of the right and upward speeds of the b system (which can be approximately equivalent to the sum of the mean square values ​​of the right and upward speeds of the b system), thereby suppressing the influence of abnormal observations on the search results, realizing the anti-error of the optimization algorithm, and further improving the robustness of the robust Kalman filter algorithm (i.e., PSO-RKF algorithm) optimized based on the intelligent optimization algorithm.

[0134] In the embodiment of the present application, based on the fitness value of each measurement noise matrix and the process optimal measurement noise matrix, adaptive update is performed, including: each measurement noise matrix iteratively updates its own speed and position at time k, and each measurement noise matrix adjusts the trajectory according to its own and group experience to move closer to the optimal measurement noise matrix; the update formula is as follows:

[0135]

[0136] Where d = 1, 2, ..., K and i = 1, 2, ..., N are the search space dimension and population size respectively; r1, r2 are random numbers between 0 and 1; c1, c2 are learning factors, which represent the ability of the measurement noise matrix to learn from itself and other measurement noise matrices, respectively, and usually take values ​​between 0 and 2; w g represents the inertia weight, which is used to adjust the diversity of the measurement noise matrix; the measurement noise matrix velocity v∈[v min ,v max ]; m is the number of generations of the current population; Respectively represent the current position and speed of the measurement noise matrix, represents the speed of the measurement noise matrix i in the dth dimension and the m+1th time step, represents the position of the measurement noise matrix i in the dth dimension and the m+1th time step; They represent the positions of the current individual and group optimal particles (i.e., the optimal measurement noise matrix) respectively.

[0137] In an embodiment of the present application, in order to improve the accuracy of the optimization algorithm and prevent the solution of the optimization algorithm from falling into the local optimum, the present application adopts a method of linearly reducing the inertia weight to improve the performance of the optimization algorithm and prevent it from falling into the local optimal solution. Inertia weight plays an important role in balancing local search and global search. The larger the inertia weight value, the more inclined to global search; conversely, the smaller the inertia weight value, the more inclined to local search. In traditional optimization algorithms, the inertia weight is generally a fixed value. In an embodiment of the present application, in order to make the algorithm's search capability stronger, even if the global search capability is strong in the early stage to prevent falling into the local optimal solution, and the local search capability is strong in the later stage to solve a more accurate optimal solution, the inertia weight is defined as a value w that decreases linearly with the number of iterations. g .

[0138] In the embodiment of the present application, the inertia weight w g as follows:

[0139]

[0140] Where n2 represents the current iteration number of the optimization process; N1 represents the total number of iterations. max and w min They represent the upper and lower bounds of a given inertia weight respectively. In practical applications, the upper and lower bounds of the inertia weight are defined as engineering experience values.

[0141] In another embodiment of the present application, the cost function may be:

[0142]

[0143] in, represents the attitude matrix in the inertial navigation data after the i-th measurement noise matrix is ​​corrected at time k.

[0144] In one embodiment of the present application, Figure 2 As shown, taking the particle optimization algorithm as an example, the determination of the optimal measurement noise matrix of the present application includes: (1) initializing the particle swarm parameters, initializing the position and velocity of the particle swarm in the parameter interval, randomly generating a number of particles and initializing the optimal particle; (2) taking the measurement noise covariance matrix at time k as a particle, using the cost function and the fitness function to evaluate the fitness value of each particle, and comparing it with the fitness value of the optimal particle. If the fitness value is better, that is, the b-series velocity estimation error is smaller, then replace the optimal particle to make the filtering performance at time k better; that is, the particle that satisfies the maximum fitness value and the minimum mean square error of the b-series velocity error is the optimal particle. (3) According to formula (22) and formula (23), particle x i =R i,kIteratively update its own speed and position at time k. Each particle adjusts its trajectory to move closer to the optimal particle based on its own and group experience. (4) Repeat steps (2) to (4) to update the optimal particle until the number of iterations or accuracy requirements are met, and output the optimal particle R best,k , R best,k is the optimal measurement noise matrix.

[0145] In the embodiment of the present application, when obtaining the optimal measurement noise matrix, the optimal measurement noise matrix is ​​substituted into formula (14) or formula (15) to obtain the corresponding Kalman gain. The optimal measurement noise matrix is ​​substituted into formulas (16) to (18) to complete the measurement update process of the robust KF algorithm (i.e., PSO-RKF algorithm) optimized by the optimization algorithm. Figure 3 The filter in is set to PSO-RKF, and the robust adaptive initial alignment of the SINS / DVL or SINS / OD integrated navigation system based on the PSO-RKF algorithm can be realized. In particular, when the expansion factor is set to 1, the PSO-RKF algorithm will degenerate into an adaptive KF algorithm (i.e., PSO-KF) algorithm optimized based on the optimization algorithm.

[0146] Step S140: Correct the initial state prediction result according to the auxiliary measurement data and the Kalman gain to obtain the state prediction result of the navigation target at the current moment and the state covariance matrix corresponding to the state prediction result.

[0147] In another embodiment of the present application, the present application extends the application of the intelligent optimization algorithm to the Kalman filter framework, proposes a robust Kalman filter algorithm (i.e., RPSO-RKF) optimized based on the robust intelligent optimization algorithm, establishes a cost function and a fitness function suitable for underwater SINS / DVL navigation fusion or land SINS / OD navigation fusion, and improves the cost function in a robust manner under non-Gaussian conditions, and performs adaptive search and estimation on the measurement noise covariance matrix, thereby further improving the estimation performance of the filtering algorithm under complex conditions.

[0148] In another embodiment of the present application, the robust optimization algorithm of the present application can also be extended and applied to SINS / DVL and SINS / OD initial alignment and combined navigation based on nonlinear Kalman filter algorithms such as extended Kalman filter (EKF) and unscented Kalman filter (UKF) and their robust improvement algorithms.

[0149] The technical effect of the present application is further illustrated below in conjunction with simulation experiments.

[0150] The adaptive search and estimation performance of PSO-RKF and RPSO-RKF (this application) for the measurement noise array under non-Gaussian conditions is verified by selecting 900s of DVL measured data. Figure 4Based on the Wiener approximation theorem, any non-Gaussian noise distribution can be represented or adequately approximated by a finite sum of Gaussian noise distributions with known probability density. Assume that the actual probability distribution of the observed noise is as follows:

[0151] ρ actual =(1-α)N(0,R c )+αN(0,R p ) (26)

[0152] Among them, the interference factor α satisfies 0≤α≤0.1; R c is the measurement noise covariance matrix of DVL output velocity data; R p is the interference noise covariance matrix with a large standard deviation. When the interference factor α deviates from 0, the distribution of formula (19) is also called a thick-tailed distribution. The root mean square error (RMSE) of position, velocity and attitude and the time average of the root mean square error (TRMSE) are selected as indicators for judging the filtering performance. The RMSE and TRMSE of position, velocity and attitude are defined as shown in formula (27)-formula (28) respectively.

[0153]

[0154] The symbols "att", "v", and "p" represent attitude, velocity, and position, respectively, and M represents the number of Monte Carlo simulation experiments. Represents the true value of the error of attitude, velocity, and position at time k, Represents the error estimate of attitude, velocity, and position at time k.

[0155] Setting up R c =200·R0, and introduce it into Figure 4 In the DVL output data shown. Since in practical applications, the prior information of the observed quantity, that is, the measurement noise covariance matrix, is usually unknown, the initial measurement noise matrix is ​​set to R0=diag(0.01 2 , 0.01 2 )m 2 / s 2 50 Monte Carlo initial alignment tests were performed using KF, RKF, PSO-RKF, and RPSO-RKF, respectively, and the interference factor α was set to 0.1. The test results are shown in Figures 5 to 15 shown.

[0156] Depend on Figures 5 to 15It can be clearly seen that when the DVL output is contaminated by non-Gaussian noise such as wild values, the initial alignment error curve obtained by KF shows a divergent trend, and its attitude, velocity, and position alignment performance are significantly lower than those of RKF, PSO-RKF, and RPSO-RKF. This is because KF is not robust. When the observed quantity is contaminated by non-Gaussian noise, it will greatly reduce the KF filtering performance, thereby reducing the initial alignment accuracy. The attitude and velocity alignment accuracy of RKF, PSO-RKF, and RPSO-RKF are similar. The heading alignment accuracy of RPSO-RKF is significantly better than that of RKF and PSO-RKF. The position alignment accuracy of RPSO-RKF is significantly better than that of PSO-RKF, but is similar to that of RKF. In order to further compare the initial alignment performance of different methods, the TRMSE (R1=750s, T2=900s) corresponding to different methods are calculated according to the experimental results as shown in Table 1.

[0157] Table 1 TRMSE corresponding to different methods

[0158]

[0159] It can be seen from Table 1 that under the same conditions, the initial alignment accuracy of PSO-RKF attitude and velocity is similar to that of RKF, and the position alignment accuracy is lower than that of RKF. The initial alignment performance of RPSO-RKF is better than that of RKF and PSO-RKF. This is because 1) during the entire alignment process, the inaccurate measurement noise covariance matrix is ​​assigned to RKF, which reduces the expansion factor λ k , which reduces the accuracy of the solution and thus reduces the alignment performance of RKF. 2) Although PSO-RKF has the ability to adaptively search and estimate the measurement noise array, in the cost function shown in formula (25), if the DVL output at time k is contaminated by non-Gaussian noise, the b-system velocity error will diverge, which will also reduce the accuracy of the fitness value calculated by formula (21), and thus make it difficult for the PSO algorithm to search for the measurement noise array corresponding to the "optimal" particle to achieve the optimal filtering performance at time k. 3) RPSO-RKF uses the cost function shown in formula (25) and uses the evaluation index to judge the observed quantity. If the observed quantity is normal, the cost function value is equal to the sum of the mean square error of the right, forward, and upward velocity errors of the b-system; conversely, if the observed quantity is abnormal, the cost function value is equal to the sum of the mean square error of the right and upward velocity errors of the b-system, thereby suppressing the influence of non-Gaussian noise on the cost function of the PSO algorithm, which is conducive to searching for a better measurement noise array and thus achieving the optimal filtering performance at time k.

[0160] In a Monte Carlo experiment, PSO-RKF and RPSO-RKF were used for initial alignment, and the velocity error of the b system was obtained as follows: Figure 16-Figure 17 shown.

[0161] Depend on Figure 4It can be seen that the DVL output is contaminated by high-intensity wild values ​​at 303s, 654s, 657s, 674s, and 740s. Fig.16 It can be seen that in the process of using PSO-RKF to intelligently search and estimate the measurement noise array, when the DVL output is contaminated by wild values, the b-system velocity error also jumps. This also shows that the cost function shown in formula (25) cannot suppress the influence of non-Gaussian noise such as wild values ​​at 303s, 654s, 657s, 674s, and 740s on the fitness value solution results, which leads to the inability to achieve better filtering performance at these moments, affecting the initial alignment effect. Fig.17 It can be seen that at 303s, 654s, 657s, 674s, and 740s when the DVL output is contaminated by wild values, the b-system velocity error does not jump, and the error tends to be stable overall. This is because, in the process of intelligent search and estimation of the measurement noise array using RPSO-RKF, the abnormal observation is first identified by the judgment factor, and then the cost function shown in formula (20) is used to solve the cost function value and the adaptation value, thereby effectively suppressing the influence of non-Gaussian noise such as wild values ​​on the filtering performance and improving the initial alignment accuracy. The experimental results further verify that RPSO-RKF can intelligently search and estimate the measurement noise array under non-Gaussian conditions to improve the filtering alignment performance; and its advantages in initial alignment performance compared with KF, RKF, and PSO-RKF under the same conditions.

[0162] Corresponding to the above method, the embodiment of the present application also provides a Kalman filter navigation information fusion device based on intelligent optimization algorithm optimization, such as Fig.18 As shown, the Kalman filter navigation information fusion device based on intelligent optimization algorithm optimization includes:

[0163] The acquisition unit 1810 is used to acquire the inertial navigation data and auxiliary measurement data of the navigation target at the current moment, as well as the historical state prediction result and the historical state covariance matrix obtained at the previous moment adjacent to the current moment;

[0164] The prediction unit 1820 is used to predict the state of the navigation target at the current moment according to the historical state prediction result and the historical state covariance matrix, and obtain the initial state prediction result and the initial state covariance matrix of the navigation target;

[0165] The estimation unit 1830 is used to perform adaptive search and estimation on the measurement noise matrix of the navigation target based on the inertial navigation data and the auxiliary measurement data to obtain the optimal measurement noise matrix of the navigation target;

[0166] A calculation unit 1840, configured to calculate a Kalman gain according to an initial state covariance matrix and an optimal measurement noise matrix;

[0167] The correction unit 1850 is used to correct the initial state prediction result according to the auxiliary measurement data and the Kalman gain to obtain the state prediction result of the navigation target at the current moment and the state covariance matrix corresponding to the state prediction result; wherein the state prediction result is the fusion result of the inertial navigation data and the auxiliary measurement data.

[0168] The functions of the various functional units of the Kalman filter navigation information fusion device based on intelligent optimization algorithm optimization provided in the above-mentioned embodiments of the present application can be achieved through the above-mentioned method steps. Therefore, the specific working process and beneficial effects of each unit in the Kalman filter navigation information fusion device based on intelligent optimization algorithm optimization provided in the embodiments of the present application will not be repeated here.

[0169] The present application also provides an electronic device, such as Fig.19 As shown, it includes a processor 1910 , a communication interface 1920 , a memory 1930 and a communication bus 1940 , wherein the processor 1910 , the communication interface 1920 , and the memory 1930 communicate with each other via the communication bus 1940 .

[0170] Memory 1930, used for storing computer programs;

[0171] The processor 1910 is used to implement the following steps when executing the program stored in the memory 1930:

[0172] The inertial navigation data and auxiliary measurement data of the navigation target at the current moment, as well as the historical state prediction result and the historical state covariance matrix obtained at the previous moment adjacent to the current moment, are obtained; the state of the navigation target at the current moment is predicted according to the historical state prediction result and the historical state covariance matrix, respectively, to obtain the initial state prediction result and the initial state covariance matrix of the navigation target; based on the inertial navigation data and the auxiliary measurement data, the measurement noise matrix of the navigation target is adaptively searched and estimated to obtain the optimal measurement noise matrix of the navigation target; the Kalman gain is calculated according to the initial state covariance matrix and the optimal measurement noise matrix; the initial state prediction result is corrected according to the auxiliary measurement data and the Kalman gain, to obtain the state prediction result of the navigation target at the current moment and the state covariance matrix corresponding to the state prediction result; wherein the state prediction result is the fusion result of the inertial navigation data and the auxiliary measurement data.

[0173] The communication bus mentioned above can be a peripheral component interconnect standard (PCI) bus or an extended industrial standard architecture (EISA) bus, etc. The communication bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, only one thick line is used in the figure, but it does not mean that there is only one bus or one type of bus. The communication interface is used for communication between the above-mentioned electronic device and other devices. The memory can include a random access memory (RAM) and can also include a non-volatile memory (NVM), such as at least one disk storage. Optionally, the memory can also be at least one storage device located away from the aforementioned processor.

[0174] The above-mentioned processor can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components. The implementation methods and beneficial effects of the various components of the electronic device in the above-mentioned embodiments to solve the problems can be found in Figure 1 The various steps in the illustrated embodiment are implemented, therefore, the specific working process and beneficial effects of the electronic device provided by the embodiment of the present application are not repeated here.

[0175] In another embodiment provided in the present application, a computer-readable storage medium is also provided, which stores instructions. When the computer-readable storage medium is run on a computer, the computer executes the Kalman filter navigation information fusion method based on intelligent optimization algorithm optimization described in any of the above embodiments.

[0176] In another embodiment provided in the present application, a computer program product comprising instructions is also provided, which, when executed on a computer, enables the computer to execute the Kalman filter navigation information fusion method based on intelligent optimization algorithm optimization described in any of the above embodiments.

[0177] Those skilled in the art will appreciate that the embodiments in the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt a complete hardware embodiment, a complete software embodiment, or a form of an embodiment combining software and hardware. Moreover, the present application may adopt a form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0178] The embodiments of the present application are described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0179] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 These computer program instructions can also be loaded onto a computer or other programmable data processing device, so that a series of operation steps are executed on the computer or other programmable device to produce a computer-implemented process, so that the instructions executed on the computer or other programmable device provide for implementing the process in the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0180] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications falling within the scope of the embodiments of the present application.

[0181] Obviously, those skilled in the art can make various changes and modifications to the embodiments in the present application without departing from the spirit and scope of the embodiments in the present application. Thus, if these modifications and variations of the embodiments in the present application fall within the scope of the claims and their equivalents in the embodiments of the present application, the embodiments of the present application are also intended to include these modifications and variations.

Claims

1. A Kalman filter navigation information fusion method based on intelligent optimization algorithm optimization, characterized in that: The method comprises: Obtain the inertial navigation data and auxiliary measurement data of the navigation target at the current moment, as well as the historical state prediction results and historical state covariance matrix obtained at the previous moment adjacent to the current moment; According to the historical state prediction result and the historical state covariance matrix, the state of the navigation target at the current moment is predicted to obtain the initial state prediction result and the initial state covariance matrix of the navigation target; Based on the inertial navigation data and the auxiliary measurement data, adaptively search and estimate the measurement noise matrix of the navigation target to obtain the optimal measurement noise matrix of the navigation target; Calculating a Kalman gain according to the initial state covariance matrix and the optimal measurement noise matrix; According to the auxiliary measurement data and the Kalman gain, the initial state prediction result is corrected to obtain the state prediction result of the navigation target at the current moment and the state covariance matrix corresponding to the state prediction result; wherein the state prediction result is the fusion result of the inertial navigation data and the auxiliary measurement data.

2. The method according to claim 1, characterized in that Based on the inertial navigation data and the auxiliary measurement data, adaptively searching and estimating the measurement noise matrix of the navigation target to obtain the optimal measurement noise matrix of the navigation target includes: Obtain configuration initialization parameters and multiple measurement noise matrices; Based on the pre-constructed cost function and fitness function, the fitness value of each measurement noise matrix is ​​calculated; wherein the cost function is configured based on the minimum mean square error of the speed error of the navigation target of the integrated navigation system as the optimization target; and the fitness function is the inverse of the cost function; According to the fitness value of each measurement noise matrix, an optimal measurement noise matrix is ​​selected from each measurement noise matrix; Based on the fitness value of each measurement noise matrix and the process optimal measurement noise matrix, adaptive update is performed until the configured termination condition is met, and the optimal measurement noise matrix is ​​output.

3. The method according to claim 2, characterized in that Before calculating the fitness value of each measurement noise matrix based on the pre-constructed cost function and fitness function, the method further includes: Calculate the evaluation index of the auxiliary measurement data at the current moment; If the evaluation index is not greater than the configured evaluation threshold, the auxiliary measurement data at the current moment is normal measurement data; If the value is greater than the configured judgment threshold, the auxiliary measurement data at the current moment is abnormal measurement data.

4. The method according to claim 3, characterized in that Before calculating the Kalman gain corresponding to the measurement noise matrix, the method further includes: Based on the judgment result of whether the auxiliary measurement data is abnormal measurement data, a corresponding target cost function is matched.

5. The method according to claim 4, characterized in that Based on the target cost function and the corresponding fitness function, the fitness value of each measurement noise matrix is ​​calculated, including: For any measurement noise matrix, calculating the Kalman gain corresponding to the measurement noise matrix according to the initial state covariance matrix and the measurement noise matrix; The initial state prediction result and the initial state covariance matrix are measured and updated according to the Kalman gain to obtain the state estimation result and the state estimation covariance matrix corresponding to the measurement noise matrix; Using the state estimation result and the state estimation covariance matrix to correct the inertial navigation data at the current moment, to obtain the inertial navigation data corrected by the measurement noise matrix; The inertial navigation data corrected by the measurement noise matrix and the auxiliary measurement data at the current moment are input into the target cost function and the corresponding fitness function to obtain the fitness value of the measurement noise matrix.

6. The method according to claim 5, characterized in that Calculating the Kalman gain corresponding to the measurement noise matrix according to the initial state covariance matrix and the measurement noise matrix includes: If the auxiliary measurement data at the current moment is normal measurement data, the Kalman gain corresponding to the measurement noise matrix is ​​calculated according to the initial state covariance matrix and the measurement noise matrix.

7. The method according to claim 5, characterized in that Calculating the Kalman gain corresponding to the measurement noise matrix according to the initial state covariance matrix and the measurement noise matrix, further comprising: If the auxiliary measurement data at the current moment is abnormal measurement data, the measurement noise matrix is ​​amplified by using the configured expansion factor to obtain an amplified measurement noise matrix; The Kalman gain corresponding to the measurement noise matrix is ​​calculated according to the initial state covariance matrix and the amplified measurement noise matrix.

8. A Kalman filter navigation information fusion device based on intelligent optimization algorithm optimization, characterized in that: The device comprises: An acquisition unit, used to acquire the inertial navigation data and auxiliary measurement data of the navigation target at the current moment, as well as the historical state prediction result and historical state covariance matrix obtained at the previous moment adjacent to the current moment; A prediction unit, used to predict the state of the navigation target at the current moment according to the historical state prediction result and the historical state covariance matrix, and obtain the initial state prediction result and the initial state covariance matrix of the navigation target; an estimating unit, configured to perform adaptive search and estimation on a measurement noise matrix of the navigation target based on the inertial navigation data and the auxiliary measurement data, so as to obtain an optimal measurement noise matrix of the navigation target; A calculation unit, used for calculating the Kalman gain according to the initial state covariance matrix and the optimal measurement noise matrix; A correction unit is used to correct the initial state prediction result according to the auxiliary measurement data and the Kalman gain to obtain the state prediction result of the navigation target at the current moment and the state covariance matrix corresponding to the state prediction result; wherein the state prediction result is a fusion result of the inertial navigation data and the auxiliary measurement data.

9. An electronic device, characterized in that: The electronic device comprises a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory communicate with each other via the communication bus; Memory, used to store computer programs; A processor, for implementing any of the methods described in claims 1-7 when executing a program stored in a memory.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images