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 PDF

Info

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
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201510782989.0A
Other languages
Chinese (zh)
Other versions
CN105806363A (en
Inventor
徐晓苏
杨博
徐祥
周峰
王捍兵
田泽鑫
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Southeast University filed Critical Southeast University
Priority to CN201510782989.0A priority Critical patent/CN105806363B/en
Publication of CN105806363A publication Critical patent/CN105806363A/en
Application granted granted Critical
Publication of CN105806363B publication Critical patent/CN105806363B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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 starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

The present invention discloses a kind of underwater large misalignment angle alignment methods of the SINS/DVL based on SRQKF, includes the following steps：Step 1：Establish Nonlinear Error Models and Nonlinear Filtering Formulae of the strapdown inertial navigation system SINS under large misalignment angle；Step 2：It is quadratured middle multivariable Gauss points and its coefficient configuration method and square root filtering method using Gauss Hermite, structure square root is quadratured Kalman filter SRQKF；Step 3：Misalignment is estimated using the square root Kalman filtering SRQKF that quadratures, and corrects strapdown attitude matrix, obtains accurate strapdown attitude matrix and attitude angle.The present invention improves the underwater alignment precision of carrier strapdown system and alignment speed.

Description

The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF
Technical field
The present invention relates to strap-down 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 large-amplitude sloshing state, to make strap-down 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 non-linear filtering method The initial alignment of lower aircraft.
In recent years, various non-linear 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 high-precision, 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 Gauss-Hermite 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 timesu、φ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,3-axis acceleration constant error is descended for b systems,For the random of the lower 3-axis 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 zero-mean white-noise process.The nonlinear state equation can letter It is denoted as：Using sampling period T as filtering cycle, quadravalence Runge-Kutta integration method can be used, It is step-length by its discretization using T, is as follows：
Assuming that the value x that the state vector chosen is carved at the beginning0It is known that and note t1=t+T/2 can then be changed by following It will for formulaIt is discrete：
Δx1=[f (x, t)+ω (t)] T
Δx2=[f (x, t1)+Δx1/2+ω(t1)]T
Δx3=[f (x, t1)+Δx2/2+ω(t1)]T
Δx4=[f (x, t+T)+Δ x3+ω(t+T)]T
xk+1=xk+(Δx1+2Δx2+2Δx3+Δx4)/6
Remember that discrete rear state filtering equation is：xk=f (xk-1)+ωk-1
Access speed establishes non-linear 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 non-thread Property measurement equation is：
Wherein, νbFor 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 step-length, z=h (x, t)+ν (t) is discrete It turns to：zk=h (xk,tk)+ν(tk), obtaining the measurement equation after discretization is：zk=h (xk)+ν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 Gauss-Hermite Method and square root filtering method, structure square root are quadratured Kalman filter SRQKF, specially：
Step 2.1：Gauss-Hermite quadrature middle multivariable Gauss points and its coefficient configuration method it is as follows：
Gauss-Hermite formula of quadraturing are：
Wherein, x is stochastic variable, and g (x) is the function about x, xiIt is referred to as Gauss points, AiIt 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 xi(Hm(x) zero) and m Gauss coefficients Ai；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 nxRandom vector is tieed up, each component is mutual indepedent, and obeys N (0,1) Gaussian Profile, each component Seek each m i-th of Gauss coefficients a that can then obtain vector x of Gauss points and Gauss coefficientsiWith 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；aijFor Gauss points ξijCorresponding 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 formulakState vectors are tieed up for 10, and each component is independent, Gaussian distributed；zkFor 2 dimension observation vectors；ωk-1With νkRespectively process noise and measurement noise, meet：
In formula, δijFor δ 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 P0：
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 P0Cholesky decomposition is carried out, initial error variance is obtained The feature square root S of battle array0：
P0=S0S0 T；
Utilize the S at k-1 momentk-1|k-1Calculate Gauss integration point Xl,k-1|k-1S is then utilized if filtering for the first time0Calculate Gauss Point：
Calculate the Gauss integration point that nonlinear state function is propagated
Calculating state one-step prediction
Calculate the feature square root S of one-step prediction variance matrixk|k-1：
S in formulaQ,kIt 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, Sk-1|k-1R gusts of upper triangular matrix in taking QR to decompose.
Step 2.2.3：The measurement update method of SRQKF is as follows：
Calculate one-step prediction state Gauss integration point Xl,k|k-1：
Calculate the Gauss integration point Z that non-linear measurement function is propagatedl,k|k-1：
Zl,k|k-1=h (Xl,k-1|k-1) l=1,2 ..., M.
Calculate one-step prediction measuring value
Calculate the feature square root S that one-step prediction measures variance matrixzz,k|k-1：
Szz,k|k-1=qr ([Zk|k-1 SR,k])
S in formulaR,kIt is decomposed for the Cholesky of observed quantity noise variance matrix, i.e.,：Zk|k-1It is expressed as：
Calculate one-step prediction covariance matrix Pxz,k|k-1：
X in formulak|k-1It is expressed as：
Calculate gain matrix K：
Calculating state updated value
Calculate the feature square root S of state variance battle arrayk|k：
Sk|k=qr ([Xk|k-1-KZk|k-1 KSR,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 Gauss-Hermite 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 filteringkWith prediction variance matrix Pk|k-1Cholesky 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：xk=f (xk-1)+ωk-1.
2. access speed amount is measuring value, non-linear measurement filtering equations are established：
Discretization postscript is：zk=h (xk)+vk.
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 xijWith Gauss coefficients
Recycle formula：
Acquire the Gauss points a of vector xiWith Gauss coefficients ξi。
4.SRQKF state variablesAnd its mean square deviation P0Initialization 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 P0Cholesky decomposition is carried out, initial error variance matrix is obtained Feature square root S0：
P0=S0S0 T
Utilize the S at k-1 momentk-1|k-1Calculate Gauss integration point Xl,k-1|k-1S is then utilized if filtering for the first time0Calculate Gauss Point Xl,k-1|k-1：
Calculate the Gauss integration point that nonlinear state function is propagated
Calculating state one-step prediction
Calculate the feature square root S of one-step prediction variance matrixk|k-1：
The measurement update method of 6.SRQKF is as follows：
Calculate one-step prediction state Gauss integration point Xl,k|k-1：
Calculate the Gauss integration point Z that non-linear measurement function is propagatedl,k|k-1：
Zl,k|k-1=h (Xl,k-1|k-1) l=1,2 ..., M.
Calculate one-step prediction measuring value
Calculate the feature square root S that one-step prediction measures variance matrixzz,k|k-1：
Szz,k|k-1=qr ([Zk|k-1 SR,k])
Calculate one-step prediction covariance matrix Pxz,k|k-1：
X in formulak|k-1It is expressed as：
Calculate gain matrix K：
Calculating state updated value
Calculate the feature square root S of state variance battle arrayk|k：
Sk|k=qr ([Xk|k-1-KZk|k-1 KSR,k])
The flow chart of SRQKF filters is as shown in Figure 3.
7. the state of progress is closed-loop 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 three-axis 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：
P0=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)

1. a kind of underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF, which is characterized in that include the following steps：
Step 1：Establish Nonlinear Error Models and nonlinear filtering side of the strapdown inertial navigation system SINS under large misalignment angle Journey；
Step 2：It is filtered using quadrature middle multivariable Gauss points and its coefficient configuration method and square roots of Gauss-Hermite Wave 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, is obtained Accurate strapdown attitude matrix and attitude angle；
Wherein, the step 1 includes：
Step 1.1：It is navigational coordinate system n to choose " northeast day " geographic coordinate system；Selection carrier " right front upper " coordinate system is carrier Coordinate system b；N systems successively turn to b systems by 3 Eulerian angles；Attitude matrix between n systems and b systems is denoted asTrue attitude angle It is denoted asWherein, ψ is true course angle, and θ is true pitch angle, and γ is true roll angle；True velocity is remembered ForThe 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' systems, and the attitude matrix between n' systems and b systems is denoted asN systems successively turn to n' systems by 3 times, and angle of rotation is respectively φ three timesu、φ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, whereinFor gyro x-axis constant error, For gyro y-axis constant error,For gyro z-axis constant error,The random error of three axis accelerometer is descended for b systems,3-axis acceleration constant error is descended for b systems,The random error of 3-axis acceleration is descended for b systems, For the reality output of accelerometer,For the mathematical platform angular velocity of rotation calculated,For earth rotation angular speed,For Angular velocity of rotation of the mathematical platform calculated relative to the earth,For correspondenceMeter Calculate error；For the inverse matrix of Eulerian angles differential equation coefficient matrix；For the attitude matrix between n' systems and n systems, 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；Gyro x-axis, y-axis, z-axis under b systems is normal It is worth errorAnd 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 zero-mean white-noise process, which can be abbreviated as：If filtering cycle is T, quadravalence Runge-Kutta integration method can be used, with the value of filtering cycle T It is step-length by its discretization, is as follows：
Assuming that the value x that the state vector chosen is carved at the beginning0It is known that and note t1=t+T/2 can then pass through following iteration public affairs Formula willIt is discrete：
Δx1=[f (x, t)+ω (t)] T
Δx2=[f (x, t1)+Δx1/2+ω(t1)]T
Δx3=[f (x, t1)+Δx2/2+ω(t1)]T
Δx4=[f (x, t+T)+Δ x3+ω(t+T)]T
xk+1=xk+(Δx1+2Δx2+2Δx3+Δx4)/6
Remember that discrete rear state filtering equation is：xk=f (xk-1)+ωk-1
Access speed establishes non-linear 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, the amount of nonlinearity of foundation Surveying equation is：
Wherein, νbFor the true velocity under b systems；For the actual speed under b systems, the i.e. speed of DVL outputs；Bidimensional is to see before taking z Measured value；ν is zero mean Gaussian white noise process, which can be abbreviated as：Z=h (x, t)+ν (t)；
Simple discretization is equally carried out using the value of filtering cycle T as step-length, is turned to z=h (x, t)+ν (t) are discrete：zk=h (xk)+νk；
Following Nonlinear Filtering Formulae is formed by state equation and measurement equation：
2. a kind of underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF as described in claim 1, it is characterised in that The step 2：It is filtered using quadrature middle multivariable Gauss points and its coefficient configuration method and square roots of Gauss-Hermite Wave method, structure square root are quadratured Kalman filter SRQKF, specially：
Step 2.1：Gauss-Hermite quadrature middle multivariable Gauss points and its coefficient configuration method it is as follows：
Gauss-Hermite formula of quadraturing are：
Wherein, x is stochastic variable, and g (x) is the function about x, xiIt is referred to as Gauss points, AiIt is referred to as Gauss coefficients, m is to set Fixed integral points；
Utilize formula：
It can be in the hope of m Gauss point and m Gauss coefficients Ai；It, can be by the desired value of g (x) using Gauss points and Gauss coefficients E (g (x)) is expressed as：
Wherein,
Assuming that x is nxRandom vector is tieed up, each component is mutual indepedent, and obeys N (0,1) Gaussian Profile, and each component is sought Each m i-th of Gauss coefficients a that can then obtain vector x of Gauss points and Gauss coefficientsiWith Gauss point vectors ξi：
Wherein,For the corresponding Gauss points of each component of vector x；It indicates by single component Gauss points structure At Gauss points vector, altogetherIt is a；aijFor Gauss points ξijCorresponding 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 formulakState vectors are tieed up for 10, and each component is independent, Gaussian distributed；zkFor 2 dimension observation vectors；ωk-1And νkPoint It Wei not process noise and measurement noise, satisfaction：
In formula, δijFor δ 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 P0：
Step 2.2.2：The method for updating time of SRQKF is as follows：
When being filtered for the first time to initial variance battle array P0Cholesky decomposition is carried out, the feature for obtaining initial error variance matrix is flat Root S0：
P0=S0S0 T；
Utilize the S at k-1 momentk-1|k-1Calculate Gauss integration point Xl,k-1|k-1S is then utilized if filtering for the first time0Calculate Gauss integration Point：
Calculate the Gauss integration point that nonlinear state function is propagated
Calculating state one-step prediction
Calculate the feature square root S of one-step prediction variance matrixk|k-1：
S in formulaQ,kIt is decomposed for the Cholesky of system noise variance matrix, i.e.,： It is expressed as：
The QR of qr () representing matrix is decomposed, Sk-1|k-1R gusts of upper triangular matrix in taking QR to decompose；
Step 2.2.3：The measurement update method of SRQKF is as follows：
Calculate one-step prediction state Gauss integration point Xl,k|k-1：
Calculate the Gauss integration point Z that non-linear measurement function is propagatedl,k|k-1：
Zl,k|k-1=h (Xl,k-1|k-1) l=1,2 ..., M.
Calculate one-step prediction measuring value
Calculate the feature square root S that one-step prediction measures variance matrixzz,k|k-1：
Szz,k|k-1=qr ([Zk|k-1 SR,k])
S in formulaR,kIt is decomposed for the Cholesky of observed quantity noise variance matrix, i.e.,：Zk|k-1It is expressed as：
Calculate one-step prediction covariance matrix Pxz,k|k-1：
X in formulak|k-1It is expressed as：
Calculate gain matrix K：
Calculating state updated value
Calculate the feature square root S of state variance battle arrayk|k：
Sk|k=qr ([Xk|k-1-KZk|k-1 KSR,k])。
3. a kind of underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF as described in claim 1, it is characterised in that 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：
Calculate the attitude matrix between n' systems and n systemsComputational 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 systemsCalculate essence True strapdown attitude matrixComputational methods are as follows：
Step 3.3：According to strapdown attitude matrixEulerian angles are calculated, computational methods are as follows：
Step 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 Gauss-Hermite Method, structure square root are quadratured Kalman filter SRQKF.
CN201510782989.0A 2015-11-16 2015-11-16 The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF Active CN105806363B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510782989.0A CN105806363B (en) 2015-11-16 2015-11-16 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) 2015-11-16 2015-11-16 The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF

Publications (2)

Publication Number Publication Date
CN105806363A CN105806363A (en) 2016-07-27
CN105806363B true CN105806363B (en) 2018-08-21

Family

ID=56465549

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510782989.0A Active CN105806363B (en) 2015-11-16 2015-11-16 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 (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106949906B (en) * 2017-03-09 2020-04-24 东南大学 Large misalignment angle rapid alignment method based on integral extended state observer
CN107063245B (en) * 2017-04-19 2020-12-25 东南大学 SINS/DVL combined navigation filtering method based on 5-order SSRCKF
CN110031882B (en) * 2018-08-02 2023-05-30 哈尔滨工程大学 External measurement information compensation method based on SINS/DVL integrated navigation system
CN109141475B (en) * 2018-09-14 2020-06-05 苏州大学 Initial alignment method for SINS robust traveling under assistance of DVL (dynamic velocity logging)
CN109443379B (en) * 2018-09-28 2020-07-21 东南大学 SINS/DV L underwater anti-shaking alignment method of deep-sea submersible vehicle
CN109612470A (en) * 2019-01-14 2019-04-12 广东工业大学 A kind of single station passive navigation method based on fuzzy volume Kalman filtering
CN110375773B (en) * 2019-07-29 2021-04-20 兰州交通大学 Attitude initialization method for MEMS inertial navigation system
CN110514203B (en) * 2019-08-30 2022-06-28 东南大学 Underwater integrated navigation method based on ISR-UKF
CN113670335B (en) * 2021-08-18 2023-10-24 河海大学 Underwater carrier initial alignment method based on DVL assistance and vector truncation K matrix
CN113959462B (en) * 2021-10-21 2023-09-12 北京机电工程研究所 Quaternion-based inertial navigation system self-alignment method

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101639365A (en) * 2009-07-22 2010-02-03 哈尔滨工程大学 Offshore alignment method of autonomous underwater vehicle based on second order interpolating filter
CN102654406A (en) * 2012-04-11 2012-09-05 哈尔滨工程大学 Initial alignment method for moving bases based on combination of nonlinear prediction filtering and cubature Kalman filtering
CN103344260A (en) * 2013-07-18 2013-10-09 哈尔滨工程大学 Initial alignment method of large azimuth misalignment angle of strapdown inertial navigation system based on RBCKF (rao-black-wellised cubature kalman filter)
CN103644913A (en) * 2013-12-25 2014-03-19 东南大学 Direct navigation model-based unscented Kalman nonlinear initial alignment method
CN104374405A (en) * 2014-11-06 2015-02-25 哈尔滨工程大学 MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering
CN104655131A (en) * 2015-02-06 2015-05-27 东南大学 Initial inertial navigation alignment method based on terated strong tracking spherical simplex radial cubature Kalman filter (ISTSSRCKF)

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
ITTO20130645A1 (en) * 2013-07-30 2015-01-31 St Microelectronics Srl METHOD AND CALIBRATION SYSTEM IN REAL TIME OF A GYROSCOPE

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101639365A (en) * 2009-07-22 2010-02-03 哈尔滨工程大学 Offshore alignment method of autonomous underwater vehicle based on second order interpolating filter
CN102654406A (en) * 2012-04-11 2012-09-05 哈尔滨工程大学 Initial alignment method for moving bases based on combination of nonlinear prediction filtering and cubature Kalman filtering
CN103344260A (en) * 2013-07-18 2013-10-09 哈尔滨工程大学 Initial alignment method of large azimuth misalignment angle of strapdown inertial navigation system based on RBCKF (rao-black-wellised cubature kalman filter)
CN103644913A (en) * 2013-12-25 2014-03-19 东南大学 Direct navigation model-based unscented Kalman nonlinear initial alignment method
CN104374405A (en) * 2014-11-06 2015-02-25 哈尔滨工程大学 MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering
CN104655131A (en) * 2015-02-06 2015-05-27 东南大学 Initial inertial navigation alignment method based on terated strong tracking spherical simplex radial cubature Kalman filter (ISTSSRCKF)

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"A New Nonlinear Filtering Method for Ballistic Target Tracking";Chunling Wu，等;《12th International Conference on Information Fusion》;20090709;2062-2067 *
"平方根求积分卡尔曼滤波器";巫春玲，等;《电子学报》;20090531;第37卷(第5期);987-992 *

Also Published As

Publication number Publication date
CN105806363A (en) 2016-07-27

Similar Documents

Publication Publication Date Title
CN105806363B (en) The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF
CN105737823B (en) A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF
CN104655131B (en) Inertial navigation Initial Alignment Method based on ISTSSRCKF
CN106871928B (en) Strap-down inertial navigation initial alignment method based on lie group filtering
CN101949703B (en) Strapdown inertial/satellite combined navigation filtering method
CN104344837B (en) Speed observation-based redundant inertial navigation system accelerometer system level calibration method
CN101706284B (en) Method for increasing position precision of optical fiber gyro strap-down inertial navigation system used by ship
CN109931955B (en) Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering
CN110398257A (en) The quick initial alignment on moving base method of SINS system of GPS auxiliary
CN106885570A (en) A kind of tight integration air navigation aid based on robust SCKF filtering
CN105865452B (en) A kind of mobile platform position and orientation estimation method based on indirect Kalman filtering
CN108318038A (en) A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method
CN103630137A (en) Correction method used for attitude and course angles of navigation system
CN104697526A (en) Strapdown inertial navitation system and control method for agricultural machines
CN109596144B (en) GNSS position-assisted SINS inter-travel initial alignment method
CN103575299A (en) Alignment and error correction method for double-axis rotational inertial navigation system based on appearance measurement information
CN101915579A (en) Novel CKF(Crankshaft Fluctuation Sensor)-based SINS (Ship Inertial Navigation System) large misalignment angle initially-aligning method
CN112595350B (en) Automatic calibration method and terminal for inertial navigation system
CN110954102B (en) Magnetometer-assisted inertial navigation system and method for robot positioning
CN106052716A (en) Method for calibrating gyro errors online based on star light information assistance in inertial system
CN102645223B (en) Serial inertial navigation vacuum filtering correction method based on specific force observation
CN106940193A (en) A kind of ship self adaptation based on Kalman filter waves scaling method
CN107063245A (en) A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF
CN108592943B (en) Inertial system coarse alignment calculation method based on OPREQ method
CN103674064B (en) Initial calibration method of strapdown inertial navigation system

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