Navigation accuracy improvement method, system, device and medium based on adaptive filtering

Abstract

The present invention relates to the field of composite navigation and provides a method, system, device, and medium for improving navigation accuracy based on adaptive filtering. The method comprises obtaining magnetometer parameters, accelerometer parameters, and gyroscope parameters from a navigation data sensor; obtaining observation noise covariance, performing motion detection on an observed target using the accelerometer parameters, thereby obtaining target velocity observation noise covariance; calculating observation quaternions, obtaining quaternion derivatives, and constructing observation equations, establishing a system input equation system, and obtaining a discrete input equation system; calculating error quaternions and obtaining estimated residuals, and obtaining residual variances based on the estimated residuals; multiplying the residual variances by the attitude observation noise covariance to obtain target attitude observation noise covariance, obtaining a Kalman gain using the target velocity observation noise covariance and the target attitude observation noise covariance, performing Kalman filtering, and outputting the attitude and velocity of the observed target. The present invention can improve navigation accuracy.

Description

Technical Field

[0001] The present invention relates to the field of composite navigation technology, and in particular to a method, system, device and medium for improving navigation accuracy based on adaptive filtering. Background Art

[0002] Personal outdoor navigation devices are capable of providing real-time, accurate, and reliable positioning information in both known and unknown environments. These devices are independent of external signals and operate reliably in a wide range of environments. In specialized environments, navigation systems must be able to achieve autonomous navigation and positioning, relying solely on their own sensors, independent of external infrastructure, and adaptable to diverse and complex environments. To ensure reliability, personal outdoor navigation devices typically integrate multiple sensors to acquire data, avoiding the large navigation errors and poor adaptability associated with a single data source. To effectively utilize this data, Kalman filtering is performed to reduce the impact of noise.

[0003] In traditional Kalman filtering, obtaining the Kalman gain requires switching the observation model. This process can cause the system state to diverge, leading to increased error. Using a fixed Kalman gain for filtering also results in over-filtering of data with small errors and under-filtering of data with large errors, increasing the error. This also prevents the Kalman gain from being flexibly adjusted to suit the accuracy of different sensors. Summary of the Invention

[0004] The present invention aims to solve at least one of the technical problems existing in the related art. To this end, the present invention provides a navigation accuracy improvement method, system, device and medium based on adaptive filtering to achieve high-precision navigation that does not rely on external signals.

[0005] The present invention provides a method for improving navigation accuracy based on adaptive filtering, comprising:

[0006] S1: Arrange a navigation data sensor on the observation target and obtain magnetometer parameters, accelerometer parameters and gyroscope parameters from the navigation data sensor;

[0007] S2: Obtain the observation noise covariance including the velocity observation noise covariance and the attitude observation noise covariance. Perform motion detection on the observed target through the accelerometer parameters. When the observed target is stationary, the velocity observation noise covariance takes a minimum value, otherwise it takes a maximum value, thereby obtaining the target velocity observation noise covariance.

[0008] S3: Calculate the observation quaternion through the magnetometer parameters and gyroscope parameters, obtain the quaternion derivative and construct the observation equation, and establish the system input equation group based on the quaternion derivative and the observation equation to obtain the discrete input equation group;

[0009] S4: Calculate the quaternion estimate using the discrete input equation, calculate the error quaternion using the quaternion estimate, obtain the estimated residual using the error quaternion, and obtain the residual variance based on the estimated residual using the sliding window method;

[0010] S5: Multiply the residual variance by the attitude observation noise covariance to obtain the target attitude observation noise covariance. The Kalman gain is obtained through the target velocity observation noise covariance and the target attitude observation noise covariance, and Kalman filtering is performed to output the attitude and velocity of the observed target.

[0011] According to the method for improving navigation accuracy based on adaptive filtering provided by the present invention, in step S1, the navigation data sensor includes a gyroscope, an accelerometer, and a magnetometer.

[0012] According to the method for improving navigation accuracy based on adaptive filtering provided by the present invention, step S2 further includes:

[0013] S21: obtaining the observation noise covariance including the velocity observation noise covariance and the attitude observation noise covariance, obtaining the accelerometer parameters of the accelerometer, and determining the motion state of the observation target according to the accelerometer parameters;

[0014] S22: Determine the minimum and maximum values ​​of the speed observation noise covariance, and judge whether the observed target is stationary based on the motion state. When the observed target is stationary, the speed observation noise covariance takes the minimum value, otherwise it takes the maximum value, thereby obtaining the target speed observation noise covariance.

[0015] According to the method for improving navigation accuracy based on adaptive filtering provided by the present invention, step S3 further includes:

[0016] S31: Calculate a first quaternion intermediate variable and a second quaternion intermediate variable using magnetometer parameters and gyroscope parameters, and obtain the observation quaternion through the first quaternion intermediate variable and the second quaternion intermediate variable;

[0017] S32: Obtain the quaternion derivative according to the observation quaternion and the gyroscope parameter, construct a measurement matrix, and construct an observation equation using the measurement matrix and the quaternion derivative;

[0018] S33: Establishing a system input equation group according to the quaternion derivative and the observation equation, and discretizing the system input equation group to obtain a discrete input equation group.

[0019] According to the method for improving navigation accuracy based on adaptive filtering provided by the present invention, step S4 further includes:

[0020] S41: Calculate the quaternion estimate using the discrete input equation group, calculate the error quaternion according to the observed quaternion and the quaternion estimate, determine the empirical coefficient, and obtain the estimated residual through the empirical coefficient and the error quaternion;

[0021] S42: Determine a window size, and obtain the residual variance according to the estimated residual and the window size using the sliding window method.

[0022] According to the navigation accuracy improvement method based on adaptive filtering provided by the present invention, in step S41, after obtaining the estimated residual, the mean of the estimated residual is calculated. If the mean of the estimated residual is not 0, the observation noise covariance is subjected to Cholesky decomposition to obtain a whitening matrix. The whitened residual is obtained through the whitening matrix, and the whitened residual is used as the estimated residual.

[0023] According to the navigation accuracy improvement method based on adaptive filtering provided by the present invention, in step S5, the Kalman gain is obtained by the target velocity observation noise covariance and the target attitude observation noise covariance, and the magnetometer parameters, the accelerometer parameters and the gyroscope parameters are Kalman filtered according to the Kalman gain to obtain filtering parameters, and the attitude and speed of the observed target are obtained according to the filtering parameters.

[0024] The present invention also provides a navigation accuracy improvement system based on adaptive filtering, comprising:

[0025] Parameter acquisition module: used to arrange navigation data sensors on the observation target and obtain magnetometer parameters, accelerometer parameters and gyroscope parameters from the navigation data sensors;

[0026] Target velocity observation noise covariance module: used to obtain the observation noise covariance including velocity observation noise covariance and attitude observation noise covariance. The accelerometer parameters are used to detect the movement of the observed target. When the observed target is stationary, the velocity observation noise covariance takes the minimum value, otherwise it takes the maximum value, thereby obtaining the target velocity observation noise covariance.

[0027] Discrete Input Equation Group Module: This module is used to calculate the observation quaternion using magnetometer parameters and gyroscope parameters, obtain the quaternion derivative and construct the observation equation. The system input equation group is established based on the quaternion derivative and the observation equation, thereby obtaining the discrete input equation group.

[0028] Residual variance module: used to calculate the quaternion estimate using the discrete input equation, calculate the error quaternion from the quaternion estimate, obtain the estimated residual from the error quaternion, and obtain the residual variance based on the estimated residual using the sliding window method;

[0029] Attitude and velocity output module: used to multiply the residual variance with the attitude observation noise covariance to obtain the target attitude observation noise covariance, obtain the Kalman gain through the target velocity observation noise covariance and the target attitude observation noise covariance, perform Kalman filtering, and output the attitude and velocity of the observed target.

[0030] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps of any of the above-described methods for improving navigation accuracy based on adaptive filtering are implemented.

[0031] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the steps of any of the above-mentioned methods for improving navigation accuracy based on adaptive filtering are implemented.

[0032] The above one or more technical solutions in the embodiments of the present invention have at least one of the following technical effects:

[0033] The adaptive filtering-based navigation accuracy improvement method, system, device, and medium provided by the present invention directly obtain the Kalman gain by calculating the target observation noise covariance, eliminating the need to perform pseudo observations with a velocity of zero, and thus eliminating the need to switch the observation model. This avoids the divergence of the system state caused by hard switching of the observation model and sudden velocity changes. The Kalman filtering process fully utilizes accelerometer data when the vehicle is stationary, while automatically increasing the reliance on magnetometer data when the vehicle is in motion. This overcomes the shortcomings of traditional inertial navigation devices, such as large errors and error divergence over time, and the susceptibility of accelerometers to interference from the vehicle's linear acceleration.

[0034] Additional aspects and advantages of the present invention will be set forth in part in the description which follows and, in part, will be obvious from the description which follows, or may be learned by practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0036] Figure 1 It is a flow chart of the method for improving navigation accuracy based on adaptive filtering provided by the present invention.

[0037] Figure 2 It is a structural diagram of the navigation accuracy improvement system based on adaptive filtering provided by the present invention.

[0038] Figure 3 It is a structural diagram of the navigation accuracy improvement device based on adaptive filtering provided by the present invention.

[0039] Reference numerals:

[0040] 100, parameter acquisition module; 200, target velocity observation noise covariance module; 300, discrete input equation group module; 400, residual variance module; 500, attitude velocity output module; 810, processor; 820, communication interface; 830, memory; 840, communication bus. DETAILED DESCRIPTION

[0041] To make the purpose, technical solutions and advantages of the present invention clearer, the technical solutions in the present invention will be clearly and completely described below. Obviously, the embodiments described are part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention. The following embodiments are used to illustrate the present invention, but are not used to limit the scope of the present invention.

[0042] In the description of the embodiments of the present invention, it should be noted that the terms “first”, “second” and “third” are used for descriptive purposes only and should not be understood as indicating or implying relative importance.

[0043] In the description of the embodiments of the present invention, it should be noted that, unless otherwise specified or limited, the terms "connected" and "connection" should be understood in a broad sense. For example, they can refer to fixed connections, detachable connections, or integral connections; mechanical connections, electrical connections; and direct connections or indirect connections through an intermediary. Those skilled in the art will understand the specific meanings of the above terms in the embodiments of the present invention based on the specific circumstances.

[0044] In the description of this specification, the reference terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" mean that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the embodiment of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art can combine and combine different embodiments or examples described in this specification and features of different embodiments or examples without contradiction.

[0045] The following combination Figures 1 to 3 Describe the specific embodiment of the present invention:

[0046] Figure 1 FIG is a flow chart of the method for improving navigation accuracy based on adaptive filtering provided by the present invention. The process of executing the method for improving navigation accuracy based on adaptive filtering is as follows: Figure 1 shown.

[0047] The present invention provides a method for improving navigation accuracy based on adaptive filtering, comprising:

[0048] S1: Arrange a navigation data sensor on the observation target and obtain magnetometer parameters, accelerometer parameters and gyroscope parameters from the navigation data sensor;

[0049] Furthermore, the purpose of this stage is to arrange navigation data sensors to obtain navigation parameters. Specifically, in step S1, the navigation data sensors include a gyroscope, an accelerometer, and a magnetometer.

[0050] Regarding the above steps, the specific implementation methods in this embodiment are as follows:

[0051] First, the target for observation must be determined. In this embodiment, the target is an individual in a field environment. Next, navigation data sensors must be placed on the target. These sensors include a gyroscope, an accelerometer, and a magnetometer. The magnetometer acquires magnetometer parameters related to the Earth's magnetic field, the accelerometer acquires accelerometer parameters as the target moves, and the gyroscope acquires gyroscope parameters as the target moves.

[0052] S2: Obtain the observation noise covariance including the velocity observation noise covariance and the attitude observation noise covariance. Perform motion detection on the observed target through the accelerometer parameters. When the observed target is stationary, the velocity observation noise covariance takes a minimum value, otherwise it takes a maximum value, thereby obtaining the target velocity observation noise covariance.

[0053] Furthermore, the purpose of this stage is to obtain the observation noise covariance and perform speed detection, and obtain the target speed observation noise covariance according to the motion state of the observed target. Specifically, step S2 further includes:

[0054] S21: obtaining the observation noise covariance including the velocity observation noise covariance and the attitude observation noise covariance, obtaining the accelerometer parameters of the accelerometer, and determining the motion state of the observation target according to the accelerometer parameters;

[0055] S22: Determine the minimum and maximum values ​​of the speed observation noise covariance, and judge whether the observed target is stationary based on the motion state. When the observed target is stationary, the speed observation noise covariance takes the minimum value, otherwise it takes the maximum value, thereby obtaining the target speed observation noise covariance.

[0056] Regarding the above steps, the specific implementation methods in this embodiment are as follows:

[0057] First, we need to obtain the initial navigation data sensor observation noise covariance, including velocity observation noise covariance and attitude observation noise covariance. And the velocity observation noise covariance and attitude observation noise covariance The presentation form is matrix: observation noise covariance for:

[0058]

[0059] in, is a 3×3 matrix of zeros.

[0060] Then, the accelerometer parameters obtained by the accelerometer are obtained. The accelerometer parameters can be used to judge the motion state of the observed target by integrating the acceleration. When the observed target is stationary and not moving, the value in the velocity observation noise covariance is set to a minimum value. In this embodiment, the value is , then it means that the data obtained from the accelerometer parameters can be trusted, and the Kalman gain of the accelerometer will become larger, so that the output of the attitude and velocity of the observed target will rely more on the data output by the accelerometer. When the observed target is in a non-stationary state, the value in the velocity observation noise covariance is set to a maximum value. In this embodiment, the value is At this time, although the accelerometer can still obtain the velocity of the observed target by integrating the acceleration, the noise of the accelerometer during the motion process is relatively large. The large velocity observation noise covariance will make the Kalman gain of the accelerometer lower during the Kalman filtering process, thus distrusting the data output by the accelerometer.

[0061] S3: Calculate the observation quaternion through the magnetometer parameters and gyroscope parameters, obtain the quaternion derivative and construct the observation equation, and establish the system input equation group based on the quaternion derivative and the observation equation to obtain the discrete input equation group;

[0062] Furthermore, the purpose of this stage is to calculate the observation quaternion, thereby obtaining the quaternion derivative and constructing the observation equation, establishing the system input equation group, and finally obtaining the discrete input equation group. Specifically, step S3 further includes:

[0063] S31: Calculate a first quaternion intermediate variable and a second quaternion intermediate variable using magnetometer parameters and gyroscope parameters, and obtain the observation quaternion through the first quaternion intermediate variable and the second quaternion intermediate variable;

[0064] S32: Obtain the quaternion derivative according to the observation quaternion and the gyroscope parameter, construct a measurement matrix, and construct an observation equation using the measurement matrix and the quaternion derivative;

[0065] S33: Establishing a system input equation group according to the quaternion derivative and the observation equation, and discretizing the system input equation group to obtain a discrete input equation group.

[0066] The specific implementation of the above steps in this embodiment is as follows:

[0067] First, you need to obtain the magnetometer parameter a obtained by the magnetometer and the accelerometer parameter b obtained by the accelerometer, so that you can calculate the first quaternion intermediate variable v and the second quaternion intermediate variable s:

[0068]

[0069] Then the observation quaternion q can be obtained through the first quaternion intermediate variable and the second quaternion intermediate variable:

[0070]

[0071] Where T represents transpose, is the component of the first quaternion intermediate variable on the x-axis in the left-hand rectangular coordinate system where the observation target is located, is the component of the first quaternion intermediate variable on the y-axis in the left-hand rectangular coordinate system where the observation target is located, is the component of the first quaternion intermediate variable on the z-axis in the left-handed rectangular coordinate system where the observation target is located.

[0072] Then, the quaternion derivative can be obtained based on the observed quaternion and gyroscope parameters :

[0073]

[0074]

[0075] in, represents the state conversion coefficient of the observation quaternion, is the component of the gyroscope parameter on the x-axis in the left-hand rectangular coordinate system where the observation target is located, is the component of the gyroscope parameter on the y-axis in the left-hand rectangular coordinate system where the observation target is located, is the component of the gyroscope parameter on the z-axis in the left-hand rectangular coordinate system where the observation target is located.

[0076] Then construct the observation matrix C. Here, the observation matrix is ​​the identity matrix. The observation equation can be constructed using the observation matrix and quaternion derivatives: , where y is the system observation variable.

[0077] Then the system input equations are established based on the quaternion derivatives and observation equations:

[0078]

[0079] in, is the process noise at time t, is the observation noise at time t. If the Kalman filter is directly applied to the observation quaternion, the unit constraint will be destroyed. Therefore, it is necessary to discretize the system input equations by small angle perturbations to obtain the discrete input equations:

[0080]

[0081] in, is the process noise at time k, is the observation noise at time k, is the small angle perturbation of the observation quaternion at time k, is the small angle perturbation of the observation quaternion at time k+1, is the state transfer matrix at time k, is the increase in the system observation variable at time k. Among them, the state transfer matrix is:

[0082]

[0083] in, is the time interval between time k and time k+1, I is the identity matrix, is the angular velocity at time k when the small angle disturbance occurs, and:

[0084]

[0085] Here, for small-angle perturbations, since the angular perturbation is three-dimensional and corresponds to the last three elements of the observation quaternion, the expression for angular velocity is a 3×3 matrix. System noise is considered to be independent white noise between navigation data sensors. This allows us to obtain system noise, which includes both process noise and observation noise.

[0086] S4: Calculate the quaternion estimate using the discrete input equation, calculate the error quaternion using the quaternion estimate, obtain the estimated residual using the error quaternion, and obtain the residual variance based on the estimated residual using the sliding window method;

[0087] Furthermore, the purpose of this stage is to calculate the error quaternion, thereby obtaining the estimated residual and obtaining the residual variance. Specifically, step S4 further includes:

[0088] S41: Calculate the quaternion estimate using the discrete input equation group, calculate the error quaternion according to the observed quaternion and the quaternion estimate, determine the empirical coefficient, and obtain the estimated residual through the empirical coefficient and the error quaternion;

[0089] S42: Determine a window size, and obtain the residual variance according to the estimated residual and the window size using the sliding window method.

[0090] In step S41, after obtaining the estimated residual, the mean of the estimated residual is calculated. If the mean of the estimated residual is not 0, the observation noise covariance is subjected to Cholesky decomposition to obtain a whitening matrix. The whitened residual is obtained through the whitening matrix, and the whitened residual is used as the estimated residual.

[0091] Regarding the above steps, the specific implementation methods in this embodiment are as follows:

[0092] First, since the first equation of the discrete input equation group can predict the small angle perturbation obtained after the observation quaternion is discretized, the quaternion estimate for the k-th moment can be obtained by predicting the small angle perturbation through the discrete input equation group. , so that the error quaternion at time k can be calculated by treating time k as the current time :

[0093]

[0094] in, is quaternion multiplication.

[0095] Then determine the empirical coefficient P, where the empirical coefficient is 0.2~0.5 according to experience, and obtain the estimated residual at time k through the empirical coefficient and error quaternion :

[0096]

[0097] in, is the component of the error quaternion at time k on the x-axis in the left-hand rectangular coordinate system where the target is located, is the component of the error quaternion at time k on the y-axis in the left-hand rectangular coordinate system where the observed target is located, is the component of the error quaternion at time k on the z-axis in the left-handed rectangular coordinate system where the observed target is located.

[0098] Then determine the window size, and use the sliding window method to estimate the residual according to the window size to obtain the residual variance of the estimated residual. Here, if the system noise is too large or too small, the value of the residual variance may be too large or too small, which may cause the system to fail. To this end, set the upper limit of the variance, here we take , set the lower limit of variance, here we take If the residual variance is greater than the upper limit of the variance, the residual variance is directly taken as the upper limit of the variance. Similarly, if the residual variance is less than the lower limit of the variance, the residual variance is directly taken as the lower limit of the variance.

[0099] Normally, the mean of the estimated residuals should be 0, but in some cases the mean of the estimated residuals may not be 0. In this case, it is necessary to perform Cholesky decomposition on the observation noise covariance to obtain the whitening matrix L. Then, the whitening matrix is ​​multiplied by the error quaternion to obtain the whitened quaternion. The whitened quaternion is then used as the error quaternion, and the whitened residual is calculated using the above steps. Using the whitened residual as the estimated residual can make the mean of the estimated residual 0.

[0100] S5: Multiply the residual variance by the attitude observation noise covariance to obtain the target attitude observation noise covariance. The Kalman gain is obtained through the target velocity observation noise covariance and the target attitude observation noise covariance, and Kalman filtering is performed to output the attitude and velocity of the observed target.

[0101] Furthermore, the purpose of this stage is to obtain a Kalman gain and perform Kalman filtering to output the attitude and velocity of the observed target. Specifically, in step S5, a Kalman gain is obtained by using the target velocity observation noise covariance and the target attitude observation noise covariance. Kalman filtering is performed on the magnetometer parameters, the accelerometer parameters, and the gyroscope parameters based on the Kalman gain to obtain filtering parameters. The attitude and velocity of the observed target are obtained based on the filtering parameters.

[0102] Regarding the above steps, the specific implementation methods in this embodiment are as follows:

[0103] Multiplying the residual variance by the attitude observation noise covariance yields the target attitude observation noise covariance. In Kalman filtering, the Kalman gain is determined by the observation noise covariance. A smaller observation noise covariance indicates higher credibility for the corresponding data, leading to a higher Kalman gain, thereby retaining more of this data. Specifically, when the target velocity observation noise covariance is small, the Kalman gain for the accelerometer parameters is higher, while a smaller Kalman gain for the magnetometer parameters is higher.

[0104] Therefore, the Kalman gain in the Kalman filtering process can be derived from the noise covariance of the target velocity observation and the target attitude observation. Using the Kalman gain, the magnetometer, accelerometer, and gyroscope parameters are subjected to Kalman filtering to obtain the filter parameters. Using these filtered parameters, the attitude and velocity of the observed target can be calculated, providing navigation and positioning for the target.

[0105] The present invention directly obtains the Kalman gain by calculating the target observation noise covariance, avoids the system state divergence caused by hard switching of the observation model and sudden speed change, overcomes the shortcomings of traditional inertial navigation devices that have large errors and error divergence over time, and the accelerometer is easily disturbed by the carrier linear acceleration.

[0106] The following describes a navigation accuracy improvement device based on adaptive filtering provided by the present invention. The navigation accuracy improvement device based on adaptive filtering described below and the navigation accuracy improvement method based on adaptive filtering described above can refer to each other.

[0107] Figure 2 The structural diagram of the navigation accuracy improvement system based on adaptive filtering is shown as follows: Figure 2 As shown, the method for improving navigation accuracy based on adaptive filtering as described above includes:

[0108] Parameter acquisition module 100: used to arrange navigation data sensors on the observation target and obtain magnetometer parameters, accelerometer parameters and gyroscope parameters from the navigation data sensors;

[0109] Target velocity observation noise covariance module 200: used to obtain the observation noise covariance including the velocity observation noise covariance and the attitude observation noise covariance, and perform motion detection on the observation target through accelerometer parameters. When the observation target is stationary, the velocity observation noise covariance takes a minimum value, otherwise it takes a maximum value, thereby obtaining the target velocity observation noise covariance;

[0110] Discrete input equation group module 300: used to calculate the observation quaternion through the magnetometer parameters and the gyroscope parameters, obtain the quaternion derivative and construct the observation equation, and establish the system input equation group according to the quaternion derivative and the observation equation, thereby obtaining the discrete input equation group;

[0111] Residual variance module 400: used to calculate quaternion estimation using discrete input equations, calculate error quaternion using the quaternion estimation, obtain estimated residual using the error quaternion, and obtain residual variance based on the estimated residual using a sliding window method;

[0112] The attitude and velocity output module 500 is used to multiply the residual variance by the attitude observation noise covariance to obtain the target attitude observation noise covariance, obtain the Kalman gain through the target velocity observation noise covariance and the target attitude observation noise covariance, perform Kalman filtering, and output the attitude and velocity of the observed target.

[0113] on the other hand, Figure 3 An example of a physical structure diagram of an electronic device is shown below. Figure 3As shown, the electronic device may include: a processor 810, a communication interface 820, a memory 830, and a communication bus 840, wherein the processor 810, the communication interface 820, and the memory 830 communicate with each other via the communication bus 840. The processor 810 may call the logic instructions in the memory 830 to execute a navigation accuracy improvement method based on adaptive filtering, which includes:

[0114] S1: Arrange a navigation data sensor on the observation target and obtain magnetometer parameters, accelerometer parameters and gyroscope parameters from the navigation data sensor;

[0115] S2: Obtain the observation noise covariance including the velocity observation noise covariance and the attitude observation noise covariance. Perform motion detection on the observed target through the accelerometer parameters. When the observed target is stationary, the velocity observation noise covariance takes a minimum value, otherwise it takes a maximum value, thereby obtaining the target velocity observation noise covariance.

[0116] S3: Calculate the observation quaternion through the magnetometer parameters and gyroscope parameters, obtain the quaternion derivative and construct the observation equation, and establish the system input equation group based on the quaternion derivative and the observation equation to obtain the discrete input equation group;

[0117] S4: Calculate the quaternion estimate using the discrete input equation, calculate the error quaternion using the quaternion estimate, obtain the estimated residual using the error quaternion, and obtain the residual variance based on the estimated residual using the sliding window method;

[0118] S5: Multiply the residual variance by the attitude observation noise covariance to obtain the target attitude observation noise covariance. The Kalman gain is obtained through the target velocity observation noise covariance and the target attitude observation noise covariance, and Kalman filtering is performed to output the attitude and velocity of the observed target.

[0119] Furthermore, the logic instructions in the aforementioned memory 830 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the portion that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product, stored in a storage medium, includes instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to perform all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, mobile hard drives, read-only memories (ROMs), random access memories (RAMs), magnetic disks, or optical disks.

[0120] In another aspect, the present invention further provides a non-transitory computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the method for improving navigation accuracy based on adaptive filtering provided by the above methods is implemented, and the method includes:

[0121] S1: Arrange a navigation data sensor on the observation target and obtain magnetometer parameters, accelerometer parameters and gyroscope parameters from the navigation data sensor;

[0122] S2: Obtain the observation noise covariance including the velocity observation noise covariance and the attitude observation noise covariance. Perform motion detection on the observed target through the accelerometer parameters. When the observed target is stationary, the velocity observation noise covariance takes a minimum value, otherwise it takes a maximum value, thereby obtaining the target velocity observation noise covariance.

[0123] S3: Calculate the observation quaternion through the magnetometer parameters and gyroscope parameters, obtain the quaternion derivative and construct the observation equation, and establish the system input equation group based on the quaternion derivative and the observation equation to obtain the discrete input equation group;

[0124] S4: Calculate the quaternion estimate using the discrete input equation, calculate the error quaternion using the quaternion estimate, obtain the estimated residual using the error quaternion, and obtain the residual variance based on the estimated residual using the sliding window method;

[0125] S5: Multiply the residual variance by the attitude observation noise covariance to obtain the target attitude observation noise covariance. The Kalman gain is obtained through the target velocity observation noise covariance and the target attitude observation noise covariance, and Kalman filtering is performed to output the attitude and velocity of the observed target.

[0126] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, i.e., they may be located in one location or distributed across multiple network units. Some or all of the modules may be selected based on actual needs to achieve the objectives of the present embodiment. Persons of ordinary skill in the art will be able to understand and implement the present invention without inventive effort.

[0127] Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented using software plus a necessary general-purpose hardware platform, or of course, hardware. Based on this understanding, the essence of the above technical solution, or the portion that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a magnetic disk, or an optical disk, and includes a number of instructions for causing a computer device (such as a personal computer, server, or network device) to execute the methods described in each embodiment or certain portions of the embodiments.

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

Claims

1. A navigation accuracy improvement method based on adaptive filtering, characterized in that: include: S1: Arrange a navigation data sensor on the observation target and obtain magnetometer parameters, accelerometer parameters and gyroscope parameters from the navigation data sensor; S2: Obtain the observation noise covariance including the velocity observation noise covariance and the attitude observation noise covariance. Perform motion detection on the observed target through the accelerometer parameters. When the observed target is stationary, the velocity observation noise covariance takes a minimum value, otherwise it takes a maximum value, thereby obtaining the target velocity observation noise covariance. S3: Calculate the observation quaternion through the magnetometer parameters and gyroscope parameters, obtain the quaternion derivative and construct the observation equation, and establish the system input equation group based on the quaternion derivative and the observation equation to obtain the discrete input equation group; S4: Calculate the quaternion estimate using the discrete input equation, calculate the error quaternion using the quaternion estimate, obtain the estimated residual using the error quaternion, and obtain the residual variance based on the estimated residual using the sliding window method; S5: Multiply the residual variance by the attitude observation noise covariance to obtain the target attitude observation noise covariance. The Kalman gain is obtained through the target velocity observation noise covariance and the target attitude observation noise covariance, and Kalman filtering is performed to output the attitude and velocity of the observed target.

2. The method for improving navigation accuracy based on adaptive filtering according to claim 1, characterized in that: In step S1 , the navigation data sensor includes a gyroscope, an accelerometer, and a magnetometer.

3. The method for improving navigation accuracy based on adaptive filtering according to claim 1, wherein: Step S2 further comprises: S21: obtaining the observation noise covariance including the velocity observation noise covariance and the attitude observation noise covariance, obtaining the accelerometer parameters of the accelerometer, and determining the motion state of the observation target according to the accelerometer parameters; S22: Determine the minimum and maximum values ​​of the speed observation noise covariance, and judge whether the observed target is stationary based on the motion state. When the observed target is stationary, the speed observation noise covariance takes the minimum value, otherwise it takes the maximum value, thereby obtaining the target speed observation noise covariance.

4. The method for improving navigation accuracy based on adaptive filtering according to claim 1, wherein: Step S3 further comprises: S31: Calculate a first quaternion intermediate variable and a second quaternion intermediate variable using magnetometer parameters and gyroscope parameters, and obtain the observation quaternion through the first quaternion intermediate variable and the second quaternion intermediate variable; S32: Obtain the quaternion derivative according to the observation quaternion and the gyroscope parameter, construct a measurement matrix, and construct an observation equation using the measurement matrix and the quaternion derivative; S33: Establishing a system input equation group according to the quaternion derivative and the observation equation, and discretizing the system input equation group to obtain a discrete input equation group.

5. The method for improving navigation accuracy based on adaptive filtering according to claim 1, wherein: Step S4 further comprises: S41: Calculate the quaternion estimate using the discrete input equation group, calculate the error quaternion according to the observed quaternion and the quaternion estimate, determine the empirical coefficient, and obtain the estimated residual through the empirical coefficient and the error quaternion; S42: Determine a window size, and obtain the residual variance according to the estimated residual and the window size using the sliding window method.

6. The method for improving navigation accuracy based on adaptive filtering according to claim 5, characterized in that: In step S41, after obtaining the estimated residual, the mean of the estimated residual is calculated. If the mean of the estimated residual is not 0, the observation noise covariance is subjected to Cholesky decomposition to obtain a whitening matrix. The whitened residual is obtained through the whitening matrix, and the whitened residual is used as the estimated residual.

7. The method for improving navigation accuracy based on adaptive filtering according to claim 1, characterized in that: In step S5, a Kalman gain is obtained by the target velocity observation noise covariance and the target attitude observation noise covariance, and Kalman filtering is performed on the magnetometer parameters, the accelerometer parameters, and the gyroscope parameters according to the Kalman gain to obtain filtering parameters, and the attitude and speed of the observed target are obtained according to the filtering parameters.

8. A navigation accuracy improvement system based on adaptive filtering, configured to execute the navigation accuracy improvement method based on adaptive filtering according to any one of claims 1 to 7, characterized in that: include: Parameter acquisition module: used to arrange navigation data sensors on the observation target and obtain magnetometer parameters, accelerometer parameters and gyroscope parameters from the navigation data sensors; Target velocity observation noise covariance module: used to obtain the observation noise covariance including velocity observation noise covariance and attitude observation noise covariance. The accelerometer parameters are used to detect the movement of the observed target. When the observed target is stationary, the velocity observation noise covariance takes the minimum value, otherwise it takes the maximum value, thereby obtaining the target velocity observation noise covariance. Discrete Input Equation Group Module: This module is used to calculate the observation quaternion using magnetometer parameters and gyroscope parameters, obtain the quaternion derivative and construct the observation equation. The system input equation group is established based on the quaternion derivative and the observation equation, thereby obtaining the discrete input equation group. Residual variance module: used to calculate the quaternion estimate using the discrete input equation, calculate the error quaternion from the quaternion estimate, obtain the estimated residual from the error quaternion, and obtain the residual variance based on the estimated residual using the sliding window method; Attitude and velocity output module: used to multiply the residual variance with the attitude observation noise covariance to obtain the target attitude observation noise covariance, obtain the Kalman gain through the target velocity observation noise covariance and the target attitude observation noise covariance, perform Kalman filtering, and output the attitude and velocity of the observed target.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the steps of the method for improving navigation accuracy based on adaptive filtering as described in any one of claims 1 to 7 are implemented.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method for improving navigation accuracy based on adaptive filtering as described in any one of claims 1 to 7 are implemented.

Images