CN105737823B - A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF - Google Patents

A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF Download PDF

Info

Publication number
CN105737823B
CN105737823B CN201610070674.8A CN201610070674A CN105737823B CN 105737823 B CN105737823 B CN 105737823B CN 201610070674 A CN201610070674 A CN 201610070674A CN 105737823 B CN105737823 B CN 105737823B
Authority
CN
China
Prior art keywords
error
sins
rank
systems
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
CN201610070674.8A
Other languages
Chinese (zh)
Other versions
CN105737823A (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 CN201610070674.8A priority Critical patent/CN105737823B/en
Publication of CN105737823A publication Critical patent/CN105737823A/en
Application granted granted Critical
Publication of CN105737823B publication Critical patent/CN105737823B/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
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Abstract

The present invention discloses a kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF, which is characterized in that includes the following steps：Step 1：Establish the integrated navigation system nonlinearity erron equation based on SINS error equations and linear measurement equation；Step 2：Utilize five rank spherical surface radial direction volume rule criteria constructions, five rank volume Kalman filter；Step 3：SINS is filtered with GPS, CNS information exported using five rank CKF, is merged, the optimal estimation of navigational parameter is obtained.

Description

A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF
Technical field
The present invention relates to strap-down inertial and integrated navigation technology field, especially a kind of SINS/ based on five rank CKF GPS/CNS Combinated navigation methods.
Background technology
In SINS/GPS/CNS integrated navigation systems, GPS can provide accurate velocity information, therefore combined system at any time The velocity error of inertial navigation can be controlled the accuracy rating in GPS by system.CNS systems can provide accurate attitude angle information, warp The measurement accuracy of attitude angle can be improved after combination, but due to the limitation of objective condition, offer course information can only be interrupted, this will It asks in the time interval of no course information, the estimation of course error must stablize.For these reasons, it is necessary to be led in combination A kind of more advanced filtering algorithm is introduced in boat system, overcomes system model parameter inaccurate or influence with environmental change, with Adaptability of the system model for external environment is improved, and then high-precision navigation information is provided.
Therefore, CKF filtering methods are studied, further increase its filtering performance, and be applied to SINS/GPS/CNS groups The Nonlinear Filtering Problem in navigation system is closed, either in theory, or all has significance in engineering practice.
Invention content
Goal of the invention：To overcome the deficiencies in the prior art, the present invention provides a kind of raising filtering accuracy and filters The GPS/SINS/CNS Combinated navigation methods based on 5 rank CKF of weave efficiency.
Technical solution：In order to solve the above technical problems, the present invention provides the GPS/SINS/CNS combinations based on five rank CKF Air navigation aid, which is characterized in that include the following steps：
Step 1：Establish the integrated navigation system nonlinearity erron equation based on SINS error equations and linear measurement Equation；
Step 2：Utilize five rank spherical surface radial direction volume rule criteria constructions, five rank volume Kalman filter；
Step 3：SINS is filtered with GPS, CNS information exported using five rank CKF, is merged, navigational parameter is obtained Optimal estimation and erection rate information, location information and strapdown attitude matrix, to obtain accurate navigation information.
Further, the step 1 establishes the integrated navigation system nonlinearity erron side based on SINS error equations Journey and linear measurement equation are specially：
Step 1.1：It is navigational coordinate system n systems to choose " northeast day " geographic coordinate system；Choose carrier " right front upper " coordinate system For carrier coordinate system b systems；N systems successively turn to b systems by 3 Eulerian angles, three Eulerian angles be denoted as course angle ψ ∈ (- π, π], Pitch angleRoll angle γ ∈ (- π, π]；Attitude matrix between n systems and b systems is denoted asTrue attitude angle note ForTrue velocity is denoted asTrue geographic coordinate system is P=[L λ H]T, In, L is latitude, and λ is longitude, and H is height；The attitude angle that SINS is calculated is denoted asSpeed is denoted asGeographic coordinate system isThe mathematical platform that SINS is calculated is denoted as n ', and n ' is Attitude matrix between b systems is denoted asRemember that attitude error isVelocity error For：Site error is：It is then non-linear Error model is 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 difference is zero-mean white-noise process,3-axis acceleration constant error is descended for b systems,For n The random error of the lower 3-axis acceleration of system is zero-mean white-noise process,For the reality output of accelerometer,It is under b systems Mathematical platform angular velocity of rotation,For the mathematical platform angular velocity of rotation calculated,For earth rotation angular speed,For solution Angular velocity of rotation of the mathematical platform of calculating relative to the earth,For correspondenceCalculating Error.RM、RNRadius of curvature is enclosed in respectively local meridian circle fourth of the twelve Earthly Branches west,For the inverse matrix of Eulerian angles differential equation coefficient matrix；It is the attitude matrix between n systems, C for n 'ωWithSpecially：
Step 1.2：It is as follows that the nonlinearity erron equation and linear measurement equation establish process：
Access speed errorEulerian angles platform error angle φe、φn、φu；Site error δ L, δ λ；Under b systems Accelerometer constant errorAnd gyroscope constant value errorAnd 12 dimension state vectors are constituted,H is approximately zero：
Bidimensional state before only being taken with P,WithFor zero-mean white-noise process；.The nonlinear state equation It can be abbreviated as：Using sampling period T as filtering cycle, quadravalence Runge-Kutta integration side can be used Method 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
It is as follows that linear measurement equation establishes process：
Choose GPS velocity errorMe、MnRespectively the range rate error item of GPS receiver is sat in east orientation north orientation Component on parameter；
Velocity measurement amount vector definition is as follows：
In formula, Hv(t)=[diag [1,1] 02×10]
Under strapdown operating mode, the posture information exported by CNS can obtain the three-axis attitude information of carrierAnd the three-axis attitude information that SINS can also provide carrier by inertial reference calculation isTherefore the two is subtracted each other to the three-axis attitude error angle Δ φ that carrier can be obtained：
Δ φ '=M × Δ φ
M is attitude error angle transition matrix in formula：
The measurement model that Δ φ ' is established to system in integrated navigation system as observation, passes through the side of optimal estimation The error of inertia device in method real-time estimation navigation system, and SINS is modified with this；
Measurement equation is as follows：
In formula, Hφ=[03×2 I3×3 03×7]
By being analyzed above it is found that the measurement equation combined entirely is：
Equally using T as filtering cycle, and simple discretization is carried out using T as step-length.
Following filtering equations are formed by state equation and measurement equation：
Further, the step 2) utilizes five rank spherical surface radial direction volume rule criteria constructions, five rank volume Kalman filtering Implement body is：
Step 2.1：Five rank spherical surface radial direction volume criterion are as follows：
F (x) is arbitrary function, RnFor integral domain, the approximate solution of its analytic value is obtained with approximate method.
Take x=ry and yTY=1, r ∈ [0, ∞), therefore above-mentioned integral can be write as in spherical surface radial coordinate system：
UnFor n N-dimension unit spheres, σ () is UnOn element.Integral I (f) be separated into Spherical integral and Radial is integrated：
Assuming that the N of Spherica integral S (r)sSampling approaches：
Radial integrates the N of RrrSampling approaches：
Obtain the N of I (f)r×NsSampling approach for：
Spherical criterion：The calculation formula of S (r) is as follows：
Wherein[s]nFor [e]nPoint Set, be denoted as Respectively：
[e]n[s]nInterior vector number is respectively 2n and 2n (n-1), [e]i[s]iRespectively [e]n[s]nI-th row；
It solves：
Radial rules：
Equation group is solved using general Gauss-Laguerre Integral Rules
Step 2.2：The construction method of five rank volume Kalman filter is as follows：
For the filtering equations of foundation：
X in formulakState vectors are tieed up for 12, and each component is independent, Gaussian distributed；zkFor 5 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.
Five rank CKF algorithms are as follows：
Step 2.2.1：Time updates：
1. assuming the Posterior probability distribution at k-1 momentIt is known that Cholesky is decomposed：
Pk-1=Sk-1Sk-1 T
1. calculating volume point：
In formula, i=1,2 ..., 2n；J=1,2 ..., 2n (n-1)
1. propagating volume point by state equation：
x1,i *=f (x1,i)；x2,i *=f (x2,i)
x3,j *=f (x3,j)；x4,j *=f (x4,j)
1. estimating the predicted value of k moment state and error battle array：
K=3- α；
Step 2.2.2：Measure update
1. Cholesky decomposed Psxx：
1. calculating volume point：
1. propagating volume point by observational equation：
z1,i=h (y1,i), z2,i=h (y2,i)
z3,i=h (y3,i), z4,i=h (y4,i)
1. estimating the observation predicted value at k moment：
1. calculating auto-correlation and cross-correlation covariance matrix：
1. completing filtering using Guass-Bayes filter frames：
Since the measurement equation is linear equation, update is measured so simplifying：
1. Cholesky decomposed Psxx：
1. calculating one-step prediction measuring value
1. calculating auto-correlation and cross-correlation covariance matrix：
1. completing filtering using Guass-Bayes filter frames：
Further, the step 3) is filtered SINS with GPS, CNS information exported using five rank CKF, merges, The optimal estimation of navigational parameter and erection rate information, location information and strapdown attitude matrix are obtained, accurately to be led Boat information be specially：
Step 3.1：After five rank CKF filters complete primary filtering, according to 12 dimension quantity of state estimations of filter output Value：Carry out velocity information, location information with And the amendment of strapdown attitude matrix.
Step 3.2：According to obtained in step 3.1Carry out velocity information amendment：
Obtained speed is resolved for SINS, when GPS does not have signal i.e. calculated VGPSAs true speed Information.
Step 3.3：According to obtained φ in step 3.1e、φn、φuCarry out posture information amendment：
It is the attitude matrix between n systems to calculate n 'Computational methods are as follows：
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 matrixComputational methods are as follows：
According to strapdown attitude matrixEulerian angles are calculated, computational methods are as follows：
Step 3.4：Following location information amendment is carried out according to obtained δ L, δ λ in step 3.1：
Wherein,It is that SINS resolves obtained latitude, L ' is acquired accurate latitude after correcting；It is that SINS is resolved Obtained latitude, λ ' are acquired accurate longitudes after correcting.
Advantageous effect：The present invention has the following advantages the prior art：
1. the present invention establishes the error mould of integrated navigation according to the general principles of SINS, GPS and CNS and respective advantage and disadvantage Type.Learn from other's strong points to offset one's weaknesses, the complementary of them is made full use of to complete integrated navigation task.
It is very limited 2. tradition CKF is based on the raising of three rank volume criterion precision, to solve this problem, is dividing in text It is proposed that a kind of five rank CKF algorithms effectively increase CKF arithmetic accuracies and with filtering very well on the basis of analysis high-order volume criterion Efficiency.
3. the present invention provides velocity information using GPS, CNS improves attitude angle information, helps to improve the filtering of five rank CKF Precision and convergence rate.
Description of the drawings
Fig. 1 is pitch angle Error Graph of the present invention.
Fig. 2 is roll angle Error Graph of the present invention.
Fig. 3 is course angle Error Graph of the present invention.
Fig. 4 is east orientation speed Error Graph of the present invention.
Fig. 5 is north orientation speed Error Graph of the present invention.
Fig. 6 is latitude error figure of the present invention.
Fig. 7 is longitude error figure of the present invention.
Fig. 8 is aircraft path profile of the present invention.
Fig. 9 is the system schema figure of the present invention.
Figure 10 is the SINS/GPS/CNS Combinated navigation method schematic diagrams the present invention is based on five rank CKF
Figure 11 is the five rank CKF filter flow figures of the present invention
Specific implementation mode
The present invention is further described below in conjunction with the accompanying drawings.
As shown in Figure 9 and Figure 10, it is the system schema figure and navigation principle 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；GPS device exports the speed of carrier Information；CNS equipment exports posture information, and the information that SINS and GPS, CNS are exported is input in five rank CKF filters simultaneously, and Into being filtered for row information, filtering is as follows：
1. according to SINS/GPS/CNS integrated navigation characteristics, 12 dimension state vectors are chosen：
Set up state equation：
Discretization postscript is：xk=f (xk-1)+ωk-1.
2. access speed amount and attitude angle are measuring value, establish and measure filtering equations：
Discretization postscript is：xk=f (xk-1)+ωk-1
3. the time updates
1. assuming the Posterior probability distribution at k-1 momentIt is known that Cholesky is decomposed：
Pk-1=Sk-1Sk-1 T
2. calculating volume point：
In formula, i=1,2 ..., 2n；J=1,2 ..., 2n (n-1)
3. propagating volume point by state equation：
x1,i *=f (x1,i)；x2,i *=f (x2,i)
x3,j *=f (x3,j)；x4,j *=f (x4,j)
4. estimating the predicted value of k moment state and error battle array：
K=3- α；
4. measuring update
1. Cholesky decomposed Psxx：
2. calculating one-step prediction measuring value
3. calculating auto-correlation and cross-correlation covariance matrix：
4. completing filtering using Guass-Bayes filter frames:
5. after five rank CKF filters complete primary filtering, according to 12 dimension quantity of state estimated values of filter output：Carry out velocity information, location information and victory Join the amendment of attitude matrix.
1. according to obtainedCarry out velocity information amendment：
Obtained speed is resolved for SINS, when GPS does not have signal i.e. calculated VGPSAs true speed Information.
2. according to obtained φe、φn、φuCarry out posture information amendment：
It is the attitude matrix between n systems to calculate n 'Computational methods are as follows：
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 matrixComputational methods are as follows：
According to strapdown attitude matrixEulerian angles are calculated, computational methods are as follows：
3. carrying out location information amendment according to obtained δ L, δ λ：
Wherein,It is that SINS resolves obtained latitude, L ' is acquired accurate latitude after correcting；It is that SINS is resolved Obtained latitude, λ ' are acquired accurate longitudes after correcting.
Illustrate beneficial effects of the present invention using following example：
1) kinematic parameter is arranged：
Initial time aircraft is emulated at 32 degree of north latitude, 118 degree of east longitude；And rotating around pitch axis, axis of roll with sine Function makees twin shaft oscillating motion, pitch angle θ, roll angle γ amplitude of waving be respectively：9,12, rolling period is respectively：16s、 20s, initial heading angle are 30 degree, and initial misalignment is：Δ θ=15 °, Δ γ=15 °, Δ ψ=100 °；Emulate initial time The aircraft is moved with uniform velocity with the speed of 10m/s, and motion path is as shown in figure 8, speed when turning round is 9 °/s, when navigation Between 6000s.
2) parameter setting of sensor：
The gyroscope constant value drift of the strapdown system of aircraft is 0.01 °/h：, Modelling of Random Drift of Gyroscopes is 0.01 °/h：, accelerate Degree meter is biased to：50mg, accelerometer random error are：50mg, GPS output speed error is：0.1m/s.
It is 1 point that CNS, which exports attitude error,.
3) analysis of simulation result：
It is emulated using the Combinated navigation method of the SINS/GPS/CNS provided by the invention based on five rank CKF, Fig. 1, 2,3 be pitching angle error in navigation procedure of the present invention, the curve of rolling angle error and course angle error；Fig. 4,5 lead for the present invention East orientation and north orientation speed error curve during boat；Fig. 6,7 be navigation procedure middle latitude of the present invention and longitude error curve.It can be with It was found that：After aircraft flight 1000s, pitch angle is restrained substantially, about 0.07 ° of error；Roll angle restrains substantially after 500s, and error is about 0.07°；Course angle restrains substantially after 100s, about 0.02 ° of error.East orientation speed restrains substantially after 1550s, error 0.12m/s. North orientation speed restrains substantially after 1500s, error 0.1m/s.Very little, maximum latitude error are 26 points to latitude, longitude error always, Maximum longitude error is 0.031 point.It is found by analysis, after navigating by water 6000s, 0.0002 ° of pitching angle error is within, roll angle Error is within 0.0009 °, and course angle error is within 0.02；East orientation speed error is within 0.01m/s, north orientation speed error Within 0.033m/s, for latitude error within 1.63 points, longitude error is within 0.0064 point.
The above result shows that five rank CKF algorithms in SINS/GPS/CNS integrated navigations for that can make systematic error quick It restrains and stablizes within smaller error range.
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 GPS/SINS/CNS Combinated navigation methods based on five rank CKF, which is characterized in that include the following steps：
Step 1：Establish the integrated navigation system nonlinearity erron equation based on SINS error equations and linear measurement equation；
Step 1.1：It is navigational coordinate system n systems to choose " northeast day " geographic coordinate system；It is to carry to choose carrier " right front upper " coordinate system Body coordinate system b systems；N systems successively turn to b systems by 3 Eulerian angles, three Eulerian angles be denoted as course angle ψ ∈ (- π, π], pitching AngleRoll angle γ ∈ (- π, π]；Attitude matrix between n systems and b systems is denoted asTrue attitude angle is denoted asTrue velocity is denoted asTrue geographic coordinate system is P=[L λ H]T, wherein L is latitude, and λ is longitude, and H is height；The attitude angle that SINS is calculated is denoted asSpeed is denoted asGeographic coordinate system isThe mathematical platform that SINS is calculated is denoted as n ', and n ' is Attitude matrix between b systems is denoted asRemember that attitude error isVelocity error For：Site error is：It is then non-linear Error model is as follows：
Wherein,The constant error of three axis accelerometer is descended for b systems,It is the lower three axis accelerometer of b systems with chance error Difference is zero-mean white-noise process,3-axis acceleration constant error is descended for b systems,For n systems lower three The random error of axle acceleration is zero-mean white-noise process,For the reality output of accelerometer,It is that mathematics is flat under b systems Platform angular velocity of rotation,For the mathematical platform angular velocity of rotation calculated,For earth rotation angular speed,It calculates Angular velocity of rotation of the mathematical platform relative to the earth,For correspondenceCalculating error, RM、RNRadius of curvature is enclosed in respectively local meridian circle fourth of the twelve Earthly Branches west,For the inverse matrix of Eulerian angles differential equation coefficient matrix；For N ' is the attitude matrix between n systems, CωWithSpecially：
Step 1.2：It is as follows that the nonlinearity erron equation and linear measurement equation establish process：
Access speed errorEulerian angles platform error angle φe、φn、φu；Site error δ L, δ λ；Acceleration under b systems Degree meter constant errorAnd gyroscope constant value errorAnd 12 dimension state vectors are constituted, H is approximately zero：
Bidimensional state before only being taken with P,WithFor zero-mean white-noise process；The nonlinear state equation can be abbreviated For：Using sampling period T as filtering cycle, quadravalence Runge-Kutta integration method can be used, with 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
It is as follows that linear measurement equation establishes process：
Choose GPS velocity errorMe、MnRespectively the range rate error item of GPS receiver is in east orientation north orientation reference axis Component；
Velocity measurement amount vector definition is as follows：
In formula, Hv(t)=[diag [1,1] 02×10]
Under strapdown operating mode, the posture information exported by CNS can obtain the three-axis attitude information of carrierAnd the three-axis attitude information that SINS can also provide carrier by inertial reference calculation isTherefore the two is subtracted each other to the three-axis attitude error angle Δ φ that carrier can be obtained：
Δ φ '=M × Δ φ
M is attitude error angle transition matrix in formula：
The measurement model that Δ φ ' is established to system in integrated navigation system as observation, it is real by the method for optimal estimation When estimation navigation system in inertia device error, and SINS is modified with this；
Measurement equation is as follows：
In formula, Hφ=[03×2 I3×3 03×7]
By being analyzed above it is found that the measurement equation combined entirely is：
Equally using T as filtering cycle, and simple discretization is carried out using T as step-length,
Following filtering equations are formed by state equation and measurement equation：
Step 2：Utilize five rank spherical surface radial direction volume rule criteria constructions, five rank volume Kalman filter；
Step 3：SINS is filtered with GPS, CNS information exported using five rank CKF, is merged, obtains navigational parameter most Excellent estimation and erection rate information, location information and strapdown attitude matrix, to obtain accurate navigation information.
2. the GPS/SINS/CNS Combinated navigation methods as described in claim 1 based on five rank CKF, which is characterized in that described Step 2) is specially using five rank spherical surface radial direction volume rule criteria constructions, five rank volume Kalman filter：
Step 2.1：Five rank spherical surface radial direction volume criterion are as follows：
F (x) is arbitrary function, RnFor integral domain, the approximate solution of its analytic value is obtained with approximate method,
Take x=ry, and yTY=1, r ∈ [0, ∞), therefore above-mentioned integral can be write as in spherical surface radial coordinate system：
UnFor n N-dimension unit spheres, σ () is UnOn element, integral I (f) be separated into Spherical integral and Radial product Point：
Assuming that the N of Spherica integral S (r)sSampling approaches：
Radial integrates the N of RrrSampling approaches：
Obtain the N of I (f)r×NsSampling approach for：
Spherical criterion：The calculation formula of S (r) is as follows：
Wherein[s]nFor [e]nPoint Set, be denoted as Respectively For：
[e]n[s]nInterior vector number is respectively 2n and 2n (n-1), [e]i[s]iRespectively [e]n[s]nI-th row；
It solves：
Radial rules：
Equation group is solved using general Gauss-Laguerre Integral Rules
Step 2.2：The construction method of five rank volume Kalman filter is as follows：
For the filtering equations of foundation：
X in formulakState vectors are tieed up for 12, and each component is independent, Gaussian distributed；zkFor 5 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,
Five rank CKF algorithms are as follows：
Step 2.2.1：Time updates：
1. assuming the Posterior probability distribution at k-1 momentIt is known that Cholesky is decomposed：
Pk-1=Sk-1Sk-1 T
2. calculating volume point：
In formula, i=1,2 ..., 2n；J=1,2 ..., 2n (n-1)
3. propagating volume point by state equation：
x1,i *=f (x1,i)；x2,i *=f (x2,i)
x3,j *=f (x3,j)；x4,j *=f (x4,j)
4. estimating the predicted value of k moment state and error battle array：
Step 2.2.2：Measure update
1. Cholesky decomposed Psxx：
2. calculating volume point：
3. propagating volume point by observational equation：
z1,i=h (y1,i), z2,i=h (y2,i)
z3,i=h (y3,i), z4,i=h (y4,i)
4. estimating the observation predicted value at k moment：
5. calculating auto-correlation and cross-correlation covariance matrix：
6. completing filtering using Guass-Bayes filter frames：
Since the measurement equation is linear equation, update is measured so simplifying：
1. Cholesky decomposed Psxx：
2. calculating one-step prediction measuring value
3. calculating auto-correlation and cross-correlation covariance matrix：
4. completing filtering using Guass-Bayes filter frames：
3. a kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF as described in claim 1, it is characterised in that institute It states step 3) to be filtered SINS with GPS, CNS information exported using five rank CKF, merge, obtains the optimal of navigational parameter Estimate simultaneously erection rate information, location information and strapdown attitude matrix, is specially to obtain accurate navigation information：
Step 3.1：After five rank CKF filters complete primary filtering, according to 12 dimension quantity of state estimated values of filter output：Carry out velocity information, location information and victory Join the amendment of attitude matrix；
Step 3.2：According to obtained in step 3.1Carry out velocity information amendment：
Obtained speed is resolved for SINS, when GPS does not have signal i.e. calculated VGPSAs true velocity information；
Step 3.3：According to obtained φ in step 3.1e、φn、φuCarry out posture information amendment：
It is the attitude matrix between n systems to calculate n 'Computational methods are as follows：
It is the attitude matrix between b systems according to n 'It is the attitude matrix between n systems with n 'Calculate accurate strapdown Attitude matrixComputational methods are as follows：
According to strapdown attitude matrixEulerian angles are calculated, computational methods are as follows：
Step 3.4：Following location information amendment is carried out according to obtained δ L, δ λ in step 3.1：
Wherein,It is that SINS resolves obtained latitude, L ' is acquired accurate latitude after correcting；It is that SINS resolves gained The latitude arrived, λ ' are acquired accurate longitudes after correcting.
CN201610070674.8A 2016-02-01 2016-02-01 A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF Active CN105737823B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610070674.8A CN105737823B (en) 2016-02-01 2016-02-01 A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610070674.8A CN105737823B (en) 2016-02-01 2016-02-01 A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF

Publications (2)

Publication Number Publication Date
CN105737823A CN105737823A (en) 2016-07-06
CN105737823B true CN105737823B (en) 2018-09-21

Family

ID=56242156

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610070674.8A Active CN105737823B (en) 2016-02-01 2016-02-01 A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF

Country Status (1)

Country Link
CN (1) CN105737823B (en)

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106291645B (en) * 2016-07-19 2018-08-21 东南大学 The volume kalman filter method coupled deeply suitable for higher-dimension GNSS/INS
CN106871905B (en) * 2017-03-02 2020-02-11 哈尔滨工业大学 Gaussian filtering substitution framework combined navigation method under non-ideal condition
CN107063245B (en) * 2017-04-19 2020-12-25 东南大学 SINS/DVL combined navigation filtering method based on 5-order SSRCKF
CN107992877B (en) * 2017-10-11 2020-05-19 衢州学院 Two-stage high-order volume information filtering method
CN108225373B (en) * 2017-12-22 2020-04-24 东南大学 Large misalignment angle alignment method based on improved 5-order cubature Kalman
CN108594272B (en) * 2018-08-01 2020-09-15 北京航空航天大学 Robust Kalman filtering-based anti-deception jamming integrated navigation method
CN109443355B (en) * 2018-12-25 2020-10-27 中北大学 Visual-inertial tight coupling combined navigation method based on self-adaptive Gaussian PF
CN109443353B (en) * 2018-12-25 2020-11-06 中北大学 Visual-inertial tight coupling combined navigation method based on fuzzy self-adaptive ICKF
CN110132267B (en) * 2019-05-10 2021-06-08 上海航天控制技术研究所 Optical fiber inertial navigation system of air-space-ground integrated aircraft and optical fiber inertial navigation on-orbit alignment method
CN110006427B (en) * 2019-05-20 2020-10-27 中国矿业大学 BDS/INS tightly-combined navigation method in low-dynamic high-vibration environment
CN110672130B (en) * 2019-11-19 2021-09-07 北方工业大学 EKF (extended Kalman filter) alignment method of inertial/polarized light integrated navigation system under large misalignment angle
CN110672131B (en) * 2019-11-19 2021-08-10 北方工业大学 UKF (unscented Kalman Filter) alignment method for inertial/polarized light integrated navigation system under large misalignment angle
CN113432608A (en) * 2021-02-03 2021-09-24 东南大学 Generalized high-order CKF algorithm based on maximum correlation entropy and suitable for INS/CNS integrated navigation system
CN114674313B (en) * 2022-03-31 2023-04-04 淮阴工学院 Unmanned distribution vehicle navigation positioning method based on CKF algorithm and integrating GPS/BDS and SINS

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1834980A (en) * 2006-03-29 2006-09-20 北京航空航天大学 SINS/CNS/GPS Combined navigation semi-entity copying system
CN101246012A (en) * 2008-03-03 2008-08-20 北京航空航天大学 Combinated navigation method based on robust dissipation filtering
CN101706281A (en) * 2009-11-13 2010-05-12 南京航空航天大学 Inertia/astronomy/satellite high-precision integrated navigation system and navigation method thereof
CN101949703A (en) * 2010-09-08 2011-01-19 北京航空航天大学 Strapdown inertial/satellite combined navigation filtering method
CN102519470A (en) * 2011-12-09 2012-06-27 南京航空航天大学 Multi-level embedded integrated navigation system and navigation method
CN102830414A (en) * 2012-07-13 2012-12-19 北京理工大学 Combined navigation method based on SINS/GPS (strapdown inertial navigation system/global position system)
CN103134491A (en) * 2011-11-30 2013-06-05 上海宇航系统工程研究所 Integrated navigation system of strapdown inertial navigation system (SINS)/central nervous system (CNS)/global navigation satellite system (GNSS) of geostationary earth orbit (GEO) transfer vehicle
CN103226022A (en) * 2013-03-27 2013-07-31 清华大学 Alignment method and system for movable base used for integrated navigation system
CN103411616A (en) * 2013-07-05 2013-11-27 东南大学 Vehicle-mounted integrated navigation method based on simplified inertia measurement assembly
CN103487820A (en) * 2013-09-30 2014-01-01 东南大学 Vehicle-mounted strapdown/satellite tight-combination seamless navigation method
CN103674030A (en) * 2013-12-26 2014-03-26 中国人民解放军国防科学技术大学 Dynamic measuring device and method for plumb line deviation kept on basis of astronomical attitude reference
CN103969672A (en) * 2014-05-14 2014-08-06 东南大学 Close combination navigation method of multi-satellite system and strapdown inertial navigation system
CN104034329A (en) * 2014-06-04 2014-09-10 南京航空航天大学 Multi-integrated navigation processing device under launch inertial system and navigation method of multi-integrated navigation processing device
CN104635251A (en) * 2013-11-08 2015-05-20 中国地质大学（北京） Novel INS (inertial navigation system)/ GPS (global position system) combined position and orientation method

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6449559B2 (en) * 1998-11-20 2002-09-10 American Gnc Corporation Fully-coupled positioning process and system thereof

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1834980A (en) * 2006-03-29 2006-09-20 北京航空航天大学 SINS/CNS/GPS Combined navigation semi-entity copying system
CN101246012A (en) * 2008-03-03 2008-08-20 北京航空航天大学 Combinated navigation method based on robust dissipation filtering
CN101706281A (en) * 2009-11-13 2010-05-12 南京航空航天大学 Inertia/astronomy/satellite high-precision integrated navigation system and navigation method thereof
CN101949703A (en) * 2010-09-08 2011-01-19 北京航空航天大学 Strapdown inertial/satellite combined navigation filtering method
CN103134491A (en) * 2011-11-30 2013-06-05 上海宇航系统工程研究所 Integrated navigation system of strapdown inertial navigation system (SINS)/central nervous system (CNS)/global navigation satellite system (GNSS) of geostationary earth orbit (GEO) transfer vehicle
CN102519470A (en) * 2011-12-09 2012-06-27 南京航空航天大学 Multi-level embedded integrated navigation system and navigation method
CN102830414A (en) * 2012-07-13 2012-12-19 北京理工大学 Combined navigation method based on SINS/GPS (strapdown inertial navigation system/global position system)
CN103226022A (en) * 2013-03-27 2013-07-31 清华大学 Alignment method and system for movable base used for integrated navigation system
CN103411616A (en) * 2013-07-05 2013-11-27 东南大学 Vehicle-mounted integrated navigation method based on simplified inertia measurement assembly
CN103487820A (en) * 2013-09-30 2014-01-01 东南大学 Vehicle-mounted strapdown/satellite tight-combination seamless navigation method
CN104635251A (en) * 2013-11-08 2015-05-20 中国地质大学（北京） Novel INS (inertial navigation system)/ GPS (global position system) combined position and orientation method
CN103674030A (en) * 2013-12-26 2014-03-26 中国人民解放军国防科学技术大学 Dynamic measuring device and method for plumb line deviation kept on basis of astronomical attitude reference
CN103969672A (en) * 2014-05-14 2014-08-06 东南大学 Close combination navigation method of multi-satellite system and strapdown inertial navigation system
CN104034329A (en) * 2014-06-04 2014-09-10 南京航空航天大学 Multi-integrated navigation processing device under launch inertial system and navigation method of multi-integrated navigation processing device

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
"５阶ＣＫＦ在捷联惯导非线性对准中的应用研究";黄湘远,等;《系统工程与电子技术》;20150331;第３7卷(第３期);633-638，摘要，第2-3章 *

Also Published As

Publication number Publication date
CN105737823A (en) 2016-07-06

Similar Documents

Publication Publication Date Title
CN105737823B (en) A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF
CN103759742B (en) Serial inertial navigation nonlinear alignment method based on fuzzy adaptivecontroller technology
CN104655131B (en) Inertial navigation Initial Alignment Method based on ISTSSRCKF
CN105806363B (en) The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF
CN109211276A (en) SINS Initial Alignment Method based on GPR Yu improved SRCKF
CN106871928B (en) Strap-down inertial navigation initial alignment method based on lie group filtering
CN107525503A (en) Adaptive cascade kalman filter method based on double antenna GPS and MIMU combination
CN105698822B (en) Initial Alignment Method between autonomous type inertial navigation based on reversed Attitude Tracking is advanced
CN106153073B (en) A kind of nonlinear initial alignment method of full posture Strapdown Inertial Navigation System
CN101706284B (en) Method for increasing position precision of optical fiber gyro strap-down inertial navigation system used by ship
CN106885570A (en) A kind of tight integration air navigation aid based on robust SCKF filtering
CN109974714A (en) A kind of adaptive Unscented kalman filtering attitude data fusion method of Sage-Husa
CN105865452B (en) A kind of mobile platform position and orientation estimation method based on indirect Kalman filtering
CN109931955B (en) Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering
CN101246012B (en) Combinated navigation method based on robust dissipation filtering
CN106441291B (en) A kind of integrated navigation system and air navigation aid based on strong tracking SDRE filtering
CN106052685A (en) Two-stage separation fusion attitude and heading estimation method
CN104697526A (en) Strapdown inertial navitation system and control method for agricultural machines
CN103076026B (en) A kind of method determining Doppler log range rate error in SINS
CN107063245A (en) A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF
CN104913781A (en) Unequal interval federated filter method based on dynamic information distribution
CN104374405A (en) MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering
CN104613966B (en) A kind of cadastration off-line data processing method
Luo et al. A position loci-based in-motion initial alignment method for low-cost attitude and heading reference system
CN104482942B (en) A kind of optimal Two position method based on inertial 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