CN110146076A - A kind of SINS/DVL combined positioning method of no inverse matrix adaptive-filtering - Google Patents
A kind of SINS/DVL combined positioning method of no inverse matrix adaptive-filtering Download PDFInfo
- Publication number
- CN110146076A CN110146076A CN201910488838.2A CN201910488838A CN110146076A CN 110146076 A CN110146076 A CN 110146076A CN 201910488838 A CN201910488838 A CN 201910488838A CN 110146076 A CN110146076 A CN 110146076A
- Authority
- CN
- China
- Prior art keywords
- error
- sins
- inverse matrix
- filtering
- dvl
- 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
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
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
- G01S15/06—Systems determining the position data of a target
- G01S15/42—Simultaneous measurement of distance and other co-ordinates
-
- 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
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/86—Combinations of sonar systems with lidar systems; Combinations of sonar systems with systems not using wave reflection
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Acoustics & Sound (AREA)
- Navigation (AREA)
Abstract
A kind of SINS/DVL combined positioning method of no inverse matrix adaptive-filtering, is related to high-precision SINS/DVL integrated positioning.The present invention is to solve to cause computational efficiency low and the problem of stability difference since traditional SINS/DVL integrated navigation filtering algorithm calculating process is complicated.A kind of SINS/DVL combined positioning method of no inverse matrix adaptive-filtering of the present invention is primarily based on Strapdown Inertial Navigation System and Doppler log sensor information, obtains corresponding state initial value and observation；Then it establishes and is based on the corresponding system equation of integrated navigation error model and observational equation, improve adaptive filter algorithm with no inverse matrix and error is corrected, obtain the speed and position error information of target after correction；The control information of acquisition and inertial navigation are merged with the observation information of Doppler log finally, obtain high-precision positioning result.The present invention can reduce calculation amount, on the basis of optimizing calculating process, guarantee that positioning system is reliable and stable, improve the positioning accuracy of underwater autonomous navigation.
Description
Technical field
The present invention relates to high-precision underwater position fixing techniques.
Background technique
It is complicated and changeable due to underwater environment during actual Underwater Navigation, so that inertial navigation and Doppler count
The system noise of gift of money for a friend going on a journey integrated positioning (SINS/DVL) system and the statistical property for measuring noise often have certain time variation.
In order to which the variation to noise statistics has certain adaptive ability, filtering accuracy is improved, frequently with adaptive Kalman
Filtering algorithm handles noise.However during Kalman filter, most of operand concentrates on the mistake of inversion operation
Journey.And inversion operation, for computer, processing is complicated, poor reliability.Therefore the present invention is asked using the nothing quickly calculated
Inverse matrix algorithm improves adaptive Kalman filter algorithm, optimizes matrix inversion operation using improved filtering algorithm
Process, while guaranteeing filtering accuracy and reliability, additionally it is possible to reduce calculation amount, promote operation efficiency, it is ensured that operational process
Relatively reliable stabilization.
Summary of the invention
The purpose of the present invention is to solve SINS/DVL combined positioning method systems under complicated underwater environment due to calculating
It is higher to measure excessive and system complexity, leads to the problem of system reliability and operation efficiency difference, proposes that a kind of no inverse matrix is adaptive
The SINS/DVL combined positioning method that should be filtered.
The SINS/DVL combined positioning method of no inverse matrix adaptive-filtering of the present invention a kind of the following steps are included:
Step 1: system establishes SINS/DVL integrated positioning error model state variable X=[δ vE δvN α β γ δL δ
λ εE εN εU δvd δΔ δC]T, wherein δ vEWith δ vNFor east, north orientation speed error, α, β, γ are the misaligned angle of the platform, δ L and δ λ
For longitude error and latitude error, εE、εN、εUFor east, north, the gyroscopic drift of day direction, δ vdFor Doppler measurement velocity shifts mistake
Difference, δ Δ are bias current angle error, and δ C is scale coefficient error；
Step 2: three axis angular rate information and acceleration of the system by the gyroscope sensitive carrier in Inertial Navigation Unit
Measure to obtain three axis (east, north, day) acceleration information aE、aN、aH, speed v is obtained by Doppler logd, drift angle Δ and posture
Equal navigation informations；
Step 3: system is by the component of acceleration a in these three directionsE、aN、aHIt brings formula (1) into be integrated respectively, i.e.,
Velocity component v of the carrier along these three directions can be obtainedE、vN、vH.Three velocity components are brought into formula (2) again integrate
To the longitude L, latitude λ and depth d of carrier, wherein R is earth radius, t0To move initial time, tkFor some time in motion process
It carves；
System brings these information in formula (3)~(15) into finds out corresponding each state variable X=[the δ v of error modelE
δvN α β γ δL δλ εE εN εU δvd δΔ δC]T:
Wherein, it is constant that Ω, which is gyroscopic vibration frequency, and g is that acceleration of gravity is constant, Δ aN, Δ aEFor acceleration error；
East orientation and north orientation speed error formula:
The misaligned angle of the platform:
Location error:
Gyroscopic drift:
εE=-βEεE+wE (10)
εN=-βNεN+wN (11)
εU=-βUεU+wU (12)
Error correlation time for gyroscope in east, north, day direction, wE, wN, wUFor white Gaussian noise；
Speed, drift angle and the error of graduation of Doppler log:
δvd=-βdδvd+wd (13)
δ Δ=- βΔδΔ+wΔ (14)
δ C=0 (15)
WhereinFor the correlation time of velocity shifts error and bias current angle error, wd、wΔFor white Gaussian noise；
Step 4: system establishes system state equation and system measurements equation, as shown in formula (16) and formula (21):
State equation description are as follows:
In formula:
WSINS=[0 0 aE aN 0 0 0 wE wN wU wd wΔ 0]T (17)
State Transitive Matrices F is established according to formula (3)~(15)SINS/DVL
Wherein:
Have:
F36=-g,
F45=g,
F61=-Ω sinL,
For F6×6Have:
System measurements equation are as follows:
For HSINS/DVLAnd VSINS/DVLHave:
VSINS/DVL=[vE vN]T (23)
Here system noise variance matrix:
Measuring noise square difference battle array:
Step 5: system, which is established, improves adaptive filter algorithm without inverse matrix, and adaptive karr is improved to no inverse matrix
The state equation and measurement equation of graceful filtering algorithm system are described, such as formula (26), (27),
State equation description are as follows:
Xk=FkXk-1+GWk (26)
System measurements equation are as follows:
Wherein, XkFor the state variable value at k moment；Xk-1For the state variable at k-1 moment；F is to act on Xk-1On shape
State transformation coefficient；WkFor the state-noise value at k moment；G is to act on WkCoefficient；HkFor observation model coefficient, true shape
State space is mapped to observation space；ZkFor the observation at k moment, by the east orientation of inertial navigation, north orientation speed error and Doppler
The difference of tachometer east orientation north orientation velocity error is constituted；VkFor the observation noise value at k moment；
Step 6: by Fk, Gk, Wk, Hk, VkAnd initial state variable X brings the state equation and measurement equation of system into
One-step prediction is carried out, the measuring value that the k moment predicts is found outWith For k moment measuring value
Error；
Step 7: system is according to stability criteria, judgementSystem is sent out if setting up
It dissipates, step 8 should be executed using no inverse matrix adaptive-filtering；The system convergence if invalid, should be using no inverse matrix strong tracking
Kalman filtering executes step 9；
Step 8: system will correspond to parameter and parameter is substituted into formula (28)~(40) and calculated
λk(1)=[Hk(1)Lk(1)+λk(1)]-1 (34)
λk(2)=[Hk(2)Lk(2)+λk(2)]-1 (38)
Wherein,For the state variable value at k moment；WithThe transition value of state variable is sought for the k moment；For
By the predicted value at the state variable at k-1 moment obtained k moment；Fk,k-1To act onOn state transformation coefficient；Kk
To act onOn without inverse matrix Kalman's coefficient；λkFor transition constant and Lk Lk∈Rn×1；HkFor observation model coefficient,
Time of day space reflection is at observation spaceZkThe measurement predicted for the observation at k moment, k moment
Value isWherein Zk=[Zk(1)Zk(2)]T, Pk,k-1For prior estimate error covariance value；For k moment error covariance mistake
Cross value；PkFor Posterior estimator error covariance value；In the formula of (28)~(40)It is made an uproar respectively by time-varying
Sound valuation equation calculates to obtain formula (41)~(44):
It realizes and is operated without inverse matrix adaptive-filtering, obtain subsequent time state variable estimateExecute step 10；
Step 9: system will correspond to parameter and amount is substituted into formula (45)~(60) and calculated:
λk+1=diag [λ1(k+1),λ2(k+1),…,λm(k+1)] (48)
λk(1)=[Hk(1)Lk(1)+λk(1)]-1 (54)
λk(2)=[Hk(2)Lk(2)+λk(2)]-1 (58)
It realizes without inverse matrix strong tracking Kalman filter, obtains subsequent time state variable estimateExecute step
Ten；
Step 10: system update converts k=k+1, the state variable estimate that will be obtainedIt is denoted as new state variable
Value
Step 11: system judges whether k is equal to n, if it is, executing step 12, step 6 is otherwise executed；
Step 12: the system state variables sequence that system is generated It is defeated
Final result outAs state outcome of the current time after filtering and calibration includeIt is the correction result of the margin of error；In conjunction with working as
The observation Y of preceding moment SINS/DVL integrated positioning systemn=[vEn vNn Lnλn vdn]T, after being corrected aircraft east
To, north orientation speedAnd latitude and longitude informationAnd the velocity information of Doppler logWherein
Step 13: system judges whether SINS/DVL integrated positioning task is completed, if so, step 14 is executed, it is no
Then follow the steps two；
Step 14: SINS/DVL integrated positioning task of the system finishing without inverse matrix adaptive-filtering.
Detailed description of the invention
Fig. 1 is a kind of flow chart of the SINS/DVL combined positioning method of no inverse matrix adaptive-filtering.
Specific embodiment
Specific embodiment one
Embodiment is described with reference to Fig. 1, a kind of SINS/ of no inverse matrix adaptive-filtering described in present embodiment
DVL combined positioning method the following steps are included:
Step 1: system establishes SINS/DVL integrated positioning error model state variable X=[δ vE δvN α β γ δL δ
λ εE εN εU δvd δΔ δC]T, wherein δ vEWith δ vNFor east, north orientation speed error, α, β, γ are the misaligned angle of the platform, δ L and δ λ
For longitude error and latitude error, εE、εN、εUFor east, north, the gyroscopic drift of day direction, δ vdFor Doppler measurement velocity shifts mistake
Difference, δ Δ are bias current angle error, and δ C is scale coefficient error；
Step 2: three axis angular rate information and acceleration of the system by the gyroscope sensitive carrier in Inertial Navigation Unit
Measure to obtain three axis (east, north, day) acceleration information aE、aN、aH, speed v is obtained by Doppler logd, drift angle Δ and posture
Equal navigation informations；
Step 3: system is by the component of acceleration a in these three directionsE、aN、aHIt brings formula (1) into be integrated respectively, i.e.,
Velocity component v of the carrier along these three directions can be obtainedE、vN、vH.Three velocity components are brought into formula (2) again integrate
To the longitude L, latitude λ and depth d of carrier, wherein R is earth radius, t0To move initial time, tkFor some time in motion process
It carves；
System brings these information in formula (3)~(15) into finds out corresponding each state variable X=[the δ v of error modelE
δvN α β γ δL δλ εE εN εU δvd δΔ δC]T:
Wherein, it is constant that Ω, which is gyroscopic vibration frequency, and g is that acceleration of gravity is constant, Δ aN, Δ aEFor acceleration error；
East orientation and north orientation speed error formula:
The misaligned angle of the platform:
Location error:
Gyroscopic drift:
εE=-βEεE+wE (10)
εN=-βNεN+wN (11)
εU=-βUεU+wU (12)
Error correlation time for gyroscope in east, north, day direction, wE, wN, wUFor white Gaussian noise；
Speed, drift angle and the error of graduation of Doppler log:
δvd=-βdδvd+wd (13)
δ Δ=- βΔδΔ+wΔ (14)
δ C=0 (15)
WhereinFor the correlation time of velocity shifts error and bias current angle error, wd、wΔFor white Gaussian noise；
Step 4: system establishes system state equation and system measurements equation, as shown in formula (16) and formula (21):
State equation description are as follows:
In formula:
WSINS=[0 0 aE aN 0 0 0 wE wN wU wd wΔ 0]T (17)
State Transitive Matrices F is established according to formula (3)~(15)SINS/DVL
Wherein:
Have:
F36=-g,
F45=g,
F61=-Ω sinL,
For F6×6Have:
System measurements equation are as follows:
For HSINS/DVLAnd VSINS/DVLHave:
VSINS/DVL=[vE vN]T (23)
Here system noise variance matrix:
Measuring noise square difference battle array:
Step 5: system, which is established, improves adaptive filter algorithm without inverse matrix, and adaptive karr is improved to no inverse matrix
The state equation and measurement equation of graceful filtering algorithm system are described, such as formula (26), (27),
State equation description are as follows:
Xk=FkXk-1+GWk (26)
System measurements equation are as follows:
Wherein, XkFor the state variable value at k moment；Xk-1For the state variable at k-1 moment；F is to act on Xk-1On shape
State transformation coefficient；WkFor the state-noise value at k moment；G is to act on WkCoefficient；HkFor observation model coefficient, true shape
State space is mapped to observation space；ZkFor the observation at k moment, by the east orientation of inertial navigation, north orientation speed error and Doppler
The difference of tachometer east orientation north orientation velocity error is constituted；VkFor the observation noise value at k moment；
Step 6: by Fk, Gk, Wk, Hk, VkAnd initial state variable X brings the state equation and measurement equation of system into
One-step prediction is carried out, the measuring value that the k moment predicts is found outWith For k moment measuring value
Error；
Step 7: system is according to stability criteria, judgementSystem is sent out if setting up
It dissipates, step 8 should be executed using no inverse matrix adaptive-filtering；The system convergence if invalid, should be using no inverse matrix strong tracking
Kalman filtering executes step 9；
Step 8: system will correspond to parameter and parameter is substituted into formula (28)~(40) and calculated
λk(1)=[Hk(1)Lk(1)+λk(1)]-1 (34)
λk(2)=[Hk(2)Lk(2)+λk(2)]-1 (38)
Wherein,For the state variable value at k moment；WithThe transition value of state variable is sought for the k moment；It serves as reasons
The predicted value at the state variable at k-1 moment obtained k moment；Fk,k-1To act onOn state transformation coefficient；KkFor
It acts onOn without inverse matrix Kalman's coefficient；λkFor transition constant and Lk Lk∈Rn×1；HkFor observation model coefficient, true
Real state space is mapped to observation spaceZkThe measuring value predicted for the observation at k moment, k moment
ForWherein Zk=[Zk(1)Zk(2)]T, Pk,k-1For prior estimate error covariance value；For k moment error covariance transition
Value；PkFor Posterior estimator error covariance value；In the formula of (28)~(40)Respectively by time-varying noise
Valuation equation calculates to obtain formula (41)~(44):
It realizes and is operated without inverse matrix adaptive-filtering, obtain subsequent time state variable estimateExecute step 10；
Step 9: system will correspond to parameter and amount is substituted into formula (45)~(60) and calculated:
λk+1=diag [λ1(k+1),λ2(k+1),…,λm(k+1)] (48)
λk(1)=[Hk(1)Lk(1)+λk(1)]-1 (54)
λk(2)=[Hk(2)Lk(2)+λk(2)]-1 (58)
It realizes without inverse matrix strong tracking Kalman filter, obtains subsequent time state variable estimateExecute step
Ten；
Step 10: system update converts k=k+1, the state variable estimate that will be obtainedIt is denoted as new state variable value
Step 11: system judges whether k is equal to n, if it is, executing step 12, step 6 is otherwise executed；
Step 12: the system state variables sequence that system is generated Output
Final resultAs state outcome of the current time after filtering and calibration includeIt is the correction result of the margin of error；In conjunction with working as
The observation Y of preceding moment SINS/DVL integrated positioning systemn=[vEn vNn Lnλn vdn]T, after being corrected aircraft east
To, north orientation speedAnd latitude and longitude informationAnd the velocity information of Doppler logWherein
Step 13: system judges whether SINS/DVL integrated positioning task is completed, if so, step 14 is executed, it is no
Then follow the steps two；
Step 14: SINS/DVL integrated positioning task of the system finishing without inverse matrix adaptive-filtering.
Specific embodiment two, present embodiment are adaptively filtered to a kind of no inverse matrix described in specific embodiment one
The step of SINS/DVL combined positioning method of wave seven, is described further, in present embodiment according to filtering estimation error with
The size relation of anticipation error is as the criterion for judging whether filtering dissipates.
Specific embodiment three, present embodiment are adaptively filtered to a kind of no inverse matrix described in specific embodiment one
The step of SINS/DVL combined positioning method of wave eight and step 9 further illustrate, can guarantee in present embodiment using one kind
Filtering accuracy and reliability without inverse matrix operation method, avoid matrix inversion operation process, on the basis of reducing calculation amount, protect
Card operational process is reliable and stable, improves the positioning accuracy of long-range underwater autonomous navigation.
Specific embodiment four, present embodiment are adaptively filtered to a kind of no inverse matrix described in specific embodiment one
The further explanation of the SINS/DVL combined positioning method of wave can be kept away using no inverse matrix backoff algorithm in present embodiment
Exempt from the problem of navigation accuracy caused by sudden change and quality decline occur as the speed of a ship or plane course of interference or aircraft, it can be fast
Speed carries out real-time tracking to system mode and reduces the accumulating rate of error.
Specific embodiment five, present embodiment are adaptively filtered to a kind of no inverse matrix described in specific embodiment one
The further explanation of the SINS/DVL combined positioning method of wave, using improving adaptive Kalman filter side in present embodiment
Method can not only guarantee filtering accuracy, but also effectively filtering can be prevented to dissipate.
Specific embodiment six, present embodiment are adaptively filtered to a kind of no inverse matrix described in specific embodiment one
The further explanation of the SINS/DVL combined positioning method of wave, using no inverse matrix adaptive filter algorithm in present embodiment,
To quickly calculate without finding the inverse matrix algorithm and improve adaptive Kalman algorithm fusion, the accuracy of blending algorithm is compared to melting
Algorithm arithmetic speed before conjunction promotes 25%.
Claims (6)
1. a kind of SINS/DVL combined positioning method of no inverse matrix adaptive-filtering, it is characterised in that accompanying method includes following
Step:
Step 1: system establishes SINS/DVL integrated positioning error model state variable X=[δ vE δvN α β γ δL δλ εE
εN εU δvd δΔ δC]T, wherein δ vEWith δ vNFor east, north orientation speed error, α, β, γ are the misaligned angle of the platform, and δ L and δ λ are warp
Spend error and latitude error, εE、εN、εUFor east, north, the gyroscopic drift of day direction, δ vdFor Doppler measurement velocity shifts error, δ
Δ is bias current angle error, and δ C is scale coefficient error；
Step 2: three axis angular rate information and acceleration measuring of the system by the gyroscope sensitive carrier in Inertial Navigation Unit
Obtain three axis (east, north, day) acceleration information aE、aN、aH, speed v is obtained by Doppler logd, drift angle Δ and posture etc. lead
Boat information；
Step 3: system is by the component of acceleration a in these three directionsE、aN、aHIt brings formula (1) into be integrated respectively, can obtain
To carrier along the velocity component v in these three directionsE、vN、vH.It brings three velocity components into formula (2) again and is integrated and carried
Longitude L, latitude λ and the depth d of body, wherein R is earth radius, t0To move initial time, tkFor certain moment in motion process；
System brings these information in formula (3)~(15) into finds out corresponding each state variable X=[the δ v of error modelE δvN α
β γ δL δλ εE εN εU δvd δΔ δC]T:
Wherein, it is constant that Ω, which is gyroscopic vibration frequency, and g is that acceleration of gravity is constant, Δ aN, Δ aEFor acceleration error；
East orientation and north orientation speed error formula:
The misaligned angle of the platform:
Location error:
Gyroscopic drift:
εE=-βEεE+wE (10)
εN=-βNεN+wN (11)
εU=-βUεU+wU (12)
Error correlation time for gyroscope in east, north, day direction, wE, wN, wUFor white Gaussian noise；
Speed, drift angle and the error of graduation of Doppler log:
δvd=-βdδvd+wd (13)
δ Δ=- βΔδΔ+wΔ (14)
δ C=0 (15)
WhereinFor the correlation time of velocity shifts error and bias current angle error, wd、wΔFor white Gaussian noise；
Step 4: system establishes system state equation and system measurements equation, as shown in formula (16) and formula (21):
State equation description are as follows:
In formula:
WSINS=[0 0 aE aN 0 0 0 wE wN wU wd wΔ 0]T (17)
State Transitive Matrices F is established according to formula (3)~(15)SINS/DVL
Wherein:
Have:
For F6×6Have:
System measurements equation are as follows:
For HSINS/DVLAnd VSINS/DVLHave:
VSINS/DVL=[vE vN]T (23)
Here system noise variance matrix:
Measuring noise square difference battle array:
Step 5: system, which is established, improves adaptive filter algorithm without inverse matrix, and adaptive Kalman filter is improved to no inverse matrix
The state equation and measurement equation of wave algorithmic system are described, such as formula (26), (27),
State equation description are as follows:
Xk=FkXk-1+GWk (26)
System measurements equation are as follows:
Wherein, XkFor the state variable value at k moment；Xk-1For the state variable at k-1 moment；F is to act on Xk-1On state transformation
Coefficient；WkFor the state-noise value at k moment；G is to act on WkCoefficient；HkFor observation model coefficient, time of day space
It is mapped to observation space；ZkFor the observation at k moment, by the east orientation of inertial navigation, north orientation speed error and Doppler log
The difference of east orientation north orientation velocity error is constituted；VkFor the observation noise value at k moment；
Step 6: by Fk, Gk, Wk, Hk, VkAnd initial state variable X brings the state equation of system into and measurement equation carries out
One-step prediction finds out the measuring value that the k moment predictsWith For the mistake of k moment measuring value
Difference；
Step 7: system is according to stability criteria, judgementSystem dissipates if setting up, Ying Cai
Step 8 is executed with no inverse matrix adaptive-filtering；The system convergence if invalid, should be using no inverse matrix strong tracking Kalman
Filtering executes step 9；
Step 8: system will correspond to parameter and parameter is substituted into formula (28)~(40) and calculated
λk(1)=[Hk(1)Lk(1)+λk(1)]-1 (34)
λk(2)=[Hk(2)Lk(2)+λk(2)]-1 (38)
Wherein,For the state variable value at k moment；WithThe transition value of state variable is sought for the k moment；For by k-1
The predicted value at the state variable at moment obtained k moment；Fk,k-1To act onOn state transformation coefficient；KkFor effect
?On without inverse matrix Kalman's coefficient；λkFor transition constant and Lk Lk∈Rn×1；HkFor observation model coefficient, true shape
State space is mapped to observation spaceZkFor the observation at k moment, the measuring value that the k moment is predicted is
Wherein Zk=[Zk(1)Zk(2)]T, Pk,k-1For prior estimate error covariance value；For k moment error covariance transition value；Pk
For Posterior estimator error covariance value；In the formula of (28)~(40)Respectively by time-varying noise estimation
Equation calculates to obtain formula (41)~(44):
It realizes and is operated without inverse matrix adaptive-filtering, obtain subsequent time state variable estimateExecute step 10；
Step 9: system will correspond to parameter and amount is substituted into formula (45)~(60) and calculated:
λk+1=diag [λ1(k+1),λ2(k+1),…,λm(k+1)] (48)
λk(1)=[Hk(1)Lk(1)+λk(1)]-1 (54)
λk(2)=[Hk(2)Lk(2)+λk(2)]-1 (58)
It realizes without inverse matrix strong tracking Kalman filter, obtains subsequent time state variable estimateExecute step 10；
Step 10: system update converts k=k+1, the state variable estimate that will be obtainedIt is denoted as new state variable value
Step 11: system judges whether k is equal to n, if it is, executing step 12, step 6 is otherwise executed；
Step 12: the system state variables sequence that system is generated Output is most
Terminate fruitAs state outcome of the current time after filtering and calibration includeIt is the correction result of the margin of error；In conjunction with working as
The observation Y of preceding moment SINS/DVL integrated positioning systemn=[vEn vNn Lnλn vdn]T, after being corrected aircraft east
To, north orientation speedAnd latitude and longitude informationAnd the velocity information of Doppler logWherein
Step 13: system judges whether SINS/DVL integrated positioning task is completed, if so, executing step 14, otherwise hold
Row step 2；
Step 14: SINS/DVL integrated positioning task of the system finishing without inverse matrix adaptive-filtering.
2. the step of SINS/DVL combined positioning method of a kind of no inverse matrix adaptive-filtering according to claim 1 seven
It further illustrates, it is characterised in that the criterion whether filtering dissipates is judged, according to the big of the error and anticipation error for filtering estimation
Small relationship judges.
3. the step of SINS/DVL combined positioning method of a kind of no inverse matrix adaptive-filtering according to claim 1 eight
Further illustrated with step 9, it is characterised in that using it is a kind of can guarantee filtering accuracy and reliability without inverse matrix operation method,
Avoid matrix inversion operation process, on the basis of reducing calculation amount, guarantee that operational process is reliable and stable, improve it is long-range underwater from
The positioning accuracy of main navigation.
4. a kind of SINS/DVL combined positioning method of no inverse matrix adaptive-filtering according to claim 1 make into
One step explanation, it is characterised in that use without inverse matrix algorithm, can be avoided due to interference or aircraft speed of a ship or plane course occur
The problem of navigation accuracy caused by sudden change and quality decline quickly can carry out real-time tracking to system mode and reduce mistake
The accumulating rate of difference.
5. a kind of SINS/DVL combined positioning method of no inverse matrix adaptive-filtering according to claim 1 is made into one
Walk explanation, it is characterised in that using method for adaptive kalman filtering is improved, can not only guarantee filtering accuracy, but also can effectively hinder
Only filtering dissipates.
6. a kind of SINS/DVL combined positioning method of no inverse matrix adaptive-filtering according to claim 1 is made into one
Walk explanation, it is characterised in that using no inverse matrix improvement adaptive filter algorithm, will quickly calculate without finding the inverse matrix algorithm with
Adaptive Kalman algorithm fusion is improved, the accuracy of blending algorithm is promoted compared to the algorithm arithmetic speed before fusion
25%.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910488838.2A CN110146076B (en) | 2019-06-06 | 2019-06-06 | SINS/DVL combined positioning method without inverse matrix adaptive filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910488838.2A CN110146076B (en) | 2019-06-06 | 2019-06-06 | SINS/DVL combined positioning method without inverse matrix adaptive filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110146076A true CN110146076A (en) | 2019-08-20 |
| CN110146076B CN110146076B (en) | 2023-04-18 |
Family
ID=67590663
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910488838.2A Active CN110146076B (en) | 2019-06-06 | 2019-06-06 | SINS/DVL combined positioning method without inverse matrix adaptive filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110146076B (en) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110514203A (en) * | 2019-08-30 | 2019-11-29 | 东南大学 | A kind of underwater Combinated navigation method based on ISR-UKF |
| CN110873813A (en) * | 2019-12-02 | 2020-03-10 | 中国人民解放军战略支援部队信息工程大学 | Water flow velocity estimation method, integrated navigation method and device |
| CN111597500A (en) * | 2020-06-19 | 2020-08-28 | 北京圣涛平试验工程技术研究院有限责任公司 | Uncertainty optimization inverse reliability evaluation method and device for DVL (dynamic Voltage Locus) mounting angle |
| CN112254718A (en) * | 2020-08-04 | 2021-01-22 | 东南大学 | Motion constraint assisted underwater combined navigation method based on improved Sage-Husa adaptive filtering |
| CN112710304A (en) * | 2020-12-17 | 2021-04-27 | 西北工业大学 | Underwater autonomous vehicle navigation method based on adaptive filtering |
| CN112729291A (en) * | 2020-12-29 | 2021-04-30 | 东南大学 | SINS/DVL ocean current velocity estimation method for deep-submergence long-endurance submersible |
| CN113472318A (en) * | 2021-07-14 | 2021-10-01 | 青岛杰瑞自动化有限公司 | Hierarchical self-adaptive filtering method and system considering observation model errors |
| CN114111840A (en) * | 2021-11-12 | 2022-03-01 | 哈尔滨工业大学 | DVL error parameter online calibration method based on integrated navigation |
| CN115031727A (en) * | 2022-03-31 | 2022-09-09 | 哈尔滨工程大学 | Initial alignment method of Doppler assisted strapdown inertial navigation system based on state transformation |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102608596A (en) * | 2012-02-29 | 2012-07-25 | 北京航空航天大学 | Information fusion method for airborne inertia/Doppler radar integrated navigation system |
| CN103278163A (en) * | 2013-05-24 | 2013-09-04 | 哈尔滨工程大学 | Nonlinear-model-based SINS/DVL (strapdown inertial navigation system/doppler velocity log) integrated navigation method |
| CN104180804A (en) * | 2014-09-11 | 2014-12-03 | 东南大学 | Single reference node underwater vehicle integrated navigation method based on underwater information network |
| CN107063245A (en) * | 2017-04-19 | 2017-08-18 | 东南大学 | A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF |
| WO2018014602A1 (en) * | 2016-07-19 | 2018-01-25 | 东南大学 | Volume kalman filtering method suitable for high-dimensional gnss/ins deep coupling |
| CN109141436A (en) * | 2018-09-30 | 2019-01-04 | 东南大学 | The improved Unscented kalman filtering algorithm application method in integrated navigation under water |
| 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 |
| CN109737959A (en) * | 2019-03-20 | 2019-05-10 | 哈尔滨工程大学 | A kind of polar region Multi-source Information Fusion air navigation aid based on federated filter |
-
2019
- 2019-06-06 CN CN201910488838.2A patent/CN110146076B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102608596A (en) * | 2012-02-29 | 2012-07-25 | 北京航空航天大学 | Information fusion method for airborne inertia/Doppler radar integrated navigation system |
| CN103278163A (en) * | 2013-05-24 | 2013-09-04 | 哈尔滨工程大学 | Nonlinear-model-based SINS/DVL (strapdown inertial navigation system/doppler velocity log) integrated navigation method |
| CN104180804A (en) * | 2014-09-11 | 2014-12-03 | 东南大学 | Single reference node underwater vehicle integrated navigation method based on underwater information network |
| WO2018014602A1 (en) * | 2016-07-19 | 2018-01-25 | 东南大学 | Volume kalman filtering method suitable for high-dimensional gnss/ins deep coupling |
| CN107063245A (en) * | 2017-04-19 | 2017-08-18 | 东南大学 | A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF |
| 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 |
| CN109141436A (en) * | 2018-09-30 | 2019-01-04 | 东南大学 | The improved Unscented kalman filtering algorithm application method in integrated navigation under water |
| CN109737959A (en) * | 2019-03-20 | 2019-05-10 | 哈尔滨工程大学 | A kind of polar region Multi-source Information Fusion air navigation aid based on federated filter |
Non-Patent Citations (2)
| Title |
|---|
| 张玉龙等: "基于极大似然估计的新息自适应滤波算法", 《传感器与微系统》 * |
| 魏延辉等: "基于改进自适应滤波的SINS/DVL组合导航算法研究", 《自动化与仪表》 * |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110514203A (en) * | 2019-08-30 | 2019-11-29 | 东南大学 | A kind of underwater Combinated navigation method based on ISR-UKF |
| CN110873813A (en) * | 2019-12-02 | 2020-03-10 | 中国人民解放军战略支援部队信息工程大学 | Water flow velocity estimation method, integrated navigation method and device |
| CN111597500A (en) * | 2020-06-19 | 2020-08-28 | 北京圣涛平试验工程技术研究院有限责任公司 | Uncertainty optimization inverse reliability evaluation method and device for DVL (dynamic Voltage Locus) mounting angle |
| CN111597500B (en) * | 2020-06-19 | 2023-03-21 | 北京圣涛平试验工程技术研究院有限责任公司 | Uncertainty optimization inverse reliability evaluation method and device for DVL (dynamic Voltage Locus) mounting angle |
| CN112254718A (en) * | 2020-08-04 | 2021-01-22 | 东南大学 | Motion constraint assisted underwater combined navigation method based on improved Sage-Husa adaptive filtering |
| CN112254718B (en) * | 2020-08-04 | 2024-04-09 | 东南大学 | Motion constraint assisted underwater integrated navigation method based on improved Sage-Husa self-adaptive filtering |
| US11754400B2 (en) | 2020-08-04 | 2023-09-12 | Southeast University | Motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering |
| WO2022028286A1 (en) * | 2020-08-04 | 2022-02-10 | 东南大学 | Motion constraint-aided underwater integrated navigation method employing improved sage-husa adaptive filtering |
| CN112710304B (en) * | 2020-12-17 | 2022-12-13 | 西北工业大学 | Underwater autonomous vehicle navigation method based on adaptive filtering |
| CN112710304A (en) * | 2020-12-17 | 2021-04-27 | 西北工业大学 | Underwater autonomous vehicle navigation method based on adaptive filtering |
| CN112729291B (en) * | 2020-12-29 | 2022-03-04 | 东南大学 | SINS/DVL ocean current velocity estimation method for deep-submergence long-endurance submersible |
| CN112729291A (en) * | 2020-12-29 | 2021-04-30 | 东南大学 | SINS/DVL ocean current velocity estimation method for deep-submergence long-endurance submersible |
| CN113472318A (en) * | 2021-07-14 | 2021-10-01 | 青岛杰瑞自动化有限公司 | Hierarchical self-adaptive filtering method and system considering observation model errors |
| CN113472318B (en) * | 2021-07-14 | 2024-02-06 | 青岛杰瑞自动化有限公司 | Hierarchical self-adaptive filtering method and system considering observation model errors |
| CN114111840B (en) * | 2021-11-12 | 2022-08-26 | 哈尔滨工业大学 | DVL error parameter online calibration method based on integrated navigation |
| CN114111840A (en) * | 2021-11-12 | 2022-03-01 | 哈尔滨工业大学 | DVL error parameter online calibration method based on integrated navigation |
| CN115031727A (en) * | 2022-03-31 | 2022-09-09 | 哈尔滨工程大学 | Initial alignment method of Doppler assisted strapdown inertial navigation system based on state transformation |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110146076B (en) | 2023-04-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110146076A (en) | A kind of SINS/DVL combined positioning method of no inverse matrix adaptive-filtering | |
| CN110146075A (en) | A kind of SINS/DVL combined positioning method of gain compensation adaptive-filtering | |
| US11105633B2 (en) | Navigation system utilizing yaw rate constraint during inertial dead reckoning | |
| US20210041239A1 (en) | Integrated navigation method for mobile vehicle | |
| CN101082493B (en) | Combined positioning method of agricultural machines navigation | |
| EP2856273B1 (en) | Pose estimation | |
| US7739045B2 (en) | Rapid self-alignment of a strapdown inertial system through real-time reprocessing | |
| US9037411B2 (en) | Systems and methods for landmark selection for navigation | |
| Stančić et al. | The integration of strap-down INS and GPS based on adaptive error damping | |
| CA3003298A1 (en) | Gnss and inertial navigation system utilizing relative yaw as an observable for an ins filter | |
| US20090119016A1 (en) | Vehicular present position detection apparatus and program storage medium | |
| JP6413946B2 (en) | Positioning device | |
| CN103278837A (en) | Adaptive filtering-based SINS/GNSS (strapdown inertial navigation system/global navigation satellite system) multistage fault-tolerant integrated navigation method | |
| AU2009200190A1 (en) | Methods and systems for underwater navigation | |
| JP6488860B2 (en) | Gradient estimation apparatus and program | |
| CN110940340A (en) | Multi-sensor information fusion method based on small UUV platform | |
| KR20020080829A (en) | IMU-GPS Integrated System including error correction system, Method for reducing search space of integer ambiguity, Method for detecting Cycle slip, and position, velocity, attitude determination Method using the same | |
| Emami et al. | A low complexity integrated navigation system for underwater vehicles | |
| CN113218389B (en) | Vehicle positioning method, device, storage medium and computer program product | |
| RU2202102C2 (en) | Procedure establishing positions of mobile objects and device for its realization | |
| RU2539131C1 (en) | Strapdown integrated navigation system of average accuracy for mobile onshore objects | |
| CN111649762B (en) | Inertial Doppler full-parameter high-precision calibration method and device | |
| CN111712855A (en) | Ground information processing method and device and unmanned vehicle | |
| Feng et al. | Adaptive BDS/MEMS INS navigation algorithm for low-dynamic carrier based on motion characteristics detection and dual-antenna Position/Velocity constraints | |
| Zhang | Autonomous underwater vehicle navigation using an adaptive Kalman filter for sensor fusion |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |