Disclosure of Invention
The technical problem to be solved by the invention is as follows: the method overcomes the defects of the prior art, provides a SINS/GPS combined navigation self-adaptive dimension reduction filtering method with high precision and good real-time performance, and provides three-dimensional position information of an antenna phase center for an SAR real-time imaging system on line.
The technical solution of the invention is as follows: an adaptive dimensionality reduction filtering method of an SINS/GPS integrated navigation system for SAR motion compensation is characterized by comprising the following steps: (1) Establishing a full-dimensional (15-dimensional) filtering model base in an off-line manner, wherein the full-dimensional (15-dimensional) filtering model base comprises a state equation, an observation equation, a system noise variance array, a measurement noise variance array and a state estimation variance array of models under three different flight maneuver conditions; the three filtering models respectively correspond to three conditions of uniform linear motion, uniform accelerated linear motion and uniform circular motion of the SAR loader. (2) The system starts to work, judges that the maneuverability of the carrier changes, utilizes a state transition matrix, an observation matrix and an observability degree analyzing and calculating method based on a singular value decomposition principle on line in real time to calculate the observability degree of the 15-dimensional state variables, and rearranges the state variables from large to small according to the observability degree. (3) Selecting front d-dimensional state variables including three-dimensional positions and speeds to form a dimensionality reduction filtering model with the minimum dimensionality, wherein d is equal to the maximum arrangement serial number corresponding to the three-dimensional positions and the speed state variables; (4) Determining elements corresponding to newly added state variables in a state estimation variance matrix of the newly built dimension reduction filtering model by adopting a random generation method; (5) And starting and maintaining the dimension reduction filtering model until the mobility of the SAR aerial carrier changes again.
The principle of the invention is as follows: the filtering precision of each state variable in the SINS/GPS combined navigation system filtering model changes along with the change of the aircraft mobility of the SAR aircraft, and the observability degree quantitatively represents the filtering precision of each state variable under the current aircraft mobility condition. The SINS/GPS combined navigation self-adaptive dimension reduction filtering method for SAR motion compensation calculates the observability degree of each state variable in the current maneuvering state when the maneuvering characteristics of the SAR carrier change, and the state variables are sequenced from large to small according to the observability degree. And selecting the front d-dimensional state variables including the three-dimensional position and the speed from large to small to construct a dimensional minimum filtering model, wherein n is equal to the maximum arrangement serial number corresponding to the three-dimensional position and speed state variables. Determining a state transition matrix, a system noise variance matrix and a state estimation variance matrix corresponding to the dimensionality reduction filtering model, and finishing combined navigation filtering; and determining elements corresponding to the new supplementary state variables in the state estimation variance matrix by adopting a random generation method. And when the flight mobility of the SAR aerial carrier does not change, the current SINS/GPS combined navigation filtering model is kept. According to the invention, the calculated amount is greatly reduced by constructing the self-adaptive dimensionality reduction filtering model in real time and deleting the state variable with poor filtering precision on line; and meanwhile, high filtering precision state variables required by SAR imaging are saved. Although certain calculation cost is needed for calculating the observability of the state variables in real time on line and constructing the dimension reduction filtering model, considering that SAR imaging requires that the carrier keeps constant-speed linear motion under most conditions, as shown in FIG. 1, the time for the change of the mobility of the SAR carrier is not much, so that the method achieves the purpose of obviously reducing the overall calculation amount of the adaptive dimension reduction filtering model of the SINS/GPS combined navigation system for SAR motion compensation by using smaller calculation cost, namely calculating the observability of the state variables and constructing the dimension reduction filtering model, ensures the filtering precision same as that of the full-dimension filtering model, and meets the requirements of the SAR real-time imaging system.
Compared with the prior art, the invention has the advantages that: hair brushThe self-adaptive dimensionality reduction filtering method is adopted, so that the SINS/GPS combined navigation system for SAR motion compensation can ensure the filtering precision, obviously reduce the calculated amount and lay the technical foundation for the SAR real-time imaging system. This is due to: (1) Compared with the traditional SINS/GPS integrated navigation system filtering method, the filtering method disclosed by the invention has the advantages that the state variables are adaptively selected or rejected on line according to the observable degree of the system state variables to form a dimension reduction filtering model, and the state variables with poor filtering precision are abandoned, so that the precision of the filtering method is not influenced completely; (2)The calculation amount is reduced remarkably; because the filtering model dimension has great influence on the system calculation amount, n and m are assumed to be the dimensions of the state variable and the observation variable respectively, and the multiplication and addition times of one-step filtering are n 3 +m×n 2 . The self-adaptive dimension reduction filtering model provided by the invention mainly aims to reduce the dimension of the SINS/GPS combined navigation filtering model for SAR motion compensation, thereby achieving the purpose of reducing the calculated amount; (3) An SINS/GPS combined navigation filtering model library under different flight mobility conditions of an SAR carrier is established, the observable degree of state variables is calculated on line in real time, and a dimension reduction filtering model is constructed only by selecting from the model library, so that the system flow is greatly simplified; (4) And determining new supplementary elements in the state estimation variance matrix by adopting a random generation method when the SINS/GPS combined navigation filtering model is reconstructed on line every time. The random generation method has small calculation amount and high correlation degree with the variance of the last filtering value, and is favorable for quick convergence of the reconstructed dimension reduction filtering model.
Detailed Description
As shown in fig. 3, the specific steps of the present invention are as follows:
(1) Firstly, constructing a full-dimensional SINS/GPS integrated navigation system filtering model library, comprising the following steps: three models of uniform linear motion, uniform accelerated linear motion and uniform circular motion.
a. The uniform circular motion full-dimensional SINS/GPS integrated navigation system filter model:
15 dimensional state variable
X(t)=[φ x φ y φ z δv x δv y δv z δL δλ δh ε x ε y ε z ▽ x ▽ y ▽ z ] T The state equation of the SINS/GPS integrated navigation system is as follows:
the measurement equation is as follows:
Z(t)=H(t)X(t)+V(t)
wherein F (t) is the state transition matrix of the system:
F N is determined by the coefficients of the following set of differential equations:
l and delta L are latitude and latitude estimation errors respectively; λ, δ λ are longitude and longitude estimation errors, respectively; h, δ h are height and height estimation errors, respectively; v x ,δv x ,v y ,δv y ,v z ,δv z Estimating errors for the three weft speeds and the speed respectively; phi is a x ,φ y ,φ z Respectively estimating errors of the three-dimensional postures; epsilon x ,ε y ,ε z Respectively, three-axis gyro errors; v x ,▽ y ,▽ z Respectively, errors of a triaxial accelerometer; f. of x ,f y ,f z Respectively measuring the three-axis specific force; x , y , z The pitch angle, the roll angle and the yaw angle of the SAR aerial carrier are respectively; r m ,R n ,ω ie The radius of the prime plane is the radius of the prime plane, and the rotation angular velocity of the earth.
G (t) is a system noise transfer matrix:
wherein:
w (t) is the random error of the gyroscope and accelerometer, W (t) = [ W =
εx w
εy w
εz w
▽x w
▽y w
▽z ]
T . Observation matrix
H
v =[0
3×3 diag(1 1 1) 0
3×12 ]
H p =[0 3×6 diag(R m R n cosL 1) 0 3×9 ]
Observation vector Z (t) = [ δ v = x δv y δv z δL δλ δh] T 。
The system noise variance matrix Q is represented by W (t) = [ W εx w εy w εz w ▽x w ▽y w ▽z ] T The variances of the individual elements constitute a diagonal matrix of diagonal elements. The measurement noise variance matrix R is formed by Z (t) = [ delta v = x δv y δv z δL δλ δh] T The variance of each element constitutes a diagonal matrix of diagonal elements. The initial value of the state estimation variance matrix is determined as a 15-dimensional unit matrix, and the matrix is updated in real time according to the conventional Kalman filtering step in each subsequent combined filtering process.
All the parameters used in the formula are navigation parameters at the current moment, and are regarded as constant values in each filtering step.
b. Compared with uniform circular motion, the SINS/GPS integrated navigation system filtering model of uniform acceleration and uniform linear motion only forms a state transition matrix F N Are different.
The filtering model of the uniform acceleration linear motion full-dimensional SINS/GPS integrated navigation system is at a constant speedOrder v on the basis of circular motion filtering model
z =0, and thus omitted
In
2ω
ie sinLv
z A term of delta L and a term of delta L,
in (1)
Item (1):
15 dimensional state variable
X(t)=[φ x φ y φ z δv x δv y δv z δL δλ δh ε x ε y ε z ▽ x ▽y ▽ z ] T
The state equation of the SINS/GPS integrated navigation system is as follows:
the measurement equation is:
Z(t)=H(t)X(t)+V(t)
wherein F (t) is the state transition matrix of the system:
F N is determined by the coefficients of the following set of differential equations:
l and delta L are latitude and latitude estimation errors respectively; λ, δ λ are longitude and longitude estimation errors, respectively; h, δ h are height and height estimation errors, respectively; v. of x ,δv x ,v y ,δv y ,δv z Estimating errors for the three weft speeds and the speed respectively; phi is a x ,φ y ,φ z Respectively estimating errors of the three-dimensional postures; epsilon x ,ε y ,ε z Respectively, three-axis gyro errors; v x ,▽ y ,▽ z Respectively, errors of a triaxial accelerometer; f. of x ,f y ,f z Are respectively provided withIs a triaxial specific force measurement; x , y , z The pitch angle, the roll angle and the yaw angle of the SAR aerial carrier are respectively; r M ,R n ,ω ie The radius of the prime plane is the radius of the prime plane, and the rotation angular velocity of the earth.
G (t) is a system noise transfer matrix:
wherein:
w (t) is the random error of the gyroscope and accelerometer,
W(t)=[w
εx w
εy w
εz w
▽x w
▽y w
▽z ]
T . Observation matrix
H
v =[0
3×3 diag(1 1 1) 0
3×12 ], H
p =[0
3×6 diag(R
m R
n cosL 1) 0
3×9 ]
Observation vector Z (t) = [ δ v = x δv y δv z δL δλ δh] T 。
The system noise variance matrix Q is represented by W (t) = [ W εx w εy w εz w ▽x w ▽y w ▽z ] T The variances of the individual elements constitute a diagonal matrix of diagonal elements. The measurement noise variance matrix R is formed by Z (t) = [ delta v = x δv y δv z δL δλ δh] T The variance of each element constitutes a diagonal matrix of diagonal elements. The initial value of the state estimation variance matrix is determined as a 15-dimensional unit matrix, and the matrix is updated in real time according to the conventional Kalman filtering step in each subsequent combined filtering process. All the parameters used in the formula are navigation parameters of the current moment, and filtering is carried out at each stepThe wave is considered to be a constant value.
c. The filtering model of the uniform linear motion full-dimensional SINS/GPS integrated navigation system is based on the uniform circular motion filtering model
z =0,f
x =f
y =f
z =0, and thus omitted
In (1)
2ω
ie sin Lv
z δL、 f
y φ
z -f
z φ
y ,
F in (1)
z φ
x -f
x φ
z 、
F in (1)
x φ
y -f
y φ
x -and the like: 15 dimensional state variable
X(t)=[φ x φ y φ z δv x δv y δv z δL δλ δh ε x ε y ε z ▽ x ▽ y ▽ z ] T The state equation of the SINS/GPS integrated navigation system is as follows:
the measurement equation is:
Z(t)=H(t)X(t)+V(t)
wherein F (t) is the state transition matrix of the system:
F N is determined by the coefficients of the following set of differential equations:
l and delta L are latitude and latitude estimation errors respectively; λ, δ λ are longitude and longitude estimation errors, respectively; h, δ h are height and height estimation errors, respectively; v. of x ,δv x ,v y ,δv y ,δ z ,δv z Estimating errors for the three weft speeds and the speed respectively; phi is a x ,φ y ,φ z Respectively estimating errors of the three-dimensional postures; epsilon x ,ε y ,ε z Respectively, three-axis gyro errors; v x ,▽ y ,▽ z Respectively, the errors of the three-axis accelerometer; f. of x ,f y ,f z Respectively measuring the three-axis specific force; x , y , z The pitch angle, the roll angle and the yaw angle of the SAR aerial carrier are respectively; r m ,R n ,ω ie The radius of the prime plane is the radius of the prime plane, and the rotation angular velocity of the earth.
G (t) is a system noise transfer matrix:
wherein:
w (t) is the random error of the gyroscope and accelerometer, W (t) = [ W = εx w εy w εz w ▽x w ▽y w ▽z ] T . Observation matrixH v =[0 3×3 diag(1 1 1) 0 3×12 ],
H p =[0 3×6 diag(R m R n cosL 1) 0 3×9 ]
Observation vector Z (t) = [ δ v = x δv y δv z δL δλ δ h ] T 。
The system noise variance matrix Q is represented by W (t) = [ W εx w εy w εz w ▽x w ▽y w ▽z ] T The variance of each element constitutes a pairA diagonal matrix of diagonal elements. The measurement noise variance matrix R is formed by Z (t) = [ delta v = x δv y δv z δL δλ δh] T The variance of each element constitutes a diagonal matrix of diagonal elements. The initial value of the state estimation variance matrix is determined as a 15-dimensional unit matrix, and the matrix is updated in real time according to the conventional Kalman filtering step in each subsequent combined filtering process. All the parameters used in the formula are navigation parameters at the current moment, and are regarded as constant values in each filtering step.
(2) Secondly, the system starts to work, judges the mobility change of the carrier, identifies the current motion of the carrier as one of three conditions of uniform linear motion, uniform accelerated linear motion or uniform circular motion, calculates the observability degree on line in real time, and rearranges the state variables according to the observability degree from large to small.
When the output of the three-dimensional accelerometer is smaller than a specified threshold, the three-dimensional accelerometer is judged to be uniform-speed linear motion, when the vector sum of the three-dimensional accelerometer keeps a constant value, the three-dimensional accelerometer is judged to be uniform-speed linear motion, and when the vector sum of the three-dimensional accelerometer keeps a constant value, the three-dimensional accelerometer is judged to be uniform-speed circular motion. The period of judgment is the same as the filtering period, and is 1 second. The output judgment threshold of the accelerometer is determined by the sensitivity of the accelerometer selected by the system.
Calculating the observability degree of all 15-dimensional state variables under the condition of uniform linear motion, uniform accelerated linear motion or uniform circular motion so as to ensure that
And carrying out singular value decomposition on the T array to obtain:
T=U*S*V T
wherein: u = [ U =
1 u
2 ...u
m ],V=[v
1 v
2 ...v
n ]Are all orthogonal matrices and are provided with a plurality of parallel,
is a matrix of order m × r, where Λ = diag (σ)
1 σ
2 ... σ
r ) Is a diagonal matrix, σ
1 ≥σ
2 ≥...≥σ
r ≧ 0 is referred to as the singularity of the matrix T. According to the size of the singular value, whether the corresponding state variable can be observed or not can be judged, the observability of which state variables is high, and the observability of which variables is low.
(3) And sequencing the state variables from large to small according to the observability degree, and taking the front d-dimensional state variables including the three-dimensional position and the three-dimensional speed to form a state vector of the minimum-dimension filtering model. As shown in fig. 2.
(4) And determining elements corresponding to the newly-added state variables in the state estimation variance matrix of the newly-built dimension reduction filtering model by adopting a random generation method.
Constructing a dimension-reduction filtering model according to the determined state variables, wherein a full dimension corresponding to the mobility of the current SAR aerial carrier is adoptedBased on the SINS/GPS combined navigation filtering model, only elements corresponding to state variables contained in the dimensionality reduction filtering model in a state transition matrix F (t), a system noise transition matrix G (t) and an observation matrix H (t) are reserved; taking the corresponding element of the state estimation variance matrix of the state variable contained in the last estimation dimension reduction filtering model as the mean value, and taking the observability sigma of the variable at the last estimation time
prev Observable degree sigma of current time
now Is the variance of
And determining an element corresponding to the state variable in the filtering variance matrix at the current moment, namely the estimation variance of the newly added state variable.
(5) And under the condition that the maneuverability of the current SAR aerial carrier is kept unchanged, the dimension reduction filtering model is kept to realize SAR motion compensation. And restarting to calculate the observability measure when the mobility of the SAR aerial carrier is changed.
The steps are circularly carried out, as shown in fig. 3, the SINS/GPS combined navigation self-adaptive dimensionality reduction filtering method for SAR motion compensation can be realized, and the calculated amount is greatly reduced while the filtering precision is ensured.