CN112212862A - GPS/INS integrated navigation method for improving particle filtering - Google Patents

GPS/INS integrated navigation method for improving particle filtering Download PDF

Info

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
Application number
CN202011016999.0A
Other languages
Chinese (zh)
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.)
Tianjin University of Technology
Original Assignee
Tianjin University of Technology
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 Tianjin University of Technology filed Critical Tianjin University of Technology
Priority to CN202011016999.0A priority Critical patent/CN112212862A/en
Publication of CN112212862A publication Critical patent/CN112212862A/en
Pending legal-status Critical Current

Links

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

GPS/INS integrated navigation method for improving particle filtering
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):
Figure BDA0002699388700000021
in the formula (I), the compound is shown in the specification,
Figure BDA0002699388700000022
a differential representing a state;
x (t) represents the error state of the system, x (t) is a 15-dimensional vector:
Figure BDA0002699388700000023
wherein the content of the first and second substances,
Figure BDA0002699388700000024
in order to error the angle towards the platform,
Figure BDA0002699388700000025
is the error angle of the north platform,
Figure BDA0002699388700000026
is the error angle of the platform in the sky direction;
Figure BDA0002699388700000027
in the east direction of the speed error,
Figure BDA0002699388700000028
is the error in the speed in the north direction,
Figure BDA0002699388700000029
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;
Figure BDA00026993887000000210
for the accelerometer to be zero offset along the x-direction,
Figure BDA00026993887000000211
for the accelerometer zero offset along the y-direction,
Figure BDA00026993887000000212
zero offset for accelerometer along z direction;
a (t) is a 15 x 15 dimensional matrix,
Figure BDA00026993887000000213
wherein the content of the first and second substances,
Figure BDA00026993887000000214
Figure BDA0002699388700000031
Figure BDA0002699388700000032
Figure BDA0002699388700000033
Figure BDA0002699388700000034
Figure BDA0002699388700000035
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;
Figure BDA0002699388700000036
a strapdown matrix, b represents a carrier coordinate system, and n represents a navigation coordinate system;
b (t) is a system noise driving matrix, B (t) is a 15 × 6 dimensional matrix:
Figure BDA0002699388700000037
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):
Figure BDA0002699388700000041
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;
Figure BDA0002699388700000042
wpos(t) position observation noise;
taking the speed error as a measurement value, establishing a speed measurement equation as shown in formula (3):
Figure BDA0002699388700000043
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;
Figure BDA0002699388700000044
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)
in the formula (I), the compound is shown in the specification,
Figure BDA0002699388700000045
further, step 3 comprises:
step 3-1, initialization: when t is 0, according to the initial state distribution
Figure BDA0002699388700000046
Generating initial particles
Figure BDA0002699388700000047
And the normalized weight corresponding to the initial particle
Figure BDA0002699388700000048
i 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:
Figure BDA0002699388700000051
wherein the content of the first and second substances,
Figure BDA0002699388700000052
is the ith particle state value at time k, XkIs the state vector for time k, and,
Figure BDA0002699388700000053
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;
step 3-3, calculating particles
Figure BDA0002699388700000054
The importance weight of (2) is shown in formula (5):
Figure BDA0002699388700000055
in the formula (I), the compound is shown in the specification,
Figure BDA0002699388700000056
are particles
Figure BDA0002699388700000057
Importance of, ZkIs an observed value at the time of k,
Figure BDA0002699388700000058
in order to be a function of the likelihood,
Figure BDA0002699388700000059
q (-) is a probability transition density function, and q (-) is an importance probability density function;
if it is not
Figure BDA00026993887000000510
Then the particle is a low weight particle and step 3-4 is performed; if it is not
Figure BDA00026993887000000511
Then, 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):
Figure BDA00026993887000000512
Figure BDA00026993887000000513
in the formula of UbIs an upper boundary, LbIs the lower boundary;
Figure BDA00026993887000000514
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
Figure BDA00026993887000000515
Step 3-4b, generating a new one from the high likelihood regionParticles
Figure BDA00026993887000000516
Figure BDA00026993887000000517
Where φ is a value randomly generated in [0, 1 ];
step 3-4c, calculating particles
Figure BDA00026993887000000518
The importance weight of (2) is shown as formula (9):
Figure BDA0002699388700000061
in the formula (I), the compound is shown in the specification,
Figure BDA0002699388700000062
are particles
Figure BDA0002699388700000063
The importance weight of (a) is determined,
Figure BDA0002699388700000064
are particles
Figure BDA0002699388700000065
The likelihood function of (a) is,
Figure BDA0002699388700000066
are particles
Figure BDA0002699388700000067
The probability transition density function of (a) is,
Figure BDA0002699388700000068
are particles
Figure BDA0002699388700000069
Summary of importance ofA rate density function;
if it is not
Figure BDA00026993887000000610
Then, step 3-4d is performed; if it is not
Figure BDA00026993887000000611
Then, go back to step 3-4b to generate a new particle and calculate the importance weight of the particle according to step 3-4c until
Figure BDA00026993887000000612
Until and executing steps 3-4 d; where ξ is a second threshold ξ ═ 1/N;
step 3-4d, when
Figure BDA00026993887000000613
When, if
Figure BDA00026993887000000614
Then, step 3-5 is executed; if it is not
Figure BDA00026993887000000615
Then, return to step 3-4a while updating χbestAnd wbestSo that
Figure BDA00026993887000000616
And performing steps 3-4a to 3-4d until
Figure BDA00026993887000000617
Until 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):
Figure BDA00026993887000000618
in the formula (I), the compound is shown in the specification,
Figure BDA00026993887000000619
the estimated value is obtained without self-adaptive weight adjustment;
Figure BDA00026993887000000620
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:
Figure BDA0002699388700000071
in the formula (I), the compound is shown in the specification,
Figure BDA0002699388700000072
a differential representing a state;
x (t) represents the state of the systematic error, x (t) is a 15-dimensional vector:
Figure BDA0002699388700000073
wherein the content of the first and second substances,
Figure BDA0002699388700000074
in order to error the angle towards the platform,
Figure BDA0002699388700000075
is the error angle of the north platform,
Figure BDA0002699388700000076
is the error angle of the platform in the sky direction;
Figure BDA0002699388700000077
in the east direction of the speed error,
Figure BDA0002699388700000078
is the error in the speed in the north direction,
Figure BDA0002699388700000079
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;
Figure BDA00026993887000000710
for the accelerometer to be zero offset along the x-direction,
Figure BDA00026993887000000711
for the accelerometer zero offset along the y-direction,
Figure BDA00026993887000000712
zero offset for accelerometer along z direction;
a (t) is a 15 x 15 dimensional matrix,
Figure BDA00026993887000000713
wherein the content of the first and second substances,
Figure BDA00026993887000000714
Figure BDA0002699388700000081
Figure BDA0002699388700000082
Figure BDA0002699388700000083
Figure BDA0002699388700000084
Figure BDA0002699388700000085
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;
Figure BDA0002699388700000086
a strapdown matrix, b represents a carrier coordinate system, and n represents a navigation coordinate system;
b (t) is a system noise driving matrix, B (t) is a 15 × 6 dimensional matrix:
Figure BDA0002699388700000087
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):
Figure BDA0002699388700000091
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;
Figure BDA0002699388700000092
wpos(t) position observation noise;
taking the speed error as a measurement value, establishing a speed measurement equation as shown in formula (3):
Figure BDA0002699388700000093
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;
Figure BDA0002699388700000094
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)
in the formula (I), the compound is shown in the specification,
Figure BDA0002699388700000095
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 distribution
Figure BDA0002699388700000101
Generating initial particles
Figure BDA0002699388700000102
And normalized weight of initial particle
Figure BDA0002699388700000103
i 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:
Figure BDA0002699388700000104
wherein the content of the first and second substances,
Figure BDA0002699388700000105
is the ith particle state value at time k, XkIs the state vector for time k, and,
Figure BDA0002699388700000106
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;
step 3-3, calculating particles
Figure BDA0002699388700000107
The importance weight of (2) is shown in formula (5):
Figure BDA0002699388700000108
in the formula (I), the compound is shown in the specification,
Figure BDA0002699388700000109
are particles
Figure BDA00026993887000001010
Importance of, ZkIs an observed value at the time of k,
Figure BDA00026993887000001011
in order to be a function of the likelihood,
Figure BDA00026993887000001012
q (-) is a probability transition density function, and q (-) is an importance probability density function;
if it is not
Figure BDA00026993887000001013
Then the particle is a low weight particle and step 3-4 is performed; if it is not
Figure BDA00026993887000001014
Then, 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):
Figure BDA00026993887000001015
Figure BDA00026993887000001016
in the formula of UbIs an upper boundary, LbIs the lower boundary;
Figure BDA00026993887000001017
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
Figure BDA00026993887000001018
Step 3-4b, generating a new particle from the high likelihood region
Figure BDA00026993887000001019
Figure BDA00026993887000001020
Where φ is a value randomly generated in [0, 1 ];
step 3-4c, calculating particles
Figure BDA00026993887000001021
The importance weight of (2) is shown as formula (9):
Figure BDA0002699388700000111
in the formula (I), the compound is shown in the specification,
Figure BDA0002699388700000112
are particles
Figure BDA0002699388700000113
The importance weight of (a) is determined,
Figure BDA0002699388700000114
are particles
Figure BDA0002699388700000115
The likelihood function of (a) is,
Figure BDA0002699388700000116
are particles
Figure BDA0002699388700000117
The probability transition density function of (a) is,
Figure BDA0002699388700000118
are particles
Figure BDA0002699388700000119
An importance probability density function of;
if it is not
Figure BDA00026993887000001110
Then, step 3-4d is performed; if it is not
Figure BDA00026993887000001111
Then, returning to step 3-4b to generate a new particle and calculating the importance weight of the particle according to step 3-4c until
Figure BDA00026993887000001112
Until and executing steps 3-4 d; where ξ is a second threshold ξ ═ 1/N;
step 3-4d, when
Figure BDA00026993887000001113
When, if
Figure BDA00026993887000001114
Then, step 3-5 is executed; if it is not
Figure BDA00026993887000001115
Then, return to step 3-4a while updating χbestAnd wbestSo that
Figure BDA00026993887000001116
And performing steps 3-4a to 3-4d until
Figure BDA00026993887000001117
Until 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):
Figure BDA00026993887000001118
in the formula (I), the compound is shown in the specification,
Figure BDA00026993887000001119
the estimated value is obtained without self-adaptive weight adjustment;
Figure BDA00026993887000001120
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):
Figure FDA0002699388690000011
in the formula (I), the compound is shown in the specification,
Figure FDA0002699388690000012
a differential representing a state;
x (t) represents the error state of the system, x (t) is a 15-dimensional vector:
Figure FDA0002699388690000013
wherein the content of the first and second substances,
Figure FDA0002699388690000014
in order to error the angle towards the platform,
Figure FDA0002699388690000015
is the error angle of the north platform,
Figure FDA0002699388690000016
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;
Figure FDA0002699388690000017
for the accelerometer to be zero offset along the x-direction,
Figure FDA0002699388690000018
for the accelerometer zero offset along the y-direction,
Figure FDA0002699388690000019
zero offset for accelerometer along z direction;
a (t) is a 15 x 15 dimensional matrix,
Figure FDA00026993886900000110
wherein the content of the first and second substances,
Figure FDA00026993886900000111
Figure FDA0002699388690000021
Figure FDA0002699388690000022
Figure FDA0002699388690000023
Figure FDA0002699388690000024
Figure FDA0002699388690000025
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;
Figure FDA0002699388690000026
a strapdown matrix, b represents a carrier coordinate system, and n represents a navigation coordinate system;
b (t) is a system noise driving matrix, B (t) is a 15 × 6 dimensional matrix:
Figure FDA0002699388690000027
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):
Figure FDA0002699388690000031
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;
Figure FDA0002699388690000032
wpos(t) position observation noise;
taking the speed error as a measurement value, establishing a speed measurement equation as shown in formula (3):
Figure FDA0002699388690000033
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;
Figure FDA0002699388690000034
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)
in the formula (I), the compound is shown in the specification,
Figure FDA0002699388690000035
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 distribution
Figure FDA0002699388690000041
Generating initial particles
Figure FDA0002699388690000042
And the normalized weight corresponding to the initial particle
Figure FDA0002699388690000043
i 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:
Figure FDA0002699388690000044
wherein the content of the first and second substances,
Figure FDA0002699388690000045
is the ith particle state value at time k, XkIs the state vector for time k, and,
Figure FDA0002699388690000046
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;
step 3-3, calculating particles
Figure FDA0002699388690000047
The importance weight of (2) is shown in formula (5):
Figure FDA0002699388690000048
in the formula (I), the compound is shown in the specification,
Figure FDA0002699388690000049
are particles
Figure FDA00026993886900000410
Importance of, ZkIs an observed value at the time of k,
Figure FDA00026993886900000411
in order to be a function of the likelihood,
Figure FDA00026993886900000412
q (-) is a probability transition density function, and q (-) is an importance probability density function;
if it is not
Figure FDA00026993886900000413
Then the particle is a low weight particle and step 3-4 is performed; if it is not
Figure FDA00026993886900000414
Then, 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):
Figure FDA00026993886900000415
Figure FDA00026993886900000416
in the formula of UbIs an upper boundary, LbIs the lower boundary;
Figure FDA00026993886900000417
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
Figure FDA00026993886900000418
Step 3-4b, generating a new particle from the high likelihood region
Figure FDA0002699388690000051
Figure FDA0002699388690000052
Where φ is a value randomly generated in [0, 1 ];
step 3-4c, calculating particles
Figure FDA0002699388690000053
The importance weight of (2) is shown as formula (9):
Figure FDA0002699388690000054
in the formula (I), the compound is shown in the specification,
Figure FDA0002699388690000055
are particles
Figure FDA0002699388690000056
The importance weight of (a) is determined,
Figure FDA0002699388690000057
are particles
Figure FDA0002699388690000058
The likelihood function of (a) is,
Figure FDA0002699388690000059
are particles
Figure FDA00026993886900000510
The probability transition density function of (a) is,
Figure FDA00026993886900000511
are particles
Figure FDA00026993886900000512
An importance probability density function of;
if it is not
Figure FDA00026993886900000513
Then, step 3-4d is performed; such asFruit
Figure FDA00026993886900000514
Then, go back to step 3-4b to generate a new particle and calculate the importance weight of the particle according to step 3-4c until
Figure FDA00026993886900000515
Until and executing steps 3-4 d; where ξ is a second threshold ξ ═ 1/N;
step 3-4d, when
Figure FDA00026993886900000516
When, if
Figure FDA00026993886900000517
Then, step 3-5 is executed; if it is not
Figure FDA00026993886900000518
Then, return to step 3-4a while updating χbestAnd wbestSo that
Figure FDA00026993886900000519
And performing steps 3-4a to 3-4d until
Figure FDA00026993886900000520
Until now and perform steps 3-5.
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):
Figure FDA00026993886900000521
in the formula (I), the compound is shown in the specification,
Figure FDA00026993886900000522
the estimated value is obtained without self-adaptive weight adjustment;
Figure FDA00026993886900000523
is an estimated value obtained through the adaptive weight adjustment processing.
CN202011016999.0A 2020-09-24 2020-09-24 GPS/INS integrated navigation method for improving particle filtering Pending CN112212862A (en)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Patent Citations (8)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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
CN109000640B (en) Vehicle GNSS/INS integrated navigation method based on discrete grey neural network model
CN107525503B (en) Adaptive cascade Kalman filtering method based on combination of dual-antenna GPS and MIMU
CN112097763B (en) Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination
CN111024064B (en) SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering
CN112212862A (en) GPS/INS integrated navigation method for improving particle filtering
CN112629538A (en) Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering
CN111121766B (en) Astronomical and inertial integrated navigation method based on starlight vector
CN111536972B (en) Vehicle-mounted DR navigation method based on odometer scale factor correction
CN113203418B (en) GNSSINS visual fusion positioning method and system based on sequential Kalman filtering
CN110231029B (en) Underwater robot multi-sensor fusion data processing method
CN112504275B (en) Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm
CN111024074B (en) Inertial navigation speed error determination method based on recursive least square parameter identification
CN111722295B (en) Underwater strapdown gravity measurement data processing method
CN113029139A (en) Airport flight area vehicle differential Beidou/SINS combined navigation method based on motion detection
CN113503892A (en) Inertial navigation system moving base initial alignment method based on odometer and backtracking navigation
CA2699137A1 (en) Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering
Zorina et al. Enhancement of INS/GNSS integration capabilities for aviation-related applications
CN113566850B (en) Method and device for calibrating installation angle of inertial measurement unit and computer equipment
CN114777812A (en) Method for estimating alignment and attitude of underwater integrated navigation system during traveling
CN110873577B (en) Underwater rapid-acting base alignment method and device
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
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
CN113985466A (en) Combined navigation method and system based on pattern recognition
CN111189473A (en) Heading and attitude system gyro error compensation method based on magnetic sensor and additional meter

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