A combined navigation filtering method and system, electronic device and storage medium

Abstract

The present application relates to a kind of combined navigation filtering method, system, electronic equipment and storage medium, it is related to navigation field, the method includes: by combined navigation system real-time record the inertial navigation data and GNSS observation data of target to be positioned, determine the state vector and measurement vector of current time;The state vector and measurement vector of current time are filtered based on the multidimensional outlier robust adaptive filtering algorithm of maximum similarity, obtain the state vector after filtering;Judge whether the state vector after filtering satisfies stop iteration condition;If not satisfied, then the error component of current time inertial navigation parameter drift is fed back to the inertial navigation state of last time, and inertial navigation state is updated and the position of current time is solved, obtain updated measurement vector;Updated measurement vector is used as the measurement vector of current time after time update, filtering processing is carried out again;If satisfied, the position result of target to be positioned.The present application improves filtering precision.

Description

Technical Field

[0001] This invention relates to the field of navigation technology, and in particular to a combined navigation filtering method, system, electronic device, and storage medium. Background Technology

[0002] In integrated navigation, the conventional inertial navigation system parameter drift error correction is performed using a closed-loop feedback structure along with the final output result. In the next integrated navigation, the inertial navigation parameters are the result after the previous closed-loop feedback correction.

[0003] Currently, most fusion algorithms for INS (Inertial Navigation System) / GNSS (Global Navigation Satellite System) integrated navigation systems are based on the traditional Kalman filter framework. Kalman filtering can only obtain its optimal theoretical solution when the system structural parameters and noise statistical characteristics are accurate and unbiased. However, in integrated navigation, the changing external environment inevitably introduces errors into these two types of parameters. Especially when outliers caused by non-Gaussian noise appear in the GNSS measurement data, the accuracy of the filtering results will decrease significantly, and may even diverge. When processing measured data, using adaptive methods to adjust the state noise covariance matrix can reduce the impact of outliers to some extent. However, apart from this, the adaptive Kalman filter's adjustment method for the noise covariance matrix is ​​relatively simple, and the overall scaling makes it unable to effectively simulate the noise distribution of each subsystem.

[0004] The multidimensional outlier robust adaptive filtering algorithm based on maximum similarity takes into account the noise distribution of different state vectors and finds the optimal adjustment coefficient through iteration. Therefore, its performance far exceeds that of Kalman filtering in multidimensional and variable environments. However, the computational cost of this algorithm increases exponentially with the number of iterations, affecting computational efficiency. How to balance filtering accuracy, robustness, and computational efficiency is a very important problem that remains to be solved, limiting the role of integrated navigation under complex conditions. Summary of the Invention

[0005] The purpose of this invention is to provide a combined navigation filtering method, system, electronic device, and storage medium that reduces the number of iterations and improves filtering accuracy, thereby improving positioning accuracy.

[0006] To achieve the above objectives, the present invention provides the following solution:

[0007] A combined navigation filtering method, comprising:

[0008] The system records inertial navigation data and global navigation satellite system observation data of the target to be located in real time through a combined navigation system; the combined navigation system is a combination of an inertial navigation system and a global navigation satellite system.

[0009] The current state vector and measurement vector are determined based on the inertial navigation data and global navigation satellite system observation data of the target to be located at the current moment; the state vector is a vector composed of position, velocity, attitude and error components of inertial navigation parameter drift; the measurement vector is the difference between the position determined based on the inertial navigation data and the position determined based on the global navigation satellite system observation data; the error components of inertial navigation parameter drift include gyroscope zero bias error components and accelerometer zero bias error components;

[0010] The current state vector and measurement vector are filtered by a multidimensional outlier robust adaptive filtering algorithm based on maximum similarity to obtain the filtered state vector.

[0011] Determine whether the L2 norm of the difference between the state vectors output from two consecutive iterations before and after filtering is less than a set threshold.

[0012] If the error component of the inertial navigation parameter drift at the current moment is greater than or equal to the set threshold, the error component of the inertial navigation parameter drift at the current moment is fed back to the inertial navigation state at the previous moment to update the inertial navigation state, and the position at the current moment is calculated. The difference between the calculated position at the current moment and the position determined by the observation data of the global navigation satellite system at the current moment is calculated to obtain the updated measurement vector.

[0013] Use the updated measurement vector as the measurement vector at the current time after the time update, and return to the step "Filter the current state vector and measurement vector using the multidimensional outlier robust adaptive filtering algorithm based on maximum similarity to obtain the filtered state vector";

[0014] If the value is less than the set threshold, the position of the target to be located is determined based on the filtered state vector.

[0015] Optionally, the step of filtering the current state vector and measurement vector using a multidimensional outlier robust adaptive filtering algorithm based on maximum similarity to obtain the filtered state vector specifically includes:

[0016] A cost function based on a similarity metric is constructed based on the nominal prediction error covariance matrix and the nominal measurement noise covariance matrix.

[0017] The adjustment matrix is ​​determined by solving the cost function.

[0018] The state covariance matrix and measurement covariance matrix are corrected according to the adjustment matrix to obtain the corrected state covariance matrix and measurement covariance matrix;

[0019] The Kalman gain is calculated based on the corrected state covariance matrix and the measurement covariance matrix.

[0020] The filtered state vector is determined based on the Kalman gain.

[0021] The present invention also discloses a combined navigation filtering system, comprising:

[0022] The inertial navigation data and global navigation satellite system observation data recording module is used to record the inertial navigation data and global navigation satellite system observation data of the target to be located in real time through the integrated navigation system; the integrated navigation system is a combination of an inertial navigation system and a global navigation satellite system.

[0023] The current state vector and measurement vector determination module is used to determine the current state vector and measurement vector based on the inertial navigation data and global navigation satellite system observation data of the target to be located at the current moment. The state vector is a vector composed of position, velocity, attitude, and error components of inertial navigation parameter drift. The measurement vector is the difference between the position determined based on the inertial navigation data and the position determined based on the global navigation satellite system observation data. The error components of inertial navigation parameter drift include gyroscope zero bias error components and accelerometer zero bias error components.

[0024] The filtering module is used to filter the current state vector and measurement vector using a multidimensional outlier robust adaptive filtering algorithm based on maximum similarity to obtain the filtered state vector.

[0025] The judgment module is used to determine whether the L2 norm of the difference between the state vectors output by two adjacent iterations before and after filtering is less than a set threshold.

[0026] The updated measurement vector acquisition module, when the difference between the state vectors output by two adjacent iterations before and after filtering is greater than or equal to a set threshold, is used to feed back the error component of the current inertial navigation parameter drift to the inertial navigation state of the previous moment for inertial navigation state update, and calculate the current position. The calculated current position is then subtracted from the position determined by the current global navigation satellite system observation data to obtain the updated measurement vector.

[0027] The return module is used to return the updated measurement vector as the measurement vector at the current time after the time update to the filtering module;

[0028] The target location determination module determines the location result of the target based on the filtered state vector when the L2 norm of the difference between the state vectors output by two adjacent iterations before and after filtering is less than a set threshold.

[0029] The present invention also discloses an electronic device, comprising: at least one processor, at least one memory, and computer program instructions stored in the memory, wherein the combined navigation filtering method is implemented when the computer program instructions are executed by the processor.

[0030] The present invention also discloses a computer-readable storage medium storing computer program instructions thereon, characterized in that the combined navigation filtering method is implemented when the computer program instructions are executed by a processor.

[0031] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0032] This invention optimizes the filter structure by feeding back the error component of the inertial navigation parameter drift at the current moment to the inertial navigation state at the previous moment during iteration, calculating the current position, and obtaining the updated measurement vector. This updated measurement vector is then used as the measurement vector at the current moment after time updates and enters the next iteration of filtering. The filtered output data re-enters the loop to correct the inertial navigation. This process is repeated to optimize the filter structure. The filter iteratively approaches the optimal solution, reducing the number of iterations required to obtain the optimal solution and eliminating the influence of the inertial navigation parameter drift between two available GNSS data on the filtering prediction accuracy. This improves the filtering accuracy and thus the positioning accuracy of the target to be located. Attached Figure Description

[0033] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0034] Figure 1 This is a schematic diagram of the combined navigation filtering method of the present invention;

[0035] Figure 2 This is a schematic diagram of a combined navigation filtering system according to the present invention. Detailed Implementation

[0036] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0037] The purpose of this invention is to provide a combined navigation filtering method, system, electronic device, and storage medium that reduces the number of iterations and improves filtering accuracy, thereby improving positioning accuracy.

[0038] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0039] Example 1

[0040] Figure 1 This is a schematic diagram of a combined navigation filtering method according to the present invention, as shown below. Figure 1 As shown, a combined navigation filtering method includes:

[0041] Step 101: Record the inertial navigation data and global navigation satellite system observation data of the target to be located in real time through the integrated navigation system; the integrated navigation system is a combination of an inertial navigation system and a global navigation satellite system.

[0042] Specifically, step 101 includes: initializing the INS / GNSS integrated navigation system (a combined system of inertial navigation system and global navigation satellite system), establishing state equations and measurement equations, and recording inertial navigation data and GNSS observation data output by inertial sensors in real time.

[0043] Inertial sensors include gyroscopes and accelerometers.

[0044] Step 102: Determine the current state vector and measurement vector based on the inertial navigation data and global navigation satellite system observation data of the target to be located at the current moment; the state vector is a vector composed of position, velocity, attitude and error components of inertial navigation parameter drift; the measurement vector is the difference between the position determined based on the inertial navigation data and the position determined based on the global navigation satellite system observation data; the error components of inertial navigation parameter drift include gyroscope zero bias error components and accelerometer zero bias error components.

[0045] The measurement vector consists of the difference between the position predicted by INS and the position measured by GPS, denoted as . λ INS -λ GPS h INS -h GPS , representing the difference between the longitude predicted by INS and the longitude measured by GPS, the difference between the latitude predicted by INS and the latitude measured by GPS, and the difference between the latitude and altitude predicted by INS and the altitude measured by GPS, respectively.

[0046] Specifically, step 102 includes establishing a state model. and measurement model Determine process noise and measurement noise

[0047] Equations of state:

[0048]

[0049] Measurement equation:

[0050]

[0051] Where X(t) represents the state vector in the state space, H(t) represents the n-dimensional measurement transition matrix, F(t) represents the n-dimensional state transition matrix, and W is the noise distribution vector, which includes the variance related to the state vector: G = [σ r,1×3 ,σ v,1×3 ,σ ε,1×3 ,σ ω,1×3 ,σ f,1×3 ]; t represents time.

[0052] V represents the measurement noise matrix, which is typically zero-mean white noise with covariance R. δω 3×1 ,δf 3×1 These represent the error components for position, velocity, attitude, gyroscope bias, and accelerometer bias, respectively.

[0053] The attitude, velocity, and position of the carrier (target to be located) are updated using inertial navigation data output from inertial sensors, specifically including:

[0054] Attitude update: Integrate the angular velocity observations output by the gyroscope to calculate the attitude change of the carrier within the observation interval, and update the carrier attitude.

[0055] Velocity update: Based on the change in attitude, the specific force observation value output by the accelerometer is integrated to calculate the change in velocity of the carrier within the observation interval, and the carrier velocity is updated.

[0056] Position Update: Based on the changes in attitude and velocity, the velocity change within the observation interval is integrated to calculate the change in the vehicle's position within the observation interval, and the vehicle's position is updated to obtain the final position vector.

[0057] Record GNSS observation information Obtained from INS location updates Substituting these values ​​into the measurement equation yields the measurement vector z required for the filtering model described below. k .

[0058] The state vector update of this invention includes time update and measurement update.

[0059] Time updates include:

[0060]

[0061] in, and P k-1∣k-1 Let Q be the state vector and the prediction covariance matrix at time k-1, respectively. k Let F be the nominal state noise covariance matrix. k It is the state transition matrix, P k∣k-1 for The corresponding covariance.

[0062] The measurement update is shown in step 103.

[0063] Step 103: Filter the current state vector and measurement vector using a multidimensional outlier robust adaptive filtering algorithm based on maximum similarity to obtain the filtered state vector.

[0064] Step 103 specifically includes:

[0065] Based on the nominal prediction error covariance matrix P k∣k-1 and the nominal measurement noise covariance matrix R k We construct a cost function based on similarity metrics.

[0066] The cost function is as follows:

[0067]

[0068] Where, q * (x k ) represents the state vector x k The optimal posterior probability density, with the superscript * indicating optimality, q(x) k ) represents the state vector x k The final probability density, H k S represents the measurement transition matrix at the current time (k). k∣k-1 P represents the nominal prediction error covariance matrix. k∣k-1 The square root matrix, R represents the nominal measurement noise covariance matrix. k The square root matrix, x k This represents the state vector at the current time (k). Let z represent the post-validation mean vector of the previous time step (k-1). k This represents the measurement vector.

[0069] In the cost function, s(α,β) represents the similarity measure:

[0070]

[0071] s(α,β) is used to compare the similarity of two vectors α and β across different dimensions, and f(·) represents the similarity function. i =β i When , s(α,β) reaches its maximum value, α i β represents the value in the i-th dimension of vector α. i This represents the value in the i-th dimension of the β vector. The filtering model built upon this allows the distribution models of three different forms—attitude, position, and velocity—to simultaneously achieve optimal performance when an object is moving.

[0072] The adjustment matrix is ​​determined by solving the cost function.

[0073] The adjustment matrix includes and This is the first adjustment matrix. This is the second adjustment matrix.

[0074]

[0075]

[0076] in,

[0077]

[0078] T ki The inverse square root matrix The column vector, U kj The inverse square root matrix The column vector, μ k Let Σ represent the posterior state vector. k μ k The corresponding posterior covariance matrix, Describe the optimal auxiliary matrix A k U k1 express The first column, Describe the optimal auxiliary matrix B k .

[0079] f x (·) is the similarity function of the state vector, f z (·) A similarity function for measuring vectors.

[0080] During the filtering iteration process, x in the cost function k It will be updated with each iteration, z k Obtained directly from inertial sensors. However, in this invention, z k It is also updated simultaneously during the iteration process, so corresponding to equations (5) and (6), μ k With zk As the iteration changes, A in equations (3) and (4) k B k Matrix update yields a better adjustment matrix. and Participate in subsequent solution calculations.

[0081] The state covariance matrix and measurement covariance matrix are corrected based on the adjustment matrix to obtain the corrected state covariance matrix and measurement covariance matrix, specifically including:

[0082] The calculated adjustment matrix and For the state covariance matrix P k∣k-1 With measurement covariance matrix R k∣k-1 After correction, the adjusted state covariance matrix is ​​obtained. With measurement covariance matrix

[0083] The Kalman gain is calculated based on the corrected state covariance matrix and the measurement covariance matrix. The filtered state vector is then determined based on the Kalman gain, specifically including:

[0084] Use the adjusted and Calculate the new Kalman gain The optimal posterior mean vector after filtering is obtained. and the optimal posterior covariance matrix

[0085]

[0086]

[0087] Because the optimal posterior mean vector (the mean vector of the filtered state vectors) And the optimal posterior covariance matrix (filtered covariance matrix) They are mutually coupled. Therefore, the fixed-point iteration method is used to approximate the solution. and Therefore, the implementation of the multidimensional outlier robust adaptive filtering algorithm based on maximum similarity proposed in this invention can be obtained.

[0088] When the initial value of the iteration (preset state vector) is... And the optimal solution (filtered state vector) Close enough, and If the following inequalities hold, then the fixed-point iteration method will converge locally.

[0089]

[0090] in, The second derivative of the similarity function of state vectors is given. Let θ1 and θ2 be arbitrary positive finite real numbers, and l represent the independent variable of the similarity function f(·). Equation (10) is solved for... The posterior state vector (filtered state vector) is composed of error components from position, velocity, attitude, accelerometer, and gyroscope. Indicates the position correction amount. Indicates the speed correction amount. This represents the attitude correction amount. δω 3×1 ,δf 3×1 These represent the zero bias of the gyroscope and the zero bias of the accelerometer, respectively. The correction amount is the error component, which is the drift error of the inertial navigation parameters.

[0091] The system model for integrated navigation filtering is as follows:

[0092]

[0093] Where F is the dynamic covariance matrix, containing all parts of the INS error model, and w represents the white noise of the system model. The state vector consists of the error components of position, velocity, attitude, gyroscope bias, and accelerometer bias, expressed as: G is the noise distribution vector, which includes the variance associated with the state vector: G = [σ] r,1×3 ,σ v,1×3 ,σ ε,1×3 ,σ ω,1×3 ,σ f,1×3 ], where σ r,1×3 The noise variance, σ, represents the position correction amount. v,1×3 The noise variance, σ, represents the speed correction amount. ε,1×3 σ represents the noise variance of the attitude correction. ω,1×3 σ represents the noise variance of the accelerometer zero bias correction. f,1×3 This represents the noise variance of the gyroscope's zero bias correction.

[0094] Step 104: Determine whether the L2 norm of the difference between the state vectors output in two adjacent iterations before and after filtering is less than a set threshold.

[0095] Step 105: If the error component of the inertial navigation parameter drift at the current moment is greater than or equal to the inertial navigation state at the previous moment, the inertial navigation state is updated, and the position at the current moment is calculated. The difference between the calculated position at the current moment and the position determined by the observation data of the global navigation satellite system at the current moment is calculated to obtain the updated measurement vector.

[0096] The inertial navigation status includes gyroscope output and accelerometer output.

[0097] Each iteration yields a set of state vectors. That is, in the steps above Extract the error components δω of the gyroscope and accelerometer from the state vector. 3×1 ,δf 3×1 Feedback is sent to the inertial navigation system at the previous moment.

[0098] ω * =ω0+δω (14)

[0099] f * =f0+δf (15)

[0100] Step 105 specifically includes:

[0101] According to the formula ω * =ω0+δω updates the gyroscope zero-bias component in the inertial navigation state of the previous time step; where ω * ω0 represents the updated zero-bias output of the gyroscope at the previous moment, ω0 represents the filtered output result of the zero-bias of the gyroscope at the previous moment before the update, and δω represents the zero-bias error component of the gyroscope.

[0102] According to formula f * =f0+δf updates the accelerometer zero-bias component in the inertial navigation state of the previous time step; where f * f0 represents the accelerometer zero bias output at the previous moment after the update, f0 represents the accelerometer zero bias output at the previous moment before the update, and δf represents the accelerometer zero bias error component.

[0103] By incorporating this feedback step, the σ in the noise distribution vector G associated with the state vector is reduced. ω,1×3 ,σ f,1×3 Filtering, thereby reducing the covariance P of the corresponding predicted state. k The smaller the value, the more reliable the inertial navigation state data becomes in the next iteration.

[0104] The corrected inertial navigation mechanical arrangement obtains the updated inertial navigation state at the current moment.

[0105] The attitude at the current moment is calculated by solving the attitude update differential equation;

[0106]

[0107] in, This represents the coordinate rotation matrix from the vehicle coordinate system to the navigation coordinate system; × indicates finding the antisymmetric matrix over a vector. This represents the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system, where ω is...* is assigned to represents the angular velocity vector,

[0108]

[0109] where L and h respectively represent the longitude and elevation in the local horizontal geographic coordinate system, and N, W, and U respectively represent the north, west, and up directions in the navigation coordinate system, and respectively represent the velocities in the three axial directions of north, west, and up in the navigation coordinate system, R M is the radius of curvature of the meridian, R N is the principal radius of curvature of the prime vertical, represents the Coriolis acceleration generated by the motion of the vehicle and the Earth, represents the centripetal acceleration generated by the motion of the vehicle on the Earth's surface, ω ig represents the angular velocity of the Earth's rotation.

[0110] Calculate the velocity at the current moment according to the velocity update differential equation;

[0111]

[0112] where, is the observed value of the accelerometer, assign the value of f * to g n is the component of the gravitational acceleration in the navigation coordinate system, v m represents the differential of v n represents the coordinate rotation matrix from the vehicle coordinate system to the navigation coordinate system, v n represents the velocity in the navigation coordinate system at the previous moment.

[0113] Calculate the position at the current moment according to the position update differential equation.

[0114]

[0115] where, represents the differential of longitude, represents the eastward velocity in the navigation coordinate system, represents the differential of latitude, represents the differential of elevation.

[0116] Substitute the feedback obtained ω * into f * Substitute into in the velocity update differential equation (19), after updating the attitude and velocity, the updated solution​ Substituting into the position update differential equation, we can obtain the new inertial navigation position solution.

[0117] In integrated navigation filtering, the discrete measurement model is represented as: δz k =H k δx k +η k Where, η on the right side of the equation k The measurement noise has zero mean and a covariance of R. k .

[0118] Since the filtered state vector contains errors from the INS, the corresponding measurement vector consists of the difference between the position predicted by the INS and the position measured by the GPS.

[0119] The updated measurement vector is represented as follows:

[0120]

[0121] Where, δv k To construct the new residual vector calculated after inertial navigation bias correction, this residual vector is the updated measurement vector z. k The corrected measurement vector z will be used in the next iteration. k Perform filtering and solution. This indicates the calculated position. This indicates the location determined by observation data from the Global Navigation Satellite System. λ represents the longitude of the calculated location. INS This indicates the calculated latitude of the location, h. INS This indicates that the height of the calculated position has been determined. λ represents the longitude of a location determined by observation data from the Global Navigation Satellite System. GPS h represents the latitude of a location determined by observation data from the Global Navigation Satellite System. GPS This indicates the altitude at which the location is determined by observation data from the Global Navigation Satellite System.

[0122] Step 106: Use the updated measurement vector as the measurement vector for the current time after the time update, and return to step 103.

[0123] Step 107: If the value is less than the set threshold, the position of the target to be located is determined based on the filtered state vector.

[0124] The iterative process specifically includes: obtaining the corrected residual vector δv by subtracting the inertial position solution after adding inertial parameters from the GNSS navigation position solution. k This is used as the new measurement vector z. k With the updated state vector Substitute these values ​​back into the next filtering iteration, specifically into equations (5) and (6). Repeat this process until the L2 norm of the difference between the state correction values ​​calculated in two adjacent iterations is sufficiently small, then end the iteration and obtain the optimal filtering solution. The optimal solution includes corrections for the inertial navigation system position. The correction amount can be simply added to the position result after the inertial navigation mechanics arrangement to obtain the accurate position result after the integrated navigation filtering.

[0125] Example 2

[0126] Figure 2 This is a schematic diagram of a combined navigation filtering system according to the present invention, as shown below. Figure 2 As shown, a combined navigation filtering system includes:

[0127] The inertial navigation data and global navigation satellite system observation data recording module 201 is used to record the inertial navigation data and global navigation satellite system observation data of the target to be located in real time through the integrated navigation system; the integrated navigation system is a combination of an inertial navigation system and a global navigation satellite system.

[0128] The current state vector and measurement vector determination module 202 is used to determine the current state vector and measurement vector based on the inertial navigation data and global navigation satellite system observation data of the target to be located at the current time. The state vector is a vector composed of position, velocity, attitude and error components of inertial navigation parameter drift. The measurement vector is the difference between the position determined based on the inertial navigation data and the position determined based on the global navigation satellite system observation data. The error components of inertial navigation parameter drift include gyroscope zero bias error components and accelerometer zero bias error components.

[0129] The filtering module 203 is used to filter the current state vector and measurement vector using a multidimensional outlier robust adaptive filtering algorithm based on maximum similarity to obtain the filtered state vector.

[0130] The judgment module 204 is used to determine whether the L2 norm of the difference between the state vectors output in two adjacent iterations before and after filtering is less than a set threshold.

[0131] The updated measurement vector acquisition module 205 is used to feed back the error component of the inertial navigation parameter drift at the current moment to the inertial navigation state at the previous moment when the difference between the state vectors output by two adjacent iterations before and after filtering is greater than or equal to a set threshold, and to update the inertial navigation state. It also calculates the position at the current moment, and subtracts the calculated position at the current moment from the position determined by the global navigation satellite system observation data at the current moment to obtain the updated measurement vector.

[0132] Return module 206 is used to take the updated measurement vector as the measurement vector at the current time after the time update and return it to the filtering module 203.

[0133] The target location determination module 207 is used to determine the target location result based on the filtered state vector when the L2 norm of the difference between the state vectors output by two adjacent iterations before and after filtering is less than a set threshold.

[0134] Example 3

[0135] This invention provides an electronic device including a memory and a processor. The memory stores a computer program, and the processor runs the computer program to enable the electronic device to perform the combined navigation filtering method of Embodiment 1.

[0136] In addition, embodiments of the present invention also provide a computer-readable storage medium storing a computer program that, when executed by a processor, implements the combined navigation filtering method of Embodiment 1.

[0137] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be referred to the method section.

[0138] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A combined navigation filtering method, characterized in that, include: The system records inertial navigation data and global navigation satellite system observation data of the target to be located in real time through a combined navigation system; the combined navigation system is a combination of an inertial navigation system and a global navigation satellite system. The current state vector and measurement vector are determined based on the inertial navigation data and global navigation satellite system observation data of the target to be located at the current moment; the state vector is a vector composed of position, velocity, attitude and error components of inertial navigation parameter drift; the measurement vector is the difference between the position determined based on the inertial navigation data and the position determined based on the global navigation satellite system observation data; the error components of inertial navigation parameter drift include gyroscope zero bias error components and accelerometer zero bias error components; The current state vector and measurement vector are filtered by a multidimensional outlier robust adaptive filtering algorithm based on maximum similarity to obtain the filtered state vector. Determine whether the L2 norm of the difference between the state vectors output from two consecutive iterations before and after filtering is less than a set threshold. If the error component of the inertial navigation parameter drift at the current moment is greater than or equal to the set threshold, the error component of the inertial navigation parameter drift at the current moment is fed back to the inertial navigation state at the previous moment to update the inertial navigation state, and the position at the current moment is calculated. The difference between the calculated position at the current moment and the position determined by the observation data of the global navigation satellite system at the current moment is calculated to obtain the updated measurement vector. Use the updated measurement vector as the measurement vector at the current time after the time update, and return to the step "Filter the current state vector and measurement vector using the multidimensional outlier robust adaptive filtering algorithm based on maximum similarity to obtain the filtered state vector"; If the value is less than the set threshold, the position of the target to be located is determined based on the filtered state vector. The step of filtering the current state vector and measurement vector using a multidimensional outlier robust adaptive filtering algorithm based on maximum similarity to obtain the filtered state vector specifically includes: A cost function based on a similarity metric is constructed based on the nominal prediction error covariance matrix and the nominal measurement noise covariance matrix. The adjustment matrix is ​​determined by solving the cost function. The state covariance matrix and measurement covariance matrix are corrected according to the adjustment matrix to obtain the corrected state covariance matrix and measurement covariance matrix; The Kalman gain is calculated based on the corrected state covariance matrix and the measurement covariance matrix. The filtered state vector is determined based on the Kalman gain.

2. The integrated navigation filtering method according to claim 1, characterized in that, The process of feeding back the error component of the current inertial navigation parameter drift to the inertial navigation state of the previous moment for inertial navigation state update, calculating the current position, and subtracting the calculated current position from the position determined by the current global navigation satellite system observation data to obtain the updated measurement vector specifically includes: According to the formula ω * =ω0+δω updates the gyroscope output in the inertial navigation state; where ω * ω0 represents the gyroscope output at the previous moment after the update, ω0 represents the gyroscope output at the previous moment before the update, and δω represents the gyroscope zero bias error component. According to formula f * =f0+δf updates the accelerometer output in the inertial navigation state; where f * f0 represents the gyroscope output at the previous moment after the update, f0 represents the gyroscope output at the previous moment before the update, and δf represents the accelerometer zero bias error component. Based on the attitude update differential equation The attitude at the current moment is calculated; where, This represents the coordinate rotation matrix from the vehicle coordinate system to the navigation coordinate system; × indicates finding the antisymmetric matrix over a vector. This represents the projection of the angular velocity of the carrier coordinate system relative to the inertial coordinate system onto the carrier coordinate system, where ω is... * Assigned value Represents the angular velocity vector. Where, L represents the longitude in the local horizontal geographic coordinate system, h represents the elevation in the local horizontal geographic coordinate system, N, W, and U respectively represent the northward, westward, and upward directions in the navigation coordinate system, and respectively represent the velocities in the three axial directions of northward, westward, and upward in the navigation coordinate system, R M is the radius of curvature of the meridian, R N is the principal radius of curvature of the prime vertical, represents the Coriolis acceleration generated by the movement of the carrier relative to the Earth, represents the centripetal acceleration generated by the movement of the carrier on the Earth's surface, ω ig represents the angular velocity of the Earth's rotation; Based on the velocity update differential equation Calculate the velocity at the current moment; where, For the accelerometer observations, f * The value assigned to g n v is the component of gravitational acceleration in the navigation coordinate system. n v represents the velocity in the navigation coordinate system at the previous moment. m Indicates v n The differential, This represents the coordinate rotation matrix from the vehicle coordinate system to the navigation coordinate system; Update the differential equation based on location Calculate the current position; in, The derivative of longitude. This represents the eastward velocity in the navigation coordinate system. The derivative representing latitude, The differential of elevation.

3. The integrated navigation filtering method according to claim 1, characterized in that, The updated measurement vector is represented as follows: Among them, z k This represents the updated measurement vector. This indicates the calculated position. This indicates the location determined by observation data from the Global Navigation Satellite System. λ represents the longitude of the calculated location. INS This indicates the calculated latitude of the location, h. INS This indicates that the height of the calculated position has been determined. λ represents the longitude of a location determined by observation data from the Global Navigation Satellite System. GPS h represents the latitude of a location determined by observation data from the Global Navigation Satellite System. GPS This indicates the altitude at which the location is determined by observation data from the Global Navigation Satellite System.

4. A combined navigation filtering system, characterized in that, include: The inertial navigation data and global navigation satellite system observation data recording module is used to record the inertial navigation data and global navigation satellite system observation data of the target to be located in real time through the integrated navigation system; the integrated navigation system is a combination of an inertial navigation system and a global navigation satellite system. The current state vector and measurement vector determination module is used to determine the current state vector and measurement vector based on the inertial navigation data and global navigation satellite system observation data of the target to be located at the current moment. The state vector is a vector composed of position, velocity, attitude, and error components of inertial navigation parameter drift. The measurement vector is the difference between the position determined based on the inertial navigation data and the position determined based on the global navigation satellite system observation data. The error components of inertial navigation parameter drift include gyroscope zero bias error components and accelerometer zero bias error components. The filtering module is used to filter the current state vector and measurement vector using a multidimensional outlier robust adaptive filtering algorithm based on maximum similarity to obtain the filtered state vector. The judgment module is used to determine whether the L2 norm of the difference between the state vectors output by two adjacent iterations before and after filtering is less than a set threshold. The updated measurement vector acquisition module, when the difference between the state vectors output by two adjacent iterations before and after filtering is greater than or equal to a set threshold, is used to feed back the error component of the current inertial navigation parameter drift to the inertial navigation state of the previous moment for inertial navigation state update, and calculate the current position. The calculated current position is then subtracted from the position determined by the current global navigation satellite system observation data to obtain the updated measurement vector. The return module is used to return the updated measurement vector as the measurement vector at the current time after the time update to the filtering module; The target location determination module is used to determine the target location result based on the filtered state vector when the L2 norm of the difference between the state vectors output by two adjacent iterations before and after filtering is less than a set threshold. The step of filtering the current state vector and measurement vector using a multidimensional outlier robust adaptive filtering algorithm based on maximum similarity to obtain the filtered state vector specifically includes: A cost function based on a similarity metric is constructed based on the nominal prediction error covariance matrix and the nominal measurement noise covariance matrix. The adjustment matrix is ​​determined by solving the cost function. The state covariance matrix and measurement covariance matrix are corrected according to the adjustment matrix to obtain the corrected state covariance matrix and measurement covariance matrix; The Kalman gain is calculated based on the corrected state covariance matrix and the measurement covariance matrix. The filtered state vector is determined based on the Kalman gain.

5. An electronic device, characterized in that, include: At least one processor, at least one memory, and computer program instructions stored in the memory, which, when executed by the processor, implement the method as described in any one of claims 1-3.

6. A computer-readable storage medium having computer program instructions stored thereon, characterized in that, When the computer program instructions are executed by a processor, the method as described in any one of claims 1-3 is implemented.

Images