CN112212862A - GPS/INS integrated navigation method for improving particle filtering - Google Patents
GPS/INS integrated navigation method for improving particle filtering Download PDFInfo
- Publication number
- CN112212862A CN112212862A CN202011016999.0A CN202011016999A CN112212862A CN 112212862 A CN112212862 A CN 112212862A CN 202011016999 A CN202011016999 A CN 202011016999A CN 112212862 A CN112212862 A CN 112212862A
- Authority
- CN
- China
- Prior art keywords
- error
- formula
- particles
- velocity
- along
- 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.)
- Pending
Links
- 239000002245 particle Substances 0.000 title claims abstract description 102
- 238000000034 method Methods 0.000 title claims abstract description 20
- 238000001914 filtration Methods 0.000 title claims abstract description 18
- 238000005259 measurement Methods 0.000 claims abstract description 36
- 238000012952 Resampling Methods 0.000 claims abstract description 6
- 230000008878 coupling Effects 0.000 claims abstract description 4
- 238000010168 coupling process Methods 0.000 claims abstract description 4
- 238000005859 coupling reaction Methods 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 12
- 230000003044 adaptive effect Effects 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
Images
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
- 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/20—Instruments for performing navigational calculations
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
The invention discloses a GPS/INS integrated navigation method for improving particle filtering, which comprises the following steps: step 1, selecting GPS/INS loose coupling combined navigation, and establishing a state equation by taking position information and speed information as observed quantities; step 2, establishing a measurement equation by taking the position error and the speed error as measurement values; and 3, constructing an improved particle filter model according to the state equation established in the step 1 and the measurement equation established in the step 2 to obtain the optimal position error estimation value and the optimal speed error estimation value. The method mainly utilizes the established GPS/INS state equation and measurement equation and utilizes the improved particle filter algorithm to realize the GPS/INS integrated navigation state estimation. Different from the conventional particle filtering, the improved particle filtering adopted by the invention is updated according to measurement, and after the weight value of the particles is calculated and normalized; selection operation is provided, and diversity of particles is realized; after the weights are calculated and normalized, a resampling step is performed, giving a state estimate.
Description
Technical Field
The invention relates to satellite navigation and inertial navigation technologies, in particular to a GPS/INS integrated navigation method based on improved particle filtering.
Background
The Inertial Navigation (INS) -satellite integrated navigation (GPS) technology can obtain more comprehensive positioning and carrier attitude parameters by utilizing the complementation of integrated navigation signals, eliminates the defects of easy lock losing and error accumulation of the navigation signals of a single navigation system and is beneficial to forming a low-cost integrated navigation system. In practical applications, some applications using the integrated inertial navigation and satellite navigation system require reliable high-precision navigation, and one of the implementations of the integrated inertial navigation and satellite navigation system high-precision navigation is to use a Particle Filter (PF). Documents [1] and [2] and the like do not solve the phenomenon of insufficient particle diversity when the GPS/INS combined navigation is realized by using particle filtering; document [3] estimates the corresponding state error from the current observation and corrects the predicted error of the particle before determining the weight of the particle and resampling. But there is also a problem of insufficient amount of sample diversity.
[1] Wanglin, Linxueyuan, Sunwei, Wangmeng, improved particle filtering algorithm and application thereof in GPS/SINS combined navigation [ J ]. proceedings of naval aeronautical engineering academy, 2016,31(01):51-57.
[2] Yanluo, Li Ming, Zhang C, a new improved particle filter algorithm [ J ]. Cegan university of electronic technology, 2010,37(05): 862-.
[3] Jiangjian, Liweifeng, Yaojian, Shiguoyou, application of improved particle filter algorithm in marine integrated navigation [ J ]. Shanghai university report, 2018,39(02):17-21+65.
Disclosure of Invention
The invention aims to overcome the defects in the prior art and provides a GPS/INS integrated navigation method based on improved particle filtering. Different from the conventional particle filtering, the improved particle filtering adopted by the invention is updated according to measurement, and after the weight value of the particles is calculated and normalized; selection operation is provided, and diversity of particles is realized; after the weights are calculated and normalized, a resampling step is performed, giving a state estimate.
The technical scheme adopted by the invention is as follows: a GPS/INS combined navigation method for improving particle filtering comprises the following steps:
step 1, selecting GPS/INS loose coupling combined navigation, and establishing a state equation by taking position information and speed information as observed quantities;
step 2, establishing a measurement equation by taking the position error and the speed error as measurement values;
and 3, constructing an improved particle filter model according to the state equation established in the step 1 and the measurement equation established in the step 2 to obtain the optimal position error estimation value and the optimal speed error estimation value.
Further, in step 1, the state equation is shown as formula (1):
x (t) represents the error state of the system, x (t) is a 15-dimensional vector:
wherein,in order to error the angle towards the platform,is the error angle of the north platform,is the error angle of the platform in the sky direction;in the east direction of the speed error,is the error in the speed in the north direction,a velocity error in the direction of the day; δ λ is latitude error, δ L is longitude error, δ h is altitude error; x is along the horizontal axis of the carrier, y is along the longitudinal axis of the carrier, and z is along the vertical direction of the carrier; epsilonxFor gyroscopic drift along the x-direction,. epsilonyFor gyro drift along the y-direction, epsilonzIs gyro drift along the z direction;for the accelerometer to be zero offset along the x-direction,for the accelerometer zero offset along the y-direction,zero offset for accelerometer along z direction;
wherein, ω isieFor self-rotation of the earthSpeed; reThe curvature radius of the earth prime unitary; l is latitude and h is height; v. ofEEast speed, vNIs the north velocity, vUThe speed in the direction of the day; f. ofeComponent of the specific force measured by the accelerometer in the east direction in the geographic coordinate system, fnFor the north component of the specific force measured by the accelerometer in the geographical coordinate system, fuA component of the measured specific force for the accelerometer in the direction of the day in the geographic coordinate system; alpha is the relevant time of the gyroscope, and beta is the relevant time of the accelerometer;a strapdown matrix, b represents a carrier coordinate system, and n represents a navigation coordinate system;
w (t) is the system noise vector: w (t) ═ wx,wy,wz,ax,ay,az]TWherein w isxWhite noise along the x-direction for the gyroscope, wyWhite noise in the y-direction for the gyroscope, wzWhite noise along the z-direction for the gyroscope; a isxWhite noise for the accelerometer along the x-direction, ayWhite noise a for accelerometers along the y-directionzWhite noise for the accelerometer along the z-direction, wx、wy、wz、ax、ayAnd azThe mean values of (A) and (B) are all 0 and are all normally distributed.
Further, in step 2, the position error is a difference between position information obtained by the inertial navigation system and position information output by the GPS receiver, and the velocity error is a difference between velocity information obtained by the inertial navigation system and velocity information output by the GPS receiver;
and (3) establishing a position measurement equation by taking the position error as a measurement value as shown in the formula (2):
in the formula, Zpos(t) represents a position error measurement; lambda [ alpha ]iLatitude, L, resolved for strapdown inertial navigationiResolving the obtained longitude for the strapdown inertial navigation; lambda [ alpha ]gLatitude, L, of GPS receiver outputgLongitude as output by the GPS receiver;wpos(t) position observation noise;
taking the speed error as a measurement value, establishing a speed measurement equation as shown in formula (3):
in the formula, Zvet(t) represents a velocity error measurement; v. ofieEast velocity, v, resolved for strapdown inertial navigationinResolving the obtained north velocity for the strapdown inertial navigation; v. ofgeEast velocity, v, of GPS receiver outputgnIs the northbound speed of the GPS receiver output;wvet(t) velocity observation noise;
the measurement equation is obtained by combining the formula (2) and the formula (3), and is shown in the formula (4):
Z=H(t)X(t)+w(t) (4)
further, step 3 comprises:
step 3-1, initialization: when t is 0, according to the initial state distributionGenerating initial particlesAnd the normalized weight corresponding to the initial particlei is 1,2, …, N is the number of particles;
step 3-2, when t is more than or equal to 1, sampling the particle set:wherein,is the ith particle state value at time k, XkIs the state vector for time k, and,set of i-th state values from 0 to time k-1, Z1:kThe set of observed values at the 1 st to k th moments is shown, and q (-) is an importance probability density function;
in the formula,are particlesImportance of, ZkIs an observed value at the time of k,in order to be a function of the likelihood,q (-) is a probability transition density function, and q (-) is an importance probability density function;
if it is notThen the particle is a low weight particle and step 3-4 is performed; if it is notThen, resampling is performed and step 3-5 is performed; where, ζ is a first threshold value, ζ is 1/N;
3-4, processing low-weight particles by adopting self-adaptive weight adjustment;
and 3-5, calculating to obtain the optimal position error estimation value and the optimal speed error estimation value at the moment k.
Further, in step 3-4, the processing low-weight particles by adaptive weight adjustment includes:
and 3-4a, defining an upper boundary and a lower boundary, wherein the upper boundary is shown as a formula (6), and the lower boundary is shown as a formula (7):
in the formula of UbIs an upper boundary, LbIs the lower boundary;expressed as a new set of particles, λ, generated from the high-likelihood regionkIs the standard deviation of the set of particles; chi shapebestThe particles of highest importance weight, χbestThe importance weight of is
Where φ is a value randomly generated in [0, 1 ];
in the formula,are particlesThe importance weight of (a) is determined,are particlesThe likelihood function of (a) is,are particlesThe probability transition density function of (a) is,are particlesSummary of importance ofA rate density function;
if it is notThen, step 3-4d is performed; if it is notThen, go back to step 3-4b to generate a new particle and calculate the importance weight of the particle according to step 3-4c untilUntil and executing steps 3-4 d; where ξ is a second threshold ξ ═ 1/N;
step 3-4d, whenWhen, ifThen, step 3-5 is executed; if it is notThen, return to step 3-4a while updating χbestAnd wbestSo thatAnd performing steps 3-4a to 3-4d untilUntil now and perform steps 3-5.
Further, in step 3-5, the optimal position error estimation value and the optimal speed error estimation value at time k are calculated according to equation (10):
in the formula,the estimated value is obtained without self-adaptive weight adjustment;is an estimated value obtained through the adaptive weight adjustment processing.
The invention has the beneficial effects that:
the GPS/INS integrated navigation method for improving the particle filtering can help ship drivers to better understand the navigation environment by fusing the data of the GPS and the INS, and improve the driving automation level and the navigation safety.
The invention improves the GPS/INS combined navigation method of particle filtering, the generated particles and the weight thereof are positioned on a high likelihood region, and more reliable state estimation can be obtained. The adaptive weight adjustment method processes low-weight particles, reduces sample degradation, and finally provides an optimal estimation value.
Drawings
FIG. 1: the invention improves the GPS/INS integrated navigation method structure flow diagram of the particle filter;
FIG. 2: the improved particle filter model of the invention is a schematic diagram.
Detailed Description
In order to further understand the contents, features and effects of the present invention, the following embodiments are illustrated and described in detail with reference to the accompanying drawings:
as shown in fig. 1 and fig. 2, a GPS/INS combined navigation method with improved particle filtering includes the following steps:
step 1, selecting GPS/INS loose coupling combined navigation, and establishing a state equation by taking position information and speed information as observed quantities.
The equation of state can be expressed as:
x (t) represents the state of the systematic error, x (t) is a 15-dimensional vector:
wherein,in order to error the angle towards the platform,is the error angle of the north platform,is the error angle of the platform in the sky direction;in the east direction of the speed error,is the error in the speed in the north direction,a velocity error in the direction of the day; δ λ is latitude error, δ L is longitude error, δ h is altitude error; x is along the horizontal axis of the carrier, y is along the longitudinal axis of the carrier, and z is along the vertical direction of the carrier; epsilonxFor gyroscopic drift along the x-direction,. epsilonyFor gyro drift along the y-direction, epsilonzIs gyro drift along the z direction;for the accelerometer to be zero offset along the x-direction,for the accelerometer zero offset along the y-direction,zero offset for accelerometer along z direction;
wherein, ω isieThe rotational angular velocity of the earth; reThe curvature radius of the earth prime unitary; l is latitude and h is height; v. ofEEast speed, vNIs the north velocity, vUThe speed in the direction of the day; f. ofeComponent of the specific force measured by the accelerometer in the east direction in the geographic coordinate system, fnFor the north component of the specific force measured by the accelerometer in the geographical coordinate system, fuA component of the measured specific force for the accelerometer in the direction of the day in the geographic coordinate system; alpha is the relevant time of the gyroscope, and beta is the relevant time of the accelerometer;a strapdown matrix, b represents a carrier coordinate system, and n represents a navigation coordinate system;
w (t) is the system noise vector: w (t) ═ wx,wy,wz,ax,ay,az]TWherein w isxWhite noise along the x-direction for the gyroscope, wyWhite noise in the y-direction for the gyroscope, wzWhite noise along the z-direction for the gyroscope; a isxWhite noise for the accelerometer along the x-direction, ayWhite noise a for accelerometers along the y-directionzWhite noise for the accelerometer along the z-direction, wx、wy、wz、ax、ayAnd azThe mean values of (A) and (B) are all 0 and are all normally distributed.
And 2, establishing a measurement equation by taking the position error and the speed error as measurement values.
The position error is the difference between the position information obtained by the inertial navigation system and the position information output by the GPS receiver, and similarly, the velocity error is the difference between the velocity information obtained by the inertial navigation system and the velocity information output by the GPS receiver.
And (3) establishing a position measurement equation by taking the position error as a measurement value as shown in the formula (2):
in the formula, Zpos(t) represents a position error measurement; lambda [ alpha ]iLatitude, L, resolved for strapdown inertial navigationiResolving the obtained longitude for the strapdown inertial navigation; lambda [ alpha ]gLatitude, L, of GPS receiver outputgLongitude as output by the GPS receiver;wpos(t) position observation noise;
taking the speed error as a measurement value, establishing a speed measurement equation as shown in formula (3):
in the formula, Zvet(t) represents a velocity error measurement; v. ofieEast velocity, v, resolved for strapdown inertial navigationinResolving the obtained north velocity for the strapdown inertial navigation; v. ofgeEast velocity, v, of GPS receiver outputgnIs the northbound speed of the GPS receiver output;wvet(t) velocity observation noise;
the measurement equation obtained by combining formula (2) and formula (3) is shown as formula (4):
Z=H(t)X(t)+w(t) (4)
and 3, constructing an improved particle filter model according to the state equation established in the step 1 and the measurement equation established in the step 2 to obtain the optimal position error estimation value and the optimal speed error estimation value.
The improved particle filter model is shown in fig. 2, and specifically includes:
step 3-1, initialization: when t is 0, according to the initial state distributionGenerating initial particlesAnd normalized weight of initial particlei is 1,2, …, N is the number of particles;
step 3-2, when t is more than or equal to 1, sampling the particle set:wherein,is the ith particle state value at time k, XkIs the state vector for time k, and,set of i-th state values from 0 to time k-1, Z1:kThe set of observed values at the 1 st to k th moments is shown, and q (-) is an importance probability density function;
in the formula,are particlesImportance of, ZkIs an observed value at the time of k,in order to be a function of the likelihood,q (-) is a probability transition density function, and q (-) is an importance probability density function;
if it is notThen the particle is a low weight particle and step 3-4 is performed; if it is notThen, resampling is performed and step 3-5 is performed; where, ζ is a first threshold value, ζ is 1/N;
3-4, adopting self-adaptive weight adjustment to process low-weight particles, comprising the following steps:
and 3-4a, defining an upper boundary and a lower boundary, wherein the upper boundary is shown as a formula (6), and the lower boundary is shown as a formula (7):
in the formula of UbIs an upper boundary, LbIs the lower boundary;expressed as a new set of particles, λ, generated from the high-likelihood regionkIs the standard deviation of the set of particles; chi shapebestThe particles of highest importance weight, χbestThe importance weight of is
Where φ is a value randomly generated in [0, 1 ];
in the formula,are particlesThe importance weight of (a) is determined,are particlesThe likelihood function of (a) is,are particlesThe probability transition density function of (a) is,are particlesAn importance probability density function of;
if it is notThen, step 3-4d is performed; if it is notThen, returning to step 3-4b to generate a new particle and calculating the importance weight of the particle according to step 3-4c untilUntil and executing steps 3-4 d; where ξ is a second threshold ξ ═ 1/N;
step 3-4d, whenWhen, ifThen, step 3-5 is executed; if it is notThen, return to step 3-4a while updating χbestAnd wbestSo thatAnd performing steps 3-4a to 3-4d untilUntil now and perform steps 3-5.
And 3-5, calculating to obtain the optimal position error estimation value and the optimal speed error estimation value at the k moment according to the formula (10):
in the formula,the estimated value is obtained without self-adaptive weight adjustment;is an estimated value obtained through the adaptive weight adjustment processing.
For equation (10), the position error estimation value and the velocity error estimation value are calculated by using equation (10), and when the position error is substituted, the position error estimation value is calculated, and when the velocity error is substituted, the velocity error estimation value is calculated.
Although the preferred embodiments of the present invention have been described above with reference to the accompanying drawings, the present invention is not limited to the above-described embodiments, which are merely illustrative and not restrictive, and those skilled in the art can make many modifications without departing from the spirit and scope of the present invention as defined in the appended claims.
Claims (6)
1. A GPS/INS combined navigation method for improving particle filtering is characterized by comprising the following steps:
step 1, selecting GPS/INS loose coupling combined navigation, and establishing a state equation by taking position information and speed information as observed quantities;
step 2, establishing a measurement equation by taking the position error and the speed error as measurement values;
and 3, constructing an improved particle filter model according to the state equation established in the step 1 and the measurement equation established in the step 2 to obtain the optimal position error estimation value and the optimal speed error estimation value.
2. The method as claimed in claim 1, wherein in step 1, the equation of state is represented by equation (1):
x (t) represents the error state of the system, x (t) is a 15-dimensional vector:wherein,in order to error the angle towards the platform,is the error angle of the north platform,is the error angle of the platform in the sky direction; delta vEFor east velocity error, δ vNVelocity error in the north direction, δ vUA velocity error in the direction of the day; δ λ is latitude error, δ L is longitude error, δ h is altitude error; x is along the horizontal axis of the carrier, y is along the longitudinal axis of the carrier, and z is along the vertical direction of the carrier; epsilonxFor gyroscopic drift along the x-direction,. epsilonyFor gyro drift along the y-direction, epsilonzIs gyro drift along the z direction;for the accelerometer to be zero offset along the x-direction,for the accelerometer zero offset along the y-direction,zero offset for accelerometer along z direction;
wherein, ω isieThe rotational angular velocity of the earth; reThe curvature radius of the earth prime unitary; l is latitude and h is height; v. ofEEast speed, vNIs the north velocity, vUThe speed in the direction of the day; f. ofeComponent of the specific force measured by the accelerometer in the east direction in the geographic coordinate system, fnFor the north component of the specific force measured by the accelerometer in the geographical coordinate system, fuA component of the measured specific force for the accelerometer in the direction of the day in the geographic coordinate system; alpha is the relevant time of the gyroscope, and beta is the relevant time of the accelerometer;a strapdown matrix, b represents a carrier coordinate system, and n represents a navigation coordinate system;
w (t) is the system noise vector: w (t) ═ wx,wy,wz,ax,ay,az]TWherein w isxWhite noise along the x-direction for the gyroscope, wyWhite noise in the y-direction for the gyroscope, wzWhite noise along the z-direction for the gyroscope; a isxWhite noise for the accelerometer along the x-direction, ayWhite noise a for accelerometers along the y-directionzWhite noise for the accelerometer along the z-direction, wx、wy、wz、ax、ayAnd azThe mean values of (A) and (B) are all 0 and are all normally distributed.
3. The integrated GPS/INS navigation method with improved particle filtering as claimed in claim 1, wherein in step 2, the position error is a difference between position information calculated by the inertial navigation system and position information output by the GPS receiver, and the velocity error is a difference between velocity information calculated by the inertial navigation system and velocity information output by the GPS receiver;
and (3) establishing a position measurement equation by taking the position error as a measurement value as shown in the formula (2):
in the formula, Zpos(t) represents a position error measurement; lambda [ alpha ]iLatitude, L, resolved for strapdown inertial navigationiResolving the obtained longitude for the strapdown inertial navigation; lambda [ alpha ]gLatitude, L, of GPS receiver outputgLongitude as output by the GPS receiver;wpos(t) position observation noise;
taking the speed error as a measurement value, establishing a speed measurement equation as shown in formula (3):
in the formula, Zvet(t) represents the velocityA measure error measurement value; v. ofieEast velocity, v, resolved for strapdown inertial navigationinResolving the obtained north velocity for the strapdown inertial navigation; v. ofgeEast velocity, v, of GPS receiver outputgnIs the northbound speed of the GPS receiver output;wvet(t) velocity observation noise;
the measurement equation is obtained by combining the formula (2) and the formula (3), and is shown in the formula (4):
Z=H(t)X(t)+w(t) (4)
4. the method as claimed in claim 1, wherein step 3 comprises:
step 3-1, initialization: when t is 0, according to the initial state distributionGenerating initial particlesAnd the normalized weight corresponding to the initial particlei is 1,2, …, N is the number of particles;
step 3-2, when t is more than or equal to 1, sampling the particle set:wherein,is the ith particle state value at time k, XkIs the state vector for time k, and,set of i-th state values from 0 to time k-1, Z1:kThe set of observed values at the 1 st to k th moments is shown, and q (-) is an importance probability density function;
in the formula,are particlesImportance of, ZkIs an observed value at the time of k,in order to be a function of the likelihood,q (-) is a probability transition density function, and q (-) is an importance probability density function;
if it is notThen the particle is a low weight particle and step 3-4 is performed; if it is notThen, resampling is performed and step 3-5 is performed; where, ζ is a first threshold value, ζ is 1/N;
3-4, processing low-weight particles by adopting self-adaptive weight adjustment;
and 3-5, calculating to obtain the optimal position error estimation value and the optimal speed error estimation value at the moment k.
5. The method as claimed in claim 4, wherein the step 3-4 of processing low-weight particles with adaptive weight adjustment comprises:
and 3-4a, defining an upper boundary and a lower boundary, wherein the upper boundary is shown as a formula (6), and the lower boundary is shown as a formula (7):
in the formula of UbIs an upper boundary, LbIs the lower boundary;expressed as a new set of particles, λ, generated from the high-likelihood regionkIs the standard deviation of the set of particles; chi shapebestThe particles of highest importance weight, χbestThe importance weight of is
Where φ is a value randomly generated in [0, 1 ];
in the formula,are particlesThe importance weight of (a) is determined,are particlesThe likelihood function of (a) is,are particlesThe probability transition density function of (a) is,are particlesAn importance probability density function of;
if it is notThen, step 3-4d is performed; such asFruitThen, go back to step 3-4b to generate a new particle and calculate the importance weight of the particle according to step 3-4c untilUntil and executing steps 3-4 d; where ξ is a second threshold ξ ═ 1/N;
6. The GPS/INS integrated navigation method with improved particle filtering as claimed in claim 4, wherein in step 3-5, the optimal position error estimate and velocity error estimate at time k are calculated according to equation (10):
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011016999.0A CN112212862A (en) | 2020-09-24 | 2020-09-24 | GPS/INS integrated navigation method for improving particle filtering |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011016999.0A CN112212862A (en) | 2020-09-24 | 2020-09-24 | GPS/INS integrated navigation method for improving particle filtering |
Publications (1)
Publication Number | Publication Date |
---|---|
CN112212862A true CN112212862A (en) | 2021-01-12 |
Family
ID=74051832
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011016999.0A Pending CN112212862A (en) | 2020-09-24 | 2020-09-24 | GPS/INS integrated navigation method for improving particle filtering |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112212862A (en) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114413892A (en) * | 2022-01-19 | 2022-04-29 | 东南大学 | Novel orchard robot combined navigation method |
CN116255988A (en) * | 2023-05-11 | 2023-06-13 | 北京航空航天大学 | Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation |
CN116299597A (en) * | 2023-05-18 | 2023-06-23 | 北京航空航天大学 | Navigation enhancement co-location method based on improved particle filter algorithm |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP1879149A1 (en) * | 2006-07-10 | 2008-01-16 | Fondazione Bruno Kessler | Method and apparatus for tracking a number of objects or object parts in image sequences |
CN104048676A (en) * | 2014-06-26 | 2014-09-17 | 哈尔滨工程大学 | MEMS (Micro Electro Mechanical System) gyroscope random error compensating method based on improved particle filter |
CN104502922A (en) * | 2014-12-09 | 2015-04-08 | 沈阳航空航天大学 | Autonomous integrity monitoring method for neural network assisted particle filter GPS (global positioning system) receiver |
CN104819716A (en) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | Indoor and outdoor personal navigation algorithm based on INS/GPS (inertial navigation system/global position system) integration of MEMS (micro-electromechanical system) |
CN105046016A (en) * | 2015-08-13 | 2015-11-11 | 电子科技大学 | Particle filtering weight optimization adaptive resampling method |
CN108646710A (en) * | 2018-05-10 | 2018-10-12 | 中国民航大学 | A kind of electro-hydraulic joint steering engine method for estimating state based on improvement volume particle filter |
CN109459044A (en) * | 2018-12-17 | 2019-03-12 | 北京计算机技术及应用研究所 | A kind of vehicle-mounted MEMS inertial navigation combination navigation method of GNSS double antenna auxiliary |
CN110926465A (en) * | 2019-12-11 | 2020-03-27 | 哈尔滨工程大学 | MEMS/GPS loose combination navigation method |
-
2020
- 2020-09-24 CN CN202011016999.0A patent/CN112212862A/en active Pending
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP1879149A1 (en) * | 2006-07-10 | 2008-01-16 | Fondazione Bruno Kessler | Method and apparatus for tracking a number of objects or object parts in image sequences |
CN104048676A (en) * | 2014-06-26 | 2014-09-17 | 哈尔滨工程大学 | MEMS (Micro Electro Mechanical System) gyroscope random error compensating method based on improved particle filter |
CN104502922A (en) * | 2014-12-09 | 2015-04-08 | 沈阳航空航天大学 | Autonomous integrity monitoring method for neural network assisted particle filter GPS (global positioning system) receiver |
CN104819716A (en) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | Indoor and outdoor personal navigation algorithm based on INS/GPS (inertial navigation system/global position system) integration of MEMS (micro-electromechanical system) |
CN105046016A (en) * | 2015-08-13 | 2015-11-11 | 电子科技大学 | Particle filtering weight optimization adaptive resampling method |
CN108646710A (en) * | 2018-05-10 | 2018-10-12 | 中国民航大学 | A kind of electro-hydraulic joint steering engine method for estimating state based on improvement volume particle filter |
CN109459044A (en) * | 2018-12-17 | 2019-03-12 | 北京计算机技术及应用研究所 | A kind of vehicle-mounted MEMS inertial navigation combination navigation method of GNSS double antenna auxiliary |
CN110926465A (en) * | 2019-12-11 | 2020-03-27 | 哈尔滨工程大学 | MEMS/GPS loose combination navigation method |
Non-Patent Citations (2)
Title |
---|
MOHAMED AHWIADI,等: "An Adaptive Particle Filter Technique for System State Estimation and Prognosis", 《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》 * |
胡世明: "GPS_INS组合导航数据融合算法研究", 《中国优秀硕士学位论文全文数据库》 * |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114413892A (en) * | 2022-01-19 | 2022-04-29 | 东南大学 | Novel orchard robot combined navigation method |
CN114413892B (en) * | 2022-01-19 | 2024-01-02 | 东南大学 | Novel combined navigation method for orchard robot |
CN116255988A (en) * | 2023-05-11 | 2023-06-13 | 北京航空航天大学 | Composite anti-interference self-adaptive filtering method based on ship dynamics combined navigation |
CN116299597A (en) * | 2023-05-18 | 2023-06-23 | 北京航空航天大学 | Navigation enhancement co-location method based on improved particle filter algorithm |
CN116299597B (en) * | 2023-05-18 | 2023-08-15 | 北京航空航天大学 | Navigation enhancement co-location method based on improved particle filter algorithm |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107525503B (en) | Adaptive cascade Kalman filtering method based on combination of dual-antenna GPS and MIMU | |
CN109000640B (en) | Vehicle GNSS/INS integrated navigation method based on discrete grey neural network model | |
CN111024064B (en) | SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering | |
CN112097763B (en) | Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination | |
CN112629538A (en) | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering | |
CN112212862A (en) | GPS/INS integrated navigation method for improving particle filtering | |
CN113203418B (en) | GNSSINS visual fusion positioning method and system based on sequential Kalman filtering | |
CN106500693B (en) | A kind of AHRS algorithm based on adaptive extended kalman filtering | |
CN111121766B (en) | Astronomical and inertial integrated navigation method based on starlight vector | |
CN110221332A (en) | A kind of the dynamic lever arm estimation error and compensation method of vehicle-mounted GNSS/INS integrated navigation | |
CN111536972B (en) | Vehicle-mounted DR navigation method based on odometer scale factor correction | |
CN112504275B (en) | Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm | |
CN110231029B (en) | Underwater robot multi-sensor fusion data processing method | |
CN110207691A (en) | A kind of more unmanned vehicle collaborative navigation methods based on data-link ranging | |
CN111722295B (en) | Underwater strapdown gravity measurement data processing method | |
CN111024074B (en) | Inertial navigation speed error determination method based on recursive least square parameter identification | |
CN113029139A (en) | Airport flight area vehicle differential Beidou/SINS combined navigation method based on motion detection | |
CA2699137A1 (en) | Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering | |
CN113503892A (en) | Inertial navigation system moving base initial alignment method based on odometer and backtracking navigation | |
CN108827345A (en) | A kind of air weapon Transfer Alignment based on lever arm deflection deformation compensation | |
Zorina et al. | Enhancement of INS/GNSS integration capabilities for aviation-related applications | |
CN113008229A (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
CN112292578B (en) | Ground level measuring method, measuring device, estimating device and data acquisition device for calculation | |
CN110873577A (en) | Underwater quick-acting base alignment method and device | |
CN116558511A (en) | SINS/GPS integrated navigation method for improving Sage-Husa self-adaptive filtering |
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 | ||
WD01 | Invention patent application deemed withdrawn after publication | ||
WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20210112 |