Azimuth misalignment angle estimation method based on adaptive filtering under vibration condition

Abstract

The invention belongs to the technical field of inertia, and provides an azimuth misalignment angle estimation method based on adaptive filtering under a vibration condition aiming at the problem that the azimuth accuracy cannot be kept for a long time under the vibration condition after a strapdown inertial navigation system completes alignment in the prior art. Acquiring a navigation speed output by the strapdown inertial navigation system; s2, extracting the navigation speed by using a recursive least square method with a forgetting factor to obtain a speed error; s3, constructing a state equation and a measurement equation of azimuth misalignment angle estimation; and S4, based on the state equation and the measurement equation, obtaining an estimated value of an estimated state through recursive calculation by an adaptive filtering algorithm, namely obtaining an estimated value of the azimuth misalignment angle. The method has the advantages of being good in environmental adaptability, high in anti-interference performance and high in precision, and has wide application prospects in the military application field.

Description

Technical Field

[0001] The present invention belongs to the field of inertial technology, and in particular relates to an azimuth misalignment angle estimation method based on adaptive filtering under vibration conditions. Background Art

[0002] After completing initial alignment, the vehicle-mounted strapdown inertial navigation system typically switches from alignment to navigation. This system provides high-precision azimuth and attitude information to the vehicle-mounted weapon system in real time, while also fully tracking and recording the vehicle's actual vibration and motion through real-time navigation calculations. As is well known, strapdown inertial navigation systems have an inherent flaw in that errors accumulate over time. After initial alignment, both horizontal attitude and azimuth errors exhibit a slow divergence trend, with horizontal attitude errors diverging more slowly and azimuth errors diverging more rapidly. However, both azimuth and horizontal attitude can remain within a relatively high accuracy range within a certain period of time. Therefore, as long as the indicators are properly designed, accuracy requirements can still be met.

[0003] However, when the strapdown inertial navigation system is affected by external vibration disturbances, such as those caused by the vehicle's engine, wind disturbances, and people getting on and off the vehicle, the divergence rate of the system's azimuth and horizontal attitude errors will accelerate significantly. In particular, the divergence rate of the azimuth error is much higher than that of the horizontal attitude error. Therefore, external vibration disturbances have a significant impact on the azimuth accuracy of the vehicle-mounted strapdown inertial navigation system. It is necessary to estimate and compensate the azimuth misalignment angle of the strapdown inertial navigation system online under vibration conditions to improve the maintenance of azimuth accuracy.

[0004] Existing methods for maintaining azimuth accuracy typically require the strapdown inertial navigation system to switch from alignment to navigation mode for a period of time before switching back to alignment mode, correcting divergent horizontal attitude errors and azimuth errors through alignment methods, and then switching back to navigation mode again, repeating this cycle. However, the alignment of vehicle-mounted strapdown inertial navigation systems places strict demands on the external environment in which they are located. Typically, these strict requirements require that the vehicle be stationary during alignment, that people are prohibited from getting on or off the vehicle, or opening or closing doors, and that the vehicle engine be stopped. Clearly, these stringent requirements bring significant inconvenience to the application of vehicle-mounted weapon systems. Furthermore, existing methods struggle to maintain the azimuth accuracy of the strapdown inertial navigation system over a long period of time in the presence of vibration interference from the vehicle engine, external wind disturbances, or people getting on or off the vehicle. Therefore, there is an urgent need for a method that can online estimate and compensate for the azimuth misalignment angle of a strapdown inertial navigation system under vibration conditions. Summary of the Invention

[0005] The purpose of the present invention is to overcome the deficiencies of the prior art and provide a method for estimating an azimuth misalignment angle based on adaptive filtering under vibration conditions.

[0006] In order to solve the problems existing in the prior art, the present invention first adopts the least squares method with a forgetting factor to extract the velocity error of the strapdown inertial navigation system, and uses this as the measurement for azimuth misalignment angle estimation; on this basis, the state equation and measurement equation for azimuth misalignment angle estimation are constructed; finally, adaptive filtering is used to perform real-time online estimation of the statistical characteristics of noise to overcome the problem that the statistical characteristics of vibration interference noise are difficult to accurately know.

[0007] To achieve the above object, the technical solution adopted by the present invention is:

[0008] A method for estimating an azimuth misalignment angle based on adaptive filtering under vibration conditions comprises the following steps:

[0009] S1. In a stationary vehicle environment, obtain the navigation speed output by the strapdown inertial navigation system. The expression of the navigation speed is as follows:

[0010]

[0011] in, is the navigation speed, is the oscillation speed, is the speed error;

[0012] S2. Using the recursive least square method with a forgetting factor, extract the navigation speed obtained in step S1 to obtain a speed error;

[0013] S3. Select the strapdown inertial navigation system error as the system state, and respectively combine the error model of the strapdown inertial navigation system and the velocity error obtained in step S2 to construct the state equation and measurement equation for azimuth misalignment angle estimation. The expressions of the state equation and measurement equation are as follows:

[0014]

[0015] Z=HX+V

[0016] Where F is the system state matrix; G is the system noise driving matrix; W is the system noise sequence, H is the measurement matrix; V is the measurement noise sequence;

[0017] S4. Based on the state equation and measurement equation obtained in step S3, an estimated value of the estimated state is obtained through recursive calculation using an adaptive filtering algorithm, that is, an estimated value of the azimuth misalignment angle is obtained.

[0018] Preferably, in step S2, the expression of the recursive least square method with forgetting factor is:

[0019]

[0020] in, For the tthk The parameter estimates at time t, For the tth k+1 The estimated parameter value at time t, L k is the gain matrix, v i (k) is the tth k Observe data at all times, For the tth k Time data vector, a k is the process matrix, P k for The variance matrix, P k+1 for The variance matrix of , μ is the forgetting factor.

[0021] Preferably, step S3 comprises the following steps:

[0022] S31. Select the strapdown inertial navigation system error as the system state, and obtain the system state vector of the azimuth misalignment angle estimation according to the error model of the strapdown inertial navigation system. The expression of the system state vector is:

[0023]

[0024] Where X is the system state vector, φ E 、φ N 、φ U is the attitude error of the strapdown inertial navigation mathematical platform, δv E ,δv N ,δv U is the speed error, δL, δλ, δh are the position errors, ε bx , ε by , ε bz is the gyro random constant drift, is the random constant error of the accelerometer;

[0025] S32. According to the error model of the strapdown inertial navigation system and in combination with the system state vector obtained in step S31, a state equation for azimuth misalignment angle estimation is obtained. The expression of the state equation is as follows:

[0026]

[0027] Among them, F is the system state matrix; G is the system noise driving matrix, and W is the system noise sequence;

[0028] S33. The speed error obtained in step S2 is used as the measurement. The expression of the measurement is as follows:

[0029]

[0030] Where Z is the measurement, δv E ,δvN ,δv U is the speed error;

[0031] S34. Combining the system state vector and the measurement, a measurement equation for estimating the azimuth misalignment angle is obtained. The expression of the measurement equation is as follows:

[0032] Z=HX+V

[0033] Where H is the measurement matrix and V is the measurement noise sequence.

[0034] Preferably, step S4 comprises the following steps:

[0035] S41, based on the state equation obtained in step S3, let t k The estimated state X at time k Affected by the system noise sequence W k-1 Drive, the expression of the discrete state equation is as follows:

[0036] X k =Φ k,k-1 X k-1 +W k-1

[0037] Among them, X k t k The estimated state at time , X k-1 t k-1 The estimated state at time W k-1 is the system noise sequence, Φ k,k-1 t k-1 to t k The moment one-step transfer matrix;

[0038] S42, according to the measurement equation obtained in step S3, k The estimated state X at time k Observe and obtain the measurement Z k With status X k The following discrete measurement equation is satisfied:

[0039] Z k =H k X k +V k

[0040] Where Z k t k The measurement vector at the moment, H k is the measurement matrix, X k t k The estimated state vector at time t, V k is a measurement noise sequence with time-varying mean and time-varying covariance matrix;

[0041] S43. Based on the discrete state equation obtained in step S41 and the discrete measurement equation obtained in step S42, an estimated value of the estimated state, that is, an estimated value of the azimuth misalignment angle, is obtained through recursive calculation using an adaptive filtering algorithm.

[0042] Preferably, in step S4, the calculation formula of the estimated value of the estimated state is:

[0043]

[0044]

[0045] P k =(IK k H k )P k / k-1

[0046]

[0047] in, t k-1 to t k One-step prediction value of the estimated state at time Φ k,k-1 t k-1 to t k The one-step transfer matrix at time , t k-1 The estimated value of the estimated state at time t, t k The variance intensity matrix of the system noise at time t, t k-1 The variance intensity matrix of the system noise at time t, t k The variance intensity matrix of the moment-by-moment measurement noise, t k-1 The variance intensity matrix of the momentary measurement noise, d k is the attenuation coefficient, and d k =(1-b) / (1-b k+1 ), 0<b<1, b is the adjustment factor, Z k t k Time measurement, H k is the measurement matrix, e k t k Time filter residual, P k / k-1 is the variance matrix of the one-step prediction value, t k The estimated value of the estimated state at the moment, P k for The variance matrix, P k-1 for The variance matrix of t k The variance matrix of the system noise at time , t k-1 The variance matrix of the system noise at time , t k The variance matrix of the moment measurement noise, t k-1 The variance matrix of the moment measurement noise, K k is the filter gain, and I is the unit matrix.

[0048] Compared with the prior art, the present invention has the following beneficial effects:

[0049] The present invention uses a least squares method with a forgetting factor to extract the velocity error of a strapdown inertial navigation system, using this as a measurement for azimuth misalignment angle estimation. Based on this, a state equation and a measurement equation for azimuth misalignment angle estimation are constructed. Finally, adaptive filtering is used to perform real-time online estimation of the statistical characteristics of noise to overcome the problem of difficulty in accurately obtaining the statistical characteristics of vibration interference noise, thereby ensuring the accuracy and stability of azimuth misalignment angle estimation. This method of the present invention has the outstanding advantages of good environmental adaptability, strong anti-interference ability, and high precision, and has broad application prospects in the field of military applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 is the output of the gyroscope in the strapdown inertial navigation system under vibration conditions;

[0051] Figure 2 is the output result of the accelerometer in the strapdown inertial navigation system under vibration conditions;

[0052] Figure 3 The azimuth output results of the strapdown inertial navigation system based on different filtering under vibration conditions. DETAILED DESCRIPTION

[0053] The following is a diagram of the embodiment of the present invention. Figures 1 to 3 The technical solutions in the embodiments of the present invention are clearly and completely described. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of them. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0054] 1. Velocity error extraction based on least squares method

[0055] When the vehicle is stationary, the true velocity of the strapdown inertial navigation system (SINS) is not zero due to factors such as engine vibration, external wind disturbances, and people getting on and off the vehicle. Using the SINS output velocity as the velocity error will not only slow down the filter convergence, but also result in a certain degree of dispersion in the converged value. Therefore, a more accurate velocity error is required to measure the azimuth misalignment angle.

[0056] Although external vibration interference is present in both accelerometer measurements and navigation velocity, extracting the velocity error directly from the navigation velocity is clearly more accurate and avoids processing the velocity error introduced by the accelerometer measurement. Furthermore, the filtering technique used to extract the velocity error directly from the navigation velocity is applicable to vehicle-mounted vibration interference environments with varying frequency characteristics and also has a certain degree of suppression effect on transient interference.

[0057] In a stationary vehicle environment, the navigation speed output by the strapdown inertial navigation system consists of two parts: speed error and true speed. The true speed oscillates around zero speed, so the navigation speed output by the strapdown inertial navigation system is actually the speed error plus the oscillation speed. The navigation speed expression is as follows:

[0058]

[0059] in, is the navigation speed, is the oscillation speed, is the speed error, and the speed error expression is:

[0060]

[0061] Where t represents time, t k Indicates t k Time, t∈[t k ,t k+1 ), is the speed error at time t, D(t k ) represents t k Momentary attitude error, g p is the acceleration due to gravity, is the disturbance acceleration, is the accelerometer output, e a is the velocity error noise, For a small amount, The relative acceleration due to gravity is also small, so the velocity error can be viewed as a piecewise linear function of time.

[0062] Therefore, in order to filter out the vibration interference from the output velocity of the strapdown inertial navigation system, the least square method with forgetting factor can be used to extract the velocity error. Assume:

[0063]

[0064] Among them, v i (t) is the observation data at time t, that is, the velocity output of the strapdown inertial navigation system; a1 is the slope of the linear function, a0 is the intercept; θ is the parameter to be estimated, is the time correlation matrix, θ=[a1,a0] T , Then the recursive least squares algorithm with forgetting factor is:

[0065]

[0066] in, For the tth k The parameter estimates at time t, For the tth k+1 The estimated parameter value at time t, L k is the gain matrix, v i (k) is the tth k Observe data at all times, For the tth k Time data vector, a k is the process matrix, P k for The variance matrix, P k+1 for The variance matrix of , μ is the forgetting factor, which usually takes a value between 0 and 1.

[0067] By using the above method, after filtering out the influence of vibration interference in the navigation speed output by the vehicle-mounted strapdown inertial navigation system, the speed error of the strapdown inertial navigation system can be obtained, and this can be used as a measurement for real-time estimation of the azimuth misalignment angle.

[0068] 2. State equation and measurement equation for azimuth misalignment angle estimation

[0069] Based on the error model of the strapdown inertial navigation system, the main error term of the strapdown inertial navigation is selected as the system state, and the state equation for azimuth misalignment angle estimation can be established; using the previously obtained strapdown inertial navigation system velocity error as a measurement, combined with the system state of the azimuth misalignment angle estimation, the measurement equation for azimuth misalignment angle estimation can be derived.

[0070] First, the strapdown inertial navigation system error is selected as the system state. According to the error model of the strapdown inertial navigation system, the system state mainly includes: strapdown inertial navigation mathematical platform attitude error φ E 、φ N 、φ U , velocity error δv E ,δv N ,δv U, position error δL, δλ, δh, gyro random constant drift ε bx , ε by , ε bz , accelerometer random constant error The gyro white noise w gx 、w gy 、w gz and accelerometer white noise w ax 、w ay 、w az It is regarded as system noise and is not included in the system state.

[0071] Therefore, the expression of the system state vector X for azimuth misalignment angle estimation is:

[0072]

[0073] Where X is the system state vector, φ E 、φ N 、φ U is the attitude error of the strapdown inertial navigation mathematical platform, δv E ,δv N ,δv U is the speed error, δL, δλ, δh are the position errors, ε bx , ε by , ε bz is the gyro random constant drift, is the random constant error of the accelerometer;

[0074] According to the error model of the strapdown inertial navigation system and combined with the expression of the system state vector X, the state equation for azimuth misalignment angle estimation can be written in the form of:

[0075]

[0076] Where F is the system state matrix; G is the system noise driving matrix; W is the system noise sequence. Here, W = [w gx ,w gy ,w gz ,w ax ,w ay ,w az ] T , which satisfies E[W(t)]=0 and E[W(t)W T (τ)] = qδ(t-τ), where q is the variance intensity matrix of W.

[0077] Due to navigation errors, the navigation velocity output by the strapdown inertial navigation system consists of two components: the true velocity and the velocity error. As previously analyzed, under vehicle-mounted vibration interference, since the vehicle is stationary and motionless, the true velocity oscillates around zero. Therefore, the navigation velocity output by the strapdown inertial navigation system is actually the velocity error superimposed on the oscillation velocity. Using the velocity error extraction method based on least squares proposed above, the velocity error of the strapdown inertial navigation system can be directly obtained and used as a measurement for real-time estimation of the azimuth misalignment angle.

[0078] Therefore, the extracted strapdown inertial navigation system velocity error is used as the measurement Z, that is, the expression of the measurement Z is as follows:

[0079]

[0080] Where Z is the measurement, δv E ,δv N ,δv U is the speed error;

[0081] At this point, combining the system state vector and the measurement expression, the measurement equation for azimuth misalignment angle estimation can be derived, and its expression is:

[0082] Z=HX+V

[0083] Where H is the measurement matrix and V is the measurement noise sequence. The measurement matrix H is:

[0084]

[0085] The state equation in the present invention is continuous, while the measurement equation is discrete. In order to use the adaptive filtering algorithm to be introduced below, it is usually necessary to discretize the continuous state equation.

[0086] 3. Adaptive Filtering Algorithm for Azimuth Misalignment Angle Estimation

[0087] During the initial alignment of a vehicle-mounted strapdown inertial navigation system (SINS), external noise disturbances such as engine operating vibration, external wind disturbances, and people getting on and off the vehicle can cause external noise disturbances. Furthermore, the statistical characteristics of this disturbance noise are difficult to accurately determine. If a Kalman filter based on the accurate statistical characteristics of the noise model is used to estimate the azimuth misalignment angle, the vibration disturbances will inevitably affect the filter performance, resulting in a decrease in accuracy and, in turn, a reduction in the accuracy of the azimuth misalignment angle estimation. Therefore, to address this issue, an adaptive filtering algorithm is needed. By performing real-time online estimation of the noise statistical characteristics, this algorithm overcomes the difficulty in accurately determining the statistical characteristics of the vibration disturbance noise, thereby ensuring the accuracy and stability of the azimuth misalignment angle estimation.

[0088] In the adaptive filtering algorithm, it is assumed that the system noise and the measurement noise are uncorrelated. In order to reduce the impact of the uncertainty of the noise statistical characteristics, an adjustment factor is introduced. While using the measurement information for filtering, the statistical characteristics of the system noise and the measurement noise determined by the prior information can be continuously estimated online in real time through the time-varying noise statistical characteristics estimation algorithm, thereby autonomously adapting to the actual changes in the statistical characteristics of the system noise and the measurement noise, thereby effectively improving the filtering accuracy, especially suppressing the filter divergence. The adaptive filtering algorithm used is described as follows:

[0089] Assume t k The estimated state X at time k Affected by the system noise sequence W k-1 Drive, then according to the state equation obtained above, it satisfies the following discrete state equation:

[0090] X k =Φ k,k-1 X k-1 +W k-1

[0091] Among them, X k t k The estimated state at time , X k-1 t k-1 The estimated state at time W k-1 is the system noise sequence, Φ k,k-1 t k-1 to t k The moment one-step transfer matrix;

[0092] According to the measurement equation obtained above, the estimated state X k Observe and obtain the measurement Z k With status X k There is a linear relationship between them, which satisfies the following discrete measurement equation:

[0093] Z k =H k X k +V k

[0094] Where Z k t k The measurement vector at the moment, H k is the measurement matrix, X k t k The estimated state vector at time t, V k is a measurement noise sequence with time-varying mean and time-varying covariance matrix, W k and V k are independent noise sequences with time-varying means and time-varying covariance matrices that satisfy the following relationship:

[0095]

[0096] Among them, q k and Q k are the mean and variance matrices of the system noise, r k and R k are the mean and variance matrices of the measurement noise, respectively, and δ(k,j) is the Kronecker δ function.

[0097] Based on the above discrete state equation and discrete measurement equation, the estimated state X k Estimated value of It can be obtained through recursive calculation using the adaptive filtering algorithm. The calculation formula is as follows:

[0098]

[0099]

[0100] P k =(IK k H k )P k / k-1

[0101]

[0102] Where, t k-1 to t k One-step prediction value of the estimated state at time φ k,k-1 t k-1 to t k The one-step transfer matrix at time , t k-1 The estimated value of the estimated state at time t, t k The variance intensity matrix of the system noise at time t, t k-1 The variance intensity matrix of the system noise at time t, t k The variance intensity matrix of the moment-by-moment measurement noise, t k-1 The variance intensity matrix of the moment measurement noise, Z k t k Time measurement, H k is the measurement matrix, e k t k Time filter residual, P k / k-1 is the variance matrix of the one-step prediction value, t k The estimated value of the estimated state at the moment, Pk for The variance matrix, P k-1 for The variance matrix of t k The variance matrix of the system noise at time , t k-1 The variance matrix of the system noise at time t, t k The variance matrix of the moment measurement noise, t k-1 The variance matrix of the moment measurement noise, K k is the filter gain, I is the unit matrix; d k is the attenuation coefficient, and d k =(1-b) / (1-b k+1 ), 0<b<1, b is a tuning factor that adjusts the filter's memory length, increasing the effect of newly observed data (i.e., measurements) on the current estimate. Therefore, the choice of b must balance tracking performance for time-varying parameters with noise insensitivity.

[0103] At this time, according to the above calculation formula, it is only necessary to set the initial value of the state and its estimated mean square error P0, combined with t k Measurement of time Z k , it is possible to complete high-precision filtering calculations even when the statistical characteristics of system noise and measurement noise are unknown or undergo significant changes, thereby recursively calculating t k State estimation at time

[0104] IV. Verification and Conclusion

[0105] Relying on the actual strapdown inertial navigation system, the azimuth misalignment angle estimation method based on adaptive filtering under vibration conditions was experimentally verified to test the initial alignment accuracy and accuracy retention of the vehicle-mounted strapdown inertial navigation system under external vibration environment conditions.

[0106] The gyroscope constant drift in the vehicle-mounted strapdown inertial navigation system is better than 0.01° / h, and its white noise random walk is better than Accelerometer constant error is better than 10 -4 g, whose white noise random walk is better than The gyroscope and accelerometer output frequency is 200Hz. During initial alignment, the vehicle was located at 34°14.763′N, 108°54.579′E, and 380m above sea level. The average ground wind speed was ≤15m / s, and the instantaneous maximum wind speed was ≤22.5m / s. The initial alignment time was 5 minutes, with an azimuth alignment error of less than 3′ and a horizontal alignment error of less than 30″. During initial alignment accuracy maintenance, the vehicle engine was kept at idle speed. In addition to normal external wind disturbances, test personnel also created vibration interference by opening and closing vehicle doors, getting on and off the vehicle, and walking around the vehicle. During the test, optical aiming was used to measure the actual heading angle. Initial alignment accuracy maintenance tests were conducted using the above method, with a hold time of 4000s. The repeatability of the results was assessed six times. The test data is shown in Table 1 below.

[0107] Table 1 Initial alignment accuracy and accuracy retention test data under vibration conditions (unit: °)

[0108]

[0109] According to the above-mentioned test result, it can be seen that the azimuth misalignment angle estimation method based on adaptive filtering proposed by the present invention can play a good maintenance effect to the initial alignment accuracy of the vehicle-mounted strapdown inertial navigation system under vibration interference conditions: under the composite interference conditions such as external wind disturbance, opening and closing car doors, getting on and off the car and walking on the car, through the test of up to 4000s, the azimuth accuracy remains on 1.2042 ', and the absolute value of the azimuth misalignment angle error is a maximum of 0.01197 ° (i.e. 0.7182 '). This illustrates that the azimuth misalignment angle estimation method based on adaptive filtering proposed by the present invention effectively overcomes the problem that the azimuth error diverges faster over time after the initial alignment of the strapdown inertial navigation system, and within the time of 4000s, orientation can remain within a higher precision range.

[0110] The strapdown inertial navigation system is then installed on a vibration table for testing. The test is conducted for 2000 seconds before the vibration table is started, at least 1000 seconds during the vibration, and after the vibration. The entire test time exceeds 4000 seconds. After a set of initial alignment and accuracy maintenance tests are completed, the direction of the strapdown inertial navigation system is adjusted before the next set of tests is carried out. The output results of the gyroscope and accelerometer on the x, y, and z axes of the strapdown inertial navigation system during vibration are as follows: Figure 1 and Figure 2 As shown; Under vibration conditions, the Kalman filter and the azimuth misalignment angle estimation method based on adaptive filtering proposed in this invention are used to estimate and compensate the azimuth misalignment angle of the strapdown inertial navigation system respectively. The azimuth angle output result after compensation is as follows: Figure 3 As shown (wherein, the red curve is the result after estimation and compensation based on Kalman filtering, and the blue curve is the result after estimation and compensation based on the azimuth misalignment angle estimation method based on adaptive filtering proposed in the present invention).

[0111] according to Figure 3 It can be seen that the azimuth angle output result of the azimuth misalignment angle estimation method based on adaptive filtering proposed in the present invention is obviously more stable than the output result based on Kalman filtering. After a vibration test lasting 4000s, the azimuth angle result output by the method proposed in the present invention is stable within the range of ±0.15°, while the azimuth angle result of Kalman filtering varies within the range of ±0.5°. This shows that the azimuth misalignment angle estimation method based on adaptive filtering proposed in the present invention has higher accuracy and better convergence effect, and its effect on maintaining the azimuth accuracy of the strapdown inertial navigation system is better.

[0112] Therefore, in order to achieve a long-term maintenance of the azimuth accuracy of a vehicle-mounted strapdown inertial navigation system under vibration conditions, the present invention proposes a method for estimating the azimuth misalignment angle based on adaptive filtering. The method first uses a least squares method with a forgetting factor to extract the velocity error of the strapdown inertial navigation system, which is used as a measurement for azimuth misalignment angle estimation. On this basis, a state equation and a measurement equation for azimuth misalignment angle estimation are constructed. Finally, adaptive filtering is used to perform real-time online estimation of the statistical characteristics of noise to overcome the problem that the statistical characteristics of vibration interference noise are difficult to accurately obtain, thereby ensuring the accuracy and stability of azimuth misalignment angle estimation. Thus, a method for estimating the azimuth misalignment angle based on adaptive filtering under vibration conditions is studied. The method does not rely on any external information or external equipment, has outstanding advantages such as good environmental adaptability, easy engineering implementation, strong anti-interference ability, and high precision, and has broad application prospects in the field of military applications.

[0113] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to the embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the claims and their equivalents.

Claims

1. A method for estimating azimuth misalignment angle based on adaptive filtering under vibration conditions, characterized in that: The following steps are involved: S1. In a stationary vehicle environment, obtain the navigation speed output by the strapdown inertial navigation system. The expression of the navigation speed is as follows: in, is the navigation speed, is the oscillation speed, is the speed error; S2. Using the recursive least square method with a forgetting factor, extract the navigation speed obtained in step S1 to obtain a speed error; S3. Select the strapdown inertial navigation system error as the system state, and respectively combine the error model of the strapdown inertial navigation system and the velocity error obtained in step S2 to construct the state equation and measurement equation for azimuth misalignment angle estimation. The expressions of the state equation and measurement equation are as follows: Z=HX+V Where F is the system state matrix; G is the system noise driving matrix; W is the system noise sequence, H is the measurement matrix; V is the measurement noise sequence; S4. Based on the state equation and measurement equation obtained in step S3, an estimated value of the estimated state is obtained through recursive calculation using an adaptive filtering algorithm, that is, an estimated value of the azimuth misalignment angle is obtained.

2. The method for estimating azimuth misalignment angle based on adaptive filtering under vibration conditions according to claim 1, characterized in that: In step S2, the expression of the recursive least square method with forgetting factor is: in, For the tth k The parameter estimates at time t, For the tth k+1 The estimated parameter value at time t, L k is the gain matrix, v i (k) is the tth k Observe data at all times, For the tth k Time data vector, a k is the process matrix, P k for The variance matrix, P k+1 for The variance matrix of , μ is the forgetting factor.

3. The method for estimating azimuth misalignment angle based on adaptive filtering under vibration conditions according to claim 1, characterized in that: Step S3 includes the following steps: S31. Select the strapdown inertial navigation system error as the system state, and obtain the system state vector of the azimuth misalignment angle estimation according to the error model of the strapdown inertial navigation system. The expression of the system state vector is: X=[φ E ,f N ,f U ,δv E ,δv N ,δv U ,δL,δλ,δh,ε bx ,he by ,he bz ,▽ bx ,▽ by ,▽ bz ] T Where X is the system state vector, φ E 、φ N 、φ U is the attitude error of the strapdown inertial navigation mathematical platform, δv E ,δv N ,δv U is the speed error, δL, δλ, δh are the position errors, ε bx , ε by , ε bz is the gyro random constant drift, ▽ bx 、▽ by 、▽ bz is the random constant error of the accelerometer; S32. According to the error model of the strapdown inertial navigation system and in combination with the system state vector obtained in step S31, a state equation for azimuth misalignment angle estimation is obtained. The expression of the state equation is as follows: Among them, F is the system state matrix; G is the system noise driving matrix, and W is the system noise sequence; S33. The speed error obtained in step S2 is used as the measurement. The expression of the measurement is as follows: Where Z is the measurement, δv E ,δv N ,δv U is the speed error; S34. Combining the system state vector and the measurement, a measurement equation for estimating the azimuth misalignment angle is obtained. The expression of the measurement equation is as follows: Z=HX+V Where H is the measurement matrix and V is the measurement noise sequence.

4. The method for estimating azimuth misalignment angle based on adaptive filtering under vibration conditions according to claim 1, characterized in that: Step S4 includes the following steps: S41, based on the state equation obtained in step S3, let t k The estimated state X at time k Affected by the system noise sequence W k-1 Drive, the expression of the discrete state equation is as follows: X k =Φ k,k-1 X k-1 +W k-1 Among them, X k t k The estimated state at time , X k-1 t k-1 The estimated state at time t, W k-1 is the system noise sequence, Φ k,k-1 t k-1 to t k Moment one-step transfer matrix; S42, according to the measurement equation obtained in step S3, k The estimated state X at time k Observe and obtain the measurement Z k With status X k The following discrete measurement equation is satisfied: Z k =H k X k +V k Where Z k t k The measurement vector at the moment, H k is the measurement matrix, X k t k The estimated state vector at time t, V k is a measurement noise sequence with time-varying mean and time-varying covariance matrix; S43. Based on the discrete state equation obtained in step S41 and the discrete measurement equation obtained in step S42, an estimated value of the estimated state, that is, an estimated value of the azimuth misalignment angle, is obtained through recursive calculation using an adaptive filtering algorithm.

5. The method for estimating azimuth misalignment angle based on adaptive filtering under vibration conditions according to claim 1, characterized in that: In step 4, the calculation formula of the estimated value of the estimated state is: P k =(I-K k H k )P k / k-1 in, t k-1 to t k One-step prediction value of the estimated state at time Φ k,k-1 t k-1 to t k The one-step transfer matrix at time , t k-1 The estimated value of the estimated state at time t, t k The variance intensity matrix of the system noise at time t, t k-1 The variance intensity matrix of the system noise at time t, t k The variance intensity matrix of the moment-by-moment measurement noise, t k-1 The variance intensity matrix of the momentary measurement noise, d k is the attenuation coefficient, and d k =(1-b) / (1-b k+1 ), 0<b<1, b is the adjustment factor, Z k t k Time measurement, H k is the measurement matrix, e k t k Time filter residual, P k / k-1 is the variance matrix of the one-step prediction value, t k The estimated value of the estimated state at the moment, P k for The variance matrix, P k-1 for The variance matrix of t k The variance matrix of the system noise at time , t k-1 The variance matrix of the system noise at time , t k The variance matrix of the moment measurement noise, t k-1 The variance matrix of the moment measurement noise, K k is the filter gain, and I is the unit matrix.

Images