Periodic oscillation error suppression method for long-endurance high-precision inertial navigation system

Abstract

The invention relates to the technical field of inertial navigation, in particular to a periodic oscillation error suppression method for a long-endurance high-precision inertial navigation system. In order to effectively suppress the periodic oscillation error of inertial navigation and improve the positioning precision, the invention provides a periodic oscillation error suppression method for a long-endurance high-precision inertial navigation system, which comprises the following steps: acquiring an attitude angle error and a speed error, carrying out phase delay modulation processing on the attitude angle error, the speed error, a course angle error and a latitude error, and carrying out phase delay modulation processing on the course angle error and the latitude error. The method comprises the following steps of: synthesizing and averaging, improving the navigation precision and stability by adopting a phase delay modulation inertial navigation error suppression method of an improved robust adaptive Kalman filter, obtaining a more accurate speed by suppressing a Schller oscillation error, and then applying the speed to integrated navigation so as to reduce error divergence of an inertial navigation system, and finally, improving the navigation precision and stability by adopting a phase delay modulation inertial navigation error suppression method of the robust adaptive Kalman filter. The phase delay modulation inertial navigation error suppression method of the improved robust adaptive Kalman filter is adopted to improve the navigation precision and stability.

Description

Technical Field

[0001] The present invention relates to the field of inertial navigation technology, and in particular to a method for suppressing periodic oscillation errors of a long-flight high-precision inertial navigation system. Background Art

[0002] The inertial navigation system has the advantages of strong autonomy, high navigation positioning accuracy, and fast output frequency. It can provide high-precision navigation information such as attitude, speed, and position, and is widely used in aviation, aerospace, navigation, and especially underwater navigation. Compared with other navigation methods, such as satellite navigation and acoustic navigation, the biggest advantage of inertial navigation is its autonomy. It can perform autonomous high-precision navigation without relying on any external auxiliary information. However, its biggest disadvantage is that the error will accumulate over time, causing the final positioning error to diverge. A method for suppressing periodic oscillation errors for long-duration high-precision inertial navigation systems.

[0003] Inertial navigation errors are mainly divided into periodic oscillation errors and divergent oscillation errors. Since divergent oscillation errors are difficult to suppress, the focus is generally on how to suppress periodic errors. Periodic errors are mainly divided into Schuler periodic errors, Foucault periodic errors and Earth periodic errors. In long-duration navigation, periodic errors have become the key errors that affect navigation and positioning accuracy, so it is necessary to effectively suppress periodic errors.

[0004] At present, there are two main methods to suppress periodic errors: combined navigation and damping. When there is external auxiliary information, the error divergence can be effectively suppressed by combined navigation, but it is too dependent on external auxiliary information, which seriously undermines the autonomy of the inertial navigation system. Damping technology can effectively suppress the Schuler periodic oscillation of inertial navigation, but it will also destroy the solution loop of the inertial navigation system, losing the autonomy advantage of inertial navigation, and is only applicable when the carrier is stationary or in uniform motion. Summary of the invention

[0005] In order to effectively suppress the periodic oscillation error of the inertial navigation system without destroying the autonomy of the inertial navigation system and without affecting the solution loop of the inertial navigation system, and to improve the navigation and positioning accuracy of the inertial navigation system, the present invention proposes a periodic oscillation error suppression method for a long-flight high-precision inertial navigation system, the suppression method includes a phase delay suppression method, and the phase delay suppression method includes the following steps:

[0006] Obtain horizontal attitude angle error and horizontal velocity error;

[0007] Perform phase delay modulation processing on the horizontal attitude angle error and horizontal velocity error, synthesize and average them to suppress the Schuler oscillation error;

[0008] Get heading angle error and latitude error;

[0009] The heading angle error and latitude error are subjected to phase delay modulation processing, synthesized and averaged to suppress the earth vibration error.

[0010] Furthermore, the suppression method also includes using an improved robust adaptive Kalman filter phase delay modulation inertial navigation error suppression method to improve navigation accuracy and stability.

[0011] Furthermore, in the steps, the attitude angle error and the speed error are subjected to phase delay modulation processing, and the synthesis and averaging processing method is as follows:

[0012]

[0013] Considering ω s >>ω ie , and satisfies

[0014] In the steps, the heading angle error and the latitude error are T ie The phase delay modulation processing of / 2, synthesis and averaging processing method are as follows:

[0015]

[0016] Among them, considering ω s >>ω ie , and satisfies is the north velocity error, is the north speed. is the roll angle error, R is the radius of the earth, ω ie is the Earth's rotation angular velocity. L is the latitude, is the heading angle error, ε E It is the eastward gyro drift. is the pitch angle error, ε N is the northward gyro drift, and δL is the latitude error. is the eastward velocity error, is the eastward velocity. U is the celestial gyro drift, and g is the gravitational acceleration. E is the accelerometer zero bias in the east direction, ▽ N is the north accelerometer zero bias. λ is the longitude, and δλ is the longitude error.

[0017] Furthermore, the improved robust adaptive Kalman filter phase delay modulation inertial navigation error suppression method comprises the following steps: The state equation of the Kalman filter is as follows: k =FX k-1 +Gη S

[0018] The system status is represents the attitude angle error, Represents the three-dimensional velocity error, δp=[δL,δλ,δh] T represents the latitude, longitude and altitude errors, ε b =[ε E ,ε N ,ε U ] T Indicates the drift of the three-axis gyroscope, ▽ b =[▽ E ,▽ N ,▽ U ] T Indicates the zero bias of the three-axis accelerometer. η S is the system noise, G is η S The control matrix of . F is the state transfer matrix.

[0019] The measurement equation is as follows:

[0020]

[0021] is the three-axis velocity calculated by inertial navigation, is the three-axis velocity obtained after phase delay, H is the measurement transfer matrix, H = [0 3×3 I 3×3 0 3×3 0 3×3 0 3×3 ]. M To measure noise.

[0022] The steps of robust adaptive Kalman filtering are as follows.

[0023]

[0024]

[0025] b=0.9+me -u

[0026]

[0027] y k =g(x k )+ξ k

[0028]

[0029] α(e i,k )=β(e i,k ) / e i,k

[0030] χ=diag[α(e i,k )]

[0031]

[0032] Among them, Q k-1 is η S The covariance matrix of P. k,k-1 for The corresponding error covariance matrix. is the one-step prediction value. k is η M The corresponding covariance matrix. i,k v k The i-th parameter of . γ=1.345. d is the measurement dimension, d=3. m determines the initial fading factor value, and n controls its changing speed. During filtering, n determines whether the fading factor needs to be adjusted. k,k-1 is the state transfer matrix from time k-1 to time k. is the one-step prediction value. k-1 is the error covariance matrix corresponding to the state at time k-1. k,k-1 P k-1 One-step prediction. is the covariance matrix of the measurement with respect to the state. k is the measurement new information. u is the adaptive factor corresponding to formula (43). m is the adaptive adjustment factor, which is generally 1. b is d k The corresponding adjustment parameters of k For each moment R k The adaptive adjustment factor. is the estimated measurement noise matrix. is the measurement prediction covariance matrix. k is the covariance matrix corresponding to the state and measurement errors. k According to D k The constructed true measurement-estimate state regression equation. g(x k ) is the true state-prediction measurement regression equation. δX k,k-1 is the state information. k is the state and measurement information matrix. i,k ) is the modified regression equation. β(e i,k ) is ρ(e i,k ). α(e i,k ) is the element of the corrected residual vector. χ is the corrected residual vector. is the corrected regression equation. is the corrected measurement information. kis the Kalman filter gain. is the state estimation vector at time k. k for The corresponding covariance matrix.

[0033] Beneficial effects of the present invention:

[0034] The horizontal attitude angle error and horizontal velocity error were calculated by the above method. s / 2 phase delay modulation, and after synthetic averaging, only the earth frequency modulation exists in the time domain representation of the horizontal attitude angle error and the horizontal velocity error, and the Schuler oscillation error can be fully suppressed. By suppressing the Schuler oscillation error, a more accurate velocity can be obtained. Then the velocity is used for combined navigation, thereby reducing the error divergence of the inertial navigation system. Finally, the phase delay modulation inertial navigation error suppression method of the improved robust adaptive Kalman filter is used to improve the navigation accuracy and stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 The error oscillation curve of the inertial navigation system in the technical background of the periodic oscillation error suppression method for a long-flight high-precision inertial navigation system provided by the present invention;

[0036] Figure 2 A schematic diagram of the pitch angle error of a phase delay suppression method of a periodic oscillation error suppression method for a long-flight high-precision inertial navigation system provided by the present invention;

[0037] Figure 3 A schematic diagram of a rolling angle error of a phase delay suppression method for a periodic oscillation error suppression method for a long-duration high-precision inertial navigation system provided by the present invention;

[0038] Figure 4 A schematic diagram of a heading angle error of a periodic oscillation error suppression method and a phase delay suppression method for a long-flight high-precision inertial navigation system provided by the present invention;

[0039] Figure 5 A schematic diagram of the eastward velocity error of a phase delay suppression method for a periodic oscillation error suppression method for a long-duration high-precision inertial navigation system provided by the present invention;

[0040] Figure 6 A schematic diagram of the north velocity error of a phase delay suppression method for a periodic oscillation error suppression method for a long-duration high-precision inertial navigation system provided by the present invention;

[0041] Figure 7 A schematic diagram of the latitude error of a periodic oscillation error suppression method and a phase delay suppression method for a long-duration high-precision inertial navigation system provided by the present invention;

[0042] Figure 8 A schematic diagram of velocity error of a phase delay modulation inertial navigation error suppression method using an improved robust adaptive Kalman filter in a periodic oscillation error suppression method for a long-duration high-precision inertial navigation system provided by the present invention;

[0043] Fig. 9 A schematic diagram of position error of a phase delay modulation inertial navigation error suppression method using an improved robust adaptive Kalman filter in a periodic oscillation error suppression method for a long-duration high-precision inertial navigation system provided by the present invention. DETAILED DESCRIPTION

[0044] In order to make the objects and advantages of the present invention more clearly understood, the present invention is further described below in conjunction with embodiments; it should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0045] The preferred embodiments of the present invention are described below with reference to the accompanying drawings. It should be understood by those skilled in the art that these embodiments are only used to explain the technical principles of the present invention and are not intended to limit the protection scope of the present invention.

[0046] It should be noted that, in the description of the present invention, terms such as "up", "down", "left", "right", "inside" and "outside" indicating directions or positional relationships are based on the directions or positional relationships shown in the drawings. This is merely for the convenience of description and does not indicate or imply that the device or element must have a specific orientation, be constructed and operated in a specific orientation. Therefore, it cannot be understood as a limitation on the present invention.

[0047] In addition, it should be noted that in the description of the present invention, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For those skilled in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0048] The present invention is further described in detail below in conjunction with the accompanying drawings.

[0049] The present invention provides a method for suppressing periodic oscillation errors for long-duration and high-precision inertial navigation systems. First, a phase delay modulation method is used to accurately obtain the speed at which the Schuler oscillation error is suppressed as the "external speed". Subsequently, this speed is used as an observation value for integrated navigation, which effectively alleviates the error divergence in the inertial navigation system. In addition, in order to solve the overshoot problem during the navigation state transition process, an improved robust adaptive Kalman filter is introduced to improve the accuracy and stability. Finally, two field experiments are conducted to verify the effectiveness of the method proposed in this paper.

[0050] Problem Analysis

[0051] The error is closely related to the specific navigation trajectory of the vehicle, and it is difficult to directly obtain its analytical solution. Under the condition of a static base, the error propagation of the strapdown inertial navigation is relatively simple. In order to illustrate the propagation law of the periodic oscillation error of the strapdown inertial navigation, this paper analyzes the propagation characteristics of the inertial navigation error under the condition of a static base. The system error equation under the static base is

[0052]

[0053]

[0054] in, is the north velocity error, is the north speed. is the roll angle error, R is the radius of the earth, ω ie is the Earth's rotation angular velocity. L is the latitude, is the heading angle error, ε E It is the eastward gyro drift. is the pitch angle error, ε N is the northward gyro drift, and δL is the latitude error. is the eastward velocity error, is the eastward velocity. U is the celestial gyro drift, and g is the gravitational acceleration. E is the accelerometer zero bias in the east direction, ▽ N is the north accelerometer zero bias. λ is the longitude, and δλ is the longitude error.

[0055] Write the above formula in matrix form

[0056]

[0057] Right now

[0058]

[0059] Perform a pull transformation on the above formula:

[0060] sX(s)=AX(s)+X(0)+W(s)

[0061] X(s)=(sI-A) -1 [X(0)+W(s)] (10)

[0062] The characteristic polynomial of the system is

[0063]

[0064] In the formula, ω s is the Schuler angular frequency. The eigenvalue of the system is determined by the following equation

[0065]

[0066] From the above formula, we can get the characteristic root as

[0067] s 1,2 = ±jω ie (14)

[0068] Let the left side of (16) be L(s), then

[0069]

[0070] From the above formula, we can get the characteristic root as

[0071]

[0072] Because ω s >>ω ie sin L, then we have

[0073] s 5,4 = ±j(ω s +ω ie sinL) (18)

[0074] s 3,6 = ±j(ω s -ω ie sinL) (19)

[0075] Therefore, the characteristic root s1 can be obtained ,2 The corresponding oscillation basis term is

[0076] sinω ie t,cosω ie t (20)

[0077] Characteristic root s 5,4 The corresponding oscillation basis term is

[0078] sin[(ω s -ω ie sinL)t],cos[(ω s -ωie sinL)t] (21)

[0079] Characteristic root s 3,6 The corresponding oscillation basis term is

[0080] sin[(ω s -ω ie sinL)t],cos[(ω s -ω ie sinL)t] (22)

[0081] The oscillation errors of the three frequencies of the inertial navigation system can be obtained

[0082] (1) Schuler oscillation ω s , the period is 84.4min;

[0083] (2) Earthquakeω ie , the cycle is 24h;

[0084] (3) Foucault oscillation ω f , the frequency is ω ie sI

[0085] Since Foucault oscillations are modulated by Schuler oscillations, we focus on the suppression of Schuler oscillations and earth oscillations. Taking the heading angle error as an example, the error oscillation curve of the inertial navigation system is as follows: Figure 1 As shown (mainly Schuler oscillation error and earth oscillation error). It can be seen from the figure that the periodic oscillation error of the inertial navigation system shows a form similar to the trigonometric function oscillation. In this regard, the present invention adopts a delayed phase modulation method to suppress the periodic error of the inertial navigation system.

[0086] Method proposed

[0087] (1) Phase delay

[0088] The method based on delayed phase modulation includes two parts: phase delay and integrated navigation Kalman filter. First, the phase delay modulation is introduced.

[0089] First, the time domain expression of the inertial navigation error changing with time is given:

[0090]

[0091]

[0092] It can be seen from the above error propagation equation that the horizontal attitude angle error and the horizontal velocity error are mainly modulated by the Schuler oscillation frequency, and the heading angle error and the latitude error are mainly affected by the earth oscillation frequency. In this regard, the present invention adopts a phase delay modulation method to suppress the two periodic errors of Schuler oscillation error and earth oscillation error, which can improve the navigation and positioning accuracy of the inertial navigation system.

[0093] Since the horizontal attitude angle error and horizontal velocity error are mainly affected by the Schuler oscillation frequency ω s Therefore, equations (23), (24), (26) and (27) are modified by T s / 2(Satisfy ) phase delay modulation processing, and considering ω s >>ω ie , and synthesize and average them, the horizontal attitude angle error and horizontal velocity error after synthesis are obtained as follows:

[0094]

[0095] The horizontal attitude angle error and horizontal velocity error were calculated by the above method. s / 2 phase delay modulation and synthetic averaging processing, there is only earth frequency modulation in the time domain representation of the horizontal attitude angle error and the horizontal velocity error, and the Schuler oscillation error can be fully suppressed.

[0096] The heading angle error and latitude error are mainly affected by the earth oscillation angular frequency ω ie Therefore, we can perform T on (25), (29), ie / 2(Satisfy ) phase delay modulation processing, and considering ω s >>ω ie , and synthesize and average, the heading angle error and latitude error after synthesis are:

[0097]

[0098] (2) Robust Adaptive Kalman Filter

[0099] The heading angle error and latitude error were calculated by the above method. ie / 2 phase delay modulation and synthetic averaging, the earth oscillation errors of heading angle error and latitude error can be fully suppressed. However, its applicability to the marine environment may be limited due to two reasons. Reason 1: Compared with the land environment, the marine environment has a higher swing amplitude and frequency, which affects the performance of the phase modulation loop. In a swaying ocean, it becomes challenging to achieve a normal phase delay, resulting in a larger error in the final output average. This may hinder the effective suppression of INS oscillation errors. Reason 2: In the marine environment, the angular motion of the gyroscope is more susceptible to sway, which has a greater impact on the attitude error. On the other hand, the velocity error is mainly affected by linear acceleration. The traditional phase modulation damping method is less effective in suppressing horizontal attitude errors in the marine environment.

[0100] Based on the above analysis, the phase modulation damping method has limited application in suppressing Schuler oscillation errors in marine environments. It has little effect on attitude errors, and its effectiveness decreases during navigation. Based on this, we use phase delay modulation to suppress errors in marine environments, that is, to obtain more accurate speed by suppressing Schuler oscillation errors. This speed is then used for combined navigation, thereby reducing the error divergence of the inertial navigation system. Finally, the phase delay modulation inertial navigation error suppression method using an improved robust adaptive Kalman filter is used to improve navigation accuracy and stability.

[0101] The state equation of the Kalman filter is as follows.

[0102] X k =FX k-1 +Gη S (36)

[0103] The system status is represents the attitude angle error, Represents the three-dimensional velocity error, δp=[δL,δλ,δh] T represents the latitude, longitude and altitude errors, ε b =[ε E ,ε N ,ε U ] T Indicates the drift of the three-axis gyroscope, ▽ b =[▽ E ,▽ N, ▽ U ] T Indicates the zero bias of the three-axis accelerometer. η S is the system noise, G is η S The control matrix of . F is the state transfer matrix.

[0104] The measurement equation is as follows:

[0105]

[0106] is the three-axis velocity calculated by inertial navigation, is the three-axis velocity obtained after phase delay, H is the measurement transfer matrix, H = [0 3×3 I 3×3 0 3×3 0 3×3 0 3×3 ]. M To measure noise.

[0107] The steps of robust adaptive Kalman filtering are as follows.

[0108]

[0109] b=0.9+me -|u| (43)

[0110]

[0111]

[0112] y k =g(x k )+ξ k (51)

[0113]

[0114] α(e i,k )=β(e i,k ) / e i,k (54)

[0115] χ=diag[α(e i,k )] (55)

[0116]

[0117] Among them, Q k-1 is η S The covariance matrix of P. k,k-1 for The corresponding error covariance matrix. is the one-step prediction value. k is η M The corresponding covariance matrix. i,k v k The i-th parameter of . γ=1.345. d is the measurement dimension, d=3. m determines the initial fading factor value, and n controls its changing speed. During filtering, n determines whether the fading factor needs to be adjusted. k,k-1is the state transfer matrix from time k-1 to time k. is the one-step prediction value. k-1 is the error covariance matrix corresponding to the state at time k-1. k,k-1 P k-1 One-step prediction. is the covariance matrix of the measurement with respect to the state. k is the measurement new information. u is the adaptive factor corresponding to formula (43). m is the adaptive adjustment factor, which is generally 1. b is d k The corresponding adjustment parameters of k For each moment R k The adaptive adjustment factor. is the estimated measurement noise matrix. is the measurement prediction covariance matrix. k is the covariance matrix corresponding to the state and measurement errors. k According to D k The constructed true measurement-estimate state regression equation. g(x k ) is the true state-prediction measurement regression equation. δX k,k-1 is the state information. k is the state and measurement information matrix. i,k ) is the modified regression equation. β(e i,k ) is ρ(e i,k ). α(e i,k ) is the element of the corrected residual vector. χ is the corrected residual vector. is the corrected regression equation. is the corrected measurement information. k is the Kalman filter gain. is the state estimation vector at time k. k for The corresponding covariance matrix.

[0118] First, the effectiveness of the phase delay proposed by the present invention is verified by simulation experiments. The constant zero bias of the gyroscope and accelerometer is set to 0.003° / h and 20μg, and the corresponding random walks are and The total simulation time is 60 hours. The method proposed in this invention is used to suppress the inertial navigation error. The error comparison is as follows: Figure 2-7 As shown in the comparison diagram, it can be seen that the Schuler oscillation errors of the horizontal attitude angle and the horizontal velocity can be effectively suppressed, and the earth oscillation errors of the heading angle and the latitude can be effectively suppressed. Therefore, the phase delay proposed in the present invention can effectively suppress the periodic oscillation error of the inertial navigation.

[0119] Furthermore, the proposed method is validated using offshore test data, e.g. Figure 8 , Fig. 9 As shown, Scheme 1 is the inertial navigation calculation under undamped conditions. Scheme 2 is the error suppression based on phase delay modulation damping of inertial navigation. Scheme 3 is the phase delay modulation inertial navigation error suppression method based on Kalman filter. Scheme 4 is the phase delay modulation inertial navigation error suppression method based on improved robust adaptive Kalman filter. It can be seen from the experimental results that the proposed method can effectively suppress the error divergence during ocean navigation, and realize the improvement of the autonomous navigation accuracy of a single inertial navigation system, which further illustrates the effectiveness of the method proposed in the present invention.

[0120] The present invention is not limited to the above-mentioned specific implementation modes. Various changes made by ordinary technicians in this field based on the above-mentioned concepts without creative work are all within the protection scope of the present invention.

Claims

1. A periodic oscillation error suppression method for a long-duration high-precision inertial navigation system, the suppression method comprising a phase delay suppression method, the phase delay suppression method comprising the following steps: Obtain horizontal attitude angle error and horizontal velocity error; Perform phase delay modulation processing on the horizontal attitude angle error and horizontal velocity error, synthesize and average them to suppress the Schuler oscillation error; Get heading angle error and latitude error; The heading angle error and latitude error are subjected to phase delay modulation processing, synthesized and averaged to suppress the earth vibration error.

2. The method for suppressing periodic oscillation errors of a long-duration high-precision inertial navigation system according to claim 1, characterized in that: The suppression method also includes using an improved robust adaptive Kalman filter phase delay modulation inertial navigation error suppression method to improve navigation accuracy and stability.

3. The method for suppressing periodic oscillation errors of a long-duration high-precision inertial navigation system according to claim 1, characterized in that: In the steps, the horizontal attitude angle error and the horizontal velocity error are T s The phase delay modulation process of / 2 is to perform T on the horizontal attitude angle error and horizontal velocity error. s The phase delay modulation processing of / 2, synthesis and averaging processing method are as follows: Considering ω s >>ω ie , and satisfies In the step, the heading angle error and the latitude error are subjected to phase delay modulation processing to perform T on the heading angle error and the latitude error. ie The phase delay modulation processing of / 2, synthesis and averaging processing method are as follows: Among them, considering ω s >>ω ie , and meet is the north velocity error, is the north speed. is the roll angle error, R is the radius of the earth, ω ie is the Earth's rotation angular velocity. L is the latitude, is the heading angle error, ε E It is the eastward gyro drift. is the pitch angle error, ε N is the northward gyro drift, and δL is the latitude error. is the eastward velocity error, is the eastward velocity. U is the celestial gyro drift, and g is the gravitational acceleration. E is the accelerometer zero bias in the east direction, ▽ N is the north accelerometer zero bias. λ is the longitude, and δλ is the longitude error.

4. The method for suppressing periodic oscillation errors of a long-duration high-precision inertial navigation system according to claim 2, characterized in that: The improved robust adaptive Kalman filter phase delay modulation inertial navigation error suppression method comprises the following steps: The state equation of Kalman filter is as follows: k =FX k-1 +Gη S The system status is represents the attitude angle error, Represents the three-dimensional velocity error, δp=[δL,δλ,δh] T represents the latitude, longitude and altitude errors, ε b =[ε E ,ε N ,ε U ] T Indicates the drift of the three-axis gyroscope, Indicates the zero bias of the three-axis accelerometer. η S is the system noise, G is η S The control matrix of . F is the state transfer matrix. The measurement equation is as follows: is the three-axis velocity calculated by inertial navigation, is the three-axis velocity obtained after phase delay, H is the measurement transfer matrix, H = [0 3×3 I 3×3 0 3×3 0 3×3 0 3×3 ]. M To measure noise. The steps of robust adaptive Kalman filtering are as follows. b=0.9+me -|u| y k =g(x k )+ξ k α(e i,k )=β(e i,k ) / And i,k x=diag[α(e i,k )] Among them, Q k-1 is η S The covariance matrix of P. k,k-1 for The corresponding error covariance matrix. is the one-step prediction value. k is η M The corresponding covariance matrix. i,k v k The i-th parameter of . γ=1.

345. d is the measurement dimension, d=3. The initial fading factor value is determined and n controls the speed of its change. During filtering, n determines whether the fading factor needs to be adjusted. k,k-1 is the state transfer matrix from time k-1 to time k. is the one-step prediction value. k-1 is the error covariance matrix corresponding to the state at time k-1. k,k-1 P k-1 One-step prediction. is the covariance matrix of the measurement with respect to the state. k is the measurement new information. u is the adaptive factor corresponding to formula (43). m is the adaptive adjustment factor, which is generally 1. b is d k The corresponding adjustment parameters of k For each moment R k The adaptive adjustment factor. is the estimated measurement noise matrix. is the measurement prediction covariance matrix. k is the covariance matrix corresponding to the state and measurement errors. k According to D k The constructed true measurement-estimate state regression equation. g(x k ) is the true state-prediction measurement regression equation. δX k,k-1 is the state information. k is the state and measurement information matrix. i,k ) is the modified regression equation. β(e i,k ) is ρ(e i,k ). α(e i,k ) is the element of the corrected residual vector. χ is the corrected residual vector. is the corrected regression equation. is the corrected measurement information. k is the Kalman filter gain. is the state estimation vector at time k. k for The corresponding covariance matrix.

Images