CN110231636A - The adaptive Unscented kalman filtering method of GPS and BDS dual mode satellite navigation system - Google Patents
The adaptive Unscented kalman filtering method of GPS and BDS dual mode satellite navigation system Download PDFInfo
- Publication number
- CN110231636A CN110231636A CN201910357048.0A CN201910357048A CN110231636A CN 110231636 A CN110231636 A CN 110231636A CN 201910357048 A CN201910357048 A CN 201910357048A CN 110231636 A CN110231636 A CN 110231636A
- Authority
- CN
- China
- Prior art keywords
- noise
- gps
- hdop
- bds
- dual mode
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- 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/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/13—Receivers
- G01S19/24—Acquisition or tracking or demodulation of signals transmitted by the system
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
The invention discloses the adaptive Unscented kalman filtering methods of a kind of GPS and BDS dual mode satellite navigation system, comprising: step 1: nonlinear system modeling；Step 2: it is discrete that Newton's laws of motion are carried out, and the model that equivalent substitution makes it meet nonlinear system is carried out to it；Step 3: HDOP adaptively selects excellent algorithms selection horizontal accuracy preferably GNSS work system to provide GNSS data, substitutes into equivalent substitution nonlinear system model；The non-linear transmitting of mean value and covariance is handled using Unscented transform method, the uncertainty that Sigma point sampling solves noise is carried out using UKF algorithm, real-time estimation and amendment noise are to solve the nonlinear characteristic of integrated navigation system, based on maximum likelihood criterion derivation noise statistics estimators device to unknown or inaccurate noise statistics progress real-time estimation and amendment.The present invention can significantly improve the precision of GPS Yu BDS dual mode satellite navigation system, and have good effect when processing system model nonlinear, noise statistics are uncertain.
Description
Technical field
The present invention relates to the filtering method of satellite navigation system more particularly to a kind of GPS and BDS dual mode satellite navigation system
Adaptive Unscented kalman filtering method.
Background technique
With social development, vehicle increases, and city traffic road condition is increasingly complicated, precision of the people to location navigation instrument
More stringent requirements are proposed.But so far, the system of existing all location navigation instruments all has certain limitation, make
The demand of people cannot be fully met by obtaining these instruments.In order to improve urban transportation and meet the needs of people, realization high-precision,
A variety of Position Fixing Navigation Systems can be combined, be maximized favourable factors and minimized unfavourable ones by round-the-clock, global location, constitute one polynary organic group
Navigation system is closed, to improve the accuracy and reliability of Position Fixing Navigation System, the present invention is led using GPS and BDS dual mode satellite
Boat positioning system, adaptively selects excellent algorithm come selected level precision preferably GNSS data, in combination with adaptive using HDOP
Unscented kalman filtering algorithm to location information carry out optimal estimation, show that more accurate location data meets user's need
It wants.
In the prior art, refer to that application No. is 201710724015.6, Publication No. 109425876A, entitled " one
The Chinese invention patent application file of the improved Kalman filter method of kind raising positioning accuracy ", the defect of the technical solution exist
In for nonlinear problem, Extended Kalman filter (EKF) is decomposed using Taylor and linearizes nonlinear model, dyscalculia
And there is linearity error, it compares Unscented kalman filtering (UKF), the introducing of linearity error reduces the precision of model.
For UKF compares EKF, UKF following points advantage: firstly, being not required to linearize nonlinear system, avoid
The calculating error introduced in nonlinear system linearization procedure；Secondly, UKF use Unscented transform can reappear completely it is non-thread
The mean value and covariance of property system state amount；Again, for any nonlinear system, its filtering accuracy can reach Taylor's grade
More than three ranks of number expansion, and EKF then can only achieve the precision of single order；In addition, UKF need not calculate Jacobian matrix, in this way
So that the calculation amount of UKF greatly simplifies, calculating error is not introduced yet due to being not required to calculating Jacobian matrix.
Summary of the invention
The technical problem to be solved in the present invention is that in view of the deficiencies of the prior art, providing one kind can be improved vehicle combination
The precision of navigation, for the adaptive Unscented kalman filtering method of GPS and BDS dual mode satellite navigation system.
In order to solve the above technical problems, the present invention adopts the following technical scheme that.
A kind of adaptive Unscented kalman filtering method of GPS and BDS dual mode satellite navigation system, which comprises
Step S1, establishes nonlinear system model.Set the model of nonlinear system are as follows:
In formula, xkFor the state vector at k moment, zkFor the observation vector at k moment, fk() and hk() is respectively non-thread
The function of state and observation function of property system, wkAnd vkFor the mean value and covariance square of irrelevant white Gaussian noise and the two
Battle array meets respectively:
In formula, q is state-noise mean value, and r is observation noise mean value；
Step S2, Newton's laws of motion are discrete and equivalent substitution nonlinear system model.State vector is taken as x respectivelye=
[ve, ae]T, xn=[vn, an]T, xeAnd xnRespectively east orientation speed and north orientation speed, aeAnd anTwo state vectors are uniformly write as:
X=[v, a]T；
In following formula, the sampling interval is set as T, vk-1For the instantaneous velocity for observing initial time, vkFor the wink after T time
Shi Sudu, ak-1It is the acceleration in T time, will can be obtained after Newton's laws of motion discretization:
vk=vk-1+ak-1T；
The position and speed of east orientation and north orientation, observation vector are provided respectively using GPS and BDS dual mode receiver output statement
It is taken as z=[x, v]T, wherein the original state amount x of system0It is irrelevant with state-noise q and observation noise r, and initial shape
State obeys Gauss normal distribution,Priori mean value and P0Covariance matrix is respectively as follows:
Enable μk=Wk- q, ηk=vk- r, being substituted into nonlinear system model can obtain:
In formula, μk-1And ηkBe mean value be zero, variance be respectively Q and R white Gaussian noise.
The adaptive optimal selection model algorithm of step S3, HDOP.Extract the level in GPS and BDS dual mode receiver output statement
Component dilution of precision (HDOP), HDOP determine the confidence level of positioning accuracy, and the input value of excellent algorithm is adaptively selected as HDOP.
Firstly, establishing HDOP prediction model:
YH(i+1)=k (i+1)+b
[k, b]T=r (I, H)
Wherein, YH(i+1)For BDS and GPS HDOP difference the i+1 moment predicted value；R () is least square letter
Number；Vector I=(i, i-1 ..., i-3)T, the continuous Vector of degree for measuring four times of the expression of i >=1；Vector H=(YH(i),
YH(i-1)..., YH(i-3))T, the HDOP difference value vector of four BDS and GPS of the expression measurement of i >=1.In order to avoid the GNSS of drift believes
Number to algorithm cheating interference, work as YH(i)When >=0.4,0.4 is taken；YH(i)When≤0,0 is taken.
To YH(i), have:
YH(i)=Δ HDOPB(i)-ΔHDOPG(i)；
ΔHDOPG(i)=λG(i)-λo；
ΔHDOPB(i)=λo-λB(i)；
In formula, λG(k)For the HDOP of k moment GPS signal；λB(k)For the HDOP of k moment BDS signal；λOIt is excellent currently to select
The HDOP of satellite navigation system, O (Optimum) are optimal.
Finally, establishing the adaptive optimal selection model of HDOP:
Wherein, SiFor the practical selection signal of i-th；w1For the weight of GPS signal, w2For the weight of BDS signal；
Argmin (E) is the minimum value of vector E；Vector E=(H, YH(i+1))T。
The adaptive optimal selection model algorithm of HDOP, which determines, selects GPS or BDS work system, its GNSS data is substituted into step
The equivalent substitution nonlinear system model of S2.
Preferably, the method also includes: using the non-linear transmitting of UT conversion process mean value and covariance, become through UT
The UKF filtering algorithm for changing rear system comprises the following processes: Sigma point sampling, time update, observation updates and noise statistics are estimated
Meter.
Wherein, the selection of Sigma point adopts Sigma point according to the complexity of the intensive traffic section within the scope of certain time
Sample frequency is adjusted.For example, doubling to sample when vehicle is by transport nodes such as crossroad, bus stop, pavements
Sigma point.
In the adaptive Unscented kalman filtering method of GPS disclosed by the invention and BDS dual mode satellite navigation system, knot
It closes HDOP and adaptively selects excellent algorithm, select the preferable GNSS system data of positioning accuracy, substitute into nonlinear system model, reuse
The method of Unscented transform is to carry out processing to nonlinear system and using Unscented kalman filtering device (Unscented Kalman
Filter, UKF).HDOP adaptively selects the introducing of excellent algorithm, reduces the switching frequency of GNSS work system, increase GPS and
Robustness, stability, the reliability of BDS dual mode satellite navigation system avoid the GNSS signal of drift from causing deception dry system
It disturbs, simultaneously, reduces the error rate of GPS and BDS dual mode satellite navigation system.GPS of the present invention and BDS dual mode satellite navigation system
Adaptive Unscented kalman filtering method be remarkably improved the precision of Integrated Navigation for Land Vehicle, and it is non-thread in processing system model
Property, noise statistics it is uncertain when there is good effect.
Detailed description of the invention
Fig. 1 is the adaptive Unscented kalman filtering method flow diagram of a kind of GPS and BDS dual mode satellite navigation system；
Fig. 2 is the curve graph that GPS/BDS car-mounted terminal filters test experiments；
Fig. 3 is AKF east orientation and north orientation position measurement experimental curve diagram；
Fig. 4 is AKF east orientation and north orientation speed error testing experimental curve diagram；
Fig. 5 is UKF east orientation and north orientation location error test experiments curve graph；
Fig. 6 is UKF east orientation and north orientation speed error testing experimental curve diagram；
Fig. 7 is AUKF east orientation and north orientation location error test experiments curve graph；
Fig. 8 is AUKF east orientation and north orientation speed error testing experimental curve diagram；
Fig. 9 is the location error test experiments curve graph of AUKF filtering front and back；
Figure 10 is each filter east orientation position comparative experiments curve graph；
Figure 11 is each filter north orientation position comparative experiments curve graph；
Figure 12 is each filter east orientation speed comparative experiments curve graph；
Figure 13 is each filter north orientation speed comparative experiments curve graph.
Specific embodiment
The present invention is further described in more detail with reference to the accompanying drawings and examples.
The invention discloses the adaptive Unscented kalman filtering methods of a kind of GPS and BDS dual mode satellite navigation system, ask
Referring to Fig. 1, which comprises
Step S1, establishes nonlinear system model.Set the model of nonlinear system are as follows:
In formula, xkFor the state vector at k moment, zkFor the observation vector at k moment, fk() and hk() is respectively non-thread
The function of state and observation function of property system, wkAnd vkFor the mean value and covariance square of irrelevant white Gaussian noise and the two
Battle array meets respectively:
In formula, q is state-noise mean value, and r is observation noise mean value；
Since land vehicle can approximately be considered the motion carrier of two-dimensional surface, the movement on two-dimensional surface can be with
It is decomposed into the motion in one dimension of two orthogonal directions, the upward movement of the movement and north south such as on east-west direction.Two
Movement non-correlation on direction, so, Kalman filtering can be carried out respectively to the motion in one dimension in both direction；
Step S2, Newton's laws of motion are discrete and equivalent substitution nonlinear system model.State vector is taken as x respectivelye=
[ve, ae]T, xn=[vn, an]T, xeAnd xnRespectively east orientation speed and north orientation speed, aeAnd anTwo state vectors are uniformly write as:
X=[v, a]T；
In following formula, the sampling interval is set as T, vk-1For the instantaneous velocity for observing initial time, vkFor the wink after T time
Shi Sudu, ak-1It is the acceleration in T time, will can be obtained after Newton's laws of motion discretization:
vk=vk-1+ak-1T；
The position and speed of east orientation and north orientation, observation vector are provided respectively using GPS and BDS dual mode receiver output statement
Take z=[x, v]T, wherein the original state amount x of system0It is irrelevant with state-noise and observation noise, and original state takes
From Gauss normal distribution,Priori mean value and P0Covariance matrix is respectively as follows:
Enable μk=wk- q, ηk=vk- r, being substituted into nonlinear system model can obtain:
In formula, μk-1And ηkBe mean value be zero, variance be respectively Q and R white Gaussian noise.
The adaptive optimal selection model algorithm of step S3, HDOP.Extract the level in GPS and BDS dual mode receiver output statement
Component dilution of precision (HDOP), HDOP determine the confidence level of positioning accuracy, and the input value of excellent algorithm is adaptively selected as HDOP.
Firstly, establishing HDOP prediction model:
YH(i+1)=k (i+1)+b
[k, b]T=r (I, H)
Wherein, YH(i+1)For BDS and GPS HDOP difference the i+1 moment predicted value；R () is least square letter
Number；Vector I=(i, i-1 ..., i-3)T, the continuous Vector of degree for measuring four times of the expression of i >=1；Vector H=(YH(i),
YH(i-1)..., YH(i-3))T, the HDOP difference value vector of four BDS and GPS of the expression measurement of i >=1.In order to avoid the GNSS of drift believes
Number to algorithm cheating interference, work as YH(i)When >=0.4,0.4 is taken；YH(i)When≤0,0 is taken.
To YH(i), have:
YH(i)=Δ HDOPB(i)-ΔHDOPG(i)；
ΔHDOPG(i)=λG(i)-λo；
ΔHDOPB(i)=λo-λB(i)；
In formula, λG(k)For the HDOP of k moment GPS signal；λB(k)For the HDOP of k moment BDS signal；λOIt is excellent currently to select
The HDOP of satellite navigation system, O (Optimum) are optimal.
Finally, establishing the adaptive optimal selection model of HDOP:
Wherein, SiFor the practical selection signal of i-th；w1For the weight of GPS signal, w2For the weight of BDS signal；
Argmin (E) is the minimum value of vector E；Vector E=(H, YH(i+1))T。
In the above method, excellent algorithm is adaptively selected in conjunction with HDOP, the preferable GNSS system data of positioning accuracy is selected, by it
It substitutes into equivalent substitution nonlinear system model.HDOP adaptively selects the introducing of excellent algorithm, reduces the switching of GNSS work system
Frequency increases the robustness, stability, reliability of GPS and BDS dual mode satellite navigation system, avoids the GNSS signal of drift
Cheating interference is caused to system, simultaneously, reduces the error rate of GPS and BDS dual mode satellite navigation system.
For adaptive Unscented kalman filtering algorithm, in the present embodiment, adaptive Kalman filter is directed to system number
It learns under model is uncertain or system mode noise and the uncertain situation of observation noise statistical property and uses, it passes through filtering
Middle real-time regulating system mathematical model parameter and noise statistics parameter are filtered；UKF is primarily directed to nonlinear system
System is filtered, but conventional UKF algorithm has limitation, that is, needs accurately known system mode noise and observation noise priori
Statistical property, this make traditional UKF uncertain to system model and the uncertain nonlinear system of system noise statistical property not
Expected filter effect can be reached, cause system unstable or even dissipated, combine adaptive Kalman filter and UKF to be formed
Adaptive Unscented kalman filtering device, while being filtered calculating, using noise statistics estimators device to unknown or inaccurate
True noise statistics carry out real-time estimation and amendment, have the adaptive ability of reply noise variation, it is unknown in noise statistics
Or in inaccurate situation, still there is splendid stability and convergence；And estimation of the noise statistics estimators device to noise statistics
It is unbiased.
It when noise mean value non-zero, i.e., is not white Gaussian in the present embodiment for adaptive Unscented kalman filtering algorithm
Under noise situations, the state-noise of this system and the statistical property of observation noise are studied, Sigma point is carried out using UKF algorithm and adopts
Sample, by state-noise wk-1With measurement noise vkPut forward to handle, since state-noise and observation noise statistical property are uncertain,
Make output observation information come real-time estimation and amendment noise statistics, i.e. W using the AKF algorithm with reply noise changing capabilityk-1
And VkMean value and covariance matrix, propose adaptive Unscented kalman filtering algorithm.
In the present embodiment, the method also includes: utilize the non-linear transmitting of UT conversion process mean value and covariance, warp
The UKF filtering algorithm of system comprises the following processes after UT transformation: Sigma point sampling, time update, observation updates and noise statistics
Estimation.
Further, during the Sigma point sampling, sampling policy is corrected using ratio, to guarantee that output variable is assisted
The Positive (>=0) of variance.The selection of Sigma point, within the scope of certain time, according to the complexity pair of the intensive traffic section
The sample frequency of Sigma point is adjusted.For example, when vehicle is by transport nodes such as crossroad, bus stop, pavements,
Double sampling Sigma point.
The time updates in step, utilizes k-1 moment estimated valueWith k-1 moment error covariance matrix
Pk-1/k-1Calculate Sigma point ξI, k-1/k-1(i=0,1 ..., L), Sigma point pass through nonlinear state function fk()+q, which is propagated, to be become
For γI, k/k-1, q is system mode noise, to calculate a step status predication mean valueWith error covariance matrix Pk/k-1,
That is:
γI, k/k-1=fk-1(ξI, k/k-1)+q, i=0,1 ..., L；
Wherein, Wi mAnd Wi cFor corresponding to ξiThe weight of (i=0,1 ..., L), Wi mAnd Wi cSingle order and second order is respectively asked to unite
Count weight coefficient when characteristic；Q is the covariance of system mode noise q；
It utilizesAnd Pk/k-1And ratio corrects sampling policy to calculate Sigma point, ξI, k/k-1(i=0,1 ...,
L) pass through non-linear observation function hkIt is x that ()+r, which is propagated,I, k/k-1, R is the covariance of observation noise r, thus obtains output forecast
Mean valueAnd covariance matrixWithThat is:
xI, k/k-1=hk(ξI, k/k-1)+r, i=0,1 ..., L；
The observation updates in step, after obtaining new observed quantity, using conventional Kalman filter recurrence formula into
The actual filtering of row updates, and fundamental equation is as follows:
In formula, KkFor filtering gain matrix,
The noise statistics estimators step is realized based on noise statistics estimators device:
Set the W of equal Normal Distributionk-1And VkIt is independent from each other, is pushed away according to maximum likelihood probability Estimation criterion
The very big priori noise statistics estimators device of suboptimum applied to UKF is led, works as q, Q, r, when R is unknown, with quantity of state x0, x1..., xk
Maximum likelihood probability EstimationAndIt is acquired with maximization density；
The recurrence formula of secondary unbiased very big priori noise statistics estimators device is as follows:
The very big priori unbiased esti-mator of the suboptimum of system mode noise q are as follows:
The very big priori unbiased esti-mator of the suboptimum of system mode noise covariance Q are as follows:
The very big priori unbiased esti-mator of the suboptimum of observation noise r are as follows:
The very big priori unbiased esti-mator of the suboptimum of observation noise covariance R are as follows:
After state-noise statistical property and observation noise statistical property of the UKF in filtering has been determined, calculated using UKF
The mathematical model of system is carried out Unscented transform by method, obtains the adaptive Unscented kalman filtering device based on noise statistics estimators device
Filtering algorithm recursive process:
Priori prediction process are as follows:
In formula,For priori estimates, Wi mFor sigma point ξI, k-1/k-1Parameter used in mean value weighting；
Utilize priori estimatesWith error covariance matrix Pk-1|k-1To calculate sigma point ξI, k-1/k-1(i=0,
1 ..., L), sigma point passes through nonlinear state function f againk()+q propagation becomes γI, k/k-1, then have γI, k/k-1=fk-1
(ξI, k-1/k-1)+q, and obtain error co-variance matrix:
Posterior estimator process are as follows:
In formula,For posterior estimate, zkFor the measurand of weighting,For predicted value, zkWith
Difference be measurement process remnants, KkFor remaining gain matrix；
Gain calculation process are as follows:
In formula, residual gain KkFor keeping Posterior estimator error covariance minimum,For prior estimateAccidentally
The covariance P of differencek/k-1,For Posterior estimatorThe covariance P of errork/k；
State covariance battle array renewal process are as follows:
In formula, Pk/kFor Posterior estimator error covariance matrix, Pk/k-1For prior estimate error covariance matrix,
For residual error covariance.
Based on above various, excellent algorithm is adaptively selected in conjunction with HDOP, selects the preferable GNSS system data of positioning accuracy, it will
It substitutes into nonlinear system model, as AUKF (Adaptive Unscented Kalman Filter) filter input source.
Simultaneously, nonlinear system is handled with the method for Unscented transform, and uses Unscented kalman filtering device
(Unscented Kalman Filter, UKF).The nonlinear characteristic of GPS and BDS dual mode satellite navigation system can be solved, simultaneously
Real-time estimation and amendment are carried out to unknown or inaccurate noise statistics using noise statistics estimators device, it is adaptive compared to single
For answering Kalman filtering (Adaptive Kalman Filter, AKF) and UKF, which has reply noise variation
Adaptive ability still there is splendid stability and convergence, and make an uproar in the case where noise statistics are unknown or inaccurate situation
Sound statistical estimate device is unbiased to the estimation of noise statistics.HDOP adaptively selects the introducing of excellent algorithm, reduces GNSS job family
The switching frequency of system increases the robustness, stability, reliability of GPS and BDS dual mode satellite navigation system, avoids drift
GNSS signal causes cheating interference to system, simultaneously, reduces the error rate of GPS and BDS dual mode satellite navigation system.UKF mistake
Cheng Zhong is not required to linearize nonlinear system, avoids the linearity error introduced in nonlinear system linearization procedure；UKF
The Unscented transform of use can reappear the mean value and covariance of non-linear system status amount completely；For any nonlinear system it
Filtering accuracy can reach three ranks of taylor series expansion or more；It is not required to calculating Jacobian matrix and does not introduce calculating error,
Greatly simplify the calculation amount of UKF.
During actual test, the present embodiment carries out testing research to adaptive Unscented kalman filtering algorithm, it is assumed that
Vehicle is accelerated with the initial velocity of 10m/s, and initial parameter is selected as observation point N=40, sampling period T=1s, satellite system
Observation noise covariance battle array of uniting is [202, 202], it is as shown in Figure 2 that GPS and BDS dual-mode terminal filters test experiments comparing result.
The carrier simulated in the experiment of the present embodiment is car, which selects the running environment of vehicle in population vehicle
It is intensive, and have the location in high buildings and large mansions and tunnel, in addition, vehicle does not encounter emergency case in the process of moving；Using this
GPS the and BDS double mode vehicular terminal of embodiment is tested, and dual mode satellite navigation system selects AKF, UKF and adaptive respectively
The information fusion algorithm of Unscented kalman (AUKF) carries out performance comparison.The mathematical model of AKF is known and is linear system mould
Type, and the mathematical model of UKF and AUKF is typical nonlinear system.Due to the influence of the factors such as device itself, external environment,
State-noise and observation noise in system model be in vehicle operation it is unknown, need in real time adjust and estimation system
The state-noise and observation noise statistical parameter of system, using the noise statistics and system mathematic model of acquisition to integrated navigation
System is filtered.Using location error (the east orientation error, north orientation error, since automobile is on ground of AKF, UKF and AUKF
Ball surface operation, it is possible to approximately think that vehicle does not move and ignore vertical error vertical), velocity error test reality
It tests as shown in figs. 3 to 9.
By the experimental result of Fig. 3 to Fig. 9 it is found that the location error in vehicular motion, speed are filtered by UKF
Error is respectively less than location error, velocity error after traditional AKF filtering processing, and AKF filtering processing reaches a timing
Between can have the phenomenon that data scatter.In contrast, the location error of AKF, UKF and AUKF, velocity error comparative experiments are as schemed
10 to shown in Figure 13.By the experimental result of Figure 10 to Figure 13 it is found that in vehicle operation, AUKF is substantially better than AKF, even if
System model is the error that nonlinear, last location error, velocity error are also less than AKF and UKF, this illustrates that AUKF is calculated
Method can greatly improve the precision of Integrated Navigation for Land Vehicle, and uncertain in processing system model nonlinear, noise statistics
When have good effect.
The above is preferred embodiments of the present invention, is not intended to restrict the invention, all in technology model of the invention
Interior done modification, equivalent replacement or improvement etc. are enclosed, should be included in the range of of the invention protect.
Claims (7)
1. the adaptive Unscented kalman filtering method of a kind of GPS and BDS dual mode satellite navigation system, which is characterized in that described
Method includes:
Step S1, establishes nonlinear system model, sets the model of nonlinear system are as follows:
In formula, xkFor the state vector at k moment, zkFor the observation vector at k moment, fk() and hk() is respectively nonlinear system
Function of state and observation function, wkAnd vkDistinguish for the mean value and covariance matrix of irrelevant white Gaussian noise and the two
Meet:
In formula, q is state-noise mean value, and r is observation noise mean value；
Step S2, Newton's laws of motion discretization equivalent substitution nonlinear system model；State vector is taken as x respectivelye=[ve,
ae]T, xn=[vn, an]T, xeAnd xnRespectively east orientation speed and north orientation speed, aeAnd anTwo state vectors are uniformly write as:
X=[v, a]T；
In following formula, the sampling interval is set as T, vk-1For the instantaneous velocity for observing initial time, vkFor the instantaneous speed after T time
Degree, ak-1It is the acceleration in T time, will can be obtained after Newton's laws of motion discretization:
vk=vk-1+ak-1T；
The position and speed of east orientation and north orientation is provided respectively using GPS and BDS dual mode receiver output statement, and observation vector is taken as
Z=[x, v]T, wherein the original state amount x of system0It is irrelevant with state-noise and observation noise, and original state is obeyed
Gauss normal distribution,Priori mean value and P0Covariance matrix is respectively as follows:
Enable μk=Wk- q, ηk=vk- r, being substituted into nonlinear system model can obtain:
In formula, μk-1And ηkBe mean value be zero, variance be respectively Q and R white Gaussian noise；
The adaptive optimal selection model algorithm of step S3, HDOP extracts the horizontal component in GPS and BDS dual mode receiver output statement
Dilution of precision (HDOP), HDOP determine the confidence level of positioning accuracy, and the input value of excellent algorithm is adaptively selected as HDOP；
Firstly, establishing HDOP prediction model:
YH(i+1)=k (i+1)+b
[k, b]T=r (I, H)
Wherein, YH(i+1)For BDS and GPS HDOP difference the i+1 moment predicted value；R () is least square function；To
It measures I=(i, i-1 ..., i-3)T, the continuous Vector of degree for measuring four times of the expression of i >=1；Vector H=(YH(i), YH(i-1) ...,
YH(i-3))T, i >=1 indicate measure four BDS and GPS HDOP difference value vector, work as YH(i)When >=0.4,0.4 is taken；YH(i)When≤0,
Take 0；
To YH(i), have:
YH(i)=Δ HDOPB(i)-ΔHDOPG(i)；
ΔHDOPG(i)=λG(i)-λo；
ΔHDOPB(i)=λo-λB(i)；
In formula, λG(k)For the HDOP of k moment GPS signal；λB(k)For the HDOP of k moment BDS signal；λOCurrently to select excellent satellite
The HDOP of navigation system (GPS or BDS), O (Optimum) are optimal；
Finally, establishing the adaptive optimal selection model of HDOP:
Wherein, SiFor the practical selection signal of i-th；w1For the weight of GPS signal, w2For the weight of BDS signal；argmin(E)
For the minimum value of vector E；Vector E=(H, YH(i+1))T；
The adaptive optimal selection model algorithm of HDOP, which determines, selects GPS or BDS work system, its GNSS data is substituted into step S2's
Equivalent substitution nonlinear system model.
2. the adaptive Unscented kalman filtering method of GPS as described in claim 1 and BDS dual mode satellite navigation system,
It is characterized in that, the adaptive optimal selection model algorithm of HDOP is selected the preferable GNSS system data of positioning accuracy, substituted into
In effect substitution nonlinear system model.
3. the adaptive Unscented kalman filtering method of GPS as described in claim 1 and BDS dual mode satellite navigation system,
It is characterized in that, the method also includes: using the non-linear transmitting of UT conversion process mean value and covariance, it is after UT is converted
The UKF filtering algorithm of system comprises the following processes: Sigma point sampling, time update, observation updates and noise statistics estimators.
4. the adaptive Unscented kalman filtering method of GPS as claimed in claim 2 and BDS dual mode satellite navigation system,
It is characterized in that, during the Sigma point sampling, sampling policy is corrected using ratio, to guarantee the half of output variable covariance
Orthotropicity, the selection of Sigma point, within the scope of certain time, according to the complexity of the intensive traffic section to the sample frequency of Sigma point
It is adjusted.
5. the adaptive Unscented kalman filtering method of GPS as claimed in claim 2 and BDS dual mode satellite navigation system,
It is characterized in that, the time updates in step, utilizes k-1 moment estimated valueWith k-1 moment error covariance matrix
Pk-1/k-1Calculate Sigma point ξI, k-1/k-1(i=0,1 ..., L), Sigma point pass through nonlinear state function fk()+q, which is propagated, to be become
For γI, k/k-1, q is system mode noise, to calculate a step status predication mean valueWith error covariance matrix Pk/k-1,
That is:
γI, k/k-1=fk-1(ξI, k/k-1)+q, i=0,1 ..., L；
Wherein, Wi mAnd Wi cFor corresponding to ξiThe weight of (i=0,1 ..., L), Wi mAnd Wi cRespectively ask single order and second-order statistics special
Weight coefficient when property；Q is the covariance of system mode noise q；
It utilizesAnd Pk/k-1And ratio corrects sampling policy to calculate Sigma point, ξI, k/k-1(i=0,1 ..., L) passes through
Non-linear observation function hkIt is x that ()+r, which is propagated,I, k/k-1, R is the covariance of observation noise r, thus obtains output forecast mean valueAnd covariance matrixWithThat is:
xI, k/k-1=hk(ξI, k/k-1)+r, i=0,1 ..., L；
6. the adaptive Unscented kalman filtering method of GPS as claimed in claim 2 and BDS dual mode satellite navigation system,
Be characterized in that, the observation updates in step, after obtaining new observed quantity, using conventional Kalman filter recurrence formula into
The actual filtering of row updates, and fundamental equation is as follows:
In formula, KkFor filtering gain matrix,
7. the adaptive Unscented kalman filtering method of GPS as claimed in claim 2 and BDS dual mode satellite navigation system,
It is characterized in that, the noise statistics estimators step is realized based on noise statistics estimators device:
Set the W of equal Normal Distributionk-1And VkIt is independent from each other, application is derived according to maximum likelihood probability Estimation criterion
In the very big priori noise statistics estimators device of the suboptimum of UKF, work as q, Q, r, when R is unknown, with quantity of state xo, x1..., xkIt is very big
Likelihood probability estimationAndIt is acquired with maximization density；
The recurrence formula of secondary unbiased very big priori noise statistics estimators device is as follows:
The very big priori unbiased esti-mator of the suboptimum of system mode noise q are as follows:
The very big priori unbiased esti-mator of the suboptimum of system mode noise covariance Q are as follows:
The very big priori unbiased esti-mator of the suboptimum of observation noise r are as follows:
The very big priori unbiased esti-mator of the suboptimum of observation noise covariance R are as follows:
It, will using UKF algorithm after state-noise statistical property and observation noise statistical property of the UKF in filtering has been determined
The mathematical model of system carries out Unscented transform, obtains the adaptive Unscented kalman filtering device filtering based on noise statistics estimators device
Algorithm recursive process:
Priori prediction process are as follows:
In formula,For priori estimates, Wi mFor sigma point ξI, k-1/k-1Weight used in mean value weighting；
Utilize priori estimatesWith error covariance matrix Pk-1|k-1To calculate sigma point ξI, k-1/k-1(i=0,1 ...,
L), sigma point passes through nonlinear state function f againk()+q propagation becomes γI, k/k-1, then have γI, k/k-1=fk-1(ξI, k-1/k-1)
+ q, and obtain error co-variance matrix:
Posterior estimator process are as follows:
In formula,For posterior estimate, zkFor the measurand of weighting,For predicted value, zkWithDifference
For the remnants of measurement process, KkFor remaining gain matrix；
Gain calculation process are as follows:
In formula, residual gain KkFor keeping Posterior estimator error covariance minimum,For prior estimateThe association of error
Variance Pk/k-1,For Posterior estimatorThe covariance P of errork/k；
State covariance battle array renewal process are as follows:
In formula, Pk/kFor Posterior estimator error covariance matrix, Pk/k-1For prior estimate error covariance matrix,It is residual
Remaining error covariance.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910357048.0A CN110231636B (en) | 2019-04-29 | 2019-04-29 | Self-adaptive unscented Kalman filtering method of GPS and BDS dual-mode satellite navigation system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910357048.0A CN110231636B (en) | 2019-04-29 | 2019-04-29 | Self-adaptive unscented Kalman filtering method of GPS and BDS dual-mode satellite navigation system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110231636A true CN110231636A (en) | 2019-09-13 |
| CN110231636B CN110231636B (en) | 2021-03-26 |
Family
ID=67860975
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910357048.0A Active CN110231636B (en) | 2019-04-29 | 2019-04-29 | Self-adaptive unscented Kalman filtering method of GPS and BDS dual-mode satellite navigation system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110231636B (en) |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110972071A (en) * | 2019-10-29 | 2020-04-07 | 福建星网智慧软件有限公司 | Multi-mode positioning method, positioning server, terminal and storage medium |
| CN111323793A (en) * | 2020-03-30 | 2020-06-23 | 中国矿业大学 | GNSS pseudo-range single-point positioning state domain integrity monitoring method |
| EP3828583A1 (en) * | 2019-11-27 | 2021-06-02 | Honda Research Institute Europe GmbH | Analysis of localization errors in a mobile object |
| CN113075712A (en) * | 2021-03-17 | 2021-07-06 | 北京云恒科技研究院有限公司 | Autonomous controllable multi-system high-precision navigation equipment and navigation method |
| CN113407909A (en) * | 2021-07-15 | 2021-09-17 | 东南大学 | Tasteless algorithm for non-analytic complex nonlinear system |
| CN113709662A (en) * | 2021-08-05 | 2021-11-26 | 北京理工大学重庆创新中心 | Ultra-wideband-based autonomous three-dimensional inversion positioning method |
| CN113791432A (en) * | 2021-08-18 | 2021-12-14 | 哈尔滨工程大学 | Integrated navigation positioning method for improving AIS positioning accuracy |
| CN114348002A (en) * | 2021-12-29 | 2022-04-15 | 江苏大学 | System and method for estimating speed of hub motor driven electric automobile |
Citations (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1136838A2 (en) * | 2000-03-24 | 2001-09-26 | CLARION Co., Ltd. | GPS receiver capable of calculating accurate 2DRMS |
| US20050251328A1 (en) * | 2004-04-05 | 2005-11-10 | Merwe Rudolph V D | Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion |
| CN101976290A (en) * | 2010-11-01 | 2011-02-16 | 北京航空航天大学 | Navigation constellation optimization design and method based on decomposition thought and particle swarm fusion method |
| CN102176031A (en) * | 2011-01-06 | 2011-09-07 | 中国科学院国家授时中心 | System time difference based receiver completeness failure detection method in dual-mode navigation system |
| EP2400267A1 (en) * | 2010-06-25 | 2011-12-28 | Thales | Navigation filter for navigation system by terrain matching. |
| US20120162006A1 (en) * | 2010-12-28 | 2012-06-28 | Stmicroelectronics S.R.L. | Cinematic parameter computing method of a satellite navigation system and receiving apparatus employing the method |
| CN105659867B (en) * | 2010-07-02 | 2013-11-20 | 北京理工大学 | The visible star of satellite navigation system and the DOP value prediction system of user-based security model task |
| CN103411616A (en) * | 2013-07-05 | 2013-11-27 | 东南大学 | Vehicle-mounted integrated navigation method based on simplified inertia measurement assembly |
| CN103592657A (en) * | 2013-09-30 | 2014-02-19 | 北京大学 | Method for realizing single-mode RAIM (Receiver Autonomous Integrity Monitoring) under small number of visible satellites based on assistance of clock correction |
| CN104966126A (en) * | 2015-05-18 | 2015-10-07 | 深圳市联和安业科技有限公司 | Smart taxi-taking system |
| CN105785412A (en) * | 2016-03-03 | 2016-07-20 | 东南大学 | Vehicle rapid optimizing satellite selection positioning method based on GPS and Beidou double constellations |
| CN106646543A (en) * | 2016-12-22 | 2017-05-10 | 成都正扬博创电子技术有限公司 | High-dynamic satellite navigation signal carrier tracking method based on master-slave AUKF algorithm |
| CN106772524A (en) * | 2016-11-25 | 2017-05-31 | 安徽科技学院 | A kind of agricultural robot integrated navigation information fusion method based on order filtering |
| CN109631913A (en) * | 2019-01-30 | 2019-04-16 | 西安电子科技大学 | X-ray pulsar navigation localization method and system based on nonlinear prediction strong tracking Unscented kalman filtering |
-
2019
- 2019-04-29 CN CN201910357048.0A patent/CN110231636B/en active Active
Patent Citations (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1136838A2 (en) * | 2000-03-24 | 2001-09-26 | CLARION Co., Ltd. | GPS receiver capable of calculating accurate 2DRMS |
| US20050251328A1 (en) * | 2004-04-05 | 2005-11-10 | Merwe Rudolph V D | Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion |
| EP2400267A1 (en) * | 2010-06-25 | 2011-12-28 | Thales | Navigation filter for navigation system by terrain matching. |
| CN105659867B (en) * | 2010-07-02 | 2013-11-20 | 北京理工大学 | The visible star of satellite navigation system and the DOP value prediction system of user-based security model task |
| CN101976290A (en) * | 2010-11-01 | 2011-02-16 | 北京航空航天大学 | Navigation constellation optimization design and method based on decomposition thought and particle swarm fusion method |
| US20120162006A1 (en) * | 2010-12-28 | 2012-06-28 | Stmicroelectronics S.R.L. | Cinematic parameter computing method of a satellite navigation system and receiving apparatus employing the method |
| CN102176031A (en) * | 2011-01-06 | 2011-09-07 | 中国科学院国家授时中心 | System time difference based receiver completeness failure detection method in dual-mode navigation system |
| CN103411616A (en) * | 2013-07-05 | 2013-11-27 | 东南大学 | Vehicle-mounted integrated navigation method based on simplified inertia measurement assembly |
| CN103592657A (en) * | 2013-09-30 | 2014-02-19 | 北京大学 | Method for realizing single-mode RAIM (Receiver Autonomous Integrity Monitoring) under small number of visible satellites based on assistance of clock correction |
| CN104966126A (en) * | 2015-05-18 | 2015-10-07 | 深圳市联和安业科技有限公司 | Smart taxi-taking system |
| CN105785412A (en) * | 2016-03-03 | 2016-07-20 | 东南大学 | Vehicle rapid optimizing satellite selection positioning method based on GPS and Beidou double constellations |
| CN106772524A (en) * | 2016-11-25 | 2017-05-31 | 安徽科技学院 | A kind of agricultural robot integrated navigation information fusion method based on order filtering |
| CN106646543A (en) * | 2016-12-22 | 2017-05-10 | 成都正扬博创电子技术有限公司 | High-dynamic satellite navigation signal carrier tracking method based on master-slave AUKF algorithm |
| CN109631913A (en) * | 2019-01-30 | 2019-04-16 | 西安电子科技大学 | X-ray pulsar navigation localization method and system based on nonlinear prediction strong tracking Unscented kalman filtering |
Non-Patent Citations (7)
| Title |
|---|
| FANCHEN MENG 等: "A new fast satellite selection algorithm for BDS-GPS receivers", 《SIPS 2013 PROCEEDINGS》 * |
| YONG WANG 等: "BDS and GPS stand-alone and integrated attitude dilution of precision definition and comparison", 《SCIENCEDIRECT》 * |
| 吴玲 等: "多星座组合导航自适应信息融合滤波算法", 《航天控制》 * |
| 唐苗苗: "车载组合导航系统自适应无迹卡尔曼滤波算法研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 * |
| 尚俊娜 等: "一种顾及加权水平精度因子的室内宽带定位算法", 《中国惯性技术学报》 * |
| 白敏: "陆基导航定位系统中的HDOP模型与计算方法", 《数字通信》 * |
| 赵琳 等: "基于自适应EKF的BDS_GPS精密单点定位方法", 《系统工程与电子技术》 * |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110972071A (en) * | 2019-10-29 | 2020-04-07 | 福建星网智慧软件有限公司 | Multi-mode positioning method, positioning server, terminal and storage medium |
| EP3828583A1 (en) * | 2019-11-27 | 2021-06-02 | Honda Research Institute Europe GmbH | Analysis of localization errors in a mobile object |
| US11287281B2 (en) | 2019-11-27 | 2022-03-29 | Honda Research Institute Europe Gmbh | Analysis of localization errors in a mobile object |
| CN111323793A (en) * | 2020-03-30 | 2020-06-23 | 中国矿业大学 | GNSS pseudo-range single-point positioning state domain integrity monitoring method |
| CN111323793B (en) * | 2020-03-30 | 2021-02-05 | 中国矿业大学 | GNSS pseudo-range single-point positioning state domain integrity monitoring method |
| CN113075712B (en) * | 2021-03-17 | 2023-08-11 | 北京云恒科技研究院有限公司 | Autonomous controllable multi-system high-precision navigation equipment |
| CN113075712A (en) * | 2021-03-17 | 2021-07-06 | 北京云恒科技研究院有限公司 | Autonomous controllable multi-system high-precision navigation equipment and navigation method |
| CN113407909A (en) * | 2021-07-15 | 2021-09-17 | 东南大学 | Tasteless algorithm for non-analytic complex nonlinear system |
| CN113407909B (en) * | 2021-07-15 | 2024-01-09 | 东南大学 | Calculation method of odorless algorithm for non-analytic complex nonlinear system |
| CN113709662A (en) * | 2021-08-05 | 2021-11-26 | 北京理工大学重庆创新中心 | Ultra-wideband-based autonomous three-dimensional inversion positioning method |
| CN113709662B (en) * | 2021-08-05 | 2023-12-01 | 北京理工大学重庆创新中心 | Autonomous three-dimensional inversion positioning method based on ultra-wideband |
| CN113791432A (en) * | 2021-08-18 | 2021-12-14 | 哈尔滨工程大学 | Integrated navigation positioning method for improving AIS positioning accuracy |
| CN114348002A (en) * | 2021-12-29 | 2022-04-15 | 江苏大学 | System and method for estimating speed of hub motor driven electric automobile |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110231636B (en) | 2021-03-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110231636A (en) | The adaptive Unscented kalman filtering method of GPS and BDS dual mode satellite navigation system | |
| CN109459040B (en) | Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on RBF (radial basis function) neural network assisted volume Kalman filtering | |
| US11754400B2 (en) | Motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering | |
| WO2020087845A1 (en) | Initial alignment method for sins based on gpr and improved srckf | |
| CN109724599A (en) | A kind of Robust Kalman Filter SINS/DVL Combinated navigation method of anti-outlier | |
| AU2009200190B2 (en) | Methods and systems for underwater navigation | |
| CN110794409B (en) | Underwater single beacon positioning method capable of estimating unknown effective sound velocity | |
| CN109443379A (en) | A kind of underwater anti-shake dynamic alignment methods of the SINS/DVL of deep-sea submariner device | |
| CN110823217A (en) | Integrated navigation fault-tolerant method based on self-adaptive federal strong tracking filtering | |
| CN103776453A (en) | Combination navigation filtering method of multi-model underwater vehicle | |
| Guangcai et al. | MM estimation-based robust cubature Kalman filter for INS/GPS integrated navigation system | |
| CN110749891B (en) | Self-adaptive underwater single beacon positioning method capable of estimating unknown effective sound velocity | |
| CN110646783B (en) | Underwater beacon positioning method of underwater vehicle | |
| CN110779519B (en) | Underwater vehicle single beacon positioning method with global convergence | |
| CN103792562A (en) | Strong tracking UKF filter method based on sampling point changing | |
| CN107607977A (en) | A kind of adaptive UKF Combinated navigation methods based on the sampling of minimum degree of bias simple form | |
| CN103776449A (en) | Moving base initial alignment method for improving robustness | |
| Pagoti et al. | Development and performance evaluation of Correntropy Kalman Filter for improved accuracy of GPS position estimation | |
| Guo et al. | A robust SINS/USBL integrated navigation algorithm based on earth frame and right group error definition | |
| CN115096302A (en) | Strapdown inertial base navigation system information filtering robust alignment method, system and terminal | |
| Zhang et al. | An outlier-robust Rao–Blackwellized particle filter for underwater terrain-aided navigation | |
| CN107621632A (en) | Adaptive filter method and system for NSHV tracking filters | |
| CN113008235A (en) | Multi-source navigation information fusion method based on matrix K-L divergence | |
| CN111290008A (en) | Dynamic self-adaptive extended Kalman filtering fault-tolerant algorithm | |
| CN116772837A (en) | GNSS/SINS integrated navigation method based on interactive multi-model |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |