CN105806363A - Alignment method of an underwater large misalignment angle based on SINS (Strapdown Inertial Navigation System)/DVL (Doppler Velocity Log) of SRQKF (Square-root Quadrature Kalman Filter) - Google Patents

Alignment method of an underwater large misalignment angle based on SINS (Strapdown Inertial Navigation System)/DVL (Doppler Velocity Log) of SRQKF (Square-root Quadrature Kalman Filter) Download PDF

Info

Publication number
CN105806363A
CN105806363A CN201510782989.0A CN201510782989A CN105806363A CN 105806363 A CN105806363 A CN 105806363A CN 201510782989 A CN201510782989 A CN 201510782989A CN 105806363 A CN105806363 A CN 105806363A
Authority
CN
China
Prior art keywords
gauss
srqkf
matrix
angle
sins
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.)
Granted
Application number
CN201510782989.0A
Other languages
Chinese (zh)
Other versions
CN105806363B (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

Abstract

The invention discloses an alignment method of an underwater large misalignment angle based on SINS (Strapdown Inertial Navigation System)/DVL (Doppler Velocity Log) of SRQKF (Square-root Quadrature Kalman Filter). The alignment method comprises the following steps: step 1: establishing a nonlinear error model and a nonlinear filtering equation of the SINS under the large misalignment angle; step 2: constructing the SRQKF by utilizing a multivariate Gauss point and coefficient configuration method and a square-root filtering method in Gauss-Hermite quadrature; and step 3: estimating the misalignment angle by utilizing the SRQKF, and correcting a strapdown attitude matrix, thus obtaining accurate strapdown attitude matrix and attitude angle. The alignment method disclosed by the invention has the advantage that the underwater alignment accuracy and alignment speed of the carrier strapdown system are improved.

Description

SINS/DVL based on SRQKF large misalignment angle alignment methods under water
Technical field
The present invention relates to strap-down inertial and integrated navigation technology field, a kind of SINS/ based on SRQKF DVL large misalignment angle alignment methods under water.
Background technology
Initial aligned relationship, to the operating accuracy of inertia system, is the key technology in strapdown inertial navigation system (SINS) One of.Owing to sea situation is complex, the hull of submarine navigation device is typically in large-amplitude sloshing state, so that strap-down inertial System cannot be rapidly completed autoregistration, is in large misalignment angle state for a long time.In this case it is necessary to it is auxiliary by external information Help, complete the initial alignment under moving base, large misalignment angle.Submarine navigation device generally completes this by Doppler log (DVL) One task.
But, large misalignment angle, vibrating state it is in all the time due to hull, conventional linearity error model cannot accurate description The error Propagation Property of SINS, needs to set up Nonlinear Error Models the most accurately, uses non-linear filtering method to complete water The initial alignment of lower aircraft.
In recent years, various non-linear filtering method EKFs (EKF), Unscented kalman filtering (UKF), Volume Kalman filtering (CKF) is continuously available application, and but, they the most all also exist some limitation, and EKF is in nonlinear characteristic Time stronger, precision is the highest；Although the precision of UKF with CKF increases, but at complex conditions such as large misalignment angles, equally cannot Obtain degree of precision, and convergence rate is slower.Therefore, invention has high accuracy, and can successfully manage large misalignment angle, moving base etc. The SINS/DVL alignment methods of complex environment is highly significant.
Summary of the invention
Goal of the invention: for overcoming the deficiencies in the prior art, the invention provides SINS/DVL water based on SRQKF Lower large misalignment angle alignment methods, it is characterised in that comprise the steps:
Step 1: set up strapdown inertial navigation system SINS Nonlinear Error Models under large misalignment angle and nonlinear filtering Wave equation；
Step 2: utilize Gauss-Hermite quadrature middle multivariate Gauss point and coefficient collocation method thereof and square Root filtering method, builds square root and quadratures Kalman filter SRQKF；
Step 3: utilize square root Kalman filtering SRQKF of quadraturing to estimate misalignment, and revise strapdown attitude matrix, Obtain accurate strapdown attitude matrix and attitude angle.
Further, described step 1: set up strapdown inertial navigation system SINS nonlinearity erron under large misalignment angle Model and Nonlinear Filtering Formulae, particularly as follows:
Step 1.1: choosing " sky, northeast " geographic coordinate system is navigational coordinate system n；Choosing carrier " on before the right side " coordinate system is Carrier coordinate system b；N system successively turns to b system through 3 Eulerian angles, and three Eulerian angles are designated as course angle ψ, pitch angle θ, rolling Angle γ；Attitude matrix between n system and b system is designated asTrue attitude angle is designated asTrue velocity is designated asThe attitude angle that SINS calculates is designated as:SINS computing speed is designated as:The mathematical platform that SINS calculates is designated as n ', and n ' is that the attitude matrix between b system is designated asIt is that three times angle of rotation is respectively φ that n system successively turns to n ' through 3 timesu、φe、φn, they are called Euler's platform error Angle, remembers vectorDefinition velocity error is:Then Nonlinear Error Models is as follows:
Wherein,The constant error of three axle gyros is descended for b system,For the lower three axle gyros of b system Random error,3-axis acceleration constant error is descended for b system,For the lower 3-axis acceleration of b system Random error,For the actual output of accelerometer,For the mathematical platform angular velocity of rotation calculated,Revolve for the earth Tarnsition velocity,For the mathematical platform that calculates relative to the angular velocity of rotation of the earth,For correspondenceCalculating error.Inverse matrix for Eulerian angles differential equation coefficient matrix；For n ' be with n system it Between attitude matrix, CωWithParticularly as follows:
Step 1.2: it is as follows that described Nonlinear Filtering Formulae sets up process:
Access speed errorEulerian angles platform error angle φu、φe、φn；Gyroscope constant value error under b systemAnd the accelerometer constant error under b systemComposition 10 dimension state vectors:
Under swaying baseBeing approximately zero, nonlinear filtering state equation can be established as:
WithOnly take front bidimensional state,WithFor zero-mean white-noise process.This nonlinear state equation can It is abbreviated as:Using sampling period T as the filtering cycle, it is possible to use quadravalence Runge-Kutta integration side Method, with T for step-length by its discretization, specifically comprises the following steps that
Assume the state vector chosen value x at initial time0It is known that and note t1=t+T/2, then can be changed by following Will for formulaDiscrete:
Δ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 sets up non-linear measurement equation as observed quantity, specifically comprises the following steps that
The east orientation speed exported using carrier S INS and DVL and north orientation velocity component as match information source, the non-thread of foundation Property measurement equation is:
Wherein, νbFor the true velocity under b system；For the actual speed under b system, the i.e. speed of DVL output；Take before z two Dimension is observation；ν is zero mean Gaussian white noise process, and this nonlinear filtering measurement equation can be abbreviated as: z=h (x, t)+ν (t)；
Equally using T as the filtering cycle, and carrying out simple discretization using T as step-length, by z=h, (x, t)+ν (t) is discrete Turn 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, described step 2: utilize Gauss-Hermite quadrature middle multivariate Gauss point and coefficient configuration Method and square root filtering method, build square root and quadrature Kalman filter SRQKF, particularly as follows:
Step 2.1:Gauss-Hermite quadrature middle multivariate Gauss point and coefficient collocation method as follows:
Gauss-Hermite formula of quadraturing is:
Wherein, x is stochastic variable, and g (x) is the function about x, xiIt is referred to as Gauss point, AiIt is referred to as Gauss coefficient, m It is that the integration set is counted.
Utilize formula:
Can be in the hope of m Gauss point xi(HmThe zero point of (x)) and m Gauss coefficient Ai；Utilize Gauss point and Gauss system Number, can be expressed as the expected value E (g (x)) of g (x):
Wherein,
Assume that x is nxDimension random vector, its each component is separate, and all obeys N (0,1) Gauss distribution, each component Ask for Gauss point and each m of Gauss coefficient the i-th Gauss coefficient a that then can obtain vector xiWith Gauss point vector ξi:
Wherein,For the Gauss point that each component of vector x is corresponding；Represent by single component Gauss The Gauss point vector that point is constituted, altogetherIndividual；aijFor Gauss point ξijCorresponding Gauss coefficient；
Step 2.2: the quadrature construction method of Kalman filter SRQKF of square root is as follows:
Nonlinear Filtering Formulae for setting up:
X in formulakIt is 10 dimension state vectors, and each component is independent, Gaussian distributed；zkIt is 2 dimension observation vectors；ωk-1With νkIt is respectively process noise and measurement noise, meets:
In formula, δijFor delta-function；Q is the variance matrix of system noise；R is the variance matrix of measurement noise.
SRQKF algorithm is as follows:
Step 2.2.1: init state variableAnd mean square deviation P0:
The method for updating time of step 2.2.2:SRQKF is as follows:
When being filtered first, (k=1) is to initial variance battle array P0Carry out Cholesky decomposition, obtain initial error variance The feature square root S of battle array0:
P0=S0S0 T；
Utilize the S in k-1 momentk-1|k-1Calculate Gauss integration point Xl,k-1|k-1If filtering first, utilize S0Calculate 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,kCholesky for system noise variance matrix decomposes, it may be assumed that It is expressed as:Qr () represents The QR of matrix decomposes, Sk-1|k-1Take the upper triangular matrix of R battle array during QR decomposes.
The measurement update method of step 2.2.3: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 one-step prediction and measure the feature square root S of variance matrixzz,k|k-1:
Szz,k|k-1=qr ([Zk|k-1 SR,k])
S in formulaR,kCholesky for observed quantity noise variance matrix decomposes, it may be assumed thatZk|k-1It is expressed as:
Calculate one-step prediction covariance matrix Pxz,k|k-1:
X in formulak|k-1It is expressed as:
Calculating 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) utilize square root Kalman filtering SRQKF of quadraturing to estimate misalignment, and revise strapdown Attitude matrix, obtains accurate strapdown attitude matrix and attitude angle, particularly as follows:
Step 3.1: after SRQKF wave filter completes once to filter, according to 10 dimension quantity of state estimated values of wave filter output:
Calculating n ' is the attitude matrix between n systemComputational methods are as follows:
Step 3.2: according to the attitude matrix between n' system and b systemAnd the attitude matrix between n' system and n system Calculate accurate strapdown attitude matrixComputational methods are as follows:
C b n = C b n ′ ( C n n ′ ) T
Step 3.3: according to strapdown attitude matrixCalculating Eulerian angles, computational methods are as follows:
3.4: if attitude accuracy reaches requirement, or the alignment time reaches setting value, and alignment terminates, the return that otherwise continues runs Step 2) utilize Gauss-Hermite to quadrature middle multivariate Gauss point and coefficient collocation method thereof and square root filtering side Method, builds square root and quadratures Kalman filter SRQKF.
Beneficial effect: the present invention has the advantage that for prior art
1. the present invention is directed to underwater carrier characteristic in the case of large misalignment angle, moving base, establish nonlinear filtering Equation, the SINS alignment precision under the conditions of using the method for SRQKF filtering to be effectively increased carrier underwater complex, and Reduce the alignment time, it is ensured that carrier SINS provides reliable attitude information.
2. the method for square root filtering is introduced in QKF filtering method by the present invention, constitutes SRQKF filtering method, it is to avoid every To state variance battle array P during secondary filteringkWith prediction variance matrix Pk|k-1Carry out Cholesky decomposition, be effectively increased filtering accuracy sum The stability that value calculates.
3. the present invention uses DVL to provide the speeds match information that precision is higher, is favorably improved the filtering accuracy of SRQKF And convergence rate, thus improve the accuracy and speed of alignment.
Accompanying drawing explanation
Fig. 1 is the system schema figure of the present invention.
Fig. 2 is present invention SINS/DVL based on SRQKF large misalignment angle alignment principles figure under water
Fig. 3 is the SRQKF filter flow figure of the present invention
Fig. 4 is that SINS/DVL of the present invention large misalignment angle under water is directed at pitch angle Error Graph.
Fig. 5 is that SINS/DVL of the present invention large misalignment angle under water is directed at roll angle Error Graph
Fig. 6 is the SINS/DVL of the present invention angle error figure of large misalignment angle heading orientation under water.
Detailed description of the invention
Below in conjunction with the accompanying drawings the present invention is further described.
As depicted in figs. 1 and 2, it is system schema figure and the alignment principles figure of the present invention.
SINS strapdown settlement module collects gyro output valve and the 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 output is input simultaneously in SRQKF wave filter, and carries out at the filtering of information Reason, filtering is as follows:
1., according to the characteristic in the case of SINS/DVL under water large misalignment angle, choose 10 state vectors:
Set up nonlinear state filtering equations:
Discretization postscript is: xk=f (xk-1)+ωk-1.
2. access speed amount is measuring value, sets up non-linear measurement filtering equations:
Discretization postscript is: zk=h (xk)+vk.
3. multivariate Gauss point and the configuration of coefficient thereof, concrete configuration method is as follows:
Utilize formula:
Try to achieve the Gauss point a that each component of vector x is correspondingijWith Gauss coefficient ξij。
Recycling formula:
Try to achieve the Gauss point a of vector xiWith Gauss coefficient ξi。
4.SRQKF state variableAnd mean square deviation P0Initialization procedure is as follows:
The method for updating time of 5.SRQKF is as follows:
When being filtered first, (k=1) is to variance matrix P0Carry out Cholesky decomposition, obtain initial error variance matrix Feature square root S0:
P0=S0S0 T
Utilize the S in k-1 momentk-1|k-1Calculate Gauss integration point Xl,k-1|k-1If filtering first, utilize S0Calculate 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 one-step prediction and measure the feature square root S of 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:
Calculating 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 wave filter is as shown in Figure 3.
7. carrying out the closed-loop corrected of state, obtain accurate strapdown attitude matrix and attitude angle, concrete grammar is as follows:
The 10 dimension quantity of state estimated values according to the output of SRQKF wave filter:
Calculating n ' is the attitude matrix between n system
It it is the attitude matrix between b system according to n 'And n ' is and attitude matrix between n systemCalculate accurate Strapdown attitude matrix
According to strapdown attitude matrixCalculating Eulerian angles, 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 performs step Rapid 2 with step 3, until alignment terminate.
Use following example explanation beneficial effects of the present invention:
1) submarine navigation device kinematic parameter is arranged:
Emulation initial time submarine navigation device is at north latitude 32 °, the 15m under water of east longitude 118 °；And rotating around pitch axis, horizontal stroke Rocker and course axle make three-axis swinging motion with SIN function, pitch angle θ, roll angle γ, course angle ψ, wave amplitude respectively For: 9 °, 12 °, 14 °, rolling period is respectively as follows: 16s, 20s, 12s, and angle, initial heading is 30 °, and initial misalignment is: Δ θ= 10 °, Δ γ=10 °, Δ ψ=20 °；This submarine navigation device, with the speed of 5m/s, is linear uniform motion, hours underway 600s.
2) parameter of sensor is arranged:
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, and accelerometer random error is: 50mg, DVL output speed error is: 0.1m/s.
3) parameter of SRQKF is arranged
The initial condition arranging wave filter 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 point of the single component of QKF state vector takes 3, i.e. m=3, and corresponding Gauss point and Gauss coefficient be:
ξ2=0,
4) analysis of simulation result:
SINS/DVL based on the SRQKF large misalignment angle alignment methods under water using the present invention to provide carries out DVL auxiliary SINS is directed under water, Fig. 4, and 5,6 is pitch angle error delta θ in alignment procedures of the present invention, roll angle error delta γ, and course angle The curve of error delta ψ, it appeared that: after submarine navigation device navigation 100s, SRQKF restrains substantially, now pitch angle alignment error About 0.002 °, roll angle alignment error about 0.003 °, course angle alignment error about 0.5 °；Found by analysis, after navigation 500s, Pitch angle alignment error 0.004 ° within, roll angle alignment error within 0.002 °, course angle alignment error 0.02 ° with In.
Result above shows, for exist large misalignment angle, swaying base and DVL speed output there is error in the case of, Use the SINS/DVL large misalignment angle alignment methods under water of SRQKF of the present invention, still can ensure that the highest alignment precision and Alignment speed quickly.
The preferred embodiment of the present invention described in detail above, but, the present invention is not limited in above-mentioned embodiment Detail, in the technology concept of the present invention, technical scheme can be carried out multiple equivalents, this A little equivalents belong to protection scope of the present invention.

Claims (4)

1. SINS/DVL based on a SRQKF large misalignment angle alignment methods under water, it is characterised in that comprise the steps:
Step 1: set up strapdown inertial navigation system SINS Nonlinear Error Models under large misalignment angle and nonlinear filtering side Journey；
Step 2: utilize Gauss-Hermite to quadrature middle multivariate Gauss point and coefficient collocation method thereof and square root filter Wave method, builds square root and quadratures Kalman filter SRQKF；
Step 3: utilize square root Kalman filtering SRQKF of quadraturing to estimate misalignment, and revise strapdown attitude matrix, obtain Accurate strapdown attitude matrix and attitude angle.
A kind of SINS/DVL based on SRQKF large misalignment angle alignment methods under water, it is characterised in that Described step 1: set up strapdown inertial navigation system SINS Nonlinear Error Models under large misalignment angle and nonlinear filtering side Journey, particularly as follows:
Step 1.1: choosing " sky, northeast " geographic coordinate system is navigational coordinate system n；Choosing carrier " on before the right side " coordinate system is carrier Coordinate system b；N system successively turns to b system through 3 Eulerian angles, and three Eulerian angles are designated as course angle ψ, pitch angle θ, roll angle γ； Attitude matrix between n system and b system is designated asTrue attitude angle is designated asTrue velocity is designated asThe attitude angle that SINS calculates is designated as:SINS computing speed is designated as:The mathematical platform that SINS calculates is designated as n ', and n ' is that the attitude matrix between b system is designated asIt is that three times angle of rotation is respectively φ that n system successively turns to n ' through 3 timesu、φe、φn, they are called Euler's platform error Angle, remembers vectorDefinition velocity error is:Then Nonlinear Error Models is as follows:
Wherein,The constant error of three axle gyros is descended for b system,For the lower three axle gyros of b system with chance error Difference,3-axis acceleration constant error is descended for b system,For the lower 3-axis acceleration of b system with chance error Difference,For the actual output of accelerometer,For the mathematical platform angular velocity of rotation calculated,For earth rotation angle speed Degree,For the mathematical platform that calculates relative to the angular velocity of rotation of the earth,For correspondenceCalculating error.Inverse matrix for Eulerian angles differential equation coefficient matrix；For n ' be with n system it Between attitude matrix, CωWithParticularly as follows:
Step 1.2: it is as follows that described Nonlinear Filtering Formulae sets up process:
Access speed errorEulerian angles platform error angle φu、φe、φn；Gyroscope constant value error under b systemAnd the accelerometer constant error under b systemComposition 10 dimension state vectors:
Under swaying baseBeing approximately zero, nonlinear filtering state equation can be established as:
WithOnly take front bidimensional state,WithFor zero-mean white-noise process.This nonlinear state equation can be abbreviated For:Using sampling period T as the filtering cycle, it is possible to use quadravalence Runge-Kutta integration method, with T For step-length by its discretization, specifically comprise the following steps that
Assume the state vector chosen value x at initial time0It is known that and note t1=t+T/2, then can be public by following iteration Formula willDiscrete:
Δ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 sets up non-linear measurement equation as observed quantity, specifically comprises the following steps that
The east orientation speed exported using carrier S INS and DVL and north orientation velocity component as match information source, the amount of nonlinearity of foundation Survey equation is:
Wherein, νbFor the true velocity under b system；For the actual speed under b system, the i.e. speed of DVL output；Before taking z, bidimensional is Observation；ν is zero mean Gaussian white noise process, and this nonlinear filtering measurement equation can be abbreviated as: z=h (x, t)+ν (t)；
Equally using T as the filtering cycle, and carry out simple discretization using T as step-length, by z=h (x, t)+ν (t) is discrete 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:
A kind of SINS/DVL based on SRQKF large misalignment angle alignment methods under water, it is characterised in that Described step 2: utilize Gauss-Hermite to quadrature middle multivariate Gauss point and coefficient collocation method thereof and square root filter Wave method, builds square root and quadratures Kalman filter SRQKF, particularly as follows:
Step 2.1:Gauss-Hermite quadrature middle multivariate Gauss point and coefficient collocation method as follows:
Gauss-Hermite formula of quadraturing is:
Wherein, x is stochastic variable, and g (x) is the function about x, xiIt is referred to as Gauss point, AiBeing referred to as Gauss coefficient, m is to set Fixed integration is counted.
Utilize formula:
Can be in the hope of m Gauss point xi(HmThe zero point of (x)) and m Gauss coefficient Ai；Utilize Gauss point and Gauss coefficient, The expected value E (g (x)) of g (x) can be expressed as:
Wherein,
Assume that x is nxDimension random vector, its each component is separate, and all obeys N (0,1) Gauss distribution, and each component is asked for Gauss point and each m of Gauss coefficient the i-th Gauss coefficient a that then can obtain vector xiWith Gauss point vector ξi:
Wherein,For the Gauss point that each component of vector x is corresponding；Represent by single component Gauss point structure The Gauss point vector become, altogetherIndividual；aijFor Gauss point ξijCorresponding Gauss coefficient；
Step 2.2: the quadrature construction method of Kalman filter SRQKF of square root is as follows:
Nonlinear Filtering Formulae for setting up:
X in formulakIt is 10 dimension state vectors, and each component is independent, Gaussian distributed；zkIt is 2 dimension observation vectors；ωk-1And νkPoint Not Wei process noise and measurement noise, meet:
In formula, δijFor delta-function；Q is the variance matrix of system noise；R is the variance matrix of measurement noise.
SRQKF algorithm is as follows:
Step 2.2.1: init state variableAnd mean square deviation P0:
The method for updating time of step 2.2.2:SRQKF is as follows:
When being filtered first, (k=1) is to initial variance battle array P0Carry out Cholesky decomposition, obtain initial error variance matrix Feature square root S0:
P0=S0S0 T；
Utilize the S in k-1 momentk-1|k-1Calculate Gauss integration point Xl,k-1|k-1If filtering first, utilize S0Calculate 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,kCholesky for system noise variance matrix decomposes, it may be assumed thatIt is expressed as:
The QR of qr () representing matrix decomposes, Sk-1|k-1Take the upper triangular matrix of R battle array during QR decomposes.
The measurement update method of step 2.2.3: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 one-step prediction and measure the feature square root S of variance matrixzz,k|k-1:
Szz,k|k-1=qr ([Zk|k-1 SR,k])
S in formulaR,kCholesky for observed quantity noise variance matrix decomposes, it may be assumed thatZk|k-1It is expressed as:
Calculate one-step prediction covariance matrix Pxz,k|k-1:
X in formulak|k-1It is expressed as:
Calculating 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])。
A kind of SINS/DVL based on SRQKF large misalignment angle alignment methods under water, it is characterised in that Step 3) utilize square root Kalman filtering SRQKF of quadraturing to estimate misalignment, and revise strapdown attitude matrix, obtain accurately Strapdown attitude matrix and attitude angle, particularly as follows:
Step 3.1: after SRQKF wave filter completes once to filter, according to 10 dimension quantity of state estimated values of wave filter output:
Calculating n ' is the attitude matrix between n systemComputational methods are as follows:
Step 3.2: according to the attitude matrix between n' system and b systemAnd the attitude matrix between n' system and n systemCalculate Accurate strapdown attitude matrixComputational methods are as follows:
Step 3.3: according to strapdown attitude matrixCalculating Eulerian angles, computational methods are as follows:
3.4: if attitude accuracy reaches requirement, or the alignment time reaches setting value, and alignment terminates, and otherwise continue return operating procedure 2) Gauss-Hermite is utilized to quadrature middle multivariate Gauss point and coefficient collocation method thereof and square root filtering method, structure Jianping root is 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 true CN105806363A (en) 2016-07-27
CN105806363B 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)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106949906A (en) * 2017-03-09 2017-07-14 东南大学 One kind is based on integral form extended state observer large misalignment angle rapid alignment method
CN107063245A (en) * 2017-04-19 2017-08-18 东南大学 A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF
CN109141475A (en) * 2018-09-14 2019-01-04 苏州大学 Initial Alignment Method between a kind of DVL auxiliary SINS robust is advanced
CN109443379A (en) * 2018-09-28 2019-03-08 东南大学 A kind of underwater anti-shake dynamic alignment methods of the SINS/DVL of deep-sea submariner device
CN109612470A (en) * 2019-01-14 2019-04-12 广东工业大学 A kind of single station passive navigation method based on fuzzy volume Kalman filtering
CN110031882A (en) * 2018-08-02 2019-07-19 哈尔滨工程大学 A kind of outer measurement information compensation method based on SINS/DVL integrated navigation system
CN110375773A (en) * 2019-07-29 2019-10-25 兰州交通大学 MEMS inertial navigation system posture initial method
CN110514203A (en) * 2019-08-30 2019-11-29 东南大学 A kind of underwater Combinated navigation method based on ISR-UKF
CN113670335A (en) * 2021-08-18 2021-11-19 河海大学 Initial alignment method of underwater carrier based on DVL (dynamic Voltage scaling) assistance and vector truncation K matrix
CN113959462A (en) * 2021-10-21 2022-01-21 北京机电工程研究所 Quaternion-based inertial navigation system self-alignment method

Citations (7)

* 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
US20150033821A1 (en) * 2013-07-30 2015-02-05 Stmicroelectronics S.R.L. Method and system for gyroscope real-time calibration
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)

Patent Citations (7)

* 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)
US20150033821A1 (en) * 2013-07-30 2015-02-05 Stmicroelectronics S.R.L. Method and system for gyroscope real-time calibration
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
CHUNLING WU，等: ""A New Nonlinear Filtering Method for Ballistic Target Tracking"", 《12TH INTERNATIONAL CONFERENCE ON INFORMATION FUSION》 *
巫春玲，等: ""平方根求积分卡尔曼滤波器"", 《电子学报》 *

Cited By (16)

* 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
CN106949906A (en) * 2017-03-09 2017-07-14 东南大学 One kind is based on integral form extended state observer large misalignment angle rapid alignment method
CN107063245A (en) * 2017-04-19 2017-08-18 东南大学 A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF
CN110031882A (en) * 2018-08-02 2019-07-19 哈尔滨工程大学 A kind of outer measurement information compensation method based on SINS/DVL integrated navigation system
CN109141475A (en) * 2018-09-14 2019-01-04 苏州大学 Initial Alignment Method between a kind of DVL auxiliary SINS robust is advanced
CN109141475B (en) * 2018-09-14 2020-06-05 苏州大学 Initial alignment method for SINS robust traveling under assistance of DVL (dynamic velocity logging)
CN109443379A (en) * 2018-09-28 2019-03-08 东南大学 A kind of underwater anti-shake dynamic alignment methods of the SINS/DVL of deep-sea submariner device
CN109443379B (en) * 2018-09-28 2020-07-21 东南大学 SINS/DV L underwater anti-shaking alignment method of deep-sea submersible vehicle
WO2020062791A1 (en) * 2018-09-28 2020-04-02 东南大学 Sins/dvl-based underwater anti-shaking alignment method for deep-sea underwater vehicle
CN109612470A (en) * 2019-01-14 2019-04-12 广东工业大学 A kind of single station passive navigation method based on fuzzy volume Kalman filtering
CN110375773A (en) * 2019-07-29 2019-10-25 兰州交通大学 MEMS inertial navigation system posture initial method
CN110514203A (en) * 2019-08-30 2019-11-29 东南大学 A kind of underwater Combinated navigation method based on ISR-UKF
CN113670335A (en) * 2021-08-18 2021-11-19 河海大学 Initial alignment method of underwater carrier based on DVL (dynamic Voltage scaling) assistance and vector truncation K matrix
CN113670335B (en) * 2021-08-18 2023-10-24 河海大学 Underwater carrier initial alignment method based on DVL assistance and vector truncation K matrix
CN113959462A (en) * 2021-10-21 2022-01-21 北京机电工程研究所 Quaternion-based inertial navigation system self-alignment method
CN113959462B (en) * 2021-10-21 2023-09-12 北京机电工程研究所 Quaternion-based inertial navigation system self-alignment method

Also Published As

Publication number Publication date
CN105806363B (en) 2018-08-21

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
CN103743414B (en) Initial Alignment Method between the traveling of vehicle-mounted SINS assisted by a kind of speedometer
CN109931955B (en) Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering
CN104374388B (en) Flight attitude determining method based on polarized light sensor
CN105698822B (en) Initial Alignment Method between autonomous type inertial navigation based on reversed Attitude Tracking is advanced
CN104697526A (en) Strapdown inertial navitation system and control method for agricultural machines
CN106885570A (en) A kind of tight integration air navigation aid based on robust SCKF filtering
CN103630137A (en) Correction method used for attitude and course angles of navigation system
CN103575299A (en) Alignment and error correction method for double-axis rotational inertial navigation system based on appearance measurement information
CN109596144B (en) GNSS position-assisted SINS inter-travel initial alignment method
CN104567930A (en) Transfer alignment method capable of estimating and compensating wing deflection deformation
CN111102993A (en) Initial alignment method for shaking base of rotary modulation type strapdown inertial navigation system
CN101915579A (en) Novel CKF(Crankshaft Fluctuation Sensor)-based SINS (Ship Inertial Navigation System) large misalignment angle initially-aligning method
CN103557864A (en) Initial alignment method for micro electro mechanical system (MEMS) strap-down inertial navigation adaptive square-root cubature Kalman filtering (SCKF)
CN103674064B (en) Initial calibration method of strapdown inertial navigation system
CN102645223B (en) Serial inertial navigation vacuum filtering correction method based on specific force observation
CN112798021B (en) Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter
CN106940193A (en) A kind of ship self adaptation based on Kalman filter waves scaling method
CN102116634A (en) Autonomous dimensionality reduction navigation method for deep sky object (DSO) landing detector
CN107063245A (en) A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF
CN108508463B (en) Fourier-Hermite orthogonal polynomial based extended ellipsoid collective filtering method

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