CN110231636B - Self-adaptive unscented Kalman filtering method of GPS and BDS dual-mode satellite navigation system - Google Patents
Self-adaptive unscented Kalman filtering method of GPS and BDS dual-mode satellite navigation system Download PDFInfo
- Publication number
- CN110231636B CN110231636B CN201910357048.0A CN201910357048A CN110231636B CN 110231636 B CN110231636 B CN 110231636B CN 201910357048 A CN201910357048 A CN 201910357048A CN 110231636 B CN110231636 B CN 110231636B
- Authority
- CN
- China
- Prior art keywords
- noise
- gps
- bds
- hdop
- state
- 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.)
- Active
Links
Images
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 a self-adaptive unscented Kalman filtering method of a GPS and BDS dual-mode satellite navigation system, which comprises the following steps: the method comprises the following steps: modeling a nonlinear system; step two: performing Newton motion law dispersion, and performing equivalent substitution on the Newton motion law dispersion to enable the Newton motion law dispersion to meet a model of a nonlinear system; step three: the HDOP self-adaptive optimization algorithm selects a GNSS working system with better horizontal accuracy to provide GNSS data, and substitutes the GNSS data into an equivalent substitution nonlinear system model; the method comprises the steps of processing nonlinear transmission of mean values and covariance by using an unscented transformation method, sampling Sigma points by using a UKF algorithm to solve uncertainty of noise, estimating and correcting the noise in real time so as to solve nonlinear characteristics of a combined navigation system, and deducing a noise statistical estimator to estimate and correct unknown or inaccurate noise statistics in real time based on a maximum likelihood criterion. The invention can obviously improve the precision of the GPS and BDS dual-mode satellite navigation system and has good effect when the model nonlinearity and the noise statistical characteristic of the processing system are uncertain.
Description
Technical Field
The invention relates to a filtering method of a satellite navigation system, in particular to a self-adaptive unscented Kalman filtering method of a GPS and BDS dual-mode satellite navigation system.
Background
With the social development and the increase of vehicles, the urban traffic road conditions are increasingly complex, and people put higher requirements on the precision of positioning navigation instruments. However, all the existing systems for positioning navigation instruments have limitations, so that the instruments cannot fully meet the requirements of people. In order to improve urban traffic, meet the requirements of people, realize high-precision, all-weather and global positioning, various positioning navigation systems can be combined, the advantages are raised, the disadvantages are avoided, and a multi-element organic combined navigation system is formed, so that the precision and the reliability of the positioning navigation system are improved.
In the prior art, please refer to chinese patent application No. 201710724015.6, publication No. 109425876a, entitled "an improved kalman filtering method for improving positioning accuracy", which has a drawback in that, for the non-linear problem, Extended Kalman Filtering (EKF) linearizes a non-linear model using taylor decomposition, which is difficult to calculate and has a linear error, and compared with Unscented Kalman Filtering (UKF), the accuracy of the model is reduced by introducing the linear error.
Compared with EKF, UKF has the following advantages: firstly, the nonlinear system does not need to be linearized, and the calculation error introduced in the linearization process of the nonlinear system is avoided; secondly, the unscented transformation adopted by the UKF can completely reproduce the mean value and the covariance of the state quantity of the nonlinear system; thirdly, the filtering precision of any nonlinear system can reach more than three orders of a Taylor series expansion, and the EKF can only reach the precision of one order; in addition, the UKF does not need to calculate a Jacobian matrix, so that the calculation amount of the UKF is greatly simplified, and calculation errors are not introduced due to the fact that the Jacobian matrix does not need to be calculated.
Disclosure of Invention
The invention aims to solve the technical problem of providing a self-adaptive unscented Kalman filtering method for a GPS and BDS dual-mode satellite navigation system, which can improve the precision of vehicle integrated navigation and aims at overcoming the defects of the prior art.
In order to solve the technical problems, the invention adopts the following technical scheme.
An adaptive unscented kalman filtering method for a GPS and BDS dual-mode satellite navigation system, the method comprising:
and step S1, establishing a nonlinear system model. The nonlinear system was modeled as:
in the formula, xkIs the state vector at time k, zkIs the observation vector at time k, fk(. and h)k(. respectively) State function and Observation function of the nonlinear System, wkAnd vkThe two white Gaussian noises are uncorrelated and the mean value and the covariance matrix of the two white Gaussian noises respectively satisfy the following conditions:
in the formula, q is a state noise mean value, and r is an observation noise mean value;
and step S2, the Newton motion law is discrete and equivalently substituted for the nonlinear system model. The state vectors are respectively taken as xe＝[ve，ae]T，xn＝[vn，an]T，xeAnd xnEast and north speed, respectively, aeAnd anThe two state vectors are written together as:
x＝[v，a]T；
in the following formula, the sampling interval is set to T, vk-1To observe the instantaneous speed, v, at the starting momentkInstantaneous speed after T time, ak-1The acceleration in T time is obtained by discretizing the Newton's law of motion:
vk＝vk-1+ak-1T；
the output sentences of the GPS dual-mode receiver and the BDS dual-mode receiver are used for respectively providing the position and the speed of the east direction and the north direction, and the observation direction is measured as z ═ x, v]TWherein the initial state quantity x of the system0And stateThe noise q and the observation noise r are not related to each other, and the initial state follows Gaussian normal distribution, which
A priori mean sum P0The covariance matrices are:
let mu letk＝Wk-q，ηk＝vkR, which is substituted into the nonlinear system model to obtain:
in the formula, muk-1And ηkAre all white gaussian noise with a mean of zero and variance of Q and R, respectively.
And step S3, carrying out HDOP self-adaptive optimization model algorithm. And extracting a horizontal component precision factor (HDOP) in an output statement of the GPS and BDS dual-mode receiver, wherein the HDOP determines the reliability of the positioning precision and is used as an input value of an HDOP self-adaptive optimization algorithm.
First, an HDOP prediction model is built:
YH(i+1)＝k(i+1)+b
[k，b]T＝r(I，H)
wherein, YH(i+1)The predicted value of the HDOP difference value of the BDS and the GPS at the moment of i +1 is obtained; r (·,) is a least squares function; vector I ═ I (I, I-1,.., I-3)TI is more than or equal to 1, representing the number vector of four continuous measurements; vector H ═ YH(i)，YH(i-1)，...，YH(i-3))TAnd i is more than or equal to 1, which represents the HDOP difference vector of the BDS and the GPS measured four times. To avoid interference of drifting GNSS signals with algorithm spoofing, when YH(i)Taking 0.4 when the content is more than or equal to 0.4; y isH(i)When the content is less than or equal to 0, 0 is selected.
For YH(i)The method comprises the following steps:
YH(i)＝ΔHDOPB(i)-ΔHDOPG(i)；
ΔHDOPG(i)＝λG(i)-λo；
ΔHDOPB(i)＝λo-λB(i)；
in the formula, λG(k)HDOP for GPS signal at time k; lambda [ alpha ]B(k)HDOP which is the BDS signal at time k; lambda [ alpha ]OThe HDOP, O (optimal) of the currently preferred satellite navigation system is optimal.
And finally, establishing an HDOP self-adaptive optimization model:
wherein S isiThe signal is actually selected for the ith time; w is a1Is the weight of the GPS signal, w2Is the weight of the BDS signal; argmin (E) is the minimum of vector E; vector E ═ H, YH(i+1))T。
The HDOP adaptive optimization model algorithm decides to use the GPS or BDS working system and substitute its GNSS data into the equivalent surrogate nonlinear system model of step S2.
Preferably, the method further comprises: by utilizing the nonlinear transmission of the UT transformation processing mean value and the covariance, the UKF filtering algorithm of the system after the UT transformation comprises the following processes: sigma point sampling, time updating, observation updating and noise statistic estimation.
And selecting the Sigma points, and adjusting the sampling frequency of the Sigma points according to the complexity of the traffic road section within a certain time range. For example, when a vehicle passes through a traffic node such as a crossroad, a bus station, a sidewalk and the like, Sigma points are sampled doubly.
The invention discloses a self-adaptive Unscented Kalman filtering method of a GPS and BDS dual-mode satellite navigation system, which combines an HDOP self-adaptive optimization algorithm to select GNSS system data with better positioning precision, substitutes the GNSS system data into a nonlinear system model, processes the nonlinear system by using an Unscented transformation method and adopts an Unscented Kalman Filter (UKF). Due to the introduction of the HDOP self-adaptive optimization algorithm, the switching frequency of a GNSS working system is reduced, the robustness, stability and reliability of the GPS and BDS dual-mode satellite navigation system are improved, the deception interference of drifting GNSS signals to the system is avoided, and meanwhile, the error rate of the GPS and BDS dual-mode satellite navigation system is reduced. The self-adaptive unscented Kalman filtering method of the GPS and BDS dual-mode satellite navigation system can obviously improve the precision of vehicle integrated navigation and has good effect when the model nonlinearity and the noise statistical characteristic of the system are uncertain.
Drawings
FIG. 1 is a flow chart of an adaptive unscented Kalman filtering method for a GPS and BDS dual-mode satellite navigation system;
FIG. 2 is a graph of a GPS/BDS vehicle terminal filtering test experiment;
FIG. 3 is a graph of an AKF east and north position test experiment;
FIG. 4 is a graph of AKF east and north speed error testing experiments;
FIG. 5 is a UKF east and north position error test experiment graph;
FIG. 6 is a UKF east and north speed error test experiment graph;
FIG. 7 is a graph of an experiment for the error test of the east and north positions of AUKF;
FIG. 8 is a graph of AUKF east and north velocity error test experiments;
FIG. 9 is a graph of a position error test experiment before and after AUKF filtering;
FIG. 10 is a graph of an east position comparison experiment for each filter;
FIG. 11 is a graph of north position comparison experiments for various filters;
FIG. 12 is a graph of east speed comparison experiments for each filter;
fig. 13 is a graph of north speed comparison experiment of each filter.
Detailed Description
The invention is described in more detail below with reference to the figures and examples.
The invention discloses a self-adaptive unscented Kalman filtering method of a GPS and BDS dual-mode satellite navigation system, please refer to FIG. 1, the method comprises the following steps:
and step S1, establishing a nonlinear system model. The nonlinear system was modeled as:
in the formula, xkIs the state vector at time k, zkIs the observation vector at time k, fk(. and h)k(. respectively) State function and Observation function of the nonlinear System, wkAnd vkThe two white Gaussian noises are uncorrelated and the mean value and the covariance matrix of the two white Gaussian noises respectively satisfy the following conditions:
in the formula, q is a state noise mean value, and r is an observation noise mean value;
since a land vehicle can be approximated as a two-dimensional plane motion carrier, motion in a two-dimensional plane can be decomposed into two mutually perpendicular one-dimensional motions, such as motion in the east-west direction and motion in the north-south direction. The motion in two directions has no correlation, so that the one-dimensional motion in two directions can be respectively subjected to Kalman filtering;
and step S2, the Newton motion law is discrete and equivalently substituted for the nonlinear system model. The state vectors are respectively taken as xe＝[ve，ae]T，xn＝[vn，an]T，xeAnd xnAre respectively eastHeading and northbound speeds, aeAnd anThe two state vectors are written together as:
x＝[v，a]T；
in the following formula, the sampling interval is set to T, vk-1To observe the instantaneous speed, v, at the starting momentkInstantaneous speed after T time, ak-1The acceleration in T time is obtained by discretizing the Newton's law of motion:
vk＝vk-1+ak-1T；
providing east and north positions and velocities by using output sentences of a GPS and BDS dual-mode receiver respectively, and taking z as [ x, v ] as an observation vector]TWherein the initial state quantity x of the system0Is independent of both state noise and observation noise, and the initial state follows a Gaussian normal distribution which
A priori mean sum P0The covariance matrices are:
let mu letk＝wk-q，ηk＝vkR, which is substituted into the nonlinear system model to obtain:
in the formula, muk-1And ηkAre all white gaussian noise with a mean of zero and variance of Q and R, respectively.
And step S3, carrying out HDOP self-adaptive optimization model algorithm. And extracting a horizontal component precision factor (HDOP) in an output statement of the GPS and BDS dual-mode receiver, wherein the HDOP determines the reliability of the positioning precision and is used as an input value of an HDOP self-adaptive optimization algorithm.
First, an HDOP prediction model is built:
YH(i+1)＝k(i+1)+b
[k，b]T＝r(I，H)
wherein, YH(i+1)The predicted value of the HDOP difference value of the BDS and the GPS at the moment of i +1 is obtained; r (·,) is a least squares function; vector I ═ I (I, I-1,.., I-3)TI is more than or equal to 1, representing the number vector of four continuous measurements; vector H ═ YH(i)，YH(i-1)，...，YH(i-3))TAnd i is more than or equal to 1, which represents the HDOP difference vector of the BDS and the GPS measured four times. To avoid interference of drifting GNSS signals with algorithm spoofing, when YH(i)Taking 0.4 when the content is more than or equal to 0.4; y isH(i)When the content is less than or equal to 0, 0 is selected.
For YH(i)The method comprises the following steps:
YH(i)＝ΔHDOPB(i)-ΔHDOPG(i)；
ΔHDOPG(i)＝λG(i)-λo；
ΔHDOPB(i)＝λo-λB(i)；
in the formula, λG(k)HDOP for GPS signal at time k; lambda [ alpha ]B(k)HDOP which is the BDS signal at time k; lambda [ alpha ]OThe HDOP, O (optimal) of the currently preferred satellite navigation system is optimal.
And finally, establishing an HDOP self-adaptive optimization model:
wherein S isiThe signal is actually selected for the ith time; w is a1Is the weight of the GPS signal, w2Is the weight of the BDS signal; argmin (E) is the minimum of vector E; vector E ═ H, YH(i+1))T。
In the method, the GNSS system data with better positioning precision is selected by combining with the HDOP self-adaptive optimization algorithm and is substituted into the equivalent substitution nonlinear system model. Due to the introduction of the HDOP self-adaptive optimization algorithm, the switching frequency of a GNSS working system is reduced, the robustness, stability and reliability of the GPS and BDS dual-mode satellite navigation system are improved, the deception interference of drifting GNSS signals to the system is avoided, and meanwhile, the error rate of the GPS and BDS dual-mode satellite navigation system is reduced.
For the adaptive unscented kalman filter algorithm, in this embodiment, the adaptive kalman filter is used under the condition that the system mathematical model is uncertain or the statistical characteristics of the system state noise and the observation noise are uncertain, and performs filtering by adjusting the system mathematical model parameters and the noise statistical characteristic parameters in real time in the filtering process; the UKF mainly carries out filtering on a nonlinear system, but the conventional UKF algorithm has limitations, namely the prior statistical characteristics of system state noise and observation noise need to be accurately known, so that the traditional UKF cannot achieve the expected filtering effect on the nonlinear system with uncertain system models and uncertain system noise statistical characteristics, the system is unstable or even diverged, the adaptive Kalman filter and the UKF are combined to form the adaptive unscented Kalman filter, the noise statistical estimator is used for carrying out real-time estimation and correction on unknown or inaccurate noise statistics while carrying out filtering calculation, the UKF has the adaptive capacity of coping with noise change, and the UKF still has excellent stability and convergence under the condition of unknown or inaccurate noise statistics; and the noise statistics estimator is unbiased in its estimation of the noise statistics.
For the adaptive unscented kalman filter algorithm, in this embodiment, when the noise mean is non-zero, that is, under the condition of non-gaussian white noise, the statistical characteristics of the state noise and observation noise of the system are studied, the UKF algorithm is used to perform Sigma point sampling, and the state noise w is sampledk-1And measuring noiseSound vkThe proposed processing adopts AKF algorithm with noise change coping capability to output observation information to estimate and correct noise statistics in real time, namely W, due to uncertain statistical characteristics of state noise and observation noisek-1And VkThe mean and covariance matrix, and an adaptive unscented Kalman filtering algorithm.
In this embodiment, the method further includes: by utilizing the nonlinear transmission of the UT transformation processing mean value and the covariance, the UKF filtering algorithm of the system after the UT transformation comprises the following processes: sigma point sampling, time updating, observation updating and noise statistic estimation.
Furthermore, in the Sigma point sampling process, a proportional correction sampling strategy is adopted to ensure the semi-positive property (more than or equal to 0) of the covariance of the output variable. And selecting the Sigma points, and adjusting the sampling frequency of the Sigma points according to the complexity of the traffic road section within a certain time range. For example, when a vehicle passes through a traffic node such as a crossroad, a bus station, a sidewalk and the like, Sigma points are sampled doubly.
In the time updating step, the estimated value of the k-1 time is used
Covariance matrix P of errors at time k-1k-1/k-1Calculate Sigma Point ξi，k-1/k-1(i ═ 0, 1.., L), Sigma points are passed through a nonlinear state function fk(.) + q propagation becomes gammai，k/k-1Q is the system state noise used to calculate the one-step state prediction mean
Sum error covariance matrix Pk/k-1Namely:
γi，k/k-1＝fk-1(ξi，k/k-1)+q，i＝0，1，...，L；
wherein, Wi mAnd Wi cTo correspond to xiiWeight of (i ═ 0, 1., L), Wi mAnd Wi cRespectively calculating weight coefficients of first-order and second-order statistical characteristics; q is the covariance of the system state noise Q;
by using
And Pk/k-1And a proportional correction sampling strategy to calculate the Sigma point, xii，k/k-1(i ═ 0, 1.., L) by a non-linear observation function hk(. r) + r propagation as xi，k/k-1R is the covariance of the observed noise R, from which the output prediction mean is obtained
And covariance matrix
And
namely:
xi，k/k-1＝hk(ξi，k/k-1)+r，i＝0，1，...，L；
in the observation updating step, after a new observed quantity is obtained, actual filtering updating is carried out by using a conventional Kalman filter recursion formula, and a basic equation is as follows:
in the formula, KkIn order to filter the gain matrix of the filter,
the noise statistics estimating step is implemented based on a noise statistics estimator:
setting W to all obey normal distributionk-1And VkAre independent of each other, derive the sub-optimal maximum prior noise statistics estimator applied to UKF according to the maximum likelihood probability estimation criterion, when Q, Q, R, R are unknown, it and the state quantity x0，x1，...，xkMaximum likelihood probability estimation of
And
obtaining by using the maximization density;
the recursion formula of the secondary unbiased maximum prior noise statistic estimator is as follows:
the suboptimal maximum prior unbiased estimation of the system state noise q is as follows:
the suboptimal maximum prior unbiased estimation of the system state noise covariance Q is:
the suboptimal maximum prior unbiased estimation of the observation noise r is as follows:
the suboptimal maximum prior unbiased estimation of the observed noise covariance R is:
after the state noise statistical characteristic and the observation noise statistical characteristic of the UKF in the filtering process are determined, the unscented transformation is carried out on the mathematical model of the system by using the UKF algorithm, and the recursive process of the filtering algorithm of the self-adaptive unscented Kalman filter based on the noise statistical estimator is obtained:
the prior prediction process is as follows:
in the formula (I), the compound is shown in the specification,
is a priori estimated value, Wi mIs sigma point xii，k-1/k-1Mean value weighting parameters;
using a priori estimates
Sum error covariance matrix Pk-1|k-1To calculate the sigma point xii，k-1/k-1(i ═ 0, 1.. times, L), the sigma point is passed through a nonlinear state function fk(.) + q becomes gammai，k/k-1Then there is γi，k/k-1＝fk-1(ξi，k-1/k-1) + q and the error covariance matrix is obtained:
the posterior estimation process is as follows:
in the formula (I), the compound is shown in the specification,
for a posteriori estimate, zkIn order to be a weighted measurement variable,
to predict value, zkAnd
the difference of (a) is the residue of the measurement process, KkA gain matrix that is a residue;
the gain calculation process is as follows:
in the formula, residual gain KkFor minimizing the a posteriori estimation error covariance,
for a priori estimation
Covariance of error Pk/k-1，
For a posteriori estimation
Covariance of error Pk/k；
The state covariance matrix updating process comprises the following steps:
in the formula, Pk/kFor a posteriori estimation of error covariance matrix, Pk/k-1For the prior estimation of the error covariance matrix,
is the residual error covariance.
Based on the above formulas, combining with an HDOP adaptive optimization algorithm, GNSS system data with better positioning accuracy is selected and substituted into a nonlinear system model to be used as an input source of an AUKF (adaptive unknown Kalman Filter) filter. Simultaneously, the nonlinear system is processed by an Unscented transformation method, and an Unscented Kalman Filter (Unscented Kalman Filter, Unscented kf) is adopted. The method can solve the nonlinear characteristic of a GPS and BDS dual-mode satellite navigation system, meanwhile, a noise statistics estimator is used for estimating and correcting unknown or inaccurate noise statistics in real time, compared with single Adaptive Kalman Filtering (AKF) and UKF, the filtering algorithm has the Adaptive capacity of coping with noise change, the filtering algorithm still has excellent stability and convergence under the condition that the noise statistics is unknown or inaccurate, and the noise statistics estimator estimates the noise statistics unbiased. Due to the introduction of the HDOP self-adaptive optimization algorithm, the switching frequency of a GNSS working system is reduced, the robustness, stability and reliability of the GPS and BDS dual-mode satellite navigation system are improved, the deception interference of drifting GNSS signals to the system is avoided, and meanwhile, the error rate of the GPS and BDS dual-mode satellite navigation system is reduced. In the UKF process, the nonlinear system does not need to be linearized, and the linear error introduced in the linearization process of the nonlinear system is avoided; the unscented transformation adopted by the UKF can completely reproduce the mean value and covariance of the state quantity of the nonlinear system; the filtering precision of any nonlinear system can reach more than three orders of Taylor series expansion; the calculation of the Jacobian matrix is not needed, calculation errors are not introduced, and the calculation amount of the UKF is greatly simplified.
In the actual test process, the present embodiment performs test research on the adaptive unscented kalman filter algorithm, assuming that the vehicle makes an accelerated motion at an initial speed of 10m/s, the initial parameter is selected to be the observation point N of 40, the sampling period T of 1s, and the covariance matrix of the satellite system observation noise is [20 [2，202]The comparison result of the filtering test experiment of the GPS and BDS dual-mode terminal is shown in fig. 2.
The simulated carrier in the experiment of the embodiment is a car, the running environment of the car is selected in the section with dense population vehicles, high buildings and tunnels, and in addition, the car does not encounter an emergency in the running process; the GPS and BDS dual-mode vehicle-mounted terminal of the embodiment is applied to carry out experiments, and the dual-mode satellite navigation system respectively selects an information fusion algorithm of AKF, UKF and Adaptive Unscented Kalman (AUKF) to carry out performance comparison. The mathematical model for AKF is known and is a linear system model, whereas the mathematical models for UKF and AUKF are typical non-linear systems. Due to the influence of factors such as the device and the external environment, state noise and observation noise in the system model are unknown in the running process of the vehicle, the statistical parameters of the state noise and the observation noise of the system need to be adjusted and estimated in real time, and the obtained statistical characteristics of the noise and the system mathematical model are utilized to filter the combined navigation system. The experiment for testing the position error (east error, north error, and because the automobile runs on the earth surface, it can be approximately considered that the automobile does not move in the vertical direction and neglects the elevation error) and the speed error by using the AKF, the UKF and the AUKF is shown in fig. 3 to fig. 9.
From the experimental results of fig. 3 to 9, it can be known that the position error and the speed error of the vehicle in the process of traveling after the UKF filtering processing are both smaller than those after the conventional AKF filtering processing, and the phenomenon of data divergence exists when the AKF filtering processing reaches a certain time. In contrast, comparative experiments of the position error and the velocity error of AKF, UKF and AUKF are shown in fig. 10 to 13. From the experimental results of fig. 10 to fig. 13, it can be known that, in the vehicle running process, the AUKF is significantly better than the AKF, and even if the system model is nonlinear, the final position error and speed error are smaller than the errors of the AKF and the UKF, which indicates that the AUKF algorithm can greatly improve the precision of the vehicle combination navigation and has a good effect when processing the system model nonlinearity and the uncertain noise statistical characteristics.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents or improvements made within the technical scope of the present invention should be included in the scope of the present invention.
Claims (7)
1. An adaptive unscented Kalman filtering method for a GPS and BDS dual-mode satellite navigation system is characterized by comprising the following steps:
step S1, establishing a nonlinear system model, and setting the nonlinear system model as:
in the formula, xkIs the state vector at time k, zkIs the observation vector at time k, fk(. and h)k(. respectively) State function and Observation function of the nonlinear System, wkAnd vkThe two white Gaussian noises are uncorrelated and the mean value and the covariance matrix of the two white Gaussian noises respectively satisfy the following conditions:
in the formula, q is a state noise mean value, and r is an observation noise mean value;
s2, carrying out discretization equivalent substitution on a nonlinear system model by using a Newton motion law; the state vectors are respectively taken as xe＝[Ve，ae]T，xn＝[Vn，an]T，xeAnd xnEast and north speed, respectively, aeAnd anThe two state vectors are written together as:
x＝[v，a]T；
in the following formula, the sampling interval is set to T, vk-1To observe the instantaneous speed, v, at the starting momentkInstantaneous speed after T time, ak-1The acceleration in T time is obtained by discretizing the Newton's law of motion:
vk＝vk-1+ak-1T；
the output sentences of the GPS dual-mode receiver and the BDS dual-mode receiver are used for respectively providing the position and the speed of the east direction and the north direction, and the observation direction is measured as z ═ x, v]TWherein the initial state quantity x of the system0Is independent of both state noise and observation noise, and the initial state follows a Gaussian normal distribution which
A priori mean sum P0The covariance matrices are:
let mu letk＝wk-q，ηk＝vkR, which is substituted into the nonlinear system model to obtain:
in the formula, muk-1And ηkThe white Gaussian noises are all white Gaussian noises with the mean value of zero and the variance of Q and R respectively;
step S3, extracting a horizontal component precision factor HDOP in an output statement of the GPS and BDS dual-mode receiver by the HDOP self-adaptive optimization model algorithm, wherein the HDOP determines the reliability of the positioning precision and is used as an input value of the HDOP self-adaptive optimization algorithm;
first, an HDOP prediction model is built:
YH(i+1)＝k(i+1)+b
[k，b]T＝r(I，H)
wherein, YH(i+1)The predicted value of the HDOP difference value of the BDS and the GPS at the moment of i +1 is obtained; r (·,) is a least squares function; vector I ═ I (I, I + 1., I +3)TI is more than or equal to 1, representing the number vector of four continuous measurements; vector H ═ YH(i)，YH(i+1)，...，YH(i+3))TWhen i is more than or equal to 1, the HDOP difference vector of BDS and GPS is measured for four times, when Y isH(i)Taking 0.4 when the content is more than or equal to 0.4; y isH(i)When the content is less than or equal to 0, taking 0;
for YH(i)The method comprises the following steps:
YH(i)＝ΔHDOPB(i)-ΔHDOPG(i)；
ΔHDOPG(i)＝λG(i)-λo；
ΔHDOPB(i)＝λo-λB(i)；
in the formula, λG(k)HDOP for GPS signal at time k; lambda [ alpha ]B(k)HDOP which is the BDS signal at time k; lambda [ alpha ]OThe HDOP, O (optimal) of the currently preferred satellite navigation system is optimal;
and finally, establishing an HDOP self-adaptive optimization model:
wherein S isiThe signal is actually selected for the ith time; w is a1Being GPS signalsWeight, w2Is the weight of the BDS signal; argmin (E) is the minimum of vector E; vector E ═ H, YH(i+1))T；
The HDOP adaptive optimization model algorithm decides to use the GPS or BDS working system and substitute its GNSS data into the equivalent surrogate nonlinear system model of step S2.
2. The adaptive unscented kalman filter method of GPS and BDS dual-mode satellite navigation system of claim 1, wherein the HDOP adaptive optimization model algorithm selects GNSS system data with better positioning accuracy and substitutes it into an equivalent surrogate nonlinear system model.
3. The adaptive unscented kalman filter method of a dual mode GPS and BDS satellite navigation system of claim 1, wherein the method further comprises: by utilizing the nonlinear transmission of the UT transformation processing mean value and the covariance, the UKF filtering algorithm of the system after the UT transformation comprises the following processes: sigma point sampling, time updating, observation updating and noise statistic estimation.
4. The adaptive unscented kalman filter method of GPS and BDS dual mode satellite navigation system of claim 3, wherein in the Sigma point sampling process, a proportional modified sampling strategy is employed to ensure the semi-positivity of the covariance of the output variable, the Sigma point is selected, and the sampling frequency of the Sigma point is adjusted according to the complexity of the traffic segment within a certain time range.
5. The adaptive unscented kalman filter method for dual mode GPS and BDS satellite navigation system as claimed in claim 3, wherein the time update step uses the estimated value of time k-1
Covariance matrix P of errors at time k-1k-1/k-1Calculate Sigma Point ξi，k-1/k-1(i ═ 0, 1.., L), Sigma points pass through a nonlinear state functionNumber fk(.) + q propagation becomes gammai，k/k-1Q is the system state noise used to calculate the one-step state prediction mean
Sum error covariance matrix Pk/k-1Namely:
γi，k/k-1＝fk-1(ξi，k/k-1)+q，i＝0，1，...，L；
wherein, Wi mAnd Wi cTo correspond to xiiWeight of (i ═ O, 1., L), Wi mAnd Wi cRespectively calculating weight coefficients of first-order and second-order statistical characteristics; q is the covariance of the system state noise Q;
by using
And Pk/k-1And a proportional correction sampling strategy to calculate the Sigma point, xii，k/k-1(i ═ 0, 1.., L) by a non-linear observation function hk(. r) + r propagation as xi，k/k-1R is the covariance of the observed noise R, from which the output prediction mean is obtained
And covariance matrix
And
namely:
xi，k/k-1＝hk(ξi，k/k-1)+r，i＝0，1，...，L；
6. the adaptive unscented kalman filter method of a GPS and BDS dual-mode satellite navigation system as claimed in claim 3, wherein in the observation update step, after obtaining a new observation quantity, the actual filtering update is performed using a conventional kalman filter recursion formula, and the basic equation is as follows:
in the formula, KkIn order to filter the gain matrix of the filter,
7. the adaptive unscented kalman filter method of a dual mode GPS and BDS satellite navigation system of claim 3, wherein the noise statistics estimating step is based on a noise statistics estimator:
setting W to all obey normal distributionk-1And VkAre independent of each other, derive the sub-optimal maximum prior noise statistics estimator applied to UKF according to the maximum likelihood probability estimation criterion, when Q, Q, R, R are unknown, it and the state quantity x0，x1，...，xkMaximum likelihood probability estimation of
And
obtaining by using the maximization density;
the recursion formula of the secondary unbiased maximum prior noise statistic estimator is as follows:
the suboptimal maximum prior unbiased estimation of the system state noise q is as follows:
the suboptimal maximum prior unbiased estimation of the system state noise covariance Q is:
the suboptimal maximum prior unbiased estimation of the observation noise r is as follows:
the suboptimal maximum prior unbiased estimation of the observed noise covariance R is:
after the state noise statistical characteristic and the observation noise statistical characteristic of the UKF in the filtering process are determined, the unscented transformation is carried out on the mathematical model of the system by using the UKF algorithm, and the recursive process of the filtering algorithm of the self-adaptive unscented Kalman filter based on the noise statistical estimator is obtained:
the prior prediction process is as follows:
in the formula (I), the compound is shown in the specification,
is a priori estimated value, Wi mIs sigma point xii，k-1/k-1The weight used for mean weighting;
using a priori estimates
Sum error covariance matrix Pk-1|k-1To calculate the sigma point xii，k-1/k-1(i ═ 0, 1.. times, L), the sigma point is passed through a nonlinear state function fk(.) + q becomes gammai，k/k-1Then there is γi，k/k-1＝fk-1(ξi，k-1/k-1) + q and the error covariance matrix is obtained:
the posterior estimation process is as follows:
in the formula (I), the compound is shown in the specification,
for a posteriori estimate, zkIn order to be a weighted measurement variable,
to predict value, zkAnd
the difference of (a) is the residue of the measurement process, KkA gain matrix that is a residue;
the gain calculation process is as follows:
in the formula, residual gain KkFor minimizing the a posteriori estimation error covariance,
for a priori estimation
Covariance of error Pk/k-1，
For a posteriori estimation
Covariance of error Pk/k；
The state covariance matrix updating process comprises the following steps:
in the formula, Pk/kFor a posteriori estimation of error covariance matrix, Pk/k-1For the prior estimation of the error covariance matrix,
is the residual 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 CN110231636A (en) | 2019-09-13 |
| CN110231636B true 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) |
Families Citing this family (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 |
| EP3828583A1 (en) | 2019-11-27 | 2021-06-02 | Honda Research Institute Europe GmbH | Analysis of localization errors in a mobile object |
| 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 |
| CN113407909B (en) * | 2021-07-15 | 2024-01-09 | 东南大学 | Calculation method of odorless algorithm for non-analytic complex nonlinear system |
| 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 | 哈尔滨工程大学 | A Combined Navigation and Positioning Method to Improve AIS Positioning Accuracy |
| CN114348002B (en) * | 2021-12-29 | 2024-07-12 | 江苏大学 | Vehicle speed estimation system and method for electric vehicle driven by hub motor |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 |
| 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 |
| 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 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE60113581T2 (en) * | 2000-03-24 | 2006-05-18 | Clarion Co., Ltd. | GPS receiver capable of accurate 2DRMS calculation |
| CN101976290B (en) * | 2010-11-01 | 2012-09-05 | 北京航空航天大学 | Navigation constellation optimization design platform and method based on decomposition thought and particle swarm fusion method |
| 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 (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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. |
| 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 |
Non-Patent Citations (7)
| Title |
|---|
| A new fast satellite selection algorithm for BDS-GPS receivers;Fanchen Meng 等;《SiPS 2013 Proceedings》;20131202;第371-376页 * |
| BDS and GPS stand-alone and integrated attitude dilution of precision definition and comparison;Yong Wang 等;《ScienceDirect》;20171202;第2972-2981页 * |
| 一种顾及加权水平精度因子的室内宽带定位算法;尚俊娜 等;《中国惯性技术学报》;20181231;第26卷(第6期);第792-798页 * |
| 基于自适应EKF的BDS_GPS精密单点定位方法;赵琳 等;《系统工程与电子技术》;20160930;第38卷(第9期);第2142-2148页 * |
| 多星座组合导航自适应信息融合滤波算法;吴玲 等;《航天控制》;20101231;第28卷(第6期);第38-42、62页 * |
| 车载组合导航系统自适应无迹卡尔曼滤波算法研究;唐苗苗;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20140415;第1-92页 * |
| 陆基导航定位系统中的HDOP模型与计算方法;白敏;《数字通信》;20100425;第51-54页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110231636A (en) | 2019-09-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110231636B (en) | Self-adaptive unscented Kalman filtering method of GPS and BDS dual-mode satellite navigation system | |
| CN110779518B (en) | A single beacon localization method for underwater vehicle with global convergence | |
| Li et al. | A novel hybrid fusion algorithm for low-cost GPS/INS integrated navigation system during GPS outages | |
| Almagbile et al. | Evaluating the performances of adaptive Kalman filter methods in GPS/INS integration | |
| CN109459040B (en) | Multi-AUV co-localization method based on RBF neural network-assisted volumetric Kalman filter | |
| CN110749891B (en) | An adaptive underwater single beacon localization method for estimating unknown effective sound velocity | |
| CN109059911B (en) | A Data Fusion Method of GNSS, INS and Barometer | |
| Liu et al. | Variational Bayesian-based robust cubature Kalman filter with application on SINS/GPS integrated navigation system | |
| CN109507706B (en) | GPS signal loss prediction positioning method | |
| CN108983271A (en) | Based on RTK-GPS/INS train combined positioning method | |
| CN112577496B (en) | Multi-source fusion positioning method based on self-adaptive weight selection | |
| CN105447574B (en) | A kind of auxiliary blocks particle filter method, device and method for tracking target and device | |
| CN105180938A (en) | Particle filter-based gravity sampling vector matching positioning method | |
| CN110794409A (en) | Underwater single beacon positioning method capable of estimating unknown effective sound velocity | |
| CN104613966B (en) | A kind of cadastration off-line data processing method | |
| CN103973263A (en) | Novel approximation filter method | |
| CA2699137A1 (en) | Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering | |
| CN112269192B (en) | Quick self-adaptive dynamic Beidou monitoring real-time resolving denoising method | |
| CN118276135A (en) | Combined navigation method based on self-adaptive maximum entropy criterion Kalman filtering | |
| Avzayesh et al. | Improved-Performance Vehicle’s State Estimator Under Uncertain Model Dynam | |
| CN113654554B (en) | Fast self-adaptive dynamic inertial navigation real-time resolving denoising method | |
| CN106855626A (en) | Vector tracking method and wave filter | |
| CN107621632A (en) | Adaptive filter method and system for NSHV tracking filters | |
| CN110912535B (en) | Novel non-pilot Kalman filtering method | |
| CN113008235A (en) | Multi-source navigation information fusion method based on matrix K-L divergence |
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 |