A composite non-Gaussian filtering method for inertial / polarization combined attitude determination

Abstract

The present invention relates to a composite non-Gaussian filtering method for inertial / polarization combined attitude determination. For an inertial / polarization combined attitude determination system that is simultaneously affected by inertial device drift, unknown incident light intensity coefficient, inertial navigation and polarization measurement noise, firstly, a tightly coupled model of an inertial / polarization combined attitude determination system containing an unknown incident light intensity coefficient is established; secondly, for a situation where the incident light intensity coefficient is known, a composite non-Gaussian filter containing inertial device drift compensation is designed to achieve real-time estimation of attitude posterior distribution; finally, an expectation-maximization iterative algorithm with an embedded composite non-Gaussian filter is designed to achieve online identification of the incident light intensity coefficient. The present invention can improve the attitude solution accuracy of an inertial / polarization combined attitude determination system under multi-source error conditions, improve the adaptability of an inertial / polarization combined attitude determination system to unknown environments and meteorological conditions, and can be applied to autonomous navigation tasks of various unmanned systems such as drones / vehicles / ships in complex environments.

Description

Technical Field

[0001] The present invention relates to a composite non-Gaussian filtering method for inertial / polarization combined attitude determination, which belongs to the field of state estimation algorithm design. The method can improve the attitude solution accuracy of an inertial / polarization combined attitude determination system under multi-source error conditions, improve the adaptability of the inertial / polarization combined attitude determination system to unknown environments and meteorological conditions, and can be applied to autonomous navigation tasks of various unmanned systems such as drones / vehicles / ships in complex environments. Background Art

[0002] For the inertial / polarization combined attitude determination system, particle filtering is currently the most widely used algorithm. It does not rely on the Gaussian assumption and can theoretically achieve arbitrarily high accuracy. However, most existing particle filtering algorithms rely on precisely known model information, making it difficult to handle the uncertainty of state transfer distribution caused by inertial device drift and the uncertainty of polarization measurement model caused by unknown optical path parameters. Therefore, it is necessary to improve the adaptability of the inertial / polarization combined attitude determination system to inertial device drift and unknown optical path parameters by improving the existing particle filtering algorithm.

[0003] The above-mentioned inertial / polarization combined attitude determination system filtering solution problem can be attributed to the design problem of improved particle filter algorithm under dynamic interference and unknown parameters. At present, there are few related research results, and the existing results mainly include robust particle filter and parameter adaptive particle filter. Among them, the robust particle filter can only process interference signals with dynamic characteristics, and cannot adaptively identify model parameters (such as incident light intensity coefficient); the parameter adaptive particle filter can update the unknown model parameters in real time, but the estimation performance is difficult to guarantee when facing dynamic interference signals (such as inertial device drift). Summary of the invention

[0004] The problem to be solved by the present invention is: considering the composite non-Gaussian interferences such as the drift of inertial devices, unknown incident light intensity coefficients and measurement noise in the inertial / polarization combined attitude determination system, overcoming the problem of insufficient accuracy and robustness of the existing particle filter algorithm under the composite non-Gaussian interference situation, and proposing a composite non-Gaussian filtering method for the inertial / polarization combined attitude determination. The method uses the expectation-maximization iterative algorithm to realize the maximum likelihood online identification of the incident light intensity coefficient, and constructs a composite non-Gaussian filter containing inertial device drift compensation, and simultaneously realizes the adaptive identification of the incident light intensity coefficient, the drift compensation of the inertial device and the real-time accurate tracking of the posterior distribution of the non-Gaussian state.

[0005] The technical solution of the present invention is: a composite non-Gaussian filtering method for inertial / polarization combined attitude determination, and its implementation steps are as follows:

[0006] Step 1: Establish a tightly coupled model of the inertial / polarization combined attitude determination system including the unknown incident light intensity coefficient.

[0007] First, the state equation is established as follows:

[0008] , (1)

[0009] in, and , , They are the attitude misalignment angles in the east, north and sky directions respectively; It is the attitude angular rate of the navigation system "n-system" relative to the inertial system "i-system" in the navigation system, which is given by the real-time measurement data of the inertial device; is the coordinate transformation matrix of the “b-system” of this system relative to the n-system; is a Gaussian noise term with zero mean and variance to be identified; The inertial device drift, its dynamic characteristics can be described as the following first-order Markov process:

[0010] , (2)

[0011] in, and is the time-dependent constant of the Markov process; is the Gaussian noise term.

[0012] Secondly, the polarization sensor measurement equation is established as follows:

[0013] , (3)

[0014] in, It is the real-time measurement value of the polarization sensor; is the polarization sensor measurement noise, which follows a Gaussian distribution; is the known polarization sensor light intensity gain coefficient; is the known optical path coupling coefficient of the polarization sensor; is the known degree of polarization; is the known polarization detection angle; is the unknown incident light intensity coefficient to be identified. and The definition is as follows:

[0015] , ,

[0016] in, , is the coordinate transformation matrix of the navigation calculation system “n'-system” relative to the b-system, is the identity matrix of appropriate dimension, is the solar vector in the n-frame, and The expression is: , ;also, represents the 2-norm of a vector; and Respectively represent vector The first and second dimension components of .

[0017] Furthermore, by combining equations (1), (2), and (3), we can obtain:

[0018] , (4)

[0019] in, , , , , , , .

[0020] Finally, the model described by equation (4) is discretized to obtain the tightly coupled model of the inertial / polarization combined attitude determination system as follows:

[0021] , (5)

[0022] Among them, the coefficient matrix , and Can be based on , , And the sampling time interval Calculated; , and is a Gaussian random noise term, satisfying: , ,and ,and and is known, and It is unknown to be identified.

[0023] Step 2: For the case where the incident light intensity coefficient is known, a composite non-Gaussian filter with inertial device drift compensation is designed.

[0024] In the moment, first according to The particle set calculated by the moment filter , and the predicted values ​​of the parameters corresponding to each particle , , , ,calculate:

[0025] , (6)

[0026] in, is the pre-specified number of particles; Indicates that the position parameter is The shape parameters are The degree of freedom parameter is Student's t distribution function; , , .

[0027] Secondly, for each particle ,definition , ,in and The filtering algorithm is Then, , the loop performs the following calculation steps:

[0028] , (7)

[0029] in, is the pre-specified maximum number of iterations; To correspond to After the loop ends, the parameter estimate is updated to , , , .

[0030] Next, calculate the The predicted parameter values ​​for each particle at the moment are as follows:

[0031] , (8)

[0032] in, represents the dimension of the state vector; is a pre-specified forgetting factor, satisfying .

[0033] Finally, , calculate the particle The corresponding weights are as follows:

[0034] , (9)

[0035] in, is the particle weight; For the The estimated value of the incident light intensity coefficient obtained at the moment. According to the calculated weights of each particle, the resampling operation is performed as follows:

[0036] , (10)

[0037] in, Indicates that the particle is located at Dirac function.

[0038] Step 3: Design an expectation-maximization iterative algorithm with an embedded composite non-Gaussian filter to achieve maximum likelihood online identification of the incident light intensity coefficient.

[0039] First of all, , calculate the coefficient according to the following formula:

[0040] , (11)

[0041] in, and For the The coefficient calculated at the moment, its initial value of iteration and Both can be taken as 0; is the preset step size parameter, satisfying and ; is the one-step forward smoothing weight, satisfying .

[0042] Next, the maximum likelihood estimate of the incident light intensity coefficient is calculated according to the following formula:

[0043] , (12)

[0044] Finally, output the particle set , estimated value of incident light intensity coefficient and parameter estimates , , and , as the first The input variable of the moment filter.

[0045] The advantages of the present invention compared with the prior art are:

[0046] (1) The present invention adopts the expectation-maximization iterative algorithm to realize the online identification of the unknown incident light intensity coefficient in the polarization measurement model, overcoming the deficiency of the existing inertial / polarization combined attitude determination filtering algorithm that it is difficult to adaptively update the model parameters;

[0047] (2) The present invention adopts a composite non-Gaussian filter with inertial device drift compensation to realize the estimation of the posterior distribution of attitude, which overcomes the problem of insufficient accuracy and robustness of the existing particle filter algorithm under dynamic interference (inertial device drift). BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 It is a design flow chart of the present invention. DETAILED DESCRIPTION

[0049] like Figure 1 As shown, the specific implementation steps of the present invention are as follows:

[0050] Step 1: Establish a tightly coupled model of the inertial / polarization combined attitude determination system including the unknown incident light intensity coefficient.

[0051] First, the state equation is established as follows:

[0052] , (1)

[0053] in, and , , They are the attitude misalignment angles in the east, north and sky directions respectively; It is the attitude angular rate of the navigation system (n-system) relative to the inertial system (i-system) in the navigation system, which is given by the real-time measurement data of the inertial device; is the coordinate transformation matrix of this system (b-system) relative to n-system; is a Gaussian noise term with zero mean and variance to be identified; The inertial device drift, its dynamic characteristics can be described as the following first-order Markov process:

[0054] , (2)

[0055] in, and is the time-dependent constant of the Markov process; is the Gaussian noise term.

[0056] Secondly, the polarization sensor measurement equation is established as follows:

[0057] (3)

[0058] in, It is the real-time measurement value of the polarization sensor; is the polarization sensor measurement noise, which follows a Gaussian distribution; is the known polarization sensor light intensity gain coefficient; is the known optical path coupling coefficient of the polarization sensor; is the known degree of polarization; is the known polarization detection angle; is the unknown incident light intensity coefficient to be identified. and The definition is as follows:

[0059] , ,

[0060] in, , is the coordinate transformation matrix of the navigation calculation system “n'-system” relative to the b-system, is the identity matrix of appropriate dimension, is the solar vector in the n-frame, and The expression is: , ;also, represents the 2-norm of a vector; and Respectively represent vector The first and second dimension components of .

[0061] Furthermore, by combining equations (1), (2), and (3), we can obtain:

[0062] , (4)

[0063] in, , , , , , , .

[0064] Finally, the model described by equation (4) is discretized to obtain the tightly coupled model of the inertial / polarization combined attitude determination system as follows: , (5)

[0065] Among them, the coefficient matrix , and Can be based on , , And the sampling time interval Calculated; , and is a Gaussian random noise term, satisfying: , ,and ,and and is known, and It is unknown to be identified.

[0066] Step 2: For the case where the incident light intensity coefficient is known, a composite non-Gaussian filter with inertial device drift compensation is designed.

[0067] In the moment, first according to The particle set calculated by the moment filter , and the predicted values ​​of the parameters corresponding to each particle , , , ,calculate:

[0068] , (6)

[0069] in, is the pre-specified number of particles; Indicates that the position parameter is The shape parameters are The degree of freedom parameter is Student's t distribution function; , , .

[0070] Secondly, for each particle ,definition , ,in and The filtering algorithm is Then, , the loop performs the following calculation steps:

[0071] , (7)

[0072] in, is the pre-specified maximum number of iterations; To correspond to After the loop ends, the parameter estimate is updated to , , , .

[0073] Next, calculate the The predicted parameter values ​​for each particle at the moment are as follows:

[0074] , (8)

[0075] in, represents the dimension of the state vector; is a pre-specified forgetting factor, satisfying .

[0076] Finally, , calculate the particle The corresponding weights are as follows:

[0077] , (9)

[0078] in, is the particle weight; For the The estimated value of the incident light intensity coefficient obtained at the moment. According to the calculated weights of each particle, the resampling operation is performed as follows:

[0079] , (10)

[0080] in, Indicates that the particle is located at Dirac function.

[0081] Step 3: Design an expectation-maximization iterative algorithm with an embedded composite non-Gaussian filter to achieve maximum likelihood online identification of the incident light intensity coefficient.

[0082] First of all, , calculate the coefficient according to the following formula:

[0083] , (11)

[0084] in, and For the The coefficient calculated at the moment, its initial value of iteration and Both can be taken as 0; is an artificially selected step size parameter that satisfies and ; is the one-step forward smoothing weight, satisfying .

[0085] Next, the maximum likelihood estimate of the incident light intensity coefficient is calculated according to the following formula:

[0086] , (12)

[0087] Finally, output the particle set , estimated value of incident light intensity coefficient and parameter estimates , , and , as the first The input variable of the moment filter.

Claims

1. A composite non-Gaussian filtering method for inertial / polarization combined attitude determination, characterized in that: The implementation steps are as follows: Step 1: Establish a tightly coupled model of the inertial / polarization combined attitude determination system including unknown incident light intensity coefficients; Step 2: Design a composite non-Gaussian filter with inertial device drift compensation for the case where the incident light intensity coefficient is known; Step 3: Design an expectation-maximization iterative algorithm with an embedded composite non-Gaussian filter to achieve maximum likelihood online identification of the incident light intensity coefficient; The second step is as follows: In the moment, first according to The particle set calculated by the moment filter , and the predicted values ​​of the parameters corresponding to each particle , , , ,calculate: ， （6） in, is the pre-specified number of particles; Indicates that the position parameter is The shape parameters are The degree of freedom parameter is Student's t distribution function; , , ; Secondly, for each particle ,definition , ,in and The filtering algorithm is The moment is calculated; then, , the loop performs the following calculation steps: ，（7） in, is the pre-specified maximum number of iterations; To correspond to The virtual quantity of each particle is measured; after the cycle ends, the parameter estimate is updated to , , , ; Next, calculate the The predicted parameter values ​​for each particle at the moment are as follows: ， （8） in, represents the dimension of the state vector; is a pre-specified forgetting factor, satisfying ; Finally, , calculate the particle The corresponding weights are as follows: ， （9） in, is the particle weight; For the The estimated value of the incident light intensity coefficient obtained at the moment; according to the calculated weights of each particle, the resampling operation is performed as follows: ， （10） in, Indicates that the particle is located at Dirac function.

2. The composite non-Gaussian filtering method according to claim 1, characterized in that: The first step is as follows: First, the state equation is established as follows: ， （1） in, and , , They are the attitude misalignment angles in the east, north and sky directions respectively; It is the attitude angular rate of the navigation system "n-system" relative to the inertial system "inertial-system" in the navigation system, which is given by the real-time measurement data of the inertial device; is the coordinate transformation matrix of the "b-system" relative to the n-system; is a Gaussian noise term with zero mean and variance to be identified; The drift of inertial devices is described as the following first-order Markov process: ， （2） in, and , , is the time-dependent constant of the Markov process; is the Gaussian noise term; Secondly, the polarization sensor measurement equation is established as follows: ，（3） in, It is the real-time measurement value of the polarization sensor; is the polarization sensor measurement noise, which follows a Gaussian distribution; is the known polarization sensor light intensity gain coefficient; is the known optical path coupling coefficient of the polarization sensor; is the known degree of polarization; is the known polarization detection angle; is the unknown incident light intensity coefficient to be identified; and The definition is as follows: ， ， in, ,and is the coordinate transformation matrix of the navigation calculation system "n'-system" relative to the b-system, is the identity matrix of appropriate dimension, is the solar vector in the n-frame, and The expression is: , ;also, represents the 2-norm of a vector; and Respectively represent vector The first and second dimension components of ; Furthermore, by combining equations (1), (2), and (3), we can obtain: ， （4） in, , , , , , , ; Finally, the model described by equation (4) is discretized to obtain the tightly coupled model of the inertial / polarization combined attitude determination system as follows: ， （5） Among them, the coefficient matrix , and Can be based on , , And the sampling time interval Calculated; , and is a Gaussian random noise term, satisfying: , and ,and and is known, and It is unknown to be identified.

3. The composite non-Gaussian filtering method according to claim 1, characterized in that: The third step is as follows: First of all, , calculate the coefficient according to the following formula: ， （11） in, and For the The coefficient calculated at the moment, its initial value of iteration and Both can be taken as 0; is an artificially selected step size parameter that satisfies and ; is the one-step forward smoothing weight, satisfying ; Next, the maximum likelihood estimate of the incident light intensity coefficient is calculated according to the following formula: ， （12） Finally, output the particle set , estimated value of incident light intensity coefficient and parameter estimates , , and , as the first The input variable of the moment filter.

Images