CN102944888B - Low calculating quantity global position system (GPS) positioning method based on second-order extended Kalman - Google Patents

Abstract

The invention discloses a low calculating quantity global position system (GPS) positioning method based on second-order extended Kalman. The method includes the following steps of acquiring positioning results at the local k-1 moment, and calculating a priori estimate state quantity and a priori estimate error covariance at the current k moment; establishing an estimating equation of observed quantities; calculating a Kalman gain K<k>; and calculating a positioning result and an error covariance at the current k moment. According to the low calculating quantity GPS positioning method based on the second-order extended Kalman, performances of second-order extended Kalman filter (EKF2) are achieved, simultaneously, calculating quantities are reduced, and the method is suitable for low-end hardware.

Description

A kind of low operand GPS localization method based on second order spreading kalman

Technical field

The invention belongs to GPS localization method field, relate to a kind of GPS positioning calculation method, be specifically related to a kind of low operand GPS localization method based on second order spreading kalman, the calculated amount of the method is applicable to low side hardware.

Background technology

In GPS positioning field, conventional location algorithm has least square method (WLS), expanded Kalman filtration algorithm (EKF), second-order EKF algorithm (EKF2) at present.WLS is the most basic location clearing algorithm, but under the condition of dynamic multi-path, the locating effect of WLS is very undesirable.Therefore, we adopt the positioning calculation algorithm of EKF2.Kalman filter is described by a series of recurrence mathematical formulaes.It provides a kind of efficient computable method to carry out the state of estimation procedure, and makes to estimate that square error is minimum, is a kind of efficient predictive filtering location technology.When adopting spreading kalman algorithm to position, the anchor point being brought by multipath in the time of can effectively suppressing dynamic is shaken serious problem, meanwhile, also can reach the positioning result more far better than least square when static state.But general EKF2 algorithm is larger for the demand of calculated amount, higher to hardware requirement, is not suitable for low side hardware.The out-of-lock detection method of a kind of GPS receiver based on extended Kalman filter track loop is also provided in prior art, is mainly to calculate prior estimate mean square deviation error, setting up observational variable matrix Zk, last computer card Kalman Filtering gain Kk.The invention provides a kind of EKF2 of low operand, algorithm reduces operand when reaching EKF2 performance.

Summary of the invention

Goal of the invention: the problem and shortage existing for above-mentioned prior art, the object of this invention is to provide a kind of low operand GPS localization method based on second order spreading kalman, when reaching EKF2 performance, reduce operand, be applicable to low side hardware.

Technical scheme: for achieving the above object, the technical solution used in the present invention is a kind of low operand GPS localization method based on second order spreading kalman, comprises the steps:

(1) obtain local k-1 positioning result constantly, calculate current k prior estimation state amount and prior estimate error covariance constantly:

${\hat{X}}_k^{-}=A\&CenterDot;{\hat{X}}_{k-1},{\hat{P}}_k^{-}={A\hat{P}}_{k-1}{A}^T+Q_k$

Wherein,

for the prior estimation state amount of k receiver user constantly, for the posteriority estimated state amount of k-1 receiver user constantly,

for the prior estimate error covariance of k receiver user constantly,

for the posteriority evaluated error covariance of k-1 receiver user constantly,

A is transition matrix: $A=\left[\begin{array}{cccccccc}1& 0& 0& \mathrm{\&Delta;t}& 0& 0& 0& 0\\ 0& 1& 0& 0& \mathrm{\&Delta;t}& 0& 0& 0\\ 0& 0& 1& 0& 0& \mathrm{\&Delta;t}& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& \mathrm{\&Delta;t}\\ 0& 0& 0& 0& 0& 0& 1& 0\end{array}\right]$

Wherein, Δ t is the time interval between twice location; Q _kfor k process noise error constantly,

$Q_k=\left[\begin{array}{cccc}{10}^{-4}{I}_{3\&times;3}& 0_{3\&times;3}& 0& 0\\ 0_{3\&times;3}& {10}^3{I}_{3\&times;3}& 0& 0\\ 0& 0& {10}^5& 0\\ 0& 0& 0& 100\end{array}\right]$

Wherein, I _{3 * 3}be 3 rank unit matrixs, 0 _{3 * 3}be complete zero square formations in 3 rank;

(2) setting up observed quantity estimates

equation:

$Z_k^{-}=h_k\left({\hat{X}}_k^{-}\right)+m_{1k}$

Wherein,

$h={[{\mathrm{\&rho;}}_1,...,{\mathrm{\&rho;}}_n,{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_1,...,{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_n]}^T$ ρ _i＝||r _i ^s-ru||+δ _tu+ε _ρi

${\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_i=\left|\right|{({r_i}^s-\mathrm{ru})}^T\&CenterDot;({V_i}^s-\mathrm{Vu})\left|\right|/\left|\right|{r_i}^s-\mathrm{ru}\left|\right|+{\mathrm{\&delta;}}_{\mathrm{fu}}+{\mathrm{\&epsiv;}}_{{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_i}$

$m_{1k}=\frac{1}{2}\underset{p}{\overset{2n_k}{\mathrm{\&Sigma;}}}{e}_p\&CenterDot;{(X_k-{\hat{X}}_k^{-})}^T\&CenterDot;H_{2k}^p\&CenterDot;(X_k-{\hat{X}}_k^{-})$

represent k measured value h constantly _kin p function exist

the second derivative at place, x _j, x _lbe respectively quantity of state X _kin any two elements, p ∈ [1,2n _k],

for the position coordinates vector of satellite,

for the speed coordinate vector of satellite, ε _{ρ i}for the pseudo range measurement error of satellite,

be the measuring error of pseudorange rate of change, n is number of satellites, i ∈ [1, n], e _pfor 2n _kp column vector in dimension unit matrix, n _kfor k moment number of satellites, δ _tu, δ _fube respectively clock correction and clock correction rate of change;

(3) calculate kalman gain K _k:

$K_k=({\hat{P}}_k^{-}\&CenterDot;H_{1k}^T+\frac{1}{2}\&CenterDot;\det X_k\&CenterDot;m_{1k}^T){\left(\begin{array}{c}{H}_{1k}\&CenterDot;{\hat{P}}_k^{-}\&CenterDot;H_{1k}^T+R_k+\frac{1}{2}\&CenterDot;H_{1k}\&CenterDot;\det X_k\&CenterDot;m_{1k}^T+\frac{1}{2}\&CenterDot;m_{1k}\&CenterDot;\det X_k^T\&CenterDot;H_{1k}\\ +\underset{p,\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002455038700012.png"\; state="\{\{state\}\}">q</figure-callout>}}{\mathrm{\&Sigma;}}\frac{1}{4}(\det {T_k}^T\&CenterDot;H_{2k}^p\&CenterDot;{\hat{P}}_k^{-}\&CenterDot;H_{2k}^q\&CenterDot;\det X_k)e_p\&CenterDot;e_q^T\end{array}\right)}^{-1}$

Wherein, x _kfor the time of day amount of k receiver user constantly,

$H_{1k}={\frac{{\&PartialD;h}_k\left(X_k\right)}{{\&PartialD;X}_k}|}_{X_k={\hat{X}}_k^{-}},$ Represent k measured value h constantly _k? $X_k={\hat{X}}_k^{-}$ The first order derivative at place,

represent k measured value h constantly _kin q function exist

the second derivative at place, R _kmeasuring error covariance for pseudorange and Doppler:

v wherein _krepresent to measure noise matrix, E() represent to ask covariance;

(4) calculate current k positioning result and error covariance constantly:

${\hat{P}}_k=\mathrm{diag}\left((I-K_k{H}_{1k}){\hat{P}}_k^{-}{(I-K_k{H}_{1k})}^T\right),{\hat{X}}_k={\hat{X}}_k^{-}+K_k(Z_k-Z_k^{-})$

Wherein,

for the posteriority evaluated error covariance of k receiver user constantly, diag () is the diagonal matrix of asking matrix,

for the posteriority estimated state amount of k receiver user constantly, I is unit matrix, Z _kactual value for observed quantity;

(5) repeating said steps (1), to step (4), obtains a series of anchor points.

It should be noted that, it will be appreciated by those skilled in the art that the step (2) in the present invention: " wherein, h=... ", the h here does not add subscript k, represents generalized case, adds that subscript k is illustrated in k value constantly, other correlation parameters are in like manner.

Beneficial effect: the present invention is the method that adopts the second-order EKF algorithm of low operand to position in GPS, feature that can the dynamic change along with the change of measured value according to kalman gain in second-order EKF algorithm, can realize real-time adjustment filtering parameter, reach filtering location optimization, improved the Static and dynamic effect of location.Meanwhile, after having reduced operand, the calculated amount of calculated amount of the present invention and single order expanded Kalman filtration algorithm is similar, can in some low side devices, move, for example, can move take in the process chip that the ARM7TDMI of 80MHz dominant frequency is core.The present invention, when reaching the locating effect identical with General Second Order expanded Kalman filtration algorithm, has reduced cost.

Accompanying drawing explanation

Fig. 1 is gps system model schematic diagram;

Fig. 2 is prior art GPS receiver machine system structured flowchart;

Fig. 3 is second order spreading kalman calculation method process flow diagram of the present invention.

Embodiment

Below in conjunction with the drawings and specific embodiments, further illustrate the present invention, should understand these embodiment is only not used in and limits the scope of the invention for the present invention is described, after having read the present invention, those skilled in the art all fall within the application's claims limited range to the modification of the various equivalent form of values of the present invention.

The present invention is a kind of second order spreading kalman localization method of low operand, comprises the part such as simplification, Static positioning accuracy, Kinematic Positioning effect of various computings in the demonstration, algorithm of the obtaining of data, algorithm.Its final result is, data are obtained the data of satellite and pseudorange, the Doppler that local receiver provides from GPS base band, and the calculating by this algorithm can obtain positioning result.In the demonstration of algorithm and the simplification of algorithm computing, strictly proved theoretically feasibility and the preciseness of algorithm, meanwhile, utilize the character of data and the actual requirement of engineering in algorithm, computing has been carried out simplifying and processed, it can be moved on low side hardware.The anchor point of settling accounts out through above step, when static state, positioning precision can reach 0.5m, and the test that continues 6 hours in static multipath situation, can reach precision 10m; In the time of dynamically, in weak signal situation, can accurately locate, maximum deviation is no more than 4m, and in strong multipath situation, deviation is no more than 10m, and can guarantee the continuity of route.The present invention obtains pseudorange, doppler information, the pseudorange error covariance of satellite position, speed and local receiver.Meanwhile, from algorithm, read positioning result and the positioning error covariance in a upper moment, calculate the required intermediate parameters of algorithm and kalman gain.According to the parameter obtaining and gain, calculate current time positioning result and error covariance.Repeat said process, after 1s, then resolve next time.

The quantity of state adopting in the present invention is;

X＝[x,y,z,Vx,Vy,Vz,δ _tu,δ _fu] ^T

Wherein, ru=[x, y, z] ^tuser's coordinate vector, Vu=[Vx, Vy, Vz] ^tuser's velocity vector, δ _tu, δ _fube respectively clock correction and clock correction rate of change.

The observed quantity adopting is:

Z＝h(X)

Wherein,

$h={[{\mathrm{\&rho;}}_1,...,{\mathrm{\&rho;}}_n,{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_1,...,{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_n]}^T$ ρ _i＝||r _i ^s-ru||+δ _tu+ε _ρi

${\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_i=\left|\right|{({r_i}^s-\mathrm{ru})}^T\&CenterDot;({V_i}^s-\mathrm{Vu})\left|\right|/\left|\right|{r_i}^s-\mathrm{ru}\left|\right|+{\mathrm{\&delta;}}_{\mathrm{fu}}+{\mathrm{\&epsiv;}}_{{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_i}$

with

the position of this satellite and speed coordinate vector, ε _{ρ i}for its pseudo range measurement error, it is the measuring error of pseudorange rate of change.N is number of satellites, i ∈ [1, n].

Concrete steps of the present invention are as follows:

1, obtain the positioning result that carve for the moment this ground, calculate priori estimates and the prior estimate covariance of current time

${\hat{X}}_k^{-}=A\&CenterDot;{\hat{X}}_{k-1},{\hat{P}}_k^{-}={A\hat{P}}_{k-1}{A}^T+Q_k$

Wherein, X _k,

with

be respectively time of day amount, posteriority estimated state amount and the prior estimation state amount of k receiver user constantly,

for posteriority evaluated error covariance,

for prior estimate covariance;

A is transition matrix,

$A=\left[\begin{array}{cccccccc}1& 0& 0& \mathrm{\&Delta;t}& 0& 0& 0& 0\\ 0& 1& 0& 0& \mathrm{\&Delta;t}& 0& 0& 0\\ 0& 0& 1& 0& 0& \mathrm{\&Delta;t}& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& \mathrm{\&Delta;t}\\ 0& 0& 0& 0& 0& 0& 1& 0\end{array}\right]$

Δ t is the time interval between twice location, and this place is 1s;

Q _kfor k process noise error constantly,

$Q_k=\left[\begin{array}{cccc}{10}^{-4}{I}_{3\&times;3}& 0_{3\&times;3}& 0& 0\\ 0_{3\&times;3}& {10}^3{I}_{3\&times;3}& 0& 0\\ 0& 0& {10}^5& 0\\ 0& 0& 0& 100\end{array}\right]$

I _{3 * 3}be 3 rank unit matrixs, 0 _{3 * 3}be complete zero square formations in 3 rank.

2, setting up observed quantity estimates

equation

$Z_k^{-}=h_k\left({\hat{X}}_k^{-}\right)+m_{1k}$

Wherein,

$m_{1k}=\frac{1}{2}\underset{p}{\overset{2n_k}{\mathrm{\&Sigma;}}}{e}_p\&CenterDot;{(X_k-{\hat{X}}_k^{-})}^T\&CenterDot;H_{2k}^p\&CenterDot;(X_k-{\hat{X}}_k^{-})$

E _pfor 2n _kp column vector in dimension unit matrix, n _kfor k moment number of satellites.

3, calculate kalman gain,

$K_k=({\hat{P}}_k^{-}\&CenterDot;H_{1k}^T+\frac{1}{2}\&CenterDot;\det X_k\&CenterDot;m_1^T){\left(\begin{array}{c}{H}_{1k}{\&CenterDot;\hat{P}}_k^{-}\&CenterDot;H_{1k}^T+R_k+\frac{1}{2}\&CenterDot;H_{1k}\&CenterDot;\det X_k\&CenterDot;m_1^T+\frac{1}{2}\&CenterDot;{\mathrm{<figure-callout\; id="1"\; label="m"\; filenames="HDA00002455038700012.png"\; state="\{\{state\}\}">m</figure-callout>}}_1\&CenterDot;\det X_k^T\&CenterDot;H_{1k}\\ +\underset{p,\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002455038700012.png"\; state="\{\{state\}\}">q</figure-callout>}}{\mathrm{\&Sigma;}}\frac{1}{4}(\det {T_k}^T\&CenterDot;H_{2k}^p\&CenterDot;{\hat{P}}_k^{-}\&CenterDot;H_{2k}^q\&CenterDot;\det X_k)e_p\&CenterDot;e_q^T\end{array}\right)}^{-1}$

Wherein,

$\det X_k=X_k-{\hat{X}}_k^{-}$

$H_{1k}={\frac{{\&PartialD;h}_k\left(X_k\right)}{{\&PartialD;X}_k}|}_{X_k={\hat{X}}_k^{-}}$

Represent k measured value h constantly _k?

the first order derivative at place,

$H_{2k}^p={\left(\frac{{\&PartialD;}^2{\left(h_k\left(X_k\right)\right)}_p}{\&PartialD;x_j\&PartialD;x_l}\right)|}_{X_k={\hat{X}}_k^{-}}$

Represent k measured value h constantly _kin p function exist

the second derivative at place, x _j, x _lbe respectively quantity of state X _kin any two elements,

with

meaning identical, p, q ∈ [1,2n _k];

R _kfor pseudorange and Doppler's measuring error covariance,

$R_k=E\left(v_k{v}_k^T\right)$

V _krepresent to measure noise matrix, E () represents to ask covariance.

4, calculate positioning result and the error covariance of current time,

${\hat{P}}_k=\mathrm{diag}\left((I-K_k{H}_{1k}){\hat{P}}_k^{-}{(I-K_k{H}_{1k})}^T\right),{\hat{X}}_k={\hat{X}}_k^{-}+K_k(Z_k-Z_k^{-})$

Wherein, diag () is for asking the diagonal matrix of matrix, Z _kactual value for the observed reading of system.

5, repeat said process, will obtain a series of anchor points.

In actual motion, due to actual value X _kcannot obtain, therefore, we adopt

replace

So have $\det X_k\&ap;{\hat{X}}_k^{-}-{\hat{X}}_{k-1}=A{\hat{X}}_{k-1}-{\hat{X}}_{k-1}=(A-I){\hat{X}}_{k-1}={\left[\begin{array}{c}\mathrm{Vu}\\ 0\\ 0\\ 0\\ {\mathrm{\&delta;}}_{\mathrm{fu}}\\ 0\end{array}\right]}_{8\&times;1}$

As seen from the above equation, detX _kand m _1konly relevant with clock correction rate of change with the speed of laststate receiver.Meanwhile, detX _kand m _1kit is the correction to EKF2 algorithm.The speed of receiver and clock correction rate of change calculate by Doppler shift, and in degree of accuracy, speed is more accurate than position, and therefore, the present invention adopts Doppler shift to revise with level and smooth position on the basis of EKF2 location algorithm.

In calculated amount, we adopt following method to reduce calculated amount:

(1) in EKF2 algorithm,

q _k, R _ksymmetric positive definite diagonal matrix, be symmetric positive definite matrix, meanwhile, having many is quadratic form, sparse matrix, minimizing calculated amount that can be a large amount of when calculating;

(2) when calculating K alman Gain, need to use matrix inversion, can prove that minute mother matrix on right side in gain accounting equation is symmetric positive definite matrix, therefore utilize this character that the calculated amount of matrix inversion is reduced;

In the computing that relates to matrix A, because A is sparse matrix, so do not adopt matrix multiplication to calculate, directly in program, write out the result of each, reduce calculated amount.Meanwhile, Q _k, R _kbefore circulation, by obtaining above, do not calculate in cycling time.

Referring to Fig. 1, be depicted as gps system illustraton of model, wherein comprise: StoreFront GPS receiver 101, receiver to the pseudorange 102 of satellite, the gps satellite 103 of x time, receiver satellite 104 constantly.Main process is that gps signal, through antenna transmission, through atmospheric envelope, exists scattering, the impacts such as refraction, arrival receiver antenna.The distance that receiver is measured, not only contains the line-of-sight distance of satellite and receiver, also adds and has contained the distance that various delays cause, and becomes " pseudorange ".

Pseudorange is the basis of satnav, according to the range formula between 2, space

therefore, accurately locate need at least 4 satellites pseudorange and co-ordinates of satellite.Like this, just can adopt spreading kalman equation to calculate the coordinate of receiver.

Fig. 2 has provided GPS receiver machine system structured flowchart.Comprise radio-frequency front-end processing module, baseband signal processing module and positioning navigation module.Radio-frequency front-end processing module is by all visible gps satellite signals of antenna reception, after prefilter and prime amplifier, carry out mixing and be downconverted into intermediate-freuqncy signal with the sinusoidal wave local oscillation signal of local oscillator generation again, finally by analog to digital conversion, intermediate-freuqncy signal is transformed into the digital medium-frequency signal of discrete time.Intermediate-freuqncy signal is through catching, follow the tracks of scheduling algorithm in baseband digital signal processing module, copy the local carrier consistent with the satellite-signal receiving and local pseudo-code signal, therefrom obtain the measured values such as GPS pseudorange and carrier phase and demodulate navigation message.In baseband digital signal processing module, handle after digital medium-frequency signal, each passage is exported respectively demodulation navigation message out on the measured values such as pseudorange, Doppler shift and carrier phase of its satellite-signal of following the tracks of and signal, and information such as ephemeris parameter in these satellite measurement and navigation message are again through the processing of follow-up location navigation calculation function module, receiver finally obtains GPS positioning result, or exports various navigation informations again.

Fig. 3 has provided the software flow pattern of second order spreading kalman algorithm.First obtain the positioning result that carve for the moment this ground, calculate priori estimates and the prior estimate covariance of current time

${\hat{X}}_k^{-}=A\&CenterDot;{\hat{X}}_{k-1},{\hat{P}}_k^{-}={A\hat{P}}_{k-1}{A}^T+Q_k$

Then set up observed quantity equation

$Z_k^{-}=h_k\left({\hat{X}}_k^{-}\right)+m_{1k}$

Next calculates kalman gain K _k

Finally calculate positioning result and the error covariance of current time,

${\hat{P}}_k=\mathrm{diag}\left((I-K_k{H}_{1k}){\hat{P}}_k^{-}{(I-K_k{H}_{1k})}^T\right),{\hat{X}}_k={\hat{X}}_k^{-}+K_k(Z_k-Z_k^{-})$

Repeat again afterwards above operation and just realized continuous location.

Claims (1)

1. the low operand GPS localization method based on second order spreading kalman, comprises the steps:

(1) obtain local k-1 positioning result constantly, calculate current k prior estimation state amount and prior estimate error covariance constantly:

${\hat{X}}_k^{\_}=A\&CenterDot;{\hat{X}}_{k-1}$ ${\hat{P}}_k^{\_}=A{\hat{P}}_{k-1}{A}^T+Q_k$

Wherein,

for the prior estimation state amount of k receiver user constantly,

for the posteriority estimated state amount of k-1 receiver user constantly,

for the prior estimate error covariance of k receiver user constantly,

for the posteriority evaluated error covariance of k-1 receiver user constantly,

A is transition matrix: $A=\left[\begin{array}{cccccccc}1& 0& 0& \mathrm{\&Delta;t}& 0& 0& 0& 0\\ 0& 1& 0& 0& \mathrm{\&Delta;t}& 0& 0& 0\\ 0& 0& 1& 0& 0& \mathrm{\&Delta;t}& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& \mathrm{\&Delta;t}\\ 0& 0& 0& 0& 0& 0& 1& 0\end{array}\right]$

Wherein, Δ t is the time interval between twice location; Q _kfor k process noise error constantly,

$Q_k=\left[\begin{array}{cccc}{10}^{-4}{I}_{3\&times;3}& 0_{3\&times;3}& 0& 0\\ 0_{3\&times;3}& {10}^3{I}_{3\&times;3}& 0& 0\\ 0& 0& {10}^5& 0\\ 0& 0& 0& 100\end{array}\right]$

Wherein, I _{3 * 3}be 3 rank unit matrixs, 0 _{3 * 3}be complete zero square formations in 3 rank;

(2) setting up observed quantity estimates

equation

$Z_k^{-}=h_k\left({\hat{X}}_k^{\_}\right)+m_{1k}$

Wherein,

$h={[{\mathrm{\&rho;}}_1,\&CenterDot;\&CenterDot;\&CenterDot;,{\mathrm{\&rho;}}_n,{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_1,\&CenterDot;\&CenterDot;\&CenterDot;,{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_n]}^T$ ${\mathrm{\&rho;}}_i=\left|\right|r_i^s-\mathrm{ru}\left|\right|+{\mathrm{\&delta;}}_{\mathrm{tu}}+{\mathrm{\&epsiv;}}_{\mathrm{\&rho;i}}$

${\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_i=\left|\right|{(r_i^s-\mathrm{ru})}^T\&CenterDot;(V_i^s-\mathrm{Vu})\left|\right|/\left|\right|r_i^s-\mathrm{ru}\left|\right|+{\mathrm{\&delta;}}_{\mathrm{fu}}+{\mathrm{\&epsiv;}}_{{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_i}$

$m_{1k}=\frac{1}{2}\underset{p}{\overset{{2n}_k}{\mathrm{\&Sigma;}}}{e}_p\&CenterDot;{(X_k-{\hat{X}}_k^{\_})}^T\&CenterDot;H_{2k}^p\&CenterDot;(X_k-{\hat{X}}_k^{\_})$

represent k measured value h constantly _kin p function exist

the second derivative at place, x _j, x _lbe respectively quantity of state X _kin any two elements, p ∈ [1,2n _k],

for the position coordinates vector of satellite, for the speed coordinate vector of satellite, for the pseudo range measurement error of satellite,

be the measuring error of pseudorange rate of change, n is number of satellites, i ∈ [1, n], e _pfor 2n _kp column vector in dimension unit matrix, n _kfor k moment number of satellites, δ _tu, δ _fube respectively clock correction and clock correction rate of change;

(3) calculate kalman gain K _k:

$K_k=({\hat{P}}_k^{\_}\&CenterDot;H_{1k}^T+\frac{1}{2}\&CenterDot;\det X_k\&CenterDot;m_{1k}^T){\left(\begin{array}{c}{H}_{1k}\&CenterDot;{\hat{P}}_k^{\_}\&CenterDot;H_{1k}^T+R_k+\frac{1}{2}\&CenterDot;H_{1k}\&CenterDot;\det X_k\&CenterDot;m_{1k}^T+\frac{1}{2}\&CenterDot;m_{1k}\&CenterDot;\det X_k^T\&CenterDot;H_{1k}\\ +\underset{p,q}{\mathrm{\&Sigma;}}\frac{1}{4}(\det X_k^T\&CenterDot;H_{2k}^p\&CenterDot;{\hat{P}}_k^{\_}\&CenterDot;H_{2k}^q\&CenterDot;\det X_k)e_p\&CenterDot;e_q^T\end{array}\right)}^{-1}$

Wherein,

x _kfor the time of day amount of k receiver user constantly,

represent k measured value h constantly _k?

the first order derivative at place,

represent k measured value h constantly _kin q function exist

the second derivative at place, R _kmeasuring error covariance for pseudorange and Doppler:

v wherein _krepresent to measure noise matrix, E () represents to ask covariance;

(4) calculate current k positioning result and error covariance constantly:

${\hat{P}}_k=\mathrm{diag}\left((I-K_k{H}_{1k}){\hat{P}}_k^{\_}{(I-K_k{H}_{1k})}^T\right)$ ${\hat{X}}_k={\hat{X}}_k^{\_}+K_k(Z_k-Z_k^{-})$

Wherein, for the posteriority evaluated error covariance of k receiver user constantly, diag () is the diagonal matrix of asking matrix,

for the posteriority estimated state amount of k receiver user constantly, I is unit matrix, Z _kactual value for observed quantity;

(5) repeating said steps (1), to step (4), obtains a series of anchor points.

Images