CN101907714B - GPS aided positioning system and method based on multi-sensor data fusion - Google Patents

Abstract

The invention relates to the technical field of navigation positioning, particularly to a navigation positioning technique based on multi-sensor data fusion. A GPS aided positioning system based on multi-sensor data fusion comprises a GPS navigation system, a gyroscope and accelerometer combined system, a data processing and fusion module and a map-matching module, wherein the GPS navigation system comprises a GPS module and a data collecting and processing module thereof and is used for acquiring the data of a carrier, such as GPS position, speed, time, number of satellites, and the degree of position accuracy; the gyroscope and accelerometer combined system comprises a gyroscope, an accelerometer and a data collecting and processing module and is used for acquiring the angular velocity, the speed and the accelerated speed of the carrier; the data processing and fusion module integrates the data acquired by the GPS navigation system and the gyroscope and accelerometer combined system so as to obtain the position information of the carrier; and the map-matching module corrects the error of position information of the carrier outputted by a digital electronic map and the data processing and fusion module so as to obtain the final position information of the carrier. The invention also provides a GPS aided positioning method based on multi-sensor data fusion.

Description

GPS assisted location method based on Fusion

Technical field

The present invention relates to technical field of navigation and positioning, be specifically related to the navigator fix technology based on Fusion.

Background technology

Existing navigator fix technology mostly is single navigational system.

Such as the most frequently used GPS (Global Positioning System) system, gps system is a kind of radio navigation system, as the new and high technology that is applied to the earliest navigation positioning system, have in the world, at any time, provide the continuously characteristics of high-precision three-dimensional Position, Velocity and Time information for the user under the meteorological condition arbitrarily, navigation accuracy is high, do not disperse in time, namely long-time stability are good.

But the GPS location depends on the satellite constellation that receiver is seen, so under some specific environment, GPS receiver lossing signal can not provide correct service, the location out of true has limited being widely used of it.

The reason of out of true location comprises: 1. multipath effect, and namely buildings is to the reflection of gps signal; 2. in the shade, " valley " that namely forms between high building and the high building in the city, under the dense vegetation, the signal reception is relatively poor; 3. in the tunnel, the signal losing lock that causes of underground parking; 4. prolonged initialization time in the area that receives signal difference; 5. some dynamic effects are such as the significant speedup of automobile and deceleration etc.Above reason can't be located by the GPS receiver or bearing accuracy descends, error even greater than 500 meters.

For another example dead reckoning DR (Dead Reckoning) system, DR system are for finding constantly a kind of system of position of vehicle, and it is to calculate according to particular course and the distance travelled of a certain position of having travelled of vehicle.Distance travelled is determined by time and the speed that magnetic compass and differential mileometer calculate.Its collection vehicle motion sensor information is carried out self-contained continuous navigation, and it is made of the sensor of measuring the course and the sensor of measuring distance, compares with GPS, and DR can independently locate, and the train locating information that does not exist the problem such as block to cause is lost.But the initial position of DR system can't independently obtain, and reckoning is a cumulative process, and different measuring error and the errors of calculation constantly all can be accumulated, As time goes on, if long-time separately use, the bearing accuracy of dead reckoning might descend.Therefore, can only guarantee the bearing accuracy of short time.

Therefore, single navigational system all can not satisfy the bearing accuracy demand under various complicated cases.

Summary of the invention

In view of this, in order addressing the above problem, to the invention discloses the GPS aided positioning system based on Fusion, can to improve bearing accuracy.

The object of the present invention is achieved like this: the GPS aided positioning system based on Fusion comprises

The GPS navigation system is comprised of GPS module and data acquisition thereof and processing module, is used for obtaining GPS position, speed, time, satellite number and the positional precision degree of strength of carrier;

Gyroscope and accelerometer combined system are comprised of gyroscope, accelerometer and data acquisition thereof and processing module, are used for obtaining angular velocity, speed and the acceleration of carrier;

Data are processed and Fusion Module, and the data that GPS navigation system and gyroscope and accelerometer combined system are obtained merge, and obtain carrier positions information; And

Map-matching module, digital electronic map and data are processed and the error of the carrier positions information that Fusion Module is exported is proofreaied and correct, and obtain the final position information of carrier.

Further, described data are processed and Fusion Module comprises data processing module and data fusion module, and described data fusion module is Kalman filter;

Further, described data processing module comprises Data correction module and dead reckoning module, the data that described Data correction module records gyroscope, accelerometer are transformed into the coordinate system with the GPS navigation systematic unity, and the data that described dead reckoning module records according to gyroscope, accelerometer are calculated the positional information of carrier;

Further, described map-matching module feeds back to the dead reckoning module with final position information, as the calculating initial value of dead reckoning module.

The present invention also provides the GPS assisted location method based on Fusion that can improve bearing accuracy, comprises the steps:

1) by the GPS navigation system, obtains position, speed, time, satellite number and the positional precision degree of strength of carrier;

2) by gyroscope and accelerometer combined system, obtain carrier angular velocity, speed and acceleration;

3) with step 1,2) data obtained merge, and obtain carrier positions information;

4) to step 3) the carrier positions information and the Digital map that obtain mate, and obtain the final position information of carrier and export.

Further, described step 3) specifically comprise the steps:

31) whether judge the satellite number greater than threshold value, if greater than threshold value, then export the GPS locating information to data fusion module, as no, then do not export the GPS locating information to data fusion module;

32) according to step 2) the carrier angular velocity, speed and the acceleration that obtain and map-matching module feedback carrier final position information carry out dead reckoning;

33) judge whether bearer rate is 0, in this way, the locating information that then obtains take dead reckoning is as carrier positions information, as no, and execution in step 34);

34) locating information of GPS navigation system acquisition and the locating information of dead reckoning acquisition are merged;

Further, be to carry out data fusion by Kalman filter described step 34);

Further, described step 34) specifically comprise the steps:

341) extracting position (x from the GPS locating information _g, y _g), speed v _g, direction θ _g, the positional precision degree of strength is calculated module extraction DR position (x from navigation _d, y _d), speed v _d, direction θ _d

342) according to positional precision degree of strength self-adaptation adjustment information distribution factor:

β ₂＝1-β ₁；

${\mathrm{\&beta;}}_1=\left\{\begin{array}{cc}0.99& \mathrm{PDOP}\&le;1\\ 1/\mathrm{PDOP}& 1<\mathrm{PDOP}<4\\ 1/4\mathrm{PDOP}& 4\&le;\mathrm{PDOP}\end{array}\right.;$

In the following formula, β ₁And β ₂Be respectively GPS locating information and navigation and calculate the information distribution factor of the locating information that obtains, PDOP is the positional precision degree of strength;

343) assign weight from the beginning according to the information distribution factor:

x ₁＝β ₁·x _g+β ₂·x _d

y ₁＝β ₁·y _g+β ₂·y _d；

344) by inferring that algorithm obtains positional information:

x ₂＝x ₀+(β ₁·v _g+β ₂v _d)·cos(β ₁·θ _g+β ₂·θ _d)

y ₂＝y ₀+(β ₁·v _g+β ₂v _d)·sin(β ₁·θ _g+β ₂·θ _d)；

X in the following formula ₀, y ₀Positional information for the previous moment;

345) process x ₁, x ₂, y ₁, y ₂The weight of distribution factor:

$v_t=\frac{\sqrt{{(x_{02}-x_{01})}^2+{(y_{02}-y_{01})}^2}}{T};$

$a_2=\frac{v_t-({\mathrm{\&beta;}}_1\&CenterDot;v_g+{\mathrm{\&beta;}}_2{\&CenterDot;v}_d)}{v_t};$

α ₁＝1-α ₂；

v _tTo calculate out bearer rate, x by the first two moment positional information ₀₁, y ₀₁And x ₀₂, y ₀₂Be respectively the constantly positional information of position of the first two, α ₁X ₁, y ₁Weight, α ₂X ₂, y ₂Weight;

346) obtain carrier positions information by following formula:

x＝α ₁·x ₁+α ₂·x ₂

y＝α ₁·y ₁+α ₂·y ₂。

The invention has the beneficial effects as follows: but the three-dimensional geographic coordinate in the GPS the real time measure corresponding dynamic moment of carrier utilized, and GAIS can be in order to determine the attitude of carrier in real time, in the situation of GPS normal operation, giving data with gps signal processes and Fusion Module, assign weight, make GPS information as main information, GAIS is as supplementary; When running into barriers such as " urban canyons ", gps signal can disappear or weaken, at this moment adopt GAIS information as main information, GPS information is as supplementary, some positions just in time can be used as the initial position of combination before GPS lost efficacy, had after the initial position, utilized GAIS just can constantly make estimation in the position of mobile vehicle to next.By follow-on Kalman filtering the two data of obtaining is merged, then adopt map-matching algorithm further to improve the precision of GPS and GAIS, finally can determine the orientation of mobile vehicle.Utilize the data of final navigation information feedback compensation GAIS, can obtain more accurate data.Utilize the characteristics of multi sensor combination location technology message complementary sense, adopt improved Kalman filter that gained information is carried out data fusion, obtain than the more accurate locator data in single-sensor location, applicable surface is wider, and can satisfy the location needs of mobile vehicle under the dynamic duty environment of complexity, can effectively improve the Performance And Reliability of mobile vehicle navigator fix, meet the requirement of high precision navigator fix.

Description of drawings

In order to make the purpose, technical solutions and advantages of the present invention clearer, the present invention is described in further detail below in conjunction with accompanying drawing:

Fig. 1 shows the structural representation of the GPS aided positioning system that the present invention is based on Fusion;

Fig. 2 shows the schematic flow sheet of data fusion step in the GPS assisted location method that the present invention is based on Fusion;

Fig. 3 shows the structural representation of map-matching module in the GPS aided positioning system that the present invention is based on Fusion;

Fig. 4 shows vehicle movement model synoptic diagram.

Embodiment

Below will be described in detail the preferred embodiments of the present invention.

Referring to Fig. 1, the GPS aided positioning system based on Fusion of present embodiment comprises GPS navigation system, gyroscope and accelerometer combined system, data processing and Fusion Module and map-matching module.

The GPS navigation system is comprised of GPS module and data acquisition thereof and processing module, is used for obtaining the GPS locating information of carrier, comprises position, speed, time, satellite number and positional precision degree of strength; The ultimate principle of GPS location technology is that satellite instantaneous position with high-speed motion is as the known data of starting at, satellite is paid no attention to ephemeris parameter and the temporal information that sends self disconnectedly, after user side GPS navigation system receives these information, adopt the method for space length resection, calculate the three-dimensional position of receiver.Specifically, the receiver of supposing the user carve at a time the method that adopts wireless distance finding recorded respectively receiver to transmitting station apart from d1, d2 and d3, if the location aware of 3 transmitting stations, then respectively take 3 transmitting stations as the centre of sphere, make 3 spheres take respective distances as radius, get final product the locus that intersection goes out receiver, in addition, also can obtain speed, time, satellite number and positional precision degree of strength PDOP (the Position Dilution Of Precision) data of carrier.

GAIS (Gyro and Accelerometer Integrated System) gyroscope and accelerometer combined system, formed by gyroscope, accelerometer and data acquisition thereof and processing module, be used for obtaining the data such as angular velocity, speed and acceleration of carrier; Gyroscope is a kind of exact instrument, so precision is his important performance indexes.In the not high rotator type gyroscope of precision, main disturbance torque is from the moment of friction in the frame bearing, and do not overlap reasons such as unbalance dynamic with gyro motor by gyroscope center of gravity and frame center.Gyrostatic principle is: when extraneous turning rate input is arranged, can produce gyroscopic couple.His big or small direction is relevant with adding angular velocity.It has gyroscopic inertia and precession two large characteristics, and wherein gyroscopic inertia is that the gyro of High Rotation Speed has very large inertia, makes its main spindle's keep stablizing constant at inertial space; Precession is under the effect of certain starting condition and certain moment of face, and gyroscope can when not stopping rotation, also ceaselessly rotate around another fixing rotating shaft.Present gyroscope is divided into, piezoelectric gyroscope, micro-mechanical gyroscope, fibre optic gyroscope, lasergyro all is electronic type, can and accelerometer, the magnetic resistance chip, GPS makes navigation control system.

Take lasergyro as example, by gyrostatic measurement, can obtain angular velocity and the corner of carrier.Therefore we can obtain following expression formula:

Δv＝kΩ (1)

$N=\underset{0}{\overset{\mathrm{\&Gamma;}}{\&Integral;}}\mathrm{k\&Omega;dt}=\mathrm{k\&theta;}---\left(2\right)$

$k=\frac{8A}{<L>\mathrm{\&lambda;}}---\left(3\right)$

Δ v is beat frequency in the formula; K is scale factor; Ω is rotational angular velocity; A is the annular resonance cavity area;＜L〉be the ring resonator loop-length; λ is optical maser wavelength.The output pulse frequency of laser gyro is proportional to Ω, and the output umber of pulse is proportional to θ, utilizes this output umber of pulse can obtain the angle increment of carrier, thereby can carry out resolving of mathematical platform.

The accelerometer principle of work: accelerometer forms by detecting quality (also claiming sensitive-mass), supporting, potentiometer, spring, damper and housing.Detecting the supported constraint of quality can only move along an axis, and this axle often is called input shaft or sensitive axes.When instrument housing was done accelerated motion along with carrier along the sensitive axes direction, according to Newton's law, the detection quality with certain inertia tried hard to keep its original motion state constant.To produce relative motion between it and the housing, make camber of spring, so detect quality thereupon accelerated motion under the effect of spring force.When the inertial force that produces when spring force and the accelerated motion of detection quality balances each other, detect between quality and the housing and just no longer include relative motion, the at this moment distortion of spring reflects by the size of measuring acceleration.Potentiometer is converted to electric signal to acceleration signal as displacement sensor, for output.Accelerometer is the oscillatory system of a single-degree-of-freedom in essence, can measure the frequency of vibration signal, the data that record of accelerometer is carried out quadratic integral just can obtain displacement.

In this module, the data that collect are processed, utilize gyroscope to measure the angular velocity of rotation of mobile vehicle and the speed that accelerometer is measured, the data such as acceleration.

Data are processed and Fusion Module, described data are processed and Fusion Module comprises data processing module and data fusion module, described data fusion module is Kalman filter, described data processing module comprises Data correction module and dead reckoning module, the data that described Data correction module records gyroscope, accelerometer are transformed into the coordinate system with the GPS navigation systematic unity, and the data that described dead reckoning module records according to gyroscope, accelerometer are calculated the positional information of carrier; The data that data are processed and Fusion Module obtains GPS navigation system and gyroscope and accelerometer combined system merge, and obtain carrier positions information, and the concrete grammar of fusion describes in detail in the content of back;

Map-matching module, digital electronic map and data are processed and the error of the carrier positions information that Fusion Module is exported is proofreaied and correct, obtain the final position information of carrier, and final position information is fed back to the dead reckoning module, as the calculating initial value of dead reckoning module.

GPS assisted location method based on Fusion of the present invention comprises the steps:

1) by the GPS navigation system, obtains position, speed, time, satellite number and the positional precision degree of strength of carrier;

2) by gyroscope and accelerometer combined system, obtain carrier angular velocity, speed and acceleration;

3) with step 1,2) data obtained merge, and obtain carrier positions information; Specifically comprise the steps:

31) whether judge the satellite number greater than threshold value, if greater than threshold value, then export the GPS locating information to data fusion module, as no, then do not export the GPS locating information to data fusion module;

32) according to step 2) the carrier angular velocity, speed and the acceleration that obtain and map-matching module feedback carrier final position information carry out dead reckoning;

Navigation calculates that concrete steps are as follows:

Referring to Fig. 4, the beginning Department of Geography coordinate system that the vehicle movement model is established vehicle originates in (x ₀, y ₀), k position (x constantly _k, y _k) can be represented by following expression formula:

$x_k=x_0+\underset{i=0}{\overset{k-1}{\mathrm{\&Sigma;}}}{S}_i\&CenterDot;\cos {\mathrm{\&theta;}}_i$

$y_k=y_0+\underset{i=0}{\overset{k-1}{\mathrm{\&Sigma;}}}{S}_i\&CenterDot;\sin {\mathrm{\&theta;}}_i$

${\mathrm{\&theta;}}_k={\mathrm{\&theta;}}_0+\underset{i=0}{\overset{k-1}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{\mathrm{\&theta;}}_i$

S in the following formula _iThe length of vehicle loxodrome, i.e. voyage, θ _iRepresent the course, i.e. the angle of line direction and x direction before the vehicle, Δ θ _iT _iConstantly to t _I+1The variable quantity in vehicle course constantly.When the sampling period, T was constant, the following formula formula was rewritten as:

$x_k=x_0+\underset{i=0}{\overset{k-1}{\mathrm{\&Sigma;}}}{v}_i\&CenterDot;T\&CenterDot;\cos {\mathrm{\&theta;}}_i$

$y_k=y_0+\underset{i=0}{\overset{k-1}{\mathrm{\&Sigma;}}}{v}_i\&CenterDot;T\&CenterDot;\sin {\mathrm{\&theta;}}_i$

${\mathrm{\&theta;}}_k={\mathrm{\&theta;}}_0+\underset{i=0}{\overset{k-1}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}_i\&CenterDot;T$

V in the following formula _iT _iSpeed constantly, ω _iT _iThe angular velocity of moment vehicle.

In the reckoning process, error can in order to reduce this error, be extracted final data conduct initial value in following formula that feedback is come along with the cumulative rises of time, so just can avoid this long error, the x in the following formula ₀, y ₀, θ ₀The data of being come by feedback determine.

33) judge whether bearer rate is 0, in this way, the locating information that then obtains take dead reckoning is as carrier positions information, as no, and execution in step 34)

34) locating information of GPS navigation system acquisition and the locating information of dead reckoning acquisition are merged; Specifically comprise the steps:

341) extracting position (x from the GPS locating information _g, y _g), speed v _g, direction θ _g, the positional precision degree of strength is calculated module extraction DR position (x from navigation _d, y _d), speed v _d, direction θ _d

342) be used for reflecting GPS location quality according to positional precision degree of strength in the GPS output parameter (Position Dilution Of Precision, PDOP) value.Usually, when the PDOP value less than 4 the time, GPS has substantially desirable bearing accuracy, positioning error E＜20m; When the PDOP value less than 1 the time, GPS has very high bearing accuracy, E＜10m; In case the PDOP value of GPS is greater than 4, bearing accuracy is difficult to be guaranteed, error even arrive hundreds of rice.According to the PDOP value of GPS and the empirical model of bearing accuracy, according to positional precision degree of strength self-adaptation adjustment information distribution factor:

β ₂＝1-β ₁；

${\mathrm{\&beta;}}_1=\left\{\begin{array}{cc}0.99& \mathrm{PDOP}\&le;1\\ 1/\mathrm{PDOP}& 1<\mathrm{PDOP}<4\\ 1/4\mathrm{PDOP}& 4\&le;\mathrm{PDOP}\end{array}\right.;$

In the following formula, β ₁And β ₂Be respectively GPS locating information and navigation and calculate the information distribution factor of the locating information that obtains, PDOP is the positional precision degree of strength;

343) assign weight from the beginning according to the information distribution factor:

x ₁＝β ₁·x _g+β ₂·x _d

y ₁＝β ₁·y _g+β ₂·y _d；

344) by inferring that algorithm obtains positional information:

x ₂＝x ₀+(β ₁·v _g+β ₂v _d)·cos(β ₁·θ _g+β ₂·θ _d)

y ₂＝y ₀+(β ₁·v _g+β ₂v _d)·sin(β ₁·θ _g+β ₂·θ _d)；

X in the following formula ₀, y ₀Positional information for the previous moment;

345) process x ₁, x ₂, y ₁, y ₂The weight of distribution factor:

$v_t=\frac{\sqrt{{(x_{02}-x_{01})}^2+{(y_{02}-y_{01})}^2}}{T};$

$a_2=\frac{v_t-({\mathrm{\&beta;}}_1\&CenterDot;v_g+{\mathrm{\&beta;}}_2{\&CenterDot;v}_d)}{v_t};$

α ₁＝1-α ₂；

v _tTo calculate out bearer rate, x by the first two moment positional information ₀₁, y ₀₁And x ₀₂, y ₀₂Be respectively the constantly positional information of position of the first two, α ₁X ₁, y ₁Weight, α ₂X ₂, y ₂Weight;

346) obtain carrier positions information by following formula:

x＝α ₁·x ₁+α ₂·x ₂

y＝α ₁·y ₁+α ₂·y ₂。

4) to step 3) the carrier positions information and the Digital map that obtain mate, and obtain the final position information of carrier and export.

The integrated navigation of GPS and GAIS system has improved precision and the reliability of positioning system to a certain extent, but still there is certain error in locator data, and when gps data was lost, the error of GAIS system can be accumulated and be become large.In real system, usually adopt map-matching algorithm further to improve the precision of GPS and GAIS system.

Map match is a gordian technique in the Vehicular navigation system.It is proofreaied and correct vehicle location and electronic chart error by software approach, determines real-time and accurately vehicle location, is the basis of realizing other crucial navigation features such as vehicle guiding.

Map-matching algorithm is usually take pattern recognition theory as the basis, with certain vehicle location point or certain section garage's geometric locus as sample to be matched, with the location point near all roads this point or this geometric locus or road curve as template, by the coupling between sample to be matched and template, get the highest template of similarity as matching result.We suppose that electronic map data is accurately before the coupling, and vehicle travels at road all the time, and we can be divided into matching process 2 following stages on this basis.

The one, road is selected, and mainly is that road is carried out segmentation, extracts roadway characteristic information, then adopts suitable search rule and matching algorithm according to the mobile vehicle information that provides when front sensor, seeks a most possible road in map data base.

The method of determining is: suppose P (X, Y) be GPS location output point, at first centered by P, 200m is that radius delimited a scope frame, determine to be included in " atomic diagram " in the scope frame according to this scope frame upper left corner and lower right corner latitude and longitude coordinates, the road of storing in these " atomic diagram " is the candidate road section relevant with P.When P is in the border of atomic diagram, the atomic diagram that comprises in the scope frame may be for a plurality of, but because road graticule has been carried out the piecemeal processing, each atomic diagram small, even if process the candidate road section data in a plurality of atomic diagram, still can guarantee higher matching speed.

The 2nd, road coupling with mobile vehicle current location coupling and be presented on this road, is used for eliminating the positioning error of sensor.Judge that the current information of travelling in which bar highway section of vehicle mainly contains 3: current vehicle location point is apart from the projector distance of candidate road section; How much topological relations in the current driving direction of car and the angle between road direction and candidate road section and last coupling highway section.Conventional map-matching algorithm is the projector distance that the map-matching algorithm of distance-based projection is only considered candidate road section, its core concept is: calculate successively point to be matched to the projector distance in each candidate highway section, get more afterwards the minimum highway section of distance and be the coupling highway section.

The above only preferably is not limited to the present invention for of the present invention, and obviously, those skilled in the art can carry out various changes and modification and not break away from the spirit and scope of the present invention the present invention.Like this, if of the present invention these are revised and modification belongs within the scope of claim of the present invention and equivalent technologies thereof, then the present invention also is intended to comprise these changes and modification interior.

Claims (1)

1. based on the GPS assisted location method of Fusion, it is characterized in that: comprise the steps:

1) by the GPS navigation system, obtains position, speed, time, satellite number and the positional precision degree of strength of carrier;

2) by gyroscope and accelerometer combined system, obtain carrier angular velocity, speed and acceleration;

3) with step 1,2) data obtained merge, and obtain carrier positions information;

4) to step 3) the carrier positions information and the Digital map that obtain mate, and obtain the final position information of carrier and export;

Described step 3) specifically comprises the steps:

31) whether judge the satellite number greater than threshold value, if greater than threshold value, then export the GPS locating information to data fusion module, as no, then do not export the GPS locating information to data fusion module;

32) according to step 2) the carrier angular velocity, speed and the acceleration that obtain and map-matching module feedback carrier final position information carry out dead reckoning;

33) judge whether bearer rate is 0, in this way, the locating information that then obtains take dead reckoning is as carrier positions information, as no, and execution in step 34);

34) locating information of GPS navigation system acquisition and the locating information of dead reckoning acquisition are merged;

Described step 34) be to carry out data fusion by Kalman filter;

Described step 34) specifically comprises the steps:

341) extracting position (x from the GPS locating information _g, y _g), speed v _g, direction θ _g, the positional precision degree of strength is calculated module extraction DR position (x from navigation _d, y _d), speed v _d, direction θ _d

342) according to positional precision degree of strength self-adaptation adjustment information distribution factor:

β ₂＝1-β ₁；

${\mathrm{\&beta;}}_1=\left\{\begin{array}{cc}0.99& \mathrm{PDOP}\&le;1\\ 1/\mathrm{PDOP}& 1<\mathrm{PDOP}<4\\ 1/4\mathrm{PDOP}& 4\&le;\mathrm{PDOP}\end{array}\right.;$

In the following formula, β ₁And β ₂Be respectively GPS locating information and navigation and calculate the information distribution factor of the locating information that obtains, PDOP is the positional precision degree of strength;

343) assign weight from the beginning according to the information distribution factor:

x ₁＝β ₁·x _g+β ₂·x _d

y ₁＝β ₁·y _g+β ₂·y _d；

344) by inferring that algorithm obtains positional information:

x ₂＝x ₀+(β ₁·v _g+β ₂v _d)·cos(β ₁·θ _g+β ₂·θ _d)

y ₂＝y ₀+(β ₁·v _g+β ₂v _d)·sin(β ₁·θ _g+β ₂·θ _d)；

X in the following formula ₀, y ₀Positional information for the previous moment;

345) process x ₁, x ₂, y ₁, y ₂The weight of distribution factor:

$v_t=\frac{\sqrt{{(x_{02}-x_{01})}^2+{(y_{02}-y_{01})}^2}}{T};$

${\mathrm{\&alpha;}}_2=\frac{v_t-({\mathrm{\&beta;}}_1\&CenterDot;v_g+{\mathrm{\&beta;}}_2\&CenterDot;v_d)}{v_t};$

α ₁＝1-α ₂；

v _tTo calculate out bearer rate, x by the first two moment positional information ₀₁, y ₀₁And x ₀₂, y ₀₂Be respectively the constantly positional information of position of the first two, α ₁X ₁, y ₁Weight, α ₂X ₂, y ₂Weight;

346) obtain carrier positions information by following formula:

x＝α ₁·x ₁+α ₂·x ₂

y＝α ₁·y ₁+α ₂·y ₂。

Images