CN104020480A - Satellite navigation method for interactive multi-model UKF with self-adapting factors - Google Patents

Abstract

The invention relates to a satellite navigation method for an interactive multi-model UKF with self-adapting factors, and belongs to the field of navigation control. The method comprises the steps that firstly, an auto-covariance matrix of a residual sequence is used as a correcting value, and a smoothing filter is designed for fusing the correcting value with the old self-adapting factor to obtain the new self-adapting factor; secondly, a covariance matrix of process noise is adjusted in real time by utilizing the self-adapting factor, and therefore positioning errors, caused by unknown noise statistics properties, in a system are effectively reduced; afterwards, a model set M is set by the adoption of the interactive multi-model (IMM) algorithm; finally, soft handover between models is achieved by adjusting model probability in real time according to measurement residual errors, so that positioning errors caused by inaccurate system models are reduced. According to the method, the positioning accuracy of a complex motor carrier in a satellite navigation system is effectively improved with models and noise statistical properties unknown.

Description

The satellite navigation method of a kind of interactive multi-model UKF with adaptive factor

Technical field

The satellite navigation method that the present invention relates to a kind of interactive multi-model UKF with adaptive factor, belongs to Navigation Control field.

Background technology

Receiver location algorithm is the key that affects Positioning Accuracy of Satellite Navigation System.Least square method is simple with its principle for a long time, is easy to the advantages such as realization and is widely used.But least square method does not connect estimated value in the same time not, the positioning result dispersion, disorderly and unsystematic that seems, difficulty has been manufactured in this optimization to navigation algorithm.For overcoming least square method in the deficiency aspect estimated performance, nonlinear filtering algorithm is applied to navigation field by scholars.For the filtering problem of nonlinear system, because needs are processed infinite dimensional integral operation, be difficult in theory find strict optimum solution, be generally all to solve by approximate method.Current non-linear filtering method is mainly divided three classes, and the first kind is the method for approximation to function, and the method that mainly adopts Taylor series expansion or interpolation polynomial to launch is similar to nonlinear function, and it is represented as EKF and filtering interpolation; Equations of The Second Kind is that deterministic sampling is approximate, and it is by selecting one group of sigma point as sample, and propagates the rear average of testing of acquisition system and variance by nonlinear function.Its Typical Representative is Unscented kalman filtering; The 3rd class is that stochastic sampling is approximate, mainly refers to the emulation based on monte carlo method, and its representative is particle filter.Particle filter is optimum in theory, and is applicable to the situation of non-Gaussian noise.But there is the problems such as particle is degenerated, calculated amount is large, real-time is poor in particle filter.

EKF (Extend Kalman Filter, EKF) is the Typical Representative of approximation to function method, is also one of filtering algorithm being most widely used, and is widely used in a lot of fields.But EKF adopts the method meeting production model error of single order taylor series expansion approximately linear, causes filtering accuracy to decline.For overcoming EKF in the deficiency aspect filtering accuracy, scholars have proposed Unscented kalman filtering (Unsigned Kalman Filter, UKF), become one of approximate typical method of Equations of The Second Kind method-deterministic sampling.UKF selects one group of sigma point as sample, and propagate the rear average of testing of acquisition system and variance by nonlinear function, avoid linearization of nonlinear system process, its estimated accuracy can reach the precision of second order Taylor series expansion, and the EKF that calculated amount is suitable with it can only reach the precision that single order launches.

But EKF and UKF exist common deficiency to need system model and noise statistics accurately, for a lot of systems, these conditions are all difficult to meet, and based on this, scholars have proposed adaptive filter algorithm.Adaptive filter algorithm mainly contains noise variance matching method and model matching method at present.(1) the general method that adopts real-time adjustment process noise covariance matrix of noise variance matching method realizes, wherein taking Sage-Husa adaptive algorithm as representative.Sage-Huse adaptive filter algorithm has simply obtained certain utilization with its principle, but it is more responsive to filtering initial value, and very easily causes filtering divergence, therefore in the time of application, will carry out the inhibition of filtering divergence, causes algorithm loaded down with trivial details, and calculated amount is large; A.H.Mohamed proposes the adaptive algorithm based on maximum likelihood estimation criterion, and this algorithm is according to estimating derived the more new formula of Q battle array and R battle array of residual error.Halil Ersin Soken is the value that matrix Q increase adaptive factor is adjusted matrix Q in real time, and in the time that estimation is accurate, Q battle array remains unchanged, and in the time estimating deviation occurs, is redefined the value of noise matrix by measurement residual error.These two kinds of method real-times are good, but all need to calculate Jacobian matrix, increased the calculated amount of UKF.And these two kinds of methods all rely on measurement residual error to determine noise covariance matrix completely, a little less than estimating stability; For strengthening the stability of above algorithm, scholars have proposed master-slave mode AUKF, and it,, upgrade noise covariance matrix according to measuring residual error from UKF, and utilizes this variance matrix to remove to proofread and correct main UKF, this algorithm stability better but calculated amount is large; Fuzzy adaptive filtering algorithm is formulated fuzzy rule to measure residual error as input, adjusts in real time the value of Q battle array, and algorithm is simple but flexibility ratio is poor; Wang Qiuting increases the accuracy of model by increasing adaptive factor to process noise matrix in its PhD dissertation, and principle is simple, and calculated amount is little, but accurate not to the statistics of measurement residual error.(2) model matching method mainly adopts IMM algorithm, and this algorithm arranges Models Sets M, and according to estimating the real-time adjustment model probability of residual error, the soft handover between implementation model.The method has solved the low problem of filtering accuracy that single model does not cause carrier movement state description comprehensively.IMM algorithm starts to be applied to target following most, has also obtained increasing application in navigation field now.Experiment showed, in the time that system model is uncertain, the method can obviously improve the positioning precision of navigational system, but this algorithm cannot be proofreaied and correct the error that noise statistics the unknown causes.

Summary of the invention

The present invention is directed to complicated motor-driven carrier movement model and noise statistics the unknown in satellite navigation system and cause problem that filtering accuracy is low to propose the satellite navigation method of a kind of interactive multi-model UKF with adaptive factor, effectively improved the positioning precision of complicated motor-driven carrier under the condition of model and noise statistics the unknown in satellite navigation system.

The object of the invention is to be achieved through the following technical solutions.

A satellite navigation method of interactive multi-model UKF with adaptive factor, specifically comprises the steps:

Step 1: set up system model

Step 1.1: the state equation of setting up the ground carrier based on GPS (GPS).

For describe motion state and the clock correction drift frequency of carrier comprehensively, by the state variable of sign X GPS (GPS) state equation, $X={[x,\stackrel{\·}{x},\stackrel{\·\·}{x,}y,\stackrel{\·}{y},\stackrel{\·\·}{y},z,\stackrel{\·}{z},\stackrel{\·\·}{z,}{\mathrm{\δt}}_u,\mathrm{\δ}{\stackrel{\·}{t}}_u]}^T.$ Wherein, x, represent respectively the lower X of world geodetic system system (World Geodetic System mono-1984Coordinate System, WGS-84 coordinate system) _tposition, speed and acceleration on axle; Y, represent respectively Y under WGS-84 coordinate system _tposition, speed and acceleration on axle; Z, represent respectively Z under WGS-84 coordinate system _tposition, speed and acceleration on axle; δ t _uwith represent respectively carrier receiver clock correction and clock correction drift frequency.Set up Models Sets (representing by symbol M), the current statistical model (CS represents with symbol) that Models Sets M comprises description carrier movement state, normal acceleration model (CA represents with symbol) and constant speed Turn Models (CT represents with symbol), be M=[CS, CA, CT].The changing value of receiver clock error is described with single order Markov model, and the state equation of current statistical model CS is as shown in formula (1); The state equation of normal acceleration MODEL C A is as shown in formula (2); The state equation of constant speed Turn Models CT is as shown in formula (3).

$X_k^1={\mathrm{\Φ}}_1{X}_{k-1}^1+{\mathrm{\Ψ}}_{k-1}^1{w}_{k-1}^1---\left(1\right)$

$X_k^2={\mathrm{\Φ}}_2{X}_{k-1}^2+{\mathrm{\Ψ}}_{k-1}^2{w}_{k-1}^2---\left(2\right)$

$X_k^3={\mathrm{\Φ}}_3{X}_{k-1}^3+{\mathrm{\Ψ}}_{k-1}^3{w}_{k-1}^3---\left(3\right)$

Wherein, the state in k moment that represents respectively system under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT, wherein i=1,2,3, that is: the state in expression system k moment under current statistical model CS; the state in expression system k moment under normal acceleration MODEL C A; the state in expression system k moment under constant speed Turn Models CT; K is positive integer; Φ _ifor the state-transition matrix under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT, Φ _i=diag (Φ _{i, x}, Φ _{i, y}, Φ _{i, z}, Φ t, u), Φ _{i, x}represent X under WGS-84 coordinate system _tstate-transition matrix on axle; represent Y under WGS-84 coordinate system _tstate-transition matrix on axle; Φ _i,zrepresent Z under WGS-84 coordinate system _tstate-transition matrix on axle; Φ _t,urepresent to describe the state-transition matrix of ground carrier receiver clock correction and clock correction drift; the process noise covariance matrix adaptive factor of system under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT; the process noise matrix of system under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT.

Use symbol the process noise covariance matrix in (k-1) moment that represents respectively system under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT, e[] computing of expression expectation value, represent to describe X under WGS-84 coordinate system _tthe process noise covariance matrix of carrier motion state on axle; represent to describe Y under WGS-84 coordinate system _tthe process noise covariance matrix of carrier motion state on axle; represent describing Z under WGS-84 coordinate system _tthe process noise covariance matrix of carrier motion state on axle; represent to describe the process noise covariance matrix of ground carrier receiver clock correction and clock correction drift;

Step 1.2: the observation equation of setting up the ground carrier based on GPS (GPS)

Adopt pseudorange to set up observation equation as observed quantity.In the k moment, the gps satellite quantity that receiver observes represents with N, sets up this moment CS model, and the pseudorange observation equation of CA model and CT model is as shown in formula (4).

${\mathrm{\ρ}}_n^k=\sqrt{{(x_k^i-x_n^k)}^2+{(y_k^i-y_n^k)}^2+{(z_k^i-z_n^k)}^2}+{\mathrm{\δt}}_u^k+{\mathrm{\ϵ}}_n^k---\left(4\right)$

Wherein, be illustrated in the pseudorange of n the gps satellite that the k moment observes, n=1,2 ... N, N represents the sum of the gps satellite that the k moment observes; represent n gps satellite X under WGS-84 coordinate system that the k moment observes _tposition on axle; represent n gps satellite Y under WGS-84 coordinate system that the k moment observes _tposition on axle; represent n gps satellite Z under WGS-84 coordinate system that the k moment observes _tposition on axle; be illustrated respectively in carrier X under WGS-84 coordinate system in k moment ground under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT _tposition on axle; be illustrated respectively in carrier Y under WGS-84 coordinate system in k moment ground under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT _tposition on axle; be illustrated respectively in carrier Z under WGS-84 coordinate system in k moment ground under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT _tposition on axle; represent k moment ground carrier receiver clock correction; the observational error of n the gps satellite that the expression k moment observes.

Convenient for below describing algorithm, set up CS model, the observation equation of CA model and CT model is formula (5).

$y_k^i=h\left(X_k^i\right)+{\mathrm{\ϵ}}_k---\left(5\right)$

Wherein, , be the observed quantity of system, $h\left(X_k^i\right)=\left[\begin{array}{c}\sqrt{{(x_k^i-x_1^k)}^2+{(y_k^i-y_1^k)}^2+{(z_k^i-z_1^k)}^2}+{\mathrm{\δt}}_u^k\\ \sqrt{{(x_k^i-x_2^k)}^2+{(y_k^i-y_2^k)}^2+{(z_k^i-z_2^k)}^2}+{\mathrm{\δt}}_u^k\\ \·\·\·\\ \sqrt{{(x_k^i-x_n^k)}^2+{(y_k^i-y_n^k)}^2+{(z_k^i-z_n^k)}^2}+{\mathrm{\δt}}_u^k\end{array}\right],$ Nonlinear function in observation equation, for the observational error of system.

Use R _kthe covariance matrix of the observational error of representation formula (5),

Step 2: CS model is set, and CA model and the filtering initial value in 0 moment of CT model arrange end time of current location tasks simultaneously.

On the basis operating in step 1, the estimation initial value of the state of CS model, CA model and CT model is set respectively, uses symbol with represent; Then the evaluated error covariance matrix initial value of CS model, CA model and CT model is set, uses symbol with represent; And end time of system operation is set.

Step 3: obtain CS model, CA model and CT model are at the filtering initial value in k moment.

The estimation initial value of CS model, CA model and the state in CT model (k-1) moment that use step 2 or step 4 obtain with , and the evaluated error covariance matrix initial value of CS model, CA model and CT model with , mix alternately by formula (6) and formula (7), obtain respectively CS model, in CA model and CT model, the estimation initial value of the state in k moment, uses respectively with represent, and CS model, in CA model and CT model, the evaluated error covariance matrix initial value in k moment, uses respectively with represent.

${\hat{X}}_{k-1}^{0j}=\underset{i=1}{\overset{3}{\mathrm{\Σ}}}{\hat{X}}_{i=1}^i{u}_{k-1|k-1}^{i|j}---\left(6\right)$

Wherein, represent respectively CS model, CA model and CT model are at the state initial value of k moment filtering; J=1,2,3, represent respectively CS model, CA model and CT model; i=1,2,3, represent respectively CS model, CA model and CT model are in the optimal estimation value of k-1 moment state; represent the mixing probability of k-1 moment model i and model j, π _ijfor model i is to the model transition probability of model j, its value is by artificially setting; for model i is at the probability in k moment, initial value by artificially setting, and each moment is upgraded by step 6 afterwards; the normaliztion constant of representative model j,

$P_{k-1}^{0j}=\underset{i=1}{\overset{3}{\mathrm{\Σ}}}{u}_{k-1|k-1}^{i|j}[P_{k-1}^i+({\hat{X}}_{k-1}^i-{\hat{X}}_{k-1}^{0j}){({\hat{X}}_{k-1}^i-{\hat{X}}_{k-1}^{0j})}^T]---\left(7\right)$

Wherein, i=1,2,3, represent respectively CS model, CA model and the CT model variance matrix at k-1 moment filtering error. represent that respectively CS model, CA model and CT model are at k moment filtering error variance matrix initial value.

Step 4: to CS model, CA model and CT model carry out self-adaptation Unscented kalman filtering (Adaptive Unsigned Kalman Filter, AUKF), obtain the filter value of each model in the k moment.Concrete operation step is:

Step 4.1: use represent the dimension of system state, the CS model obtaining in step 1, the dimension of state equation in CA model and CT model, by formula (8) computation model j in the k moment individual initial sigma (sigma) point, wherein r sigma point is with using symbol represent, and r is integer;

Wherein, represent the r row (or r is capable) of On Square-Rooting Matrices.

By formula (9) calculating first kind weight coefficient, the first-order statistics characteristic of this weight coefficient for asking each sigma to order.The first kind weight coefficient symbol that r the sigma of model j ordered represent, that is: represent r sigma point first kind weight coefficient of CS model, represent r sigma point first kind weight coefficient of CA model, represent r sigma point first kind weight coefficient of CT model; With formula (10) calculating Equations of The Second Kind weight coefficient, the second-order statistics of this weight coefficient for asking each sigma to order.The Equations of The Second Kind weight coefficient symbol that r the sigma of model j ordered represent, that is: represent r sigma point Equations of The Second Kind weight coefficient of CS model, represent r sigma point Equations of The Second Kind weight coefficient of CA model, represent r sigma point Equations of The Second Kind weight coefficient of CT model;

Wherein, λ is a composite scale parameter, and its value solves by formula (11), and parameter alpha determines sigma spread of points degree, and its value is by artificially setting, 1e ^-4≤ α≤1; Parameter beta is used for the distributed intelligence of the state variable X that describes GPS state equation, and in Gauss's situation, the optimal value of β is 2.

$\mathrm{\λ}={\mathrm{\α}}^2(\tilde{n}+\mathrm{\κ})-\tilde{n}---\left(11\right)$

Wherein, parameter κ gets 0.

Step 4.2: carry out time renewal by formula (12) to formula (14).

${\mathrm{\ξ}}_k^{j,r}={\mathrm{\Φ}}_j{\mathrm{\ξ}}_{k-1}^{j,r}---\left(12\right)$

Wherein, expression model j is r sigma (sigma) point after a step shifts in the k moment; Φ _jfor the state-transition matrix of model j.

${\hat{X}}_{k,k-1}^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\Σ}}}{w}_{j,r}^{\left(m\right)}{\mathrm{\ξ}}_k^{j,r}---\left(13\right)$

Wherein, the step status predication value of model j in the k moment;

$P_{k,k-1}^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\Σ}}}{w}_{j,r}^{\left(c\right)}({\mathrm{\ξ}}_k^{j,r}-{\hat{X}}_{k,k-1}^j){({\mathrm{\ξ}}_k^{j,r}-{\hat{X}}_{k,k-1}^j)}^T+{\mathrm{\Ψ}}_{k-1}^j{Q}_{k-1}^j{\left({\mathrm{\Ψ}}_{k-1}^j\right)}^T---\left(14\right)$

Wherein, for model j is at the one-step prediction varivance matrix in k moment. represent that model j is at the adaptive matrix in (k-1) moment, initial value by artificially setting.

Step 4.3: observe renewal by formula (15)-(17)

${\hat{y}}_k^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\Σ}}}{w}_{j,r}^{\left(m\right)}h\left({\mathrm{\ξ}}_k^{j,r}\right)---\left(15\right)$

Wherein represent that model j is in k moment observability estimate value, r represent the 0th sigma o'clock to the the mark that individual sigma is ordered, h () is the nonlinear function in the observation equation obtaining in step 1.

$P_{\mathrm{yy}}^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\Σ}}}{w}_{j,r}^{\left(c\right)}[h\left({\mathrm{\ξ}}_k^{j,r}\right)-{\hat{y}}_k^j]{[h\left({\mathrm{\ξ}}_k^{j,r}\right)-{\hat{y}}_k^j]}^T+R_k---\left(16\right)$

$P_{\mathrm{xy}}^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\Σ}}}{w}_{j,r}^{\left(c\right)}[{\mathrm{\ξ}}_k^{j,r}-{\hat{X}}_{k,k-1}^j]{[h\left({\mathrm{\ξ}}_k^{j,r}\right)-{\hat{y}}_k^j]}^T---\left(17\right)$

y is that model j upgrades error covariance matrix in the time in k moment, for model j is at k moment transposition error covariance matrix.

Step 4.4: carry out filtering renewal by formula (18) to formula (20)

$K_k^j=P_{\mathrm{xy}}^j{\left(P_{\mathrm{yy}}^j\right)}^{-1}---\left(18\right)$

Wherein for model j is at the filter gain matrix in k moment.

${\hat{X}}_k^j={\hat{X}}_{k,k-1}^j+K_k^j(y_k-{\hat{y}}_k^j)---\left(19\right)$

for the state estimation value in k moment in model j, for the observed reading of this gps system receiver in k moment, it is known quantity.

$P_k^j=P_{k,k-1}^j-K_k^j{P}_{\mathrm{yy}}^j{\left(K_k^j\right)}^T---\left(20\right)$

for the estimation error variance matrix of k moment model j.

Step 5: computation model j is at the process noise covariance matrix in k moment the adaptive matrix of battle array

Step 5.1: with formula (21) to formula (23) computation model j the k moment estimated bias

${\mathrm{\υ}}_k^j=y_k^j-{\hat{y}}_k^j---\left(21\right)$

Wherein for model j is at the residual sequence in k moment.Residual sequence covariance matrix use represent, with formula (22) calculating.

$V_k^j=E\left[{\mathrm{\υ}}_k^j{\left({\mathrm{\υ}}_k^j\right)}^T\right]=\frac{1}{k-1}\underset{s=1}{\overset{s=k}{\mathrm{\Σ}}}{\mathrm{\υ}}_s^j{\left({\mathrm{\υ}}_s^j\right)}^T=\left\{\begin{array}{cc}{\mathrm{\υ}}_k^j{\left({\mathrm{\υ}}_k^j\right)}^T,& k=1\\ V_{k-1}^j+{\mathrm{\υ}}_k^j{\left({\mathrm{\υ}}_k^j\right)}^T,& k=2\\ \frac{k-2}{k-1}(V_{k-1}^j+\frac{{\mathrm{\υ}}_k^j{\left({\mathrm{\υ}}_k^j\right)}^T}{k-2}),& k\≥3\end{array}\right.---\left(22\right)$

Wherein, s represents the variation instruction of moment from 1 to k.

Model j is at the estimated bias matrix symbol in k moment represent, and utilize formula (23) to ask for.

${\mathrm{\Δ}}_k^j=\mathrm{trace}\left(V_k^j\right)-\mathrm{trace}\left(P_{\mathrm{yy}}^j\right)---\left(23\right)$

Wherein trace () represents Matrix Calculating mark.

Step 5.2: adopt smoothing filter to ask for the process noise covariance matrix of model j adaptive factor

Process noise covariance matrix in work be mainly the value on diagonal line, therefore design adaptive factor for diagonal matrix,

${\mathrm{\Ψ}}_k^j=\mathrm{diag}\{q_k^j\left(1\right),q_k^j\left(2\right),q_k^j\left(3\right),q_k^j\left(4\right),q_k^j\left(5\right),q_k^j\left(6\right),q_k^j\left(8\right),q_k^j\left(9\right),\mathrm{1,1}\}---\left(24\right)$

Wherein represent the adaptive factor of model j in k moment process noise covariance diagonal entry, ask for formula (25) and formula (26).Adaptive matrix in latter two be that the process noise covariance matrix of 1 expression receiver clock correction and clock correction drift is without correction.

$\left\{\begin{array}{c}{\stackrel{\‾}{q}}_k^j\left(l\right)={\mathrm{\λ}}_1{\stackrel{\‾}{q}}_{k-1}^j\left(l\right)+{\mathrm{\λ}}_2{\mathrm{\Δ}}_k({\mathrm{\Δ}}_k>\mathrm{\η})\\ \begin{array}{cc}{\stackrel{\‾}{q}}_k^j\left(l\right)=1& ({\mathrm{\Δ}}_k\≤\mathrm{\η})\end{array}\end{array}---\left(25\right)\right.$

$q_k^j\left(l\right)=\sqrt{{\stackrel{\‾}{q}}_k^j\left(l\right)}---\left(26\right)$

Wherein for asking for diagonal entry intermediate variable.λ ₁and λ ₂for smothing filtering coefficient, and 0 < λ ₁< 1, λ ₁+ λ ₂=1.η is threshold parameter, and its value is by artificially setting.

Through the operation of step 4 and step 5, complete the self-adaptation UKF filtering of model j.

Step 6: operate on basis in step 5, by formula (27) to formula (29), model probability is upgraded.

$u_k^j=\frac{{\mathrm{\&Lambda;}}_k^j{\stackrel{\&OverBar;}{c}}_j}{c}---\left(27\right)$

Wherein, for model j is at the likelihood function in k moment, can calculate by formula (28); C is normaliztion constant, can calculate by formula (29).

${\mathrm{\&Lambda;}}_k^j\frac{1}{\sqrt{2\mathrm{\&pi;}\left|P_{\mathrm{yy}}^j\right|}}\exp [-0.5{\left({\mathrm{\&upsi;}}_k^j\right)}^T{\left(P_{\mathrm{yy}}^j\right)}^{-1}{\mathrm{\&upsi;}}_k^j]---\left(28\right)$

Wherein exp[] exponential function of expression taking natural constant e the end of as.

$c=\underset{j=1}{\overset{3}{\mathrm{\&Sigma;}}}{\mathrm{\&Lambda;}}_k^j{\stackrel{\&OverBar;}{c}}_j---\left(29\right)$

for the normaliztion constant of model j, π _ijfor model i is to the transition probability of model j, its value is by thinking setting, for the model probability in k-1 moment, in 0 moment by artificially setting, and afterwards each moment by formula (27) upgrade.

Step 7: operate on basis in step 6, carry out information fusion, obtain the output of k moment wave filter.

${\hat{X}}_k=\underset{j=1}{\overset{3}{\mathrm{\&Sigma;}}}{u}_k^j{\hat{X}}_k^j---\left(30\right)$

$P_k=\underset{j=1}{\overset{3}{\mathrm{\&Sigma;}}}{u}_k^j[P_k^j+({\hat{X}}_k^j-{\hat{X}}_k){({\hat{X}}_k^j-{\hat{X}}_k)}^T]---\left(31\right)$

Wherein for the system optimal estimated value that adopts interactive multi-model self-adaptation UKF (IMM-AUKF) algorithm to obtain, P _kfor adopting the error covariance matrix of the state optimization estimated value that IMM-AUKF algorithm obtains.

Step 8: judge whether current time arrives the end time of the current location tasks that step 2 arranges, if arrive end operation; Otherwise, operate on basis the state estimation value in k moment in the model j that step 4 is obtained in step 6 the estimation error variance matrix in k moment in model j the model j that step 5 obtains is at k moment process noise covariance matrix the adaptive matrix of battle array and the probability of the model j that obtains of step 6 as input value, repeated execution of steps 2, to the operation of step 7, is calculated the output valve of next moment wave filter.

Beneficial effect

The present invention propose the adaptive estimation method that is applied to satellite navigation with there to be technology to compare, it is advantageous that: cause problem that positioning precision is low to improve the precision of carrier location constant speed from two aspects for complicated motor-driven carrier movement model and noise statistics the unknown in satellite navigation system: first to utilize the auto-covariance matrix of residual sequence as correcting value, design smoothing filter merges this correcting value and outmoded adaptive factor, obtain a new adaptive factor, and utilize the real-time adjustment process noise covariance matrix of this adaptive factor, effectively reduce the positioning error that in system, noise statistics the unknown causes.Secondly, adopt interacting multiple model algorithm (IMM) that Models Sets M is set, and according to the soft handover of measuring between the real-time adjustment model probability of residual error implementation model, reduced the positioning error that system model out of true causes.

Brief description of the drawings

Fig. 1 is the schematic flow sheet of the satellite navigation method of a kind of interactive multi-model UKF with adaptive factor in the specific embodiment of the invention.

Embodiment

For better explanation objects and advantages of the present invention, below in conjunction with accompanying drawing and embodiment, the present invention will be further described.

The present embodiment will complete the location tasks of ground on-vehicle in 300 second time.This is vehicle-mounted moved with uniform velocity within 0 to 60 second time, within 60 to 102 second time, did at the uniform velocity turning motion, within 103 to 162 second time, did linear uniform motion, in 183 to 195 seconds, do at the uniform velocity turning motion, in the time of 196 seconds to 205 seconds, do uniformly accelrated rectilinear motion, in 206 seconds to 300 second times, do even deceleration rectilinear motion.

The adaptive estimation method that is applied to satellite navigation that uses the present invention to propose positions, and as shown in Figure 1, concrete steps are as follows for its flow process:

Step 1: set up system model

Step 1.1: the state equation of setting up the ground carrier based on GPS (GPS).

For describe motion state and the clock correction drift frequency of carrier comprehensively, by the state variable of sign X GPS (GPS) state equation, $X={[x,\stackrel{\&CenterDot;}{x},\stackrel{\&CenterDot;\&CenterDot;}{x,}y,\stackrel{\&CenterDot;}{y},\stackrel{\&CenterDot;\&CenterDot;}{y},z,\stackrel{\&CenterDot;}{z},\stackrel{\&CenterDot;\&CenterDot;}{z,}{\mathrm{\&delta;t}}_u,\mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_u]}^T.$ Wherein, represent respectively the lower X of world geodetic system system (World Geodetic System mono-1984Coordinate System, WGS-84 coordinate system) _tposition, speed and acceleration on axle; represent respectively Y under WGS-84 coordinate system _tposition, speed and acceleration on axle; represent respectively Z under WGS-84 coordinate system _tposition, speed and acceleration on axle; δ t _uwith represent respectively carrier receiver clock correction and clock correction drift frequency.Set up Models Sets M, the current statistical model CS that Models Sets M comprises description carrier movement state, normal acceleration MODEL C A and constant speed Turn Models CT, i.e. M=[CS, CA, CT].Wherein " present statistical model supposes that the acceleration that the acceleration in next moment is current time adds a finite value, and the value of next moment acceleration is limited, and can only near variation current acceleration." present statistical model CS has stronger adaptive faculty to environment, is to describe one of model that carrier movement state is conventional; Normal acceleration model CA accelerates in carrier movement process, and the value of acceleration is normal value; The turning motion that does constant angular speed in a plane of constant speed Turn Models CT hypothesis carrier.

The changing value of receiver clock error is described with single order Markov model, and the state equation of current statistical model CS is as shown in formula (1); The state equation of normal acceleration MODEL C A is as shown in formula (2); The state equation of constant speed Turn Models CT is as shown in formula (3).Φ in formula (1) ₁=diag{ Φ _{1, x}Φ _{1, y}Φ _{1, z}Φ _t,uwherein

${\mathrm{\&Phi;}}_{1,x}={\mathrm{\&Phi;}}_{1,y}={\mathrm{\&Phi;}}_{1,z}=\left[\begin{array}{ccc}1& T& \frac{1}{{\mathrm{\&lambda;}}_{\mathrm{\&tau;}}^2}(-1+{\mathrm{\&lambda;}}_{\mathrm{\&tau;}}T+e^{-{\mathrm{\&lambda;}}_{\mathrm{\&tau;}}T})\\ 0& 1& \frac{1}{{\mathrm{\&lambda;}}_{\mathrm{\&tau;}}}(1-e^{-{\mathrm{\&lambda;}}_{\mathrm{\&tau;}}T})\\ 0& 0& e^{-{\mathrm{\&lambda;}}_{\mathrm{\&tau;}}T}\end{array}\right],{\mathrm{\&Phi;}}_{t,u}=\left[\begin{array}{cc}1& (1-e^{-{\mathrm{\&lambda;}}_{t}T})/{\mathrm{\&lambda;}}_t\\ 0& e^{-{\mathrm{\&lambda;}}_{t}T}\end{array}\right]$

Wherein λ _τthe inverse that represents the G-time constant in each coordinate axis under WGS-84 coordinate system, being taken as 0.1, T is sampling time interval, is taken as 1; λ _tfor single order Markov time constant on the contrary, be taken as 0.1.

Φ in formula (2) ₂=diag{ Φ _{2, x}Φ _{2, y}Φ _{2, z}Φ _t,u, wherein ${\mathrm{\&Phi;}}_{2,x}={\mathrm{\&Phi;}}_{2,y}={\mathrm{\&Phi;}}_{2,z}=\left[\begin{array}{ccc}1& T& {0.5T}^2\\ 0& 1& T\\ 0& 0& 1\end{array}\right].$

In formula (3),

Φ ₃=diag{ Φ _{3, x}Φ _{3, y}Φ _{3, z}Φ _t,u, wherein ${\mathrm{\&Phi;}}_{3,x}={\mathrm{\&Phi;}}_{3,y}={\mathrm{\&Phi;}}_{3,z}=\left[\begin{array}{ccc}1& \frac{\sin \mathrm{\&omega;T}}{\mathrm{\&omega;}}& \frac{(1-\cos \mathrm{\&omega;T})}{{\mathrm{\&omega;}}^2}\\ 0& \cos \mathrm{\&omega;T}& \frac{\sin \mathrm{\&omega;T}}{\mathrm{\&omega;}}\\ 0& -\mathrm{\&omega;}\sin \mathrm{\&omega;T}& \cos \mathrm{\&omega;T}\end{array}\right],$ ω represents the differences in angle of carrier, is made as 0.2.

Use symbol the process noise covariance matrix in (k-1) moment that represents respectively system under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT, e[] computing of expression expectation value, represent to describe X under WGS-84 coordinate system _tthe process noise covariance matrix of carrier motion state on axle; represent to describe Y under WGS-84 coordinate system _tthe process noise covariance matrix of carrier motion state on axle; represent describing Z under WGS-84 coordinate system _tthe process noise covariance matrix of carrier motion state on axle; Q _{t, k-1}represent to describe the process noise covariance matrix of ground carrier receiver clock correction and clock correction drift;

$Q_{k-1}^1=\mathrm{diag}\{Q_{x,k-1}^1,Q_{y,k-1}^1,Q_{z,k-1}^1,Q_{t,k-1}\},$

$Q_{x,k-1}^1=Q_{y,k-1}^1=Q_{z,k-1}^1={2\mathrm{\&lambda;}}_{\mathrm{\&tau;}}\left[\begin{array}{ccc}\frac{T^5}{20}& \frac{T^4}{8}& \frac{T^3}{6}\\ \frac{T^4}{8}& \frac{T^3}{6}& \frac{T^2}{2}\\ \frac{T^3}{6}& \frac{T^2}{2}& T\end{array}\right]Q_{t,k-1}=\left[\begin{array}{cc}\frac{T^3}{3}& \frac{T^2}{2}\\ \frac{T^2}{2}& T\end{array}\right];$

$Q_{k-1}^2=\mathrm{diag}\{Q_{x,k-1}^2,Q_{y,k-1}^2,Q_{z,k-1}^2,Q_{t,k-1}\},$ Wherein $Q_{x,k-1}^2=Q_{y,k-1}^2=Q_{z,k-1}^2=\left[\begin{array}{ccc}\frac{T^4}{20}& \frac{T^3}{8}& \frac{T^2}{6}\\ \frac{T^3}{8}& \frac{T^2}{3}& \frac{T}{2}\\ \frac{T^2}{6}& \frac{T}{2}& 1\end{array}\right];$

$Q_{k-1}^3=\mathrm{diag}\{Q_{x,k-1}^3,Q_{y,k-1}^3,Q_{z,k-1}^3,Q_{t,k-1}\},$

Wherein,

$Q_{x,k1}^3=Q_{y,k-1}^3=Q_{z,k-1}^3=\left[\begin{array}{ccc}\frac{6\mathrm{\&omega;T}-8\sin \mathrm{\&omega;T}+\sin 2\mathrm{\&omega;T}}{{4\mathrm{\&omega;}}^5}& \frac{{2\sin }^4(\mathrm{\&omega;T}/2)}{{\mathrm{\&omega;}}^4}& \frac{-2\mathrm{\&omega;T}+4\sin \mathrm{\&omega;T}-\sin 2\mathrm{\&omega;T}}{{4\mathrm{\&omega;}}^3}\\ \frac{{2\sin }^4(\mathrm{\&omega;T}/2)}{{\mathrm{\&omega;}}^4}& \frac{2\mathrm{\&omega;T}-\sin 2\mathrm{\&omega;T}}{{4\mathrm{\&omega;}}^3}& \frac{{\sin }^2\mathrm{\&omega;T}}{{2\mathrm{\&omega;}}^2}\\ \frac{-2\mathrm{\&omega;T}+4\sin T-\sin 2\mathrm{\&omega;T}}{{4\mathrm{\&omega;}}^3}& \frac{{\sin }^2\mathrm{\&omega;t}}{{2\mathrm{\&omega;}}^2}& \frac{2\mathrm{\&omega;T}+\sin 2\mathrm{\&omega;T}}{4\mathrm{\&omega;}}\end{array}\right.$

Step 1.2: the observation equation of setting up the ground carrier based on GPS (GPS)

Adopt pseudorange to set up observation equation as observed quantity.At 1 second, in 300 seconds, 5 gps satellites that receiver observes, and utilize these 5 gps satellites to position, set up this moment CS model, and the pseudorange observation equation of CA model and CT model is as shown in formula (4).Wherein in formula (4), the span of n is n=1,2,3,4,5

Convenient for below describing algorithm, set up CS model, the observation equation of CA model and CT model is formula (5).

Use R _kthe covariance matrix of the observational error of representation formula (5),

Step 2: CS model is set, CA model and the filtering initial value in 0 moment of CT model, wherein the initial value of position, speed and receiver clock correction is obtained by least square method, and the initial value of acceleration and receiver clock correction variable quantity is made as 0; The end time that current location tasks is set is simultaneously 300 seconds.

Step 3: obtain CS model, CA model and CT model are at the filtering initial value in k moment.

The estimation initial value of CS model, CA model and the state in CT model (k-1) moment that use step 2 or step 4 obtain with and the evaluated error covariance matrix initial value of CS model, CA model and CT model with mix alternately by formula (6) and formula (7), obtain respectively CS model, the estimation initial value of the state in k moment in CA model and CT model with and CS model, the evaluated error covariance matrix initial value in k moment in CA model and CT model with

Step 4: to CS model, CA model and CT model carry out self-adaptation Unscented kalman filtering (Adaptive Unsigned Kalman Filter, AUKF), obtain the filter value of each model in the k moment.Concrete operation step is:

Step 4.1: use represent the CS model obtaining in step 1, the dimension in the state equation of CA model and CT model, by formula (8) computation model j in the k moment individual initial sigma (sigma) point;

With formula (9) calculating first kind weight coefficient the first-order statistics characteristic of this weight coefficient for asking each sigma to order; With formula (10) calculating Equations of The Second Kind weight coefficient the second-order statistics of this weight coefficient for asking each sigma to order; Wherein the β value in formula (9) and (10) is that 2, α value is 0.01, and the parameter κ in formula (11) gets 0.

Step 4.2: carry out time renewal by formula (12) to formula (14).

Wherein in formula (14) initial value be made as unit matrix.

Step 4.3: observe renewal by formula (15)-(17)

Step 4.4: carry out filtering renewal by formula (18) to formula (20)

Step 5: computation model j is at the process noise covariance matrix in k moment the adaptive matrix of battle array

Step 5.1: use formula (21) to the covariance matrix of formula (22) computation model j at the residual sequence in k moment.And utilize formula (23) to ask the estimated bias matrix of model j in the k moment.

Step 5.2: adopt the smoothing filter of formula (25) design to ask for the process noise covariance matrix of model j adaptive matrix

Realize Δ by formula (25) to the smoothing filter of formula (26) design _kfusion with outmoded adaptive factor.

In formula (26), smothing filtering coefficient is set to λ ₁=0.98, λ ₂=0.02, threshold parameter is set to 10.

Through the operation of step 4 and step 5, complete the self-adaptation UKF filtering of model j, j=1,2,3, represent respectively CS model, CA model and CT model.

Step 6: operate on basis in step 5, by formula (27) to formula (29), model probability is upgraded

Wherein model i is to the transition probability π of model j _ijbe set to $\mathrm{\&pi;}=\left[\begin{array}{ccc}0.98& 0.01& 0.01\\ 0.01& 0.98& 0.01\\ 0.01& 0.01& 0.98\end{array}\right].$

Step 7: operate on basis in step 6, carry out information fusion by formula (30) and formula (31), obtain the output of k moment wave filter.

Step 8: judge whether current time arrives 300 seconds end times of the current location tasks that step 2 arranges, if arrive end operation; Otherwise, operate on basis the state estimation value in k moment in the model j that step 4 is obtained in step 6 the estimation error variance matrix in k moment in model j the model j that step 5 obtains is at k moment process noise covariance matrix the adaptive matrix of battle array and the probability of the model j that obtains of step 6 as input value, repeated execution of steps 2, to the operation of step 7, is calculated the output valve of next moment wave filter.

For effect of the present invention is described, the IMM-AUKF method that uses respectively existing UKF algorithm, IMM-UKF and the present invention to propose positions the location tasks in embodiment, and the error his-and-hers watches result obtaining is as shown in table 1.

The experiment comparing result of table 1 UKF algorithm, IMM-UKF and IMM-AUKF algorithm

As can be seen from Table 1, compared with classic method, the positioning precision of the IMM-AUKF method that the present invention proposes is the highest.

Claims (3)

1. a satellite navigation method of the interactive multi-model UKF with adaptive factor, is characterized in that: specifically comprise the steps:

Step 1: set up system model;

Step 1.1: the state equation of setting up the ground carrier based on global position system GPS;

For describe motion state and the clock correction drift frequency of carrier comprehensively, by the state variable of sign X global position system GPS state equation, $X={[x,\stackrel{\&CenterDot;}{x},\stackrel{\&CenterDot;\&CenterDot;}{x,}y,\stackrel{\&CenterDot;}{y},\stackrel{\&CenterDot;\&CenterDot;}{y},z,\stackrel{\&CenterDot;}{z},\stackrel{\&CenterDot;\&CenterDot;}{z,}{\mathrm{\&delta;t}}_u,\mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_u]}^T;$ Wherein, x, represent respectively X under WGS-84 coordinate system _tposition, speed and acceleration on axle; Y, represent respectively Y under WGS-84 coordinate system _tposition, speed and acceleration on axle; Z, represent respectively Z under WGS-84 coordinate system _tposition, speed and acceleration on axle; δ t _uwith represent respectively carrier receiver clock correction and clock correction drift frequency; Set up Models Sets M, the current statistical model CS that Models Sets M comprises description carrier movement state, normal acceleration MODEL C A and constant speed Turn Models CT, i.e. M=[CS, CA, CT]; The changing value of receiver clock error is described with single order Markov model, and the state equation of current statistical model CS is as shown in formula (1); The state equation of normal acceleration MODEL C A is as shown in formula (2); The state equation of constant speed Turn Models CT is as shown in formula (3);

$X_k^1={\mathrm{\&Phi;}}_1{X}_{k-1}^1+{\mathrm{\&Psi;\&psi;}}_{k-1}^1{w}_{k-1}^1---\left(1\right)$

$X_k^2={\mathrm{\&Phi;}}_2{X}_{k-1}^2+{\mathrm{\&Psi;}}_{k-1}^2{w}_{k-1}^2---\left(2\right)$

$X_k^3={\mathrm{\&Phi;}}_3{X}_{k-1}^3+{\mathrm{\&Psi;}}_{k-1}^3{w}_{k-1}^3---\left(3\right)$

Wherein, the state in k moment that represents respectively system under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT, wherein i=1,2,3, that is: the state in expression system k moment under current statistical model CS; the state in expression system k moment under normal acceleration MODEL C A; the state in expression system k moment under constant speed Turn Models CT; K is positive integer; Φ _ifor the state-transition matrix under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT, Φ _i=diag (Φ _i,x, Φ _i,y, Φ _i,z, Φ _t,u), Φ _{i, x}represent X under WGS-84 coordinate system _tstate-transition matrix on axle; Φ _i,yrepresent Y under WGS-84 coordinate system _tstate-transition matrix on axle; Φ _i,zrepresent Z under WGS-84 coordinate system _tstate-transition matrix on axle; Φ _t,urepresent to describe the state-transition matrix of ground carrier receiver clock correction and clock correction drift; the process noise covariance matrix adaptive factor of system under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT; the process noise matrix of system under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT;

Use symbol the process noise covariance matrix in (k-1) moment that represents respectively system under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT, e[] computing of expression expectation value, represent to describe X under WGS-84 coordinate system _tthe process noise covariance matrix of carrier motion state on axle; represent to describe Y under WGS-84 coordinate system _tthe process noise covariance matrix of carrier motion state on axle; represent describing Z under WGS-84 coordinate system _tthe process noise covariance matrix of carrier motion state on axle; represent to describe the process noise covariance matrix of ground carrier receiver clock correction and clock correction drift;

Step 1.2: the observation equation of setting up the ground carrier based on global position system GPS;

Adopt pseudorange to set up observation equation as observed quantity; In the k moment, the gps satellite quantity that receiver observes represents with N, sets up this moment CS model, and the pseudorange observation equation of CA model and CT model is as shown in formula (4);

${\mathrm{\&rho;}}_n^k=\sqrt{{(x_k^i-x_n^k)}^2+{(y_k^i-y_n^k)}^2+{(z_k^i-z_n^k)}^2}+{\mathrm{\&delta;t}}_u^k+{\mathrm{\&epsiv;}}_n^k---\left(4\right)$

Wherein, be illustrated in the pseudorange of n the gps satellite that the k moment observes, n=1,2 ... N, N represents the sum of the gps satellite that the k moment observes; represent n gps satellite X under WGS-84 coordinate system that the k moment observes _tposition on axle; represent n gps satellite Y under WGS-84 coordinate system that the k moment observes _tposition on axle; represent n gps satellite Z under WGS-84 coordinate system that the k moment observes _tposition on axle; be illustrated respectively in carrier X under WGS-84 coordinate system in k moment ground under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT _tposition on axle; be illustrated respectively in carrier Y under WGS-84 coordinate system in k moment ground under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT _tposition on axle; be illustrated respectively in carrier Z under WGS-84 coordinate system in k moment ground under current statistical model CS, normal acceleration MODEL C A and constant speed Turn Models CT _tposition on axle; represent k moment ground carrier receiver clock correction; the observational error of n the gps satellite that the expression k moment observes;

Convenient for below describing algorithm, set up CS model, the observation equation of CA model and CT model is formula (5);

$y_k^i=h\left(X_k^i\right)+{\mathrm{\&epsiv;}}_k---\left(5\right)$

Wherein, , be the observed quantity of system, $h\left(X_k^i\right)=\left[\begin{array}{c}\sqrt{{(x_k^i-x_1^k)}^2+{(y_k^i-y_1^k)}^2+{(z_k^i-z_1^k)}^2}+{\mathrm{\&delta;t}}_u^k\\ \sqrt{{(x_k^i-x_2^k)}^2+{(y_k^i-y_2^k)}^2+{(z_k^i-z_2^k)}^2}+{\mathrm{\&delta;t}}_u^k\\ \&CenterDot;\&CenterDot;\&CenterDot;\\ \sqrt{{(x_k^i-x_n^k)}^2+{(y_k^i-y_n^k)}^2+{(z_k^i-z_n^k)}^2}+{\mathrm{\&delta;t}}_u^k\end{array}\right],$ Nonlinear function in observation equation, for the observational error of system;

Use R _kthe covariance matrix of the observational error of representation formula (5),

Step 2: CS model is set, and CA model and the filtering initial value in 0 moment of CT model arrange end time of current location tasks simultaneously;

On the basis operating in step 1, the estimation initial value of the state of CS model, CA model and CT model is set respectively, uses symbol with represent; Then the evaluated error covariance matrix initial value of CS model, CA model and CT model is set, uses symbol with represent; And end time of system operation is set;

Step 3: obtain CS model, CA model and CT model are at the filtering initial value in k moment;

The estimation initial value of CS model, CA model and the state in CT model (k-1) moment that use step 2 or step 4 obtain with and the evaluated error covariance matrix initial value of CS model, CA model and CT model with mix alternately by formula (6) and formula (7), obtain respectively CS model, in CA model and CT model, the estimation initial value of the state in k moment, uses respectively with represent, and CS model, in CA model and CT model, the evaluated error covariance matrix initial value in k moment, uses respectively with represent;

${\hat{X}}_{k-1}^{0j}=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}{\hat{X}}_{i=1}^i{u}_{k-1|k-1}^{i|j}---\left(6\right)$

Wherein, represent respectively CS model, CA model and CT model are at the state initial value of k moment filtering; J=1,2,3, represent respectively CS model, CA model and CT model; i=1,2,3, represent respectively CS model, CA model and CT model are in the optimal estimation value of k-1 moment state; represent the mixing probability of k-1 moment model i and model j, π _ijfor model i is to the model transition probability of model j, its value is by artificially setting; for model i is at the probability in k moment, initial value by artificially setting, and each moment is upgraded by step 6 afterwards; the normaliztion constant of representative model j,

$P_{k-1}^{0j}=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}{u}_{k-1|k-1}^{i|j}[P_{k-1}^i+({\hat{X}}_{k-1}^i-{\hat{X}}_{k-1}^{0j}){({\hat{X}}_{k-1}^i-{\hat{X}}_{k-1}^{0j})}^T]---\left(7\right)$

Wherein, i=1,2,3, represent respectively CS model, CA model and the CT model variance matrix at k-1 moment filtering error; represent that respectively CS model, CA model and CT model are at k moment filtering error variance matrix initial value;

Step 4: to CS model, CA model and CT model carry out self-adaptation Unscented kalman filtering AUKF, obtain the filter value of each model in the k moment;

Step 5: computation model j is at the process noise covariance matrix in k moment the adaptive matrix of battle array

Step 6: operate on basis in step 5, by formula (27) to formula (29), model probability is upgraded;

$u_k^j=\frac{{\mathrm{\&Lambda;}}_k^j{\stackrel{\&OverBar;}{c}}_j}{c}---\left(8\right)$

Wherein, for model j is at the likelihood function in k moment, can calculate by formula (28); C is normaliztion constant, can calculate by formula (29);

${\mathrm{\&Lambda;}}_k^j\frac{1}{\sqrt{2\mathrm{\&pi;}\left|P_{\mathrm{yy}}^j\right|}}\exp [-0.5{\left({\mathrm{\&upsi;}}_k^j\right)}^T{\left(P_{\mathrm{yy}}^j\right)}^{-1}{\mathrm{\&upsi;}}_k^j]---\left(9\right)$

Wherein exp[] exponential function of expression taking natural constant e the end of as;

$c=\underset{j=1}{\overset{3}{\mathrm{\&Sigma;}}}{\mathrm{\&Lambda;}}_k^j{\stackrel{\&OverBar;}{c}}_j---\left(10\right)$

for the normaliztion constant of model j, π _ijfor model i is to the transition probability of model j, its value is by thinking setting, for the model probability in k-1 moment, in 0 moment by artificially setting, and afterwards each moment by formula (27) upgrade;

Step 7: operate on basis in step 6, carry out information fusion, obtain the output of k moment wave filter;

${\hat{X}}_k=\underset{j=1}{\overset{3}{\mathrm{\&Sigma;}}}{u}_k^j{\hat{X}}_k^j---\left(11\right)$

$P_k=\underset{j=1}{\overset{3}{\mathrm{\&Sigma;}}}{u}_k^j[P_k^j+({\hat{X}}_k^j-{\hat{X}}_k){({\hat{X}}_k^j-{\hat{X}}_k)}^T]---\left(12\right)$

Wherein for the system optimal estimated value that adopts interactive multi-model self-adaptation UKF algorithm to obtain, P _kfor adopting the error covariance matrix of the state optimization estimated value that IMM-AUKF algorithm obtains;

Step 8: judge whether current time arrives the end time of the current location tasks that step 2 arranges, if arrive end operation; Otherwise, operate on basis the state estimation value in k moment in the model j that step 4 is obtained in step 6 the estimation error variance matrix in k moment in model j the model j that step 5 obtains is at k moment process noise covariance matrix the adaptive matrix of battle array and the probability of the model j that obtains of step 6 as input value, repeated execution of steps 2, to the operation of step 7, is calculated the output valve of next moment wave filter.

2. the satellite navigation method of a kind of interactive multi-model UKF with adaptive factor as claimed in claim 1, it is characterized in that: described in its step 4 to CS model, CA model and CT model carry out self-adaptation Unscented kalman filtering AUKF, obtain each model at the concrete operation step of the filter value in k moment to be:

Step 4.1: use represent the dimension of system state, the CS model obtaining in step 1, the dimension of state equation in CA model and CT model, by formula (8) computation model j in the k moment individual initial sigma point, wherein r sigma point is with using symbol represent, and r is integer;

Wherein, the r row or the r that represent On Square-Rooting Matrices are capable;

By formula (9) calculating first kind weight coefficient, the first-order statistics characteristic of this weight coefficient for asking each sigma to order; The first kind weight coefficient symbol that r the sigma of model j ordered represent, that is: represent r sigma point first kind weight coefficient of CS model, represent r sigma point first kind weight coefficient of CA model, represent r sigma point first kind weight coefficient of CT model; With formula (10) calculating Equations of The Second Kind weight coefficient, the second-order statistics of this weight coefficient for asking each sigma to order; The Equations of The Second Kind weight coefficient symbol that r the sigma of model j ordered represent, that is: represent r sigma point Equations of The Second Kind weight coefficient of CS model, represent r sigma point Equations of The Second Kind weight coefficient of CA model, represent r sigma point Equations of The Second Kind weight coefficient of CT model;

Wherein, λ is a composite scale parameter, and its value solves by formula (11), and parameter alpha determines sigma spread of points degree, and its value is by artificially setting, 1e ^-4≤ α≤1; Parameter beta is used for the distributed intelligence of the state variable X that describes GPS state equation, and in Gauss's situation, the optimal value of β is 2;

$\mathrm{\&lambda;}={\mathrm{\&alpha;}}^2(\tilde{n}+\mathrm{\&kappa;})-\tilde{n}---\left(16\right)$

Wherein, parameter κ gets 0;

Step 4.2: carry out time renewal by formula (12) to formula (14);

${\mathrm{\&xi;}}_k^{j,r}={\mathrm{\&Phi;}}_j{\mathrm{\&xi;}}_{k-1}^{j,r}---\left(17\right)$

Wherein, expression model j is r sigma point after a step shifts in the k moment; Φ _jfor the state-transition matrix of model j;

${\hat{X}}_{k,k-1}^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\&Sigma;}}}{w}_{j,r}^{\left(m\right)}{\mathrm{\&xi;}}_k^{j,r}---\left(18\right)$

Wherein, the step status predication value of model j in the k moment;

$P_{k,k-1}^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\&Sigma;}}}{w}_{j,r}^{\left(c\right)}({\mathrm{\&xi;}}_k^{j,r}-{\hat{X}}_{k,k-1}^j){({\mathrm{\&xi;}}_k^{j,r}-{\hat{X}}_{k,k-1}^j)}^T+{\mathrm{\&Psi;}}_{k-1}^j{Q}_{k-1}^j{\left({\mathrm{\&Psi;}}_{k-1}^j\right)}^T---\left(19\right)$

Wherein, for model j is at the one-step prediction varivance matrix in k moment; represent that model j is at the adaptive matrix in (k-1) moment, initial value by artificially setting;

Step 4.3: observe renewal by formula (15) to (17);

${\hat{y}}_k^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\&Sigma;}}}{w}_{j,r}^{\left(m\right)}h\left({\mathrm{\&xi;}}_k^{j,r}\right)---\left(20\right)$

Wherein represent that model j is in k moment observability estimate value, r represent the 0th sigma o'clock to the the mark that individual sigma is ordered, h () is the nonlinear function in the observation equation obtaining in step 1;

$P_{\mathrm{yy}}^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\&Sigma;}}}{w}_{j,r}^{\left(c\right)}[h\left({\mathrm{\&xi;}}_k^{j,r}\right)-{\hat{y}}_k^j]{[h\left({\mathrm{\&xi;}}_k^{j,r}\right)-{\hat{y}}_k^j]}^T+R_k---\left(21\right)$

$P_{\mathrm{xy}}^j=\underset{r=0}{\overset{2\tilde{n}}{\mathrm{\&Sigma;}}}{w}_{j,r}^{\left(c\right)}[{\mathrm{\&xi;}}_k^{j,r}-{\hat{X}}_{k,k-1}^j]{[h\left({\mathrm{\&xi;}}_k^{j,r}\right)-{\hat{y}}_k^j]}^T---\left(22\right)$

for model j upgrades error covariance matrix in the time in k moment, for model j is at k moment transposition error covariance matrix;

Step 4.4: carry out filtering renewal by formula (18) to formula (20)

$K_k^j=P_{\mathrm{xy}}^j{\left(P_{\mathrm{yy}}^j\right)}^{-1}---\left(23\right)$

Wherein for model j is at the filter gain matrix in k moment;

${\hat{X}}_k^j={\hat{X}}_{k,k-1}^j+K_k^j(y_k-{\hat{y}}_k^j)---\left(24\right)$

for the state estimation value in k moment in model j, for the observed reading of this gps system receiver in k moment, it is known quantity;

$P_k^j=P_{k,k-1}^j-K_k^j{P}_{\mathrm{yy}}^j{\left(K_k^j\right)}^T---\left(25\right)$

for the estimation error variance matrix of k moment model j.

3. the satellite navigation method of a kind of interactive multi-model UKF with adaptive factor as claimed in claim 1 or 2, is characterized in that: the j of computation model described in its step 5 is at the process noise covariance matrix in k moment adaptive matrix concrete operation step be:

Step 5.1: with formula (21) to formula (23) computation model j the k moment estimated bias

${\mathrm{\&upsi;}}_k^j=y_k^j-{\hat{y}}_k^j---\left(26\right)$

Wherein for model j is at the residual sequence in k moment; Residual sequence covariance matrix use represent, with formula (22) calculating;

$V_k^j=E\left[{\mathrm{\&upsi;}}_k^j{\left({\mathrm{\&upsi;}}_k^j\right)}^T\right]=\frac{1}{k-1}\underset{s=1}{\overset{s=k}{\mathrm{\&Sigma;}}}{\mathrm{\&upsi;}}_s^j{\left({\mathrm{\&upsi;}}_s^j\right)}^T=\left\{\begin{array}{cc}{\mathrm{\&upsi;}}_k^j{\left({\mathrm{\&upsi;}}_k^j\right)}^T,& k=1\\ V_{k-1}^j+{\mathrm{\&upsi;}}_k^j{\left({\mathrm{\&upsi;}}_k^j\right)}^T,& k=2\\ \frac{k-2}{k-1}(V_{k-1}^j+\frac{{\mathrm{\&upsi;}}_k^j{\left({\mathrm{\&upsi;}}_k^j\right)}^T}{k-2}),& k\&GreaterEqual;3\end{array}\right.---\left(27\right)$

Wherein, s represents the variation instruction of moment from 1 to k;

Model j is at the estimated bias matrix symbol in k moment represent, and utilize formula (23) to ask for;

${\mathrm{\&Delta;}}_k^j=\mathrm{trace}\left(V_k^j\right)-\mathrm{trace}\left(P_{\mathrm{yy}}^j\right)---\left(28\right)$

Wherein trace () represents Matrix Calculating mark;

Step 5.2: adopt smoothing filter to ask for the process noise covariance matrix of model j adaptive factor

Process noise covariance matrix in work be mainly the value on diagonal line, therefore design adaptive factor for diagonal matrix,

${\mathrm{\&Psi;}}_k^j=\mathrm{diag}\{q_k^j\left(1\right),q_k^j\left(2\right),q_k^j\left(3\right),q_k^j\left(4\right),q_k^j\left(5\right),q_k^j\left(6\right),q_k^j\left(8\right),q_k^j\left(9\right),\mathrm{1,1}\}---\left(29\right)$

Wherein represent the adaptive factor of model j in k moment process noise covariance diagonal entry, ask for formula (25) and formula (26); Adaptive matrix in latter two be that the process noise covariance matrix of 1 expression receiver clock correction and clock correction drift is without correction;

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{q}}_k^j\left(l\right)={\mathrm{\&lambda;}}_1{\stackrel{\&OverBar;}{q}}_{k-1}^j\left(l\right)+{\mathrm{\&lambda;}}_2{\mathrm{\&Delta;}}_k({\mathrm{\&Delta;}}_k>\mathrm{\&eta;})\\ \begin{array}{cc}{\stackrel{\&OverBar;}{q}}_k^j\left(l\right)=1& ({\mathrm{\&Delta;}}_k\&le;\mathrm{\&eta;})\end{array}\end{array}---\left(30\right)\right.$

$q_k^j\left(l\right)=\sqrt{{\stackrel{\&OverBar;}{q}}_k^j\left(l\right)}---\left(31\right)$

Wherein for asking for diagonal entry intermediate variable; λ ₁and λ ₂for smothing filtering coefficient, and 0 < λ ₁< 1, λ ₁+ λ ₂=1; η is threshold parameter, and its value is by artificially setting.