CN104655131B - Inertial navigation Initial Alignment Method based on ISTSSRCKF - Google Patents

Inertial navigation Initial Alignment Method based on ISTSSRCKF Download PDF

Info

Publication number
CN104655131B
CN104655131B CN201510063634.6A CN201510063634A CN104655131B CN 104655131 B CN104655131 B CN 104655131B CN 201510063634 A CN201510063634 A CN 201510063634A CN 104655131 B CN104655131 B CN 104655131B
Authority
CN
China
Prior art keywords
error
state
matrix
sins
equation
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
CN201510063634.6A
Other languages
Chinese (zh)
Other versions
CN104655131A (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 CN201510063634.6A priority Critical patent/CN104655131B/en
Publication of CN104655131A publication Critical patent/CN104655131A/en
Application granted granted Critical
Publication of CN104655131B publication Critical patent/CN104655131B/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

This document statement is a kind of to be based on iteration strong tracking sphere most simple phase footpath volume Kalman filtering (Iterated Strong Tracking Spherical Simplex Radial Cubature Kalman Filter, ISTSSRCKF Initial Alignment Method), CKF volume point is chosen using SSR rules；Fading factor in Strong tracking filter was incorporated among CKF time and measurement renewal equation；Gauss Newton iterative algorithms are introduced into again and corresponding new breath variance and covariance in iterative process is improved.For the Initial Alignment under the conditions of large misalignment angle and swaying base, the present invention reduces sampled point excessive in high-order CKF using SSR rule samplings, introduce strong tracking algorithm overcome model it is inaccurate when CKF hydraulic performance declines the problem of, newest measurement information is made full use of using iterative process, so as to be effectively reduced state estimation error, the initial alignment precision better than standard CKF is obtained.

Description

Inertial navigation Initial Alignment Method based on ISTSSRCKF
Technical field
The invention mainly relates to field of navigation technology, more particularly to a kind of inertial navigation based on ISTSSRCKF are initially right Quasi- method.
Background technology
Inertial navigation system first will be initially aligned before entering working condition, and SINS is by inertia sensing Device --- gyroscope and accelerometer are directly connected firmly with carrier, obtain calculating platform by computing to replace physical platform, therefore The Initial Alignment of SINS is just to solve for the attitude matrix of initial time.Initially alignment directly affects strapdown to SINS The navigation performance of system, i.e. navigation accuracy and reaction time.Autonomous underwater vehicle (AUV) be one kind combine artificial intelligence and The task controller of other advanced computing techniques, will make submarine navigation device complete predetermined mission, be unable to do without underwater navigation technology, It is the bottleneck problem for determining the development of submarine navigation device technology and application.Under actual application environment, particularly in large misalignment angle Under the conditions of acutely rocking, it is impossible to use the classical kalman filter method using linear small misalignment as Alignment model to be filtered Ripple, can only set up non-linear alignment model, and the nonlinear filtering algorithm such as traditional EKF, UKF exists essence is directed under dimensional state Relatively low and numerical value instability problem, and the shortcoming of reply uncertain factor ability are spent, therefore invention successfully manages complicated ring Alignment methods under border, the SINS Nonlinear Sloshing pedestals with certain strong tracking ability have great importance.
The content of the invention
Goal of the invention：It is not enough present in existing non-linear alignment technology in order to overcome, improved the invention provides one kind The inertial navigation Initial Alignment Method based on ISTSSRCKF of SINS alignment precisions.
Technical scheme：In order to solve the above technical problems, the invention provides at the beginning of a kind of inertial navigation based on ISTSSRCKF Beginning alignment methods, its step includes as follows：
Step 1：The SINS swaying base Alignment models set up under large misalignment angle state, the Alignment model includes SINS's Nonlinear Error Models, nonlinear filtering state model and nonlinear filtering measurement model：
It is wherein, described that to set up SINS Nonlinear Error Models processes as follows：
Step 1.1：Coordinate system is managed as navigational coordinate system n using the northeast world, it is fixed to the vector right hand with carrier front upper right Coordinate system is then constituted as carrier coordinate system b, true attitude angle isTrue velocity is True geographical coordinate is p=[L λ H]T；The attitude that SINS is actual to be calculated isSpeed isGeographical coordinate isRemember that the coordinate system that the geographical position resolved by SINS is set up is Navigational coordinate system n ' is calculated, SINS attitude angles is defined and velocity error is respectivelyThen φ, δ vn The differential equation it is as follows：
Wherein, φ=[φeφnφu]TFor attitude error, the attitude error be specially pitch angle, roll angle and Course angle error, δ vn=[δ veδvnδvu]TFor east orientation, north orientation and sky orientation speed error,For lower three axles of b systems The constant error of gyro,The random error of three axis accelerometer is descended for b systems,For the lower three axis accelerometer of b systems Constant error,The random error of three axis accelerometer is descended for b systems,For the reality output of accelerometer,Solved for SINS The speed of calculation,For the angular velocity of rotation under the navigational coordinate system of calculating,For the earth rotation angular speed of calculating,For meter The navigational coordinate system of calculation with respect to the earth angular velocity of rotation,For correspondenceCalculating miss Difference,For n systems successively anglec of rotation φe、φn、φuIt is formed direction cosine matrix to obtain n ',It is shape to be tied to n ' for b State transfer matrix, that is, the attitude matrix calculated, CwFor the coefficient matrix of the Eulerian angles differential equation, subscript T represents transposition, matrix And CwSpecially：
It is as follows that the nonlinear filtering state model sets up process：
Step 1.2：Take SINS Euler's platform error angle φe、φn、φu, velocity error δ ve、δvn, the gyroscope constant value under b systems ErrorAccelerometer constant error under b systemsConstitute state vector Under swaying baseWithIt is approximately zero, then the state equation of nonlinear filtering can be reduced to：
Wherein,Only take preceding bidimensional state, ω (t)=[ωg ωa 01×3 01×2]TFor zero-mean gaussian white noise Sound process, the nonlinear filtering state equation is abbreviated as：
The foundation of the non-linear measurement equation is as follows：
Step 1.3：It is v to remember the true velocity under b systemsb, the actual speed under b systems isThe appearance resolved using SINS State matrix handleIt is converted intoWithWithIn east orientation and north orientation speed component as match information source, then nonlinear filtering Measurement equation is：
Wherein, the preceding bidimensional for taking z is observation, and v is zero mean Gaussian white noise process, and the Continuous Nonlinear is filtered Measurement equation is abbreviated as：
Z (t)=h (x, t)+v (t)
Step 2：With sampling period TsAs filtering cycle, and with TsDiscretization is carried out for step-length, willIt is discrete to turn to xk=xk-1+[f(xk-1,tk-1)+ω(tk-1)]Ts, state equation can be obtained：
xk=f (xk-1)+ωk-1
Z is turned to by z (t)=h (x, t)+v (t) are discretek=h (xk,tk)+v(tk), measurement equation can be obtained：
zk=h (xk)+vk
Step 2.1：Following Discrete-time Nonlinear Systems are constituted by equation of state and measurement equation formula：
In formula, xkThe state vector of etching system during for k；zkFor the measuring value of k moment system modes；Stochastic system noise ωk ~N (0, Qk)；Random observation noise vk~N (0, Rk)；
Step 2.2：System mode initial value is setInitial error covariance matrix：
P0=diag { (1 °)2(1°)2(10°)2(0.1)2(0.1)2
(0.01°/h)2(0.01°/h)2(0.01°/h)2(100μg)2(100μg)2}*10
And to P0Cholesky decomposition is carried out, the feature square root S of initial error covariance matrix is obtained0；
Step 2.3：Sampled point is chosen using SSR rules, taken such as next group of vector：aj=[aj,1 aj,2 … aj,n]T, j= 1,2 ..., n+1, wherein n are the number of quantity of state, then
From ξiI-th of volume point is represented, the individual volume points of m=2 (n+1) are：
N is the number of quantity of state, n=10 herein in formula；It is abbreviated non-linear multidimensional integralFor Q (f), under SSR rules
Step 2.4：Calculating state one-step prediction valueWith one-step prediction error covariance matrix Pk/k-1：
Pass through Cholesky resolution error covariance matrixs Pk-1/k-1Obtain Sk-1/k-1：
Pk-1/k-1=Sk-1/k-1ST k-1/k-1
Calculating Cubature points (i=1,2 ..., n+1；M=2 (n+1))：
Cubature points are propagated by state equation：
X* i,k/k-1=f (Xi,k-1/k-1)
State one-step prediction value：
One-step prediction error covariance matrix：
Step 3：Utilize current measuring value zkSubtract measurement predictor mutually in the same timeObtain the residual error at current time εk, suboptimum fading factor λ is calculated according to strong tracking principlek, then utilize λkCorrect filtering time renewal process；
Step 4：With state one-step prediction valueWith one-step prediction error covariance matrix Pk/k-1It is iterated for initial value Process, carries out maximum iteration NmaxSecondary iteration, completes to measure renewal；
N thereinmaxFor the number of times of iteration, it can as needed choose, 10 are taken as herein.
Step 5：The Euler's mesa corners estimate obtained using filter filteringWith velocity estimation valueCorrect SINS solutions The attitude matrix of calculationAnd speedThe initial value that value after amendment is resolved as strapdown next time, utilizes current acquisition Gyroscope constant value error estimateWith the constant error estimate of accelerometerCorrect the gyro output of subsequent timeWith The output of accelerometerSpecific correction formula is calculated as the following formula：
If precision reach it is default it is initial to alignment request if terminate, execution step 2~5 are otherwise continued cycling through, until initial Alignment terminates.
Further, in the step 3：
It is described to calculate suboptimum fading factor λ according to strong tracking principlekCalculating process is as follows：
Utilize current measuring value zkSubtract measurement predictor mutually in the same timeThe residual error for obtaining current time is：
Take the covariance matrix of reality output sequence:
ρ is forgetting factor in formula, and span is generally 0.95≤ρ≤0.98；
Calculating matrix:
Mk=Pzz,k/k-1+Nk-Vk
P in formulazz,k/k-1For auto-correlation covariance matrix, Pxz,k/k-1For cross-correlation covariance matrix.
Calculate suboptimum fading factor λk：
The mark of tr [ ] representing matrix in formula.
Further, the iteration CKF algorithms in the step 4：
Step 4.1.1：Using various in step 2.4, state one-step prediction value is calculatedWith one-step prediction error association side Poor battle array Pk/k-1；
Step 4.1.2：Calculate fading factor λk, pass through Cholesky decomposed Psk/k-1：
Pk/k-1=Sk/k-1ST k/k-1
Calculating Cubature points (i=1,2 ..., n+1；M=2 (n+1))：
Cubature points are propagated by observational equation：
Zi,k/k-1=h (Xi,k/k-1)
Estimate the observation predicted value at k moment：
Estimate auto-correlation covariance matrix：
Estimate cross-correlation covariance matrix：
Fading factor λ is calculated according to step 3 is variousk；
Step 4.1.3：Utilize the fading factor λ obtainedk, one-step prediction error covariance matrix Pk/k-1Carry out such as down conversion：
Step 4.2.1：Measure renewal be withWithFor the iterative process of initial value.Remember the estimation of ith iteration Value and variance areWith
Step 4.2.2：New factorization and volume point is produced, the Cholesky of chol () representing matrix is decomposed：
Wherein ξjRepresent j-th of volume point；
Step 4.2.3：With the P in step 4.1.3k/k-1Recalculate step 4.1.2 various, P can be obtained successivelyzz,k/k-1、 Pxz,k/k-1。
Calculate the state and variance evaluation of ith iteration：
Step 4.2.4：If iterations is N during iteration ends, the state estimation and covariance at k moment are：
The invention discloses a kind of Initial Alignment Method based on ISTSSRCKF, CKF volume is chosen using SSR rules Point；Fading factor in Strong tracking filter was incorporated among CKF time and measurement renewal equation；Again by Gauss-Newton Iterative algorithm is introduced into and improves corresponding new breath variance and covariance in iterative process.For in large misalignment angle and swaying base bar Initial Alignment under part, the present invention reduces sampled point excessive in high-order CKF using SSR rule samplings, introduce it is strong with Track algorithm overcome model it is inaccurate when CKF hydraulic performance declines the problem of, make full use of newest measurement information using iterative process, from And state estimation error is effectively reduced, obtain the initial alignment precision better than standard CKF.
Beneficial effect：The present invention has advantages below relative to prior art：
1) the problem of CKF alignment precisions under the conditions of swaying base and large misalignment angle decline is solved, is SINS Precision navigation positioning provide high-precision initial state information.
2) sampled point is chosen using SSR rules, overcomes the problem of high-order CKF sampled points are excessive, not only reduce computing Complexity, and improve computational accuracy.
3) CKF degradation problems when in order to overcome model inaccurate, the fading factor in strong tracking algorithm is incorporated into Among CKF time and measurement renewal equation.Gauss-Newton iterative algorithms are introduced, are made full use of in an iterative process newest Measurement information simultaneously improves the new breath variance and covariance produced in iterative process, can be effectively reduced state estimation error, so that Obtain the alignment precision better than standard CKF.
Brief description of the drawings
Fig. 1 is the installation diagram of three axis accelerometer of the present invention and three axis accelerometer.
Fig. 2 is SINS swaying bases alignment scheme figure of the present invention.
Fig. 3 is the Initial Alignment Method schematic diagram of the invention based on ISTSSRCKF.
Fig. 4 waves simulation drawing for pitch angle, roll angle and course angle under swaying base of the present invention.
Fig. 5 is the simulation drawing that SINS three-axis gyroscopes of the present invention are exported.
Fig. 6 is the simulation drawing that SINS three axis accelerometers of the present invention are exported.
Fig. 7 is initial alignment pitch angle Error Graph under SINS swaying bases of the present invention.
Fig. 8 is initial alignment roll angle Error Graph under SINS swaying bases of the present invention.
Fig. 9 is initial heading orientation angle error figure under SINS swaying bases of the present invention.
Embodiment
The present invention is further described below in conjunction with the accompanying drawings.
Step 1：The SINS swaying base Alignment models set up under large misalignment angle state, the Alignment model includes SINS's Nonlinear Error Models, nonlinear filtering state model and nonlinear filtering measurement model：
It is wherein, affiliated that to set up SINS Nonlinear Error Models processes as follows：
Step 1.1：Coordinate system is managed as navigational coordinate system n using the northeast world, it is fixed to the vector right hand with carrier front upper right Coordinate system is then constituted as carrier coordinate system b, true attitude angle isTrue velocity is True geographical coordinate is p=[L λ H]T；The attitude that SINS is actual to be calculated isSpeed isGeographical coordinate isRemember that the coordinate system that the geographical position resolved by SINS is set up is Navigational coordinate system n ' is calculated, SINS attitude angles is defined and velocity error is respectivelyThen φ, δ νn The differential equation it is as follows：
Wherein, φ=[φeφnφu]TFor attitude error, the attitude error be specially pitch angle, roll angle and Course angle error, δ vn=[δ veδvnδvu]TFor east orientation, north orientation and sky orientation speed error,For lower three axles of b systems The constant error of gyro,The random error of three axis accelerometer is descended for b systems,For the lower three axis accelerometer of b systems Constant error,The random error of three axis accelerometer is descended for b systems,For the reality output of accelerometer,Solved for SINS The speed of calculation,For the angular velocity of rotation under the navigational coordinate system of calculating,For the earth rotation angular speed of calculating,For meter The navigational coordinate system of calculation with respect to the earth angular velocity of rotation,For correspondenceCalculating miss Difference,For n systems successively anglec of rotation φe、φn、φuIt is formed direction cosine matrix to obtain n ',It is shape to be tied to n ' for b State transfer matrix, that is, the attitude matrix calculated, CwFor the coefficient matrix of the Eulerian angles differential equation, subscript T represents transposition, matrix And CwSpecially：
It is as follows that the nonlinear filtering state model sets up process：
Step 1.2：Take SINS Euler's platform error angle φe、φn、φu, velocity error δ ve、δvn, the gyroscope constant value under b systems ErrorAccelerometer constant error under b systemsConstitute state vector Under swaying baseWithIt is approximately zero, then the state equation of nonlinear filtering can be reduced to：
Wherein,Only take preceding bidimensional state, ω (t)=[ωg ωa 01×3 01×2]TFor zero-mean gaussian white noise Sound process, the nonlinear filtering state equation is abbreviated as：
The foundation of the non-linear measurement equation is as follows：
Step 1.3：It is v to remember the true velocity under b systemsb, the actual speed under b systems isThe appearance resolved using SINS State matrix handleIt is converted intoWithWithIn east orientation and north orientation speed component as match information source, then nonlinear filtering Measurement equation be：
Wherein, the preceding bidimensional for taking z is observation, and v is zero mean Gaussian white noise process, and the Continuous Nonlinear is filtered Measurement equation is abbreviated as：
Z (t)=h (x, t)+v (t)
Step 2：With sampling period TsAs filtering cycle, and with TsDiscretization is carried out for step-length, willIt is discrete to turn to xk=xk-1+[f(xk-1,tk-1)+ωtk-1)]Ts, state equation can be obtained：
xk=f (xk-1)+ωk-1
Measurement equation z (t)=h (x, t)+v (t) are discrete to turn to zk=h (xk,tk)+v(tk), measurement equation can be obtained：
zk=h (xk)+vk
Step 2.1：Following Discrete-time Nonlinear Systems are constituted by equation of state and measurement equation formula：
In formula, xkThe state vector of etching system during for k；zkFor the measuring value of k moment system modes；Stochastic system noise ωk ~N (0, Qk)；Random observation noise vk~N (0, Rk)。
Step 2.2：System mode initial value is setInitial error covariance matrix：
P0=diag { (1 °)2(1°)2(10°)2(0.1)2(0.1)2
(0.01°/h)2(0.01°/h)2(0.01°/h)2(100μg)2(100μg)2}*10
And to P0Cholesky decomposition is carried out, the feature square root S of initial error covariance matrix is obtained0；
Step 2.3：Sampled point is taken using SSR rules herein, taken such as next group of vector：aj=[aj,1 aj,2 … aj,n]T,j =1,2 ..., n+1, wherein n are the number of quantity of state, then
From ξiI-th of volume point is represented, the individual volume points of m=2 (n+1) are：
N is the number of quantity of state, n=10 herein in formula；It is abbreviated non-linear multidimensional integralFor Q (f), under SSR rules
Step 2.4：Calculating state one-step prediction valueWith one-step prediction error covariance matrix Pk/k-1：Pass through Cholesky Resolution error covariance matrix Pk-1/k-1Obtain Sk-1/k-1：
Pk-1/k-1=Sk-1/k-1ST k-1/k-1
Calculating Cubature points (i=1,2 ..., n+1；M=2 (n+1))：
Cubature points are propagated by state equation：
State one-step prediction value：
One-step prediction error covariance matrix：
Step 3：Utilize current measuring value zkSubtract measurement predictor mutually in the same timeObtain the residual error at current time For：
Take the covariance matrix of reality output sequence:
ρ is forgetting factor in formula, and span is generally 0.95≤ρ≤0.98；
Calculating matrix:
Mk=Pzz,k/k-1+Nk-Vk
P in formulazz,k/k-1For auto-correlation covariance matrix, Pxz,k/k-1For cross-correlation covariance matrix.
Calculate suboptimum fading factor λk：
The mark of tr [ ] representing matrix in formula.
Step 4：With state one-step prediction valueWith one-step prediction error covariance matrix Pk/k-1It is iterated for initial value Process, carries out NmaxSecondary iteration, completes to measure renewal；
The iteration CKF algorithmic procedures are as follows：
Step 4.1.1：Using various in step 2.4, state one-step prediction value is calculatedWith one-step prediction error association side Poor battle array Pk/k-1。
Step 4.1.2：Calculate fading factor λk, pass through Cholesky decomposed Psk/k-1：
Pk/k-1=Sk/k-1ST k/k-1
Calculating Cubature points (i=1,2 ..., n+1；M=2 (n+1))：
Cubature points are propagated by observational equation：
Zi,k/k-1=h (Xi,k/k-1)
Estimate the observation predicted value at k moment：
Estimate auto-correlation covariance matrix：
Estimate cross-correlation covariance matrix：
Fading factor λ is calculated according to step 3 is variousk。
Step 4.1.3：Utilize the fading factor λ obtainedk, one-step prediction error covariance matrix Pk/k-1Carry out such as down conversion：
Step 4.2.1：Measure renewal be withAnd Pk/k-1For the iterative process of initial value.Remember the estimation of ith iteration Value and variance areWith
Step 4.2.2：New factorization and volume point is produced, the Cholesky of chol () representing matrix is decomposed：
Wherein ξjRepresent j-th of volume point.
Step 4.2.3：With the P in step 4.1.3k/k-1Recalculate step 4.1.2 various, P can be obtained successivelyzz,k/k-1、 Pxz,k/k-1。
Calculate the state and variance evaluation of ith iteration：
Step 4.2.4：If iterations is N during iteration ends, the state estimation and covariance at k moment are：
Step 5：Utilize the Euler's mesa corners estimate currently obtainedWith velocity estimation valueCorrect the appearance that SINS is resolved State matrixAnd speedThe initial value that value after amendment is resolved as strapdown next time, utilizes the gyro currently obtained Constant error estimateWith the constant error estimate of accelerometerCorrect the gyro output of subsequent timeAnd acceleration The output of meterSpecific correction formula is calculated as the following formula：
If precision reach it is initial to alignment request if terminate, execution step 2~5 are otherwise continued cycling through, until initial alignment knot Beam.
Three gyros and three accelerometers are orthogonal is fixedly mounted on inside AUV as shown in figure 1, Fig. 2 is SINS of the present invention Swaying base alignment scheme schematic diagram, Fig. 3 is the Initial Alignment Method schematic diagram of the invention based on ISTSSRCKF.
It is described below to be directed to common submarine navigation device.
Illustrate the actual effect of the present invention using following simulation example：
1) Ship Motion situation and parameter setting
The initial time of emulation at 32 ° of north latitude, the 20m under water of 118 ° of east longitude, under water in the presence of environment rotating around Pitch axis, axis of roll and course axle make the motion of three axle sinusoidal wobbles, pitch angle θ, roll angle γ and course angle ψ wave amplitude according to Secondary is 10 °, 8 °, 6 °, and corresponding rolling period is 6s, 8s, 10s, and the curve that above parameter is simulated is as shown in Figure 4.
2) parameter setting of gyro and accelerometer
Inertial navigation generally uses optical fibre gyro and flexure accelerometers, and the constant value drift of gyro is 0.02 °/h, gyro Random drift is 0.01 °/h, and the constant value of accelerometer is biased to 100 × 10-6G, the random error of accelerometer is 50 × 10-6g (g is acceleration of gravity), simulation three axis accelerometer output ωx、ωy、ωzAs shown in figure 5, three axis accelerometer exports fx、fy、fzSuch as Shown in Fig. 6.
3) analysis of simulation result
Initial misalignment is set to 10 °, 10 °, 10 °, and carrying out SINS using nonlinear filtering algorithm proposed by the present invention rocks Pedestal is initially aligned, the pitching angle error φ after the completion of alignmentx, rolling angle error φyWith course angle error φzRespectively as Fig. 7, 8th, shown in 9.Simulation result shows, in swaying base and under the conditions of there is complex water areas in the case of large misalignment angle, making Have higher alignment precision with nonlinear filtering technique proposed by the present invention, the navigator fix that can be met it is initial to alignment request.
The above is only the preferred embodiment of the present invention, it should be pointed out that：For the ordinary skill people of the art For member, under the premise without departing from the principles of the invention, some improvements and modifications can also be made, these improvements and modifications also should It is considered as protection scope of the present invention.

Claims (3)

1. a kind of inertial navigation Initial Alignment Method based on ISTSSRCKF, it is characterised in that comprise the following steps：
Step 1：The SINS swaying base Alignment models set up under large misalignment angle state, the Alignment model includes SINS non-thread Property error model, nonlinear filtering state model and nonlinear filtering measurement model：
It is wherein, described that to set up SINS Nonlinear Error Models processes as follows：
Step 1.1：Coordinate system is managed as navigational coordinate system n using the northeast world, with carrier front upper right to vector right-hand rule structure Into coordinate system as carrier coordinate system b, true attitude angle isTrue velocity isVery Real geographical coordinate is p=[L λ H]T；The attitude that SINS is actual to be calculated isSpeed isGeographical coordinate isRemember that the coordinate system that the geographical position resolved by SINS is set up is Navigational coordinate system n ' is calculated, SINS attitude angles is defined and velocity error is respectivelyThen φ, δ νn The differential equation it is as follows：
Wherein, φ=[φe φn φu]TFor attitude error, the attitude error is specially pitch angle, roll angle and course Angle error, δ vn=[δ ve δvn δvu]TFor east orientation, north orientation and sky orientation speed error,For lower three axles of b systems The constant error of gyro,The random error of three axis accelerometer is descended for b systems,For the lower three axis accelerometer of b systems Constant error,The random error of three axis accelerometer is descended for b systems,For the reality output of accelerometer,Solved for SINS The speed of calculation,For the angular velocity of rotation under the navigational coordinate system of calculating,For the earth rotation angular speed of calculating,For meter The navigational coordinate system of calculation with respect to the earth angular velocity of rotation,For correspondenceCalculating miss Difference,For n systems successively anglec of rotation φe、φn、φuIt is formed direction cosine matrix to obtain n ',It is shape to be tied to n ' for b State transfer matrix, that is, the attitude matrix calculated, CwFor the coefficient matrix of the Eulerian angles differential equation, subscript T represents transposition, matrix And CwSpecially：
It is as follows that the nonlinear filtering state model sets up process：
Step 1.2：Take SINS Euler's platform error angle φe、φn、φu, velocity error δ ve、δvn, the gyroscope constant value error under b systemsAccelerometer constant error under b systemsConstitute state vector Under swaying baseWithIt is approximately zero, then the state equation of nonlinear filtering can be reduced to：
Wherein,Only take preceding bidimensional state, ω (t)=[ωg ωa 01×3 01×2]TFor zero mean Gaussian white noise mistake Journey, the nonlinear filtering state equation is abbreviated as：
The foundation of the non-linear measurement equation is as follows：
Step 1.3：It is v to remember the true velocity under b systemsb, the actual speed under b systems isThe attitude square resolved using SINS Battle array handleIt is converted intoWithWithIn east orientation and north orientation speed component as match information source, then nonlinear filtering measurement Equation is：
Wherein, the preceding bidimensional for taking z is observation, and v is zero mean Gaussian white noise process, and the Continuous Nonlinear is filtered into measurement Equation is abbreviated as：
Z (t)=h (x, t)+v (t)
Step 2：With sampling period TsAs filtering cycle, and with TsDiscretization is carried out for step-length, will It is discrete to turn to xk=xk-1+[f(xk-1,tk-1)+ω(tk-1)]Ts, state equation can be obtained：
xk=f (xk-1)+ωk-1
Z is turned to by z (t)=h (x, t)+v (t) are discretek=h (xk,tk)+v(tk), measurement equation can be obtained：
zk=h (xk)+vk
Step 2.1：Following Discrete-time Nonlinear Systems are constituted by equation of state and measurement equation formula：
In formula, xkThe state vector of etching system during for k；zkFor the measuring value of k moment system modes；Stochastic system noise ωk~N (0,Qk)；Random observation noise vk~N (0, Rk)；
Step 2.2：System mode initial value is setInitial error covariance matrix：
P0=diag { (1 °)2(1°)2(10°)2(0.1)2(0.1)2
(0.01°/h)2(0.01°/h)2(0.01°/h)2(100μg)2(100μg)2}*10
And to P0Cholesky decomposition is carried out, the feature square root S of initial error covariance matrix is obtained0；
Step 2.3：Sampled point is chosen using SSR rules, taken such as next group of vector：aj=[aj,1 aj,2 … aj,n]T, j=1, 2 ..., n+1, wherein n are the number of quantity of state, then
From ξiI-th of volume point is represented, the individual volume points of m=2 (n+1) are：
N is the number of quantity of state, n=10 herein in formula；It is abbreviated non-linear multidimensional integralFor Q (f), under SSR rules
Step 2.4：Calculating state one-step prediction valueWith one-step prediction error covariance matrix Pk/k-1：Decomposed by Cholesky Error covariance matrix Pk-1/k-1Obtain Sk-1/k-1：
Pk-1/k-1=Sk-1/k-1ST k-1/k-1
Calculating Cubature points (i=1,2 ..., n+1；M=2 (n+1))：
Cubature points are propagated by state equation：
X* i,k/k-1=f (Xi,k-1/k-1)
State one-step prediction value：
One-step prediction error covariance matrix：
Step 3：Utilize current measuring value zkSubtract measurement predictor mutually in the same timeObtain the residual epsilon at current timek, foundation Strong tracking principle calculates suboptimum fading factor λk, then utilize λkCorrect filtering time renewal process；
Step 4：With state one-step prediction valueWith one-step prediction error covariance matrix Pk/k-1Process is iterated for initial value, Carry out maximum iteration NmaxSecondary iteration, completes to measure renewal；
Step 5：The Euler's mesa corners estimate obtained using filter filteringWith velocity estimation valueCorrect what SINS was resolved Attitude matrixAnd speedThe initial value that value after amendment is resolved as strapdown next time, utilizes the top currently obtained Spiral shell constant error estimateWith the constant error estimate of accelerometerCorrect the gyro output of subsequent timeAnd acceleration Spend the output of meterSpecific correction formula is calculated as the following formula：
If precision reach it is default it is initial to alignment request if terminate, execution step 2~5 are otherwise continued cycling through, until initial alignment Terminate.
2. the inertial navigation Initial Alignment Method according to claim 1 based on ISTSSRCKF, it is characterised in that：It is described In step 3：
It is described to calculate suboptimum fading factor λ according to strong tracking principlekCalculating process is as follows：
Utilize current measuring value zkSubtract measurement predictor mutually in the same timeThe residual error for obtaining current time is：
Take the covariance matrix of reality output sequence:
ρ is forgetting factor in formula, and span is generally 0.95≤ρ≤0.98；
Calculating matrix:
Mk=Pzz,k/k-1+Nk-Vk
P in formulazz,k/k-1For auto-correlation covariance matrix, Pxz,k/k-1For cross-correlation covariance matrix；
Calculate suboptimum fading factor λk：
In formulaThe mark of representing matrix.
3. the inertial navigation Initial Alignment Method according to claim 1 based on ISTSSRCKF, it is characterised in that：It is described Iteration CKF algorithms in step 4：
Step 4.1.1：Using various in step 2.4, state one-step prediction value is calculatedWith one-step prediction error covariance matrix Pk/k-1；
Step 4.1.2：Calculate fading factor λk, pass through Cholesky decomposed Psk/k-1：
Pk/k-1=Sk/k-1ST k/k-1
Calculating Cubature points (i=1,2 ..., n+1；M=2 (n+1))：
Cubature points are propagated by observational equation：
Zi,k/k-1=h (Xi,k/k-1)
Estimate the observation predicted value at k moment：
Estimate auto-correlation covariance matrix：
Estimate cross-correlation covariance matrix：
Fading factor λ is calculated according to step 3 is variousk；
Step 4.1.3：Utilize the fading factor λ obtainedk, one-step prediction error covariance matrix Pk/k-1Carry out such as down conversion：
Step 4.2.1：Measure renewal be withAnd Pk/k-1For the iterative process of initial value；Remember ith iteration estimate and Variance isWith
Step 4.2.2：New factorization and volume point is produced,The Cholesky of representing matrix is decomposed：
Wherein ξjRepresent j-th of volume point；
Step 4.2.3：With the P in step 4.1.3k/k-1Recalculate step 4.1.2 various, P can be obtained successivelyzz,k/k-1、 Pxz,k/k-1；
Calculate the state and variance evaluation of ith iteration：
Step 4.2.4：If iterations is N during iteration ends, the state estimation and covariance at k moment are：
CN201510063634.6A 2015-02-06 2015-02-06 Inertial navigation Initial Alignment Method based on ISTSSRCKF Active CN104655131B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510063634.6A CN104655131B (en) 2015-02-06 2015-02-06 Inertial navigation Initial Alignment Method based on ISTSSRCKF

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510063634.6A CN104655131B (en) 2015-02-06 2015-02-06 Inertial navigation Initial Alignment Method based on ISTSSRCKF

Publications (2)

Publication Number Publication Date
CN104655131A CN104655131A (en) 2015-05-27
CN104655131B true CN104655131B (en) 2017-07-18

Family

ID=53246535

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510063634.6A Active CN104655131B (en) 2015-02-06 2015-02-06 Inertial navigation Initial Alignment Method based on ISTSSRCKF

Country Status (1)

Country Link
CN (1) CN104655131B (en)

Families Citing this family (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105021212B (en) * 2015-07-06 2017-09-26 中国人民解放军国防科学技术大学 A kind of lower submariner device fast transfer alignment method of initial orientation information auxiliary
CN105806363B (en) * 2015-11-16 2018-08-21 东南大学 The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF
CN106153073B (en) * 2016-06-21 2018-10-12 东南大学 A kind of nonlinear initial alignment method of full posture Strapdown Inertial Navigation System
CN107729585B (en) * 2016-08-12 2020-08-28 贵州火星探索科技有限公司 Method for estimating noise covariance of unmanned aerial vehicle
CN106679648B (en) * 2016-12-08 2019-12-10 东南大学 Visual inertia combination SLAM method based on genetic algorithm
CN106595711A (en) * 2016-12-21 2017-04-26 东南大学 Strapdown inertial navigation system coarse alignment method based on recursive quaternion
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
CN107782309A (en) * 2017-09-21 2018-03-09 天津大学 Noninertial system vision and double tops instrument multi tate CKF fusion attitude measurement methods
CN108225373B (en) * 2017-12-22 2020-04-24 东南大学 Large misalignment angle alignment method based on improved 5-order cubature Kalman
CN108304612B (en) * 2017-12-26 2021-08-10 南京邮电大学 Iterative square root CKF (tracking of target) automobile radar target tracking method based on noise compensation
CN109211276B (en) * 2018-10-30 2022-06-03 东南大学 SINS initial alignment method based on GPR and improved SRCKF
CN109829938B (en) * 2019-01-28 2020-12-08 杭州电子科技大学 Adaptive fault-tolerant volume Kalman filtering method applied to target tracking
CN109945895B (en) * 2019-04-09 2020-11-06 扬州大学 Inertial navigation initial alignment method based on fading smooth variable structure filtering
CN110567490B (en) * 2019-08-29 2022-02-18 桂林电子科技大学 SINS initial alignment method under large misalignment angle
CN111649745B (en) * 2020-05-18 2022-04-05 北京三快在线科技有限公司 Attitude estimation method and apparatus for electronic device, and storage medium
CN115077530B (en) * 2022-06-16 2024-04-23 哈尔滨工业大学（威海） Multi-AUV collaborative navigation method and system based on strong tracking dimension-expanding ECKF algorithm

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101915579A (en) * 2010-07-15 2010-12-15 哈尔滨工程大学 Novel CKF(Crankshaft Fluctuation Sensor)-based SINS (Ship Inertial Navigation System) large misalignment angle initially-aligning method
CN103471616A (en) * 2013-09-04 2013-12-25 哈尔滨工程大学 Initial alignment method of SINS (strapdown inertial navigation system) with moving base and at large azimuth misalignment angle
CN103759742A (en) * 2014-01-22 2014-04-30 东南大学 Serial inertial navigation nonlinear alignment method based on fuzzy self-adaptation control technology
CN104062672A (en) * 2013-11-28 2014-09-24 哈尔滨工程大学 SINSGPS integrated navigation method based on strong tracking self-adaptive Kalman filtering

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8705793B2 (en) * 2009-10-08 2014-04-22 University Of Southern California Object tracking by hierarchical association of detection responses

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101915579A (en) * 2010-07-15 2010-12-15 哈尔滨工程大学 Novel CKF(Crankshaft Fluctuation Sensor)-based SINS (Ship Inertial Navigation System) large misalignment angle initially-aligning method
CN103471616A (en) * 2013-09-04 2013-12-25 哈尔滨工程大学 Initial alignment method of SINS (strapdown inertial navigation system) with moving base and at large azimuth misalignment angle
CN104062672A (en) * 2013-11-28 2014-09-24 哈尔滨工程大学 SINSGPS integrated navigation method based on strong tracking self-adaptive Kalman filtering
CN103759742A (en) * 2014-01-22 2014-04-30 东南大学 Serial inertial navigation nonlinear alignment method based on fuzzy self-adaptation control technology

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Spherical Simplex-Radial Cubature Kalman Filter;Shiyuan Wang 等;《IEEE SIGNAL PROCESSING LETTERS》;20140131;第21卷(第1期);第43-46页 *
基于数据存储与循环解算的SINS快速对准方法;刘锡祥 等;《中国惯性技术学报》;20131231;第21卷(第6期);第715-720页 *
基于简化无迹Kalman滤波的非线性SINS初始对准;张涛 等;《中国惯性技术学报》;20111031;第19卷(第5期);第537-542页 *

Also Published As

Publication number Publication date
CN104655131A (en) 2015-05-27

Similar Documents

Publication Publication Date Title
CN104655131B (en) Inertial navigation Initial Alignment Method based on ISTSSRCKF
CN103759742B (en) Serial inertial navigation nonlinear alignment method based on fuzzy adaptivecontroller technology
CN101949703B (en) Strapdown inertial/satellite combined navigation filtering method
CN104075715B (en) A kind of underwater navigation localization method of Combining with terrain and environmental characteristic
CN105737823B (en) A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF
CN103575299B (en) Utilize dual-axis rotation inertial navigation system alignment and the error correcting method of External Observation information
CN109211276A (en) SINS Initial Alignment Method based on GPR Yu improved SRCKF
CN105371844B (en) A kind of inertial navigation system initial method based on inertia/astronomical mutual assistance
CN105806363B (en) The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF
CN106052686B (en) Complete autonomous strapdown inertial navigation system based on DSPTMS320F28335
CN112697138B (en) Bionic polarization synchronous positioning and composition method based on factor graph optimization
CN106595711A (en) Strapdown inertial navigation system coarse alignment method based on recursive quaternion
CN105589064A (en) Rapid establishing and dynamic updating system and method for WLAN position fingerprint database
CN108362288B (en) Polarized light SLAM method based on unscented Kalman filtering
CN109059914B (en) Projectile roll angle estimation method based on GPS and least square filtering
CN102937450B (en) A kind of relative attitude defining method based on gyro to measure information
CN101706287A (en) Rotating strapdown system on-site proving method based on digital high-passing filtering
CN107063245A (en) A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF
CN106840211A (en) A kind of SINS Initial Alignment of Large Azimuth Misalignment On methods based on KF and STUPF combined filters
CN103674064B (en) Initial calibration method of strapdown inertial navigation system
CN103743413A (en) Installation error online estimation and north-seeking error compensation method for modulating north seeker under inclined state
CN107576327A (en) Varistructure integrated navigation system design method based on Observable degree analysis of Beidou double
CN104296780B (en) A kind of SINS autoregistrations based on gravity apparent motion and latitude computational methods
CN104697520A (en) Combined navigation method based on integrated gyroscope free strapdown inertial navigation system and GPS
CN109945895A (en) Inertial navigation Initial Alignment Method based on the smooth structure changes filtering that fades

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