CN105806363B  The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF  Google Patents
The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF Download PDFInfo
 Publication number
 CN105806363B CN105806363B CN201510782989.0A CN201510782989A CN105806363B CN 105806363 B CN105806363 B CN 105806363B CN 201510782989 A CN201510782989 A CN 201510782989A CN 105806363 B CN105806363 B CN 105806363B
 Authority
 CN
 China
 Prior art keywords
 gauss
 systems
 srqkf
 matrix
 error
 Prior art date
Links
 239000011159 matrix materials Substances 0.000 claims abstract description 65
 238000001914 filtration Methods 0.000 claims abstract description 49
 239000000969 carriers Substances 0.000 claims abstract description 11
 238000000034 methods Methods 0.000 claims description 9
 230000000875 corresponding Effects 0.000 claims description 6
 230000000644 propagated Effects 0.000 claims description 6
 230000001133 acceleration Effects 0.000 claims description 4
 238000000354 decomposition reactions Methods 0.000 claims description 4
 WCUXLLCKKVVCTQUHFFFAOYSAM potassium chloride Chemical compound data:image/svg+xml;base64,PD94bWwgdmVyc2lvbj0nMS4wJyBlbmNvZGluZz0naXNvLTg4NTktMSc/Pgo8c3ZnIHZlcnNpb249JzEuMScgYmFzZVByb2ZpbGU9J2Z1bGwnCiAgICAgICAgICAgICAgeG1sbnM9J2h0dHA6Ly93d3cudzMub3JnLzIwMDAvc3ZnJwogICAgICAgICAgICAgICAgICAgICAgeG1sbnM6cmRraXQ9J2h0dHA6Ly93d3cucmRraXQub3JnL3htbCcKICAgICAgICAgICAgICAgICAgICAgIHhtbG5zOnhsaW5rPSdodHRwOi8vd3d3LnczLm9yZy8xOTk5L3hsaW5rJwogICAgICAgICAgICAgICAgICB4bWw6c3BhY2U9J3ByZXNlcnZlJwp3aWR0aD0nMzAwcHgnIGhlaWdodD0nMzAwcHgnIHZpZXdCb3g9JzAgMCAzMDAgMzAwJz4KPCEtLSBFTkQgT0YgSEVBREVSIC0tPgo8cmVjdCBzdHlsZT0nb3BhY2l0eToxLjA7ZmlsbDojRkZGRkZGO3N0cm9rZTpub25lJyB3aWR0aD0nMzAwJyBoZWlnaHQ9JzMwMCcgeD0nMCcgeT0nMCc+IDwvcmVjdD4KPHRleHQgZG9taW5hbnQtYmFzZWxpbmU9ImNlbnRyYWwiIHRleHQtYW5jaG9yPSJlbmQiIHg9JzYwLjEzMzEnIHk9JzE1Ni42JyBzdHlsZT0nZm9udC1zaXplOjQwcHg7Zm9udC1zdHlsZTpub3JtYWw7Zm9udC13ZWlnaHQ6bm9ybWFsO2ZpbGwtb3BhY2l0eToxO3N0cm9rZTpub25lO2ZvbnQtZmFtaWx5OnNhbnMtc2VyaWY7ZmlsbDojNUJCNzcyJyA+PHRzcGFuPkNsPC90c3Bhbj48dHNwYW4gc3R5bGU9J2Jhc2VsaW5lLXNoaWZ0OnN1cGVyO2ZvbnQtc2l6ZTozMHB4Oyc+LTwvdHNwYW4+PHRzcGFuPjwvdHNwYW4+PC90ZXh0Pgo8dGV4dCBkb21pbmFudC1iYXNlbGluZT0iY2VudHJhbCIgdGV4dC1hbmNob3I9InN0YXJ0IiB4PScyNDcuNDQ5JyB5PScxNTYuNicgc3R5bGU9J2ZvbnQtc2l6ZTo0MHB4O2ZvbnQtc3R5bGU6bm9ybWFsO2ZvbnQtd2VpZ2h0Om5vcm1hbDtmaWxsLW9wYWNpdHk6MTtzdHJva2U6bm9uZTtmb250LWZhbWlseTpzYW5zLXNlcmlmO2ZpbGw6IzNCNDE0MycgPjx0c3Bhbj5LPC90c3Bhbj48dHNwYW4gc3R5bGU9J2Jhc2VsaW5lLXNoaWZ0OnN1cGVyO2ZvbnQtc2l6ZTozMHB4Oyc+KzwvdHNwYW4+PHRzcGFuPjwvdHNwYW4+PC90ZXh0Pgo8L3N2Zz4K data:image/svg+xml;base64,PD94bWwgdmVyc2lvbj0nMS4wJyBlbmNvZGluZz0naXNvLTg4NTktMSc/Pgo8c3ZnIHZlcnNpb249JzEuMScgYmFzZVByb2ZpbGU9J2Z1bGwnCiAgICAgICAgICAgICAgeG1sbnM9J2h0dHA6Ly93d3cudzMub3JnLzIwMDAvc3ZnJwogICAgICAgICAgICAgICAgICAgICAgeG1sbnM6cmRraXQ9J2h0dHA6Ly93d3cucmRraXQub3JnL3htbCcKICAgICAgICAgICAgICAgICAgICAgIHhtbG5zOnhsaW5rPSdodHRwOi8vd3d3LnczLm9yZy8xOTk5L3hsaW5rJwogICAgICAgICAgICAgICAgICB4bWw6c3BhY2U9J3ByZXNlcnZlJwp3aWR0aD0nODVweCcgaGVpZ2h0PSc4NXB4JyB2aWV3Qm94PScwIDAgODUgODUnPgo8IS0tIEVORCBPRiBIRUFERVIgLS0+CjxyZWN0IHN0eWxlPSdvcGFjaXR5OjEuMDtmaWxsOiNGRkZGRkY7c3Ryb2tlOm5vbmUnIHdpZHRoPSc4NScgaGVpZ2h0PSc4NScgeD0nMCcgeT0nMCc+IDwvcmVjdD4KPHRleHQgZG9taW5hbnQtYmFzZWxpbmU9ImNlbnRyYWwiIHRleHQtYW5jaG9yPSJlbmQiIHg9JzMxLjAyJyB5PSc0NS43MzA4JyBzdHlsZT0nZm9udC1zaXplOjIycHg7Zm9udC1zdHlsZTpub3JtYWw7Zm9udC13ZWlnaHQ6bm9ybWFsO2ZpbGwtb3BhY2l0eToxO3N0cm9rZTpub25lO2ZvbnQtZmFtaWx5OnNhbnMtc2VyaWY7ZmlsbDojNUJCNzcyJyA+PHRzcGFuPkNsPC90c3Bhbj48dHNwYW4gc3R5bGU9J2Jhc2VsaW5lLXNoaWZ0OnN1cGVyO2ZvbnQtc2l6ZToxNi41cHg7Jz4tPC90c3Bhbj48dHNwYW4+PC90c3Bhbj48L3RleHQ+Cjx0ZXh0IGRvbWluYW50LWJhc2VsaW5lPSJjZW50cmFsIiB0ZXh0LWFuY2hvcj0ic3RhcnQiIHg9JzU3LjA5MjMnIHk9JzQ1LjczMDgnIHN0eWxlPSdmb250LXNpemU6MjJweDtmb250LXN0eWxlOm5vcm1hbDtmb250LXdlaWdodDpub3JtYWw7ZmlsbC1vcGFjaXR5OjE7c3Ryb2tlOm5vbmU7Zm9udC1mYW1pbHk6c2Fucy1zZXJpZjtmaWxsOiMzQjQxNDMnID48dHNwYW4+SzwvdHNwYW4+PHRzcGFuIHN0eWxlPSdiYXNlbGluZS1zaGlmdDpzdXBlcjtmb250LXNpemU6MTYuNXB4Oyc+KzwvdHNwYW4+PHRzcGFuPjwvdHNwYW4+PC90ZXh0Pgo8L3N2Zz4K [Cl].[K+] WCUXLLCKKVVCTQUHFFFAOYSAM 0.000 claims description 3
 241000197722 Sphaeroceridae Species 0.000 claims description 2
 238000010276 construction Methods 0.000 claims description 2
 239000000686 essences Substances 0.000 claims 1
 239000011901 water Substances 0.000 description 6
 238000005516 engineering processes Methods 0.000 description 3
 238000004458 analytical methods Methods 0.000 description 2
 230000000694 effects Effects 0.000 description 2
 238000005096 rolling process Methods 0.000 description 2
 280000450078 System One companies 0.000 description 1
 238000005070 sampling Methods 0.000 description 1
 238000004088 simulation Methods 0.000 description 1
Classifications

 G—PHYSICS
 G01—MEASURING; TESTING
 G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
 G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
 G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or startingup of inertial devices
Abstract
Description
Technical field
The present invention relates to strapdown inertial and integrated navigation technology field, especially a kind of SINS/ based on SRQKF The underwater large misalignment angle alignment methods of DVL.
Background technology
Initial aligned relationship is the key technology in strapdown inertial navigation system (SINS) to the operating accuracy of inertia system One of.Since sea situation is complex, the hull of submarine navigation device is typically in largeamplitude sloshing state, to make strapdown inertial Autoregistration can not be rapidly completed in system, be in large misalignment angle state for a long time.In this case it is necessary to auxiliary by external information It helps, completes the initial alignment under moving base, large misalignment angle.Submarine navigation device usually completes this by Doppler log (DVL) One task.
However, since hull is in large misalignment angle, vibrating state always, conventional linearity error model can not accurate description The error Propagation Property of SINS, needs to establish more accurate Nonlinear Error Models, and water is completed using nonlinear filtering method The initial alignment of lower aircraft.
In recent years, various nonlinear filtering methods  Extended Kalman filter (EKF), Unscented kalman filtering (UKF), Volume Kalman filtering (CKF) is continuously available application, and but, they are again all there is some limitations, and EKF is in nonlinear characteristic Precision is not high when stronger；Although the precision of UKF and CKF increases, but in complex conditions such as large misalignment angles, equally can not Degree of precision is obtained, and convergence rate is slower.Therefore, invention has highprecision, and large misalignment angle, moving base can be successfully managed etc. The SINS/DVL alignment methods of complex environment are meaningful.
Invention content
Goal of the invention：To overcome the deficiencies in the prior art, the present invention provides the SINS/DVL water based on SRQKF Lower large misalignment angle alignment methods, which is characterized in that include the following steps：
Step 1：Establish Nonlinear Error Models and nonlinear filtering of the strapdown inertial navigation system SINS under large misalignment angle Wave equation；
Step 2：Using GaussHermite quadrature middle multivariable Gauss points and its coefficient configuration method and square Root filtering method, structure square root are quadratured Kalman filter SRQKF；
Step 3：Misalignment is estimated using the square root Kalman filtering SRQKF that quadratures, and corrects strapdown attitude matrix, Obtain accurate strapdown attitude matrix and attitude angle.
Further, the step 1：Establish nonlinearity erron moulds of the strapdown inertial navigation system SINS under large misalignment angle Type and Nonlinear Filtering Formulae, specially：
Step 1.1：It is navigational coordinate system n to choose " northeast day " geographic coordinate system；Choosing carrier " right front upper " coordinate system is Carrier coordinate system b；N systems successively turn to b systems by 3 Eulerian angles, and three Eulerian angles are denoted as course angle ψ, pitch angle θ, rolling Angle γ；Attitude matrix between n systems and b systems is denoted asTrue attitude angle is denoted asTrue velocity is denoted asThe attitude angle that SINS is calculated is denoted as：SINS computing speeds are denoted as：The mathematical platform that SINS is calculated is denoted as n ', and n ' is that the attitude matrix between b systems is denoted asIt is that angle of rotation is respectively φ three times that n systems successively turn to n ' by 3 times_{u}、φ_{e}、φ_{n}, they are referred to as Euler's platform error Vector is remembered at angleDefining velocity error is： Then Nonlinear Error Models are as follows：
Wherein,The constant error of three axis accelerometer is descended for b systems,For the lower three axis accelerometer of b systems with Chance error is poor,3axis acceleration constant error is descended for b systems,For the random of the lower 3axis acceleration of b systems Error,For the reality output of accelerometer,For the mathematical platform angular velocity of rotation calculated,For earth rotation angle speed Degree,The angular velocity of rotation for being the mathematical platform that calculates relative to the earth,For correspondenceCalculating error.For the inverse matrix of Eulerian angles differential equation coefficient matrix；It is between n systems for n ' Attitude matrix, C_{ω}WithSpecially：
Step 1.2：It is as follows that the Nonlinear Filtering Formulae establishes process：
Access speed errorEulerian angles platform error angle φ_{u}、φ_{e}、φ_{n}；Gyroscope constant value error under b systemsAnd the accelerometer constant error under b systemsConstitute 10 dimension state vectors：
Under swaying baseIt is approximately zero, nonlinear filtering state equation can be established as：
WithBidimensional state before only taking,WithFor zeromean whitenoise process.The nonlinear state equation can letter It is denoted as：Using sampling period T as filtering cycle, quadravalence RungeKutta integration method can be used, It is steplength by its discretization using T, is as follows：
Assuming that the value x that the state vector chosen is carved at the beginning_{0}It is known that and note t_{1}=t+T/2 can then be changed by following It will for formulaIt is discrete：
Δx_{1}=[f (x, t)+ω (t)] T
Δx_{2}=[f (x, t_{1})+Δx_{1}/2+ω(t_{1})]T
Δx_{3}=[f (x, t_{1})+Δx_{2}/2+ω(t_{1})]T
Δx_{4}=[f (x, t+T)+Δ x_{3}+ω(t+T)]T
x_{k+1}=x_{k}+(Δx_{1}+2Δx_{2}+2Δx_{3}+Δx_{4})/6
Remember that discrete rear state filtering equation is：x_{k}=f (x_{k1})+ω_{k1}
Access speed establishes nonlinear measurement equation as observed quantity, is as follows：
Using the east orientation speed of carrier S INS and DVL output and north orientation speed component as match information source, foundation it is nonthread Property measurement equation is：
Wherein, ν^{b}For the true velocity under b systems；For the actual speed under b systems, the i.e. speed of DVL outputs；It takes two before z Dimension is observation；ν is zero mean Gaussian white noise process, which can be abbreviated as：Z=h (x, t)+ν (t)；
Equally using T as filtering cycle, and simple discretization is carried out using T as steplength, z=h (x, t)+ν (t) is discrete It turns to：z_{k}=h (x_{k},t_{k})+ν(t_{k}), obtaining the measurement equation after discretization is：z_{k}=h (x_{k})+ν_{k}。
Following Nonlinear Filtering Formulae is formed by state equation and measurement equation：
Further, the step 2：It is configured using quadrature middle multivariable Gauss points and its coefficients of GaussHermite Method and square root filtering method, structure square root are quadratured Kalman filter SRQKF, specially：
Step 2.1：GaussHermite quadrature middle multivariable Gauss points and its coefficient configuration method it is as follows：
GaussHermite formula of quadraturing are：
Wherein, x is stochastic variable, and g (x) is the function about x, x_{i}It is referred to as Gauss points, A_{i}It is referred to as Gauss coefficients, m It is the integral points of setting.
Utilize formula：
It can be in the hope of m Gauss points x_{i}(H_{m}(x) zero) and m Gauss coefficients A_{i}；Utilize Gauss points and Gauss systems The desired value E (g (x)) of g (x), can be expressed as by number：
Wherein,
Assuming that x is n_{x}Random vector is tieed up, each component is mutual indepedent, and obeys N (0,1) Gaussian Profile, each component Seek each m ith of Gauss coefficients a that can then obtain vector x of Gauss points and Gauss coefficients_{i}With Gauss point vectors ξ_{i}：
Wherein,For the corresponding Gauss points of each component of vector x；It indicates by single component Gauss The Gauss point vectors that point is constituted, altogetherIt is a；a_{ij}For Gauss points ξ_{ij}Corresponding Gauss coefficients；
Step 2.2：Square root quadrature Kalman filter SRQKF construction method it is as follows：
For the Nonlinear Filtering Formulae of foundation：
X in formula_{k}State vectors are tieed up for 10, and each component is independent, Gaussian distributed；z_{k}For 2 dimension observation vectors；ω_{k1}With ν_{k}Respectively process noise and measurement noise, meet：
In formula, δ_{ij}For δ functions；Q is the variance matrix of system noise；R is the variance matrix for measuring noise.
SRQKF algorithms are as follows：
Step 2.2.1：Init state variableAnd its mean square deviation P_{0}：
Step 2.2.2：The method for updating time of SRQKF is as follows：
When being filtered for the first time, (k=1) is to initial variance battle array P_{0}Cholesky decomposition is carried out, initial error variance is obtained The feature square root S of battle array_{0}：
P_{0}=S_{0}S_{0} ^{T}；
Utilize the S at k1 moment_{k1k1}Calculate Gauss integration point X_{l,k1k1}S is then utilized if filtering for the first time_{0}Calculate Gauss Point：
Calculate the Gauss integration point that nonlinear state function is propagated
Calculating state onestep prediction
Calculate the feature square root S of onestep prediction variance matrix_{kk1}：
S in formula_{Q,k}It is decomposed for the Cholesky of system noise variance matrix, i.e.,： It is expressed as：Qr () is indicated The QR of matrix is decomposed, S_{k1k1}R gusts of upper triangular matrix in taking QR to decompose.
Step 2.2.3：The measurement update method of SRQKF is as follows：
Calculate onestep prediction state Gauss integration point X_{l,kk1}：
Calculate the Gauss integration point Z that nonlinear measurement function is propagated_{l,kk1}：
Z_{l,kk1}=h (X_{l,k1k1}) l=1,2 ..., M.
Calculate onestep prediction measuring value
Calculate the feature square root S that onestep prediction measures variance matrix_{zz,kk1}：
S_{zz,kk1}=qr ([Z_{kk1} S_{R,k}])
S in formula_{R,k}It is decomposed for the Cholesky of observed quantity noise variance matrix, i.e.,：Z_{kk1}It is expressed as：
Calculate onestep prediction covariance matrix P_{xz,kk1}：
X in formula_{kk1}It is expressed as：
Calculate gain matrix K：
Calculating state updated value
Calculate the feature square root S of state variance battle array_{kk}：
S_{kk}=qr ([X_{kk1}KZ_{kk1} KS_{R,k}])。
Further, step 3) estimates misalignment using the square root Kalman filtering SRQKF that quadratures, and corrects strapdown Attitude matrix obtains accurate strapdown attitude matrix and attitude angle, specially：
Step 3.1：After SRQKF filters complete primary filtering, according to 10 dimension quantity of state estimated values of filter output：
It is the attitude matrix between n systems to calculate n 'Computational methods are as follows：
Step 3.2：According to the attitude matrix between n' systems and b systemsWith the attitude matrix between n' systems and n systemsMeter Calculate accurate strapdown attitude matrixComputational methods are as follows：
Step 3.3：According to strapdown attitude matrixEulerian angles are calculated, computational methods are as follows：
3.4：If attitude accuracy reaches requirement, or the alignment time reaches setting value, and alignment terminates, and is otherwise run after return Step 2) is quadratured middle multivariable Gauss points and its coefficient configuration method and square root filtering side using GaussHermite Method, structure square root are quadratured Kalman filter SRQKF.
Advantageous effect：The present invention has the following advantages the prior art：
1. the present invention, in the characteristic of large misalignment angle, moving base, establishes nonlinear filtering for underwater carrier Equation effectively increases the Strapdown Inertial Navigation System alignment precision under the conditions of carrier underwater complex using the SRQKF methods filtered, and The alignment time is reduced, ensures that carrier Strapdown Inertial Navigation System provides reliable posture information.
2. the method for square root filtering is introduced into QKF filtering methods by the present invention, SRQKF filtering methods are constituted, are avoided every To state variance battle array P when secondary filtering_{k}With prediction variance matrix P_{kk1}Cholesky decomposition is carried out, filtering accuracy sum number is effectively increased It is worth the stability calculated.
3. the present invention using DVL provide the higher speeds match information of precision, help to improve SRQKF filtering accuracy and Convergence rate, to improve the accuracy and speed of alignment.
Description of the drawings
Fig. 1 is the system schema figure of the present invention.
Fig. 2 is that the present invention is based on the underwater large misalignment angle alignment principles figures of the SINS/DVL of SRQKF
Fig. 3 is the SRQKF filter flow figures of the present invention
Fig. 4 is the underwater large misalignment angle alignment pitch angle Error Graphs of SINS/DVL of the present invention.
Fig. 5 is the underwater large misalignment angle alignment roll angle Error Graphs of SINS/DVL of the present invention
Fig. 6 is the underwater large misalignment angle heading orientation angle error figures of SINS/DVL of the present invention.
Specific implementation mode
The present invention is further described below in conjunction with the accompanying drawings.
As depicted in figs. 1 and 2, it is the system schema figure and alignment principles figure of the present invention.
SINS strapdown settlement modules collect the gyro output valve and accelerometer of Inertial Measurement Unit (IMU) module output Output valve carries out strapdown resolving, obtains the information such as attitude angle, attitude matrix, speed, position；DVL equipment works under water, output The velocity information of carrier；The information of SINS and DVL outputs is input in SRQKF filters simultaneously, at the filtering for row information of going forward side by side Reason, filtering are as follows：
1. according to the characteristic in the case of the underwater large misalignment angles of SINS/DVL, 10 state vectors are chosen：
Set up nonlinear state filtering equations：
Discretization postscript is：x_{k}=f (x_{k1})+ω_{k1}.
2. access speed amount is measuring value, nonlinear measurement filtering equations are established：
Discretization postscript is：z_{k}=h (x_{k})+v_{k}.
3. the configuration of multivariable Gauss points and its coefficient, concrete configuration method are as follows：
Utilize formula：
Acquire the corresponding Gauss points a of each component of vector x_{ij}With Gauss coefficients
Recycle formula：
Acquire the Gauss points a of vector x_{i}With Gauss coefficients ξ_{i}。
4.SRQKF state variablesAnd its mean square deviation P_{0}Initialization procedure is as follows：
The method for updating time of 5.SRQKF is as follows：
When being filtered for the first time, (k=1) is to variance matrix P_{0}Cholesky decomposition is carried out, initial error variance matrix is obtained Feature square root S_{0}：
P_{0}=S_{0}S_{0} ^{T}
Utilize the S at k1 moment_{k1k1}Calculate Gauss integration point X_{l,k1k1}S is then utilized if filtering for the first time_{0}Calculate Gauss Point X_{l,k1k1}：
Calculate the Gauss integration point that nonlinear state function is propagated
Calculating state onestep prediction
Calculate the feature square root S of onestep prediction variance matrix_{kk1}：
The measurement update method of 6.SRQKF is as follows：
Calculate onestep prediction state Gauss integration point X_{l,kk1}：
Calculate the Gauss integration point Z that nonlinear measurement function is propagated_{l,kk1}：
Z_{l,kk1}=h (X_{l,k1k1}) l=1,2 ..., M.
Calculate onestep prediction measuring value
Calculate the feature square root S that onestep prediction measures variance matrix_{zz,kk1}：
S_{zz,kk1}=qr ([Z_{kk1} S_{R,k}])
Calculate onestep prediction covariance matrix P_{xz,kk1}：
X in formula_{kk1}It is expressed as：
Calculate gain matrix K：
Calculating state updated value
Calculate the feature square root S of state variance battle array_{kk}：
S_{kk}=qr ([X_{kk1}KZ_{kk1} KS_{R,k}])
The flow chart of SRQKF filters is as shown in Figure 3.
7. the state of progress is closedloop corrected, accurate strapdown attitude matrix and attitude angle are obtained, the specific method is as follows：
According to 10 dimension quantity of state estimated values of SRQKF filters output：
It is the attitude matrix between n systems to calculate n '
It is the attitude matrix between b systems according to n 'It is the attitude matrix between n systems with n 'It calculates accurate Strapdown attitude matrix
According to strapdown attitude matrixEulerian angles are calculated, computational methods are as follows：
If attitude accuracy reaches requirement, or the alignment time reaches setting value, and alignment terminates, and otherwise continues recursion and executes step Rapid 2 with step 3, until alignment terminate.
Illustrate beneficial effects of the present invention using following example：
1) submarine navigation device kinematic parameter is arranged：
Initial time submarine navigation device is emulated at 32 ° of north latitude, the underwater 15m of 118 ° of east longitude；And rotating around pitch axis, cross Rocker and course axis make threeaxis swinging movement with SIN function, pitch angle θ, roll angle γ, course angle ψ, wave amplitude difference For：9 °, 12 °, 14 °, rolling period is respectively：16s, 20s, 12s, initial heading angle are 30 °, and initial misalignment is：Δ θ= 10 °, Δ γ=10 °, Δ ψ=20 °；The submarine navigation device is linear uniform motion, hours underway 600s with the speed of 5m/s.
2) parameter setting of sensor：
The gyroscope constant value drift of the strapdown system of submarine navigation device is 0.01 °/h：, Modelling of Random Drift of Gyroscopes is 0.01 °/h：, Accelerometer is biased to：50mg, accelerometer random error are：50mg, DVL output speed error is：0.1m/s.
3) parameter setting of SRQKF
The primary condition that filter is arranged is：
P_{0}=diag { (0.1m/s)^{2},(0.1m/s)^{2},(10°)^{2},(10°)^{2},(20°)^{2},
(50mg)^{2},(50mg)^{2},(0.01°/h)^{2},(0.01°/h)^{2},(0.01°/h)^{2}}
Q=diag { (50mg)^{2},(50mg)^{2},(0.01°/h)^{2},(0.01°/h)^{2},(0.01°/h)^{2},0,0,0,0,0}
R=diag { (0.1m/s)^{2},(0.1m/s)^{2}}
The Gauss points of the single component of QKF state vectors take 3, i.e. m=3, corresponding Gauss points and Gauss coefficients to be：
4) analysis of simulation result：
DVL auxiliary is carried out using the underwater large misalignment angle alignment methods of the SINS/DVL provided by the invention based on SRQKF SINS is aligned under water, Fig. 4, and 5,6 be pitch angle the error delta θ, roll angle error delta γ and course angle in alignment procedures of the present invention The curve of error delta ψ, it can be found that：After submarine navigation device navigates by water 100s, SRQKF restrains substantially, at this time pitch angle alignment error About 0.002 °, about 0.003 ° of roll angle alignment error, about 0.5 ° of course angle alignment error；It is found by analysis, after navigating by water 500s, 0.004 ° of pitch angle alignment error within, roll angle alignment error within 0.002 °, course angle alignment error 0.02 ° with It is interior.
The above result shows that in the case of for being exported there are error there are large misalignment angle, swaying base and DVL speed, Using the present invention SRQKF the underwater large misalignment angle alignment methods of SINS/DVL, still can ensure very high alignment precision and Alignment speed quickly.
The preferred embodiment of the present invention has been described above in detail, still, during present invention is not limited to the embodiments described above Detail can carry out a variety of equivalents to technical scheme of the present invention within the scope of the technical concept of the present invention, this A little equivalents all belong to the scope of protection of the present invention.
Claims (3)
Priority Applications (1)
Application Number  Priority Date  Filing Date  Title 

CN201510782989.0A CN105806363B (en)  20151116  20151116  The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF 
Applications Claiming Priority (1)
Application Number  Priority Date  Filing Date  Title 

CN201510782989.0A CN105806363B (en)  20151116  20151116  The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF 
Publications (2)
Publication Number  Publication Date 

CN105806363A CN105806363A (en)  20160727 
CN105806363B true CN105806363B (en)  20180821 
Family
ID=56465549
Family Applications (1)
Application Number  Title  Priority Date  Filing Date 

CN201510782989.0A CN105806363B (en)  20151116  20151116  The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF 
Country Status (1)
Country  Link 

CN (1)  CN105806363B (en) 
Families Citing this family (4)
Publication number  Priority date  Publication date  Assignee  Title 

CN106949906B (en) *  20170309  20200424  东南大学  Large misalignment angle rapid alignment method based on integral extended state observer 
CN107063245A (en) *  20170419  20170818  东南大学  A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF 
CN109141475B (en) *  20180914  20200605  苏州大学  Initial alignment method for SINS robust traveling under assistance of DVL (dynamic velocity logging) 
CN109443379A (en) *  20180928  20190308  东南大学  A kind of underwater antishake dynamic alignment methods of the SINS/DVL of deepsea submariner device 
Citations (6)
Publication number  Priority date  Publication date  Assignee  Title 

CN101639365A (en) *  20090722  20100203  哈尔滨工程大学  Offshore alignment method of autonomous underwater vehicle based on second order interpolating filter 
CN102654406A (en) *  20120411  20120905  哈尔滨工程大学  Initial alignment method for moving bases based on combination of nonlinear prediction filtering and cubature Kalman filtering 
CN103344260A (en) *  20130718  20131009  哈尔滨工程大学  Initial alignment method of large azimuth misalignment angle of strapdown inertial navigation system based on RBCKF (raoblackwellised cubature kalman filter) 
CN103644913A (en) *  20131225  20140319  东南大学  Direct navigation modelbased unscented Kalman nonlinear initial alignment method 
CN104374405A (en) *  20141106  20150225  哈尔滨工程大学  MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering 
CN104655131A (en) *  20150206  20150527  东南大学  Initial inertial navigation alignment method based on terated strong tracking spherical simplex radial cubature Kalman filter (ISTSSRCKF) 
Family Cites Families (1)
Publication number  Priority date  Publication date  Assignee  Title 

ITTO20130645A1 (en) *  20130730  20150131  St Microelectronics Srl  Method and system calibration in real time to a gyroscope 

2015
 20151116 CN CN201510782989.0A patent/CN105806363B/en active IP Right Grant
Patent Citations (6)
Publication number  Priority date  Publication date  Assignee  Title 

CN101639365A (en) *  20090722  20100203  哈尔滨工程大学  Offshore alignment method of autonomous underwater vehicle based on second order interpolating filter 
CN102654406A (en) *  20120411  20120905  哈尔滨工程大学  Initial alignment method for moving bases based on combination of nonlinear prediction filtering and cubature Kalman filtering 
CN103344260A (en) *  20130718  20131009  哈尔滨工程大学  Initial alignment method of large azimuth misalignment angle of strapdown inertial navigation system based on RBCKF (raoblackwellised cubature kalman filter) 
CN103644913A (en) *  20131225  20140319  东南大学  Direct navigation modelbased unscented Kalman nonlinear initial alignment method 
CN104374405A (en) *  20141106  20150225  哈尔滨工程大学  MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering 
CN104655131A (en) *  20150206  20150527  东南大学  Initial inertial navigation alignment method based on terated strong tracking spherical simplex radial cubature Kalman filter (ISTSSRCKF) 
NonPatent Citations (2)
Title 

"A New Nonlinear Filtering Method for Ballistic Target Tracking";Chunling Wu，等;《12th International Conference on Information Fusion》;20090709;20622067 * 
"平方根求积分卡尔曼滤波器";巫春玲，等;《电子学报》;20090531;第37卷(第5期);987992 * 
Also Published As
Publication number  Publication date 

CN105806363A (en)  20160727 
Similar Documents
Publication  Publication Date  Title 

Li et al.  Effective adaptive Kalman filter for MEMSIMU/magnetometers integrated attitude and heading reference systems  
Dmitriyev et al.  Nonlinear filtering methods application in INS alignment  
Jiancheng et al.  Study on innovation adaptive EKF for inflight alignment of airborne POS  
CN106289246B (en)  A kind of flexible link arm measure method based on position and orientation measurement system  
RU2662460C1 (en)  Method of upgrading angular position of agricultural machine based on the nineaxis mems sensor  
Phuong et al.  A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass  
Hong  Fuzzy logic based closedloop strapdown attitude system for unmanned aerial vehicle (UAV)  
CN101246022B (en)  Optic fiber gyroscope strapdown inertial navigation system twoposition initial alignment method based on filtering  
CN102519460B (en)  Nonlinear alignment method of strapdown inertial navigation system  
CN104635251B (en)  A kind of INS/GPS integrated positionings determine appearance new method  
Han et al.  A novel method to integrate IMU and magnetometers in attitude and heading reference systems  
CN101514900B (en)  Method for initial alignment of a singleaxis rotation strapdown inertial navigation system (SINS)  
CN104197927B (en)  Submerged structure detects robot realtime navigation system and method  
Chang et al.  Initial alignment for a Doppler velocity logaided strapdown inertial navigation system with limited information  
CN101660914B (en)  Airborne starlight of coupling inertial position error and independent navigation method of inertial composition  
CN103575299B (en)  Utilize dualaxis rotation inertial navigation system alignment and the error correcting method of External Observation information  
CN101706287B (en)  Rotating strapdown system onsite proving method based on digital highpassing filtering  
CN103090870B (en)  Spacecraft attitude measurement method based on MEMS (microelectromechanical systems) sensor  
CN103630137B (en)  A kind of for the attitude of navigational system and the bearing calibration of course angle  
CN100541135C (en)  Fiberoptic gyroscope strapdown inertial navigation system initial attitude based on Doppler is determined method  
CN104457754B (en)  SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method  
Li et al.  A novel backtracking navigation scheme for autonomous underwater vehicles  
CN101963513B (en)  Alignment method for eliminating lever arm effect error of strapdown inertial navigation system (SINS) of underwater carrier  
CN103913181A (en)  Airborne distribution type POS (position and orientation system) transfer alignment method based on parameter identification  
CN101846510A (en)  Highprecision satellite attitude determination method based on star sensor and gyroscope 
Legal Events
Date  Code  Title  Description 

C06  Publication  
PB01  Publication  
C10  Entry into substantive examination  
SE01  Entry into force of request for substantive examination  
GR01  Patent grant  
GR01  Patent grant 