CN103792562A - Strong tracking UKF filter method based on sampling point changing - Google Patents

Abstract

The invention provides a strong tracking UKF filter method based on sampling point changing. The strong tracking UKF filter method based on the sampling point changing comprises the steps that (1) initial parameter setting is carried out on a system; (2) Sigma points are sampled according to an orthogonal transformation sampling point method, a corresponding prediction equation is determined, and time and measurement are updated; (3) fading factors are calculated; (4) new one-step prediction covariance is calculated by using the fading factors, the Sigma points are recalculated, and auto-covariance and cross covariance after the fading factors are introduced are obtained through nonlinear measurement function propagation; (5) filter updating is carried out to the end. According to the strong tracking UKF filter method based on the sampling point changing, the problem of non-local sampling of the system is solved effectively, the precision of the system is improved, and the system is made to have certain strong tracking capability. The strong tracking UKF filter method based on the sampling point changing can be used for solving the problems of poor robustness and filtering divergence when a model of the system is uncertain, solves the problem of the non-local sampling in the high-dimensional system, and expands the application range of strong tracking filter. In an MEMS/GPS combined navigation system, the positioning and attitude determination performance of the MEMS/GPS combined navigation system can be improved through the method.

Description

The filtering method of a kind of strong tracking UKF based on transformed samples point

Technical field

What the present invention relates to is a kind of parameter calculation method of MEMS-INS/GPS integrated navigation attitude and positional information, especially a kind of improved strong tracking Unscented kalman filtering method, and it is a kind of strong tracking filter based on conversion Sigma sampled point in essence.

Background technology

Along with to the chasing of low cost lightweight, high precision navigational system, micro inertial measurement unit (MEMS-IMU) and GPS combined system on guided weapon and naval vessel, vehicle-mounted, rocket guidance field is widely used.The data solver of navigational system is to determine the key link of navigation accuracy, wherein the height of filtering accuracy and will largely affect combined navigation system performance.

In low precision inertial navigation system, because systematic error is large and situation is complicated, the modeling of system is difficult to accurately, and disturbance is lacked to resistance, this filtering technique just need to certain robustness is dealt with problems.The proposition of strong tracking filter (STF) has solved tracking problem and the modeling uncertain problem of dynamic performance to a certain extent, but requires nonlinear function continuously differentiable and not good its theory limit that becomes of strong nonlinearity filtering performance.Have scholar propose utilize based on Unscented(without mark) conversion strong tracking filter (UTSTF) solve above problem, owing at least approaching any non-linear Gaussian Systems state Posterior Mean and covariance with three rank Taylor's precision without mark conversion, and without the Jacobin matrix that calculates nonlinear function, UT conversion just provides effective solution route for overcoming the theory limit of STF like this.But, but the Nonlinear Filtering Problem that is greater than 3 for dimension, thus easily exist the non-positive definite of variance of transmitting to cause the numerical value instability problem of filtering algorithm.In the understanding to this problem, Arasaratnam has proposed CKF algorithm just, and points out that CKF has higher precision and stability compared with UKF in higher-dimension problem.Although CKF efficiently solves the problem of UKF algorithm numerical instability, introduce again non local Sampling simultaneously.Non local Sampling refers to that (average place) is very large if the sampled point using departs from central point, may cause the decline of filtering accuracy for the nonlinear filtering algorithm of transmitting average and variance based on sampled point.

Summary of the invention

The object of the present invention is to provide a kind of precision high, expand the filtering method of the strong tracking UKF based on transformed samples point of the range of application of strong track algorithm.

The object of the present invention is achieved like this:

After carrying out system initialization, carry out as follows filtering:

Step 1:

According to transformed samples strategy, determine number, position and the corresponding weight value of sampled point,

for one-step prediction estimated value, the P of the state in k moment _kfor maintaining system, estimation variance, n there is 2n sampled point, sampled point χ _i,kbe expressed as follows

${\mathrm{\χ}}_{i,k}={\hat{x}}_k+\sqrt{P_k}\mathrm{\γ}$

γ=(γ ₁, γ ₂... γ _2n), wherein γ _k=(γ _{k, 1}, γ _{k, 2}... γ _k,n) ^t, k=1,2 ... 2n

${\mathrm{\γ}}_{k,2r-1}=\sqrt{2}\cos ((2r-1)\mathrm{k\π}/n)$

${\mathrm{\γ}}_{k,2r}=\sqrt{2}\sin ((2r-1)\mathrm{k\π}/n)$

R=1,2 ..., [n/2], [n/2] represents to be not more than the maximum integer of n/2, if n is odd number,

corresponding weight value is specifically expressed as follows:

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="0"\; label="W"\; filenames="HDA0000468760650000023.png,HDA0000468760650000024.png"\; state="\{\{state\}\}">W</figure-callout>}}_0=\mathrm{\&kappa;}/(n+\mathrm{\&kappa;})\\ W_i=1/2(n+\mathrm{\&kappa;})\\ W_{i+n}=1/2(n+\mathrm{\&kappa;})\end{array}\right.,i=1,\&CenterDot;\&CenterDot;\&CenterDot;,n$

W _irepresent the corresponding weight value of i point, i=0,1 ..., 2n, and weights meet κ is a scale factor, selects κ=3-n;

Step 2:

Time upgrades, according to the selected sampled point χ of step 1 _i,k, propagate as ξ by nonlinear state function f _{i, k+1|k}, and then obtain the step status predication value after weighting

$\left\{\begin{array}{c}{\mathrm{\&xi;}}_{i,k+1|k}=f[{\mathrm{\&chi;}}_{i,k},u_k]+q_k,i=\mathrm{0,1},\&CenterDot;\&CenterDot;\&CenterDot;,2n\\ {\hat{x}}_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^{\left(m\right)}{\mathrm{\&xi;}}_{i,k+1|k}\end{array}\right.$

$P_{k+1|k}^{\left(l\right)}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}[{\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_k]{[{\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_k]}^T+Q_k$

status predication error covariance matrix while representing not introduce fading factor;

Step 3:

Measure and upgrade, utilize the step status predication value and the error covariance matrix that in step 2, draw to adopt the sampling policy in step 1 to calculate Sigma point, by speed, position measurement function the h () propagation of GPS be

that is:

${\mathrm{\&zeta;}}_{i,k+1|k}^{\left(l\right)}=h\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)+r_{k+1}$

R _k+1for k+1 observation noise is calculated the prediction of output of not introducing fading factor

${\hat{z}}_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^m{\mathrm{\&zeta;}}_{i,k+1|k}^{\left(l\right)}$

Calculate the autocovariance of not introducing fading factor

$P_{{\tilde{z}}_{k+1}}^{\left(l\right)}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^{\left(c\right)}({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}-{\hat{z}}_{k+1|k}){({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}-{\hat{z}}_{k+1|k})}^T+R_{k+1}$

R _k+1be observation noise matrix, calculate the cross covariance of not introducing fading factor

$P_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}^{\left(l\right)}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^{\left(c\right)}({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}-{\hat{x}}_{k+1|k}){({\mathrm{\&zeta;}}_{i,k+1|k}^{\left(l\right)}-{\hat{z}}_{k+1|k})}^T;$

Step 4:

Calculate fading factor λ _k+1, establish theoretical output residual sequence and be

${\mathrm{\&lambda;}}_{k+1}=\left\{\begin{array}{c}{\mathrm{\&lambda;}}_0,{\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1,{\mathrm{\&lambda;}}_0<1\end{array}\right.,{\mathrm{\&lambda;}}_0=\mathrm{tr}\left[N_{k+1}\right]/\mathrm{tr}\left[M_{k+1}\right]$

Wherein N _k+1, M _k+1by V _k+1calculate V _k+1for the covariance matrix of reality output residual sequence, be unknown,

$V_{k+1}=\left\{\begin{array}{c}{\mathrm{\&epsiv;}}_1{\mathrm{\&epsiv;}}_1^T,k=1;\\ \frac{\mathrm{\&rho;}{V}_k+{\mathrm{\&epsiv;}}_{k+1}{\mathrm{\&epsiv;}}_{k+1}^{\mathrm{<figure-callout\; id="1"\; label="T"\; filenames="HDA0000468760650000023.png,HDA0000468760650000024.png"\; state="\{\{state\}\}">T</figure-callout>}}}{1+\mathrm{\&rho;}},k\&GreaterEqual;1\end{array}\right.$

In formula, ρ is forgetting factor, and 0< ρ≤1, gets ρ=0.95,

$N_{k+1}=V_{k+1}-R_{k+1}-{\left[P_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}^{\left(l\right)}\right]}^T{\left[P_{k+1|k}^{\left(l\right)}\right]}^{-1}{Q}_k{\left[P_{k+1|k}^{\left(l\right)}\right]}^{-1}{P}_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}^{\left(l\right)}$

$M_{k+1}=P_{{\tilde{z}}_{k+1}}^{\left(l\right)}-V_{k+1}+N_{k+1};$

Step 5:

Try to achieve fading factor λ _k+1after; Try to achieve new status predication error covariance matrix P _k+1|k:

$P_{k+1|k}={\mathrm{\&lambda;}}_{k+1}\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^c[{\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_k]{[{\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_k]}^T+Q_k$

Q _kfor system noise matrix, adopt the sampling policy in step 1 to calculate Sigma point, propagate as ζ by nonlinear measurement function h () _{i, k+1|k}, again system is measured to renewal,

${\mathrm{\&chi;}}_{i,k+1|k}={\hat{x}}_{k+1|k}+\sqrt{P_{k+1|k}}\mathrm{\&gamma;}$

ζ _i,k+1|k=h(χ _i,k+1|k)+r _k+1

Calculate the prediction of output

${\hat{z}}_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^m{\mathrm{\&zeta;}}_{i,k+1|k}$

Calculate autocovariance

$P_{{\tilde{z}}_{k+1}}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^c({\mathrm{\&chi;}}_{i,k+1|k}-{\hat{z}}_{k+1|k}){({\mathrm{\&chi;}}_{i,k+1|k}-{\hat{z}}_{k+1|k})}^T+R_{k+1}$

Calculate cross covariance

$P_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^c({\mathrm{\&chi;}}_{i,k+1|k}-{\hat{x}}_{k+1|k}){({\mathrm{\&zeta;}}_{i,k+1}-{\hat{z}}_{k+1|k})}^T;$

Step 6:

Filtering is upgraded, and calculates the scale factor K in k+1 moment _k+1, the estimated value in k+1 moment

with evaluated error covariance P _k+1:

$K_{k+1}=P_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}{P}_{{\tilde{z}}_{k+1}}^{-1}$

${\hat{x}}_{k+1}={\hat{x}}_{k+1|k}+K_{k+1}(z_{k+1}-{\hat{z}}_{k+1|k})$

$P_{k+1}=P_{k+1|k}-K_{k+1}{P}_{{\tilde{z}}_{k+1}}{K}_{k+1}^T$

Return to step 1, carry out the estimation in next moment.

The present invention proposes a kind of strong tracking UKF filtering algorithm (ST-TUKF based on transformed samples point, Strong Tracking-Transformed Unscented Kalman Filter), on the basis of traditional UKF algorithm, adopt to sigma point carry out orthogonal transformation approach non-linear Gaussian Systems state posteriority distribute, make the Transformation based on UT(Unscented) conversion TUKF (Transformed Unscented Kalman Filter) method be used for strong tracking filter.

Inventive principle of the present invention is:

(1) strong tracking filter should meet orthogonality principle, state estimation value

with predicted value x _k+1between variance minimum, and do not export in the same time residual sequence and keep orthogonal everywhere.The output sequence of supposing residual error is

meet following formula:

$E\left[(x_{k+1}-{\hat{x}}_{k+1}){(x_{k+1}-{\hat{x}}_{k+1})}^T\right]=\min$

$E\left[{\mathrm{\&epsiv;}}_{k+1+j}{\mathrm{\&epsiv;}}_{k+1}^T\right]=0,j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;;k=\mathrm{0,1}\&CenterDot;\&CenterDot;\&CenterDot;$

Strong tracking filter method forces output residual sequence to keep mutually orthogonal, the online gain matrix of adjusting in real time accordingly, and STF still can keep the tracking power to system in the time that system model is uncertain like this.

(2) logarithm value integration sampling point set carries out orthogonal transformation and can not change its numerical approximation precision, and the key of therefore studying the long-pending approximate point of same precision numerical value is the different orthogonal matrix of research.Theorem has below provided the orthogonal matrix of the general type of a n dimension

B=[B ₁;B ₂;…B _n]

B _k=(β _k,1,β _k,2,…β _k,n) ^T

${\mathrm{\&beta;}}_{k,2r-1}=\sqrt{2/n}\cos ((2r-1)\mathrm{k\&pi;}/n)$

${\mathrm{\&beta;}}_{k,2r}=\sqrt{2/n}\sin ((2r-1)\mathrm{k\&pi;}/n)$

B is orthogonal matrix, and TUKF chooses the 2n same with CKF point.

$\mathrm{\&chi;}=\left[\begin{array}{cc}\sqrt{n}{e}_k& -\sqrt{n}{e}_k\end{array}\right],$ Make γ=B χ

${\mathrm{\&chi;}}_{i,k}={\hat{x}}_k+\sqrt{P_k}\mathrm{\&gamma;}$

Wherein γ=(γ ₁, γ ₂... γ _2n), γ _k=(γ _{k, 1}, γ _{k, 2}... γ _k,n) ^t, k=1,2 ... 2n

${\mathrm{\&gamma;}}_{k,2r-1}=\sqrt{2}\cos ((2r-1)\mathrm{k\&pi;}/n)$

${\mathrm{\&gamma;}}_{k,2r}=\sqrt{2}\sin ((2r-1)\mathrm{k\&pi;}/n)$

Can find out, the sampled point based on orthogonal transformation not with the increase of dimension away from central point, can not cause non local Sampling.

Based on above two principles, TUKF is combined with strong tracking item, can form a good solution.

The technical solution adopted in the present invention is: for integrated navigation filtering technique, adopt the filtering algorithm of a kind of strong tracking UKF based on transformed samples point.For its computing method are described, consider nonlinear discrete systems as follows, wherein the statistical property of noise can come out according to the sensing data of measuring.System model design is as follows:

x _k+1=f(x _k,u _k)+w _k

z _k=h(x _k)+v _k

Wherein, k>=0th, discrete-time variable, x ∈ R ⁿfor state vector, u _k∈ R ^pto control control inputs vector, z ∈ R ^mfor output vector.F (), h () are nonlinear functions, process noise w _k, measurement noise v _k+1be respectively n peacekeeping m dimension white Gaussian noise, its statistical property can be by inertia device, test draws, concrete form is as follows:

$\left\{\begin{array}{c}E\left[w_k\right]=q_k,\mathrm{cov}[w_k,w_j^T]=Q_k{\mathrm{\&delta;}}_{\mathrm{kj}},\\ E\left[v_k\right]=r_k,\mathrm{cov}[v_k,v_j^T]=R_k{\mathrm{\&delta;}}_{\mathrm{kj}},\\ \mathrm{cov}[w_k,v_j^T]=0\end{array}\right.$

Wherein Q _k, R _kbe all positive definite symmetrical matrix, δ _kjfor kronecker-δ function.

Technical characteristics body of the present invention comprises:

(1) set up integrated navigation Filtering Model;

(2) initial parameter in integrated navigation system is arranged, comprise system noise matrix Q, observation noise matrix R, quantity of state initial estimation X ₀with Initial state estimation error covariance P ₀, initial fading factor λ ₀=1.；

(3) use the strong UKF of tracking of transformed samples point filtering algorithm to carry out filtering to state variable, estimate the navigation informations such as alliance, speed, attitude;

(4) epoch, the moment added 1, read next moment observation, returned to step (3), until finish.

Wherein the described filtering of (3) part mainly comprises:

Step 1: system variable is carried out to initialization, set initial filtering parameter.

Step 2: according to the sampling policy of orthogonal transformation sampled point, Sigma point is sampled, and calculate corresponding weight value, propagate by nonlinear state equation, obtain corresponding one-step prediction, carry out time renewal and measure upgrading.

Step 3: guarantee that residual sequence is orthogonal, thereby by calculating fading factor λ _k+1.

Step 4: utilize fading factor to calculate new one-step prediction covariance P _k+1|k, utilize the sample mode of orthogonal transformation to recalculate Sigma point, propagate by nonlinear measurement function, obtain introducing autocovariance and cross covariance after fading factor.

Step 5: carry out filtering renewal, return to step 2.Detailed process can be with reference to figure mono-.

The present invention's beneficial effect is compared with prior art mainly reflected in:

(1) compared with traditional UKF algorithm, TUKF has guaranteed the numerical stability of system, has also solved because dimension increases the non local Sampling causing, and therefore has higher precision.

(2) strong track algorithm is combined with TUKF algorithm, overcome the uncertain impact of MEMS system model, improved its tracking power to dynamic property.

The present invention proposes first: with one group of sigma point obtaining based on orthogonal transformation for the strong UKF that follows the tracks of, due to sampled point in the process of orthogonal transformation and the dimension of getting along well proportional, therefore can there is not non local Sampling, therefore can there is higher precision., solve in High Dimensional Systems due to the problem that numerical value is unstable and non local sampling causes for UKF with novel sample mode, expanded the range of application of strong track algorithm.

Accompanying drawing explanation

Fig. 1 based on transformed samples point without mark strong tracking filter process flow diagram.

Fig. 2 direct-type Kalman filtering data I/O mode.

The attitude of Fig. 3 UKF is estimated.

The velocity estimation of Fig. 4 UKF.

The location estimation of Fig. 5 UKF.

The attitude of Fig. 6 ST-TUKF is estimated.

The velocity estimation of Fig. 7 ST-TUKF.

The location estimation of Fig. 8 ST-TUKF.

Embodiment

In order to make object of the present invention, technical scheme and advantage clearer, in conjunction with specific experiment, the present invention is further elaborated.Concrete example described herein, only in order to explain the present invention, is not intended to limit the present invention.

The data of processing of the present invention are the original output information of MEMS/GPS integrated navigation system.In experimentation, MEMS is connected on locomotive, is well placed gps antenna, make it in the good scope of signal, start inertial navigation system.For reaching the top performance of MEMS, need, starting after certain hour, system be moved under the condition with certain linear velocity, then static measurement.In measuring process, add an artificial disturbance for system, then keep stationary state.

The present invention adopts the direct-type nonlinear model (data procedures is referring to Fig. 2) based on hypercomplex number, and chosen position, speed, hypercomplex number, gyroscopic drift, acceleration zero are set up state equation partially.

Systematic procedure model is as follows

$\stackrel{\&CenterDot;}{P}=V$

$\stackrel{\&CenterDot;}{V}=C_b^n(f_b-v_a)+{\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^{T}g$

$\stackrel{\&CenterDot;}{q}=-\frac{1}{2}Q\&times;({\mathrm{\&omega;}}_{\mathrm{pb}}^b-{\mathrm{\&epsiv;}}_{\mathrm{\&omega;}}+v_{\mathrm{\&omega;}})$

${\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}}_{\mathrm{\&omega;}}=v_{{\mathrm{\&epsiv;}}_{\mathrm{\&omega;}}}$

${\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}}_a=v_{{\mathrm{\&epsiv;}}_a}$

Wherein,

be respectively alliance, speed, attitude quaternion, gyroscopic drift, the inclined to one side variable quantity of acceleration zero, f _bfor acting on the specific force on carrier,

be the attitude speed of carrier, Q is the antisymmetric matrix of attitude quaternion, v _a, v _ω,

be respectively the process noise of accelerometer, gyroscope, gyroaccelerometer drift.Here suppose that process noise is zero-mean white Gaussian noise.

for carrier is tied to the direction cosine matrix that navigation is, it is nonlinear.

$C_b^n\left(q\right)=\left[\begin{array}{ccc}{{\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000468760650000023.png,HDA0000468760650000024.png"\; state="\{\{state\}\}">q</figure-callout>}}_0}^2+{{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000468760650000023.png,HDA0000468760650000024.png"\; state="\{\{state\}\}">q</figure-callout>}}_1}^2-{{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000468760650000023.png"\; state="\{\{state\}\}">q</figure-callout>}}_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000468760650000023.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)& {{\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000468760650000023.png,HDA0000468760650000024.png"\; state="\{\{state\}\}">q</figure-callout>}}_0}^2-{{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000468760650000023.png,HDA0000468760650000024.png"\; state="\{\{state\}\}">q</figure-callout>}}_1}^2+{{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000468760650000023.png"\; state="\{\{state\}\}">q</figure-callout>}}_2}^2-{q_3}^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& {{\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000468760650000023.png,HDA0000468760650000024.png"\; state="\{\{state\}\}">q</figure-callout>}}_0}^2-{{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000468760650000023.png,HDA0000468760650000024.png"\; state="\{\{state\}\}">q</figure-callout>}}_1}^2-{{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000468760650000023.png"\; state="\{\{state\}\}">q</figure-callout>}}_2}^2+{q_3}^2\end{array}\right]$

Consider the factors such as the lower and vehicular speeds of MEMS precision is slower, ignore the impact of the angular speed of earth rotation on carrier.

The measuring value of chosen position, speed is as measurement equation.System measurements model is:

$P_k^{\mathrm{GPS}}=P_{k-N}+w_p$

$V_k^{\mathrm{GPS}}=V_{k-N}+w_v$

Suppose w _p, w _vbe respectively the measurement noise of speed and position, and be zero-mean white Gaussian noise.P _k-N, V _k-Nbe alliance and the speed of considering time delay, consider that gps signal has the hysteresis of 50ms, inertial reference calculation frequency is 100Hz, therefore get N=5.

System is carried out to initialization, be initial parameter is arranged, comprise system noise matrix Q, observation noise matrix R, quantity of state initial estimation X ₀with Initial state estimation error covariance P ₀, initial fading factor λ ₀=1.

After carrying out system initialization.Described filter step is specific as follows.

Step 1:

According to transformed samples (Transformed sigma point) strategy, determine number, position and the corresponding weight value of sampled point.

for the one-step prediction estimated value of the state in k moment, P _kfor estimation variance, n maintains system and has 2n sampled point.Sampled point χ _i,kbe expressed as follows

${\mathrm{\&chi;}}_{i,k}={\hat{x}}_k+\sqrt{P_k}\mathrm{\&gamma;}$

γ=(γ ₁, γ ₂... γ _2n), wherein γ _k=(γ _{k, 1}, γ _{k, 2}... γ _k,n) ^t, k=1,2 ... 2n

${\mathrm{\&gamma;}}_{k,2r-1}=\sqrt{2}\cos ((2r-1)\mathrm{k\&pi;}/n)$

${\mathrm{\&gamma;}}_{k,2r}=\sqrt{2}\sin ((2r-1)\mathrm{k\&pi;}/n)$

R=1,2 ..., [n/2], [n/2] represents to be not more than the maximum integer of n/2, if n is odd number, corresponding weight value is specifically expressed as follows:

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="0"\; label="W"\; filenames="HDA0000468760650000023.png,HDA0000468760650000024.png"\; state="\{\{state\}\}">W</figure-callout>}}_0=\mathrm{\&kappa;}/(n+\mathrm{\&kappa;})\\ W_i=1/2(n+\mathrm{\&kappa;})\\ W_{i+n}=1/2(n+\mathrm{\&kappa;})\end{array}\right.,i=1,\&CenterDot;\&CenterDot;\&CenterDot;,n$

W _irepresent the corresponding weight value of i point, i=0,1 ..., 2n, and weights meet

here κ is a scale factor, generally selects κ=3-n.

Step 2:

Time upgrades.According to the selected sampled point χ of step 1 _i,k, propagate as ξ by nonlinear state function f _{i, k+1|k}, and then obtain the step status predication value after weighting

$\left\{\begin{array}{c}{\mathrm{\&xi;}}_{i,k+1|k}=f[{\mathrm{\&chi;}}_{i,k},u_k]+q_k,i=\mathrm{0,1},\&CenterDot;\&CenterDot;\&CenterDot;,2n\\ {\hat{x}}_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^{\left(m\right)}{\mathrm{\&xi;}}_{i,k+1|k}\end{array}\right.$

$P_{k+1|k}^{\left(l\right)}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}[{\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_k]{[{\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_k]}^T+Q_k$

status predication error covariance matrix while representing not introduce fading factor.

Step 3

Measure and upgrade.Utilize the step status predication value and the error covariance matrix that in step 2, calculate to adopt the sampling policy in step 1 to calculate Sigma point, by speed, position measurement function the h () propagation of GPS be

that is:

${\mathrm{\&zeta;}}_{i,k+1|k}^{\left(l\right)}=h\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)+r_{k+1}$

R _k+1for k+1 observation noise is calculated the prediction of output of not introducing fading factor

${\hat{z}}_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^m{\mathrm{\&zeta;}}_{i,k+1|k}^{\left(l\right)}$

Calculate the autocovariance of not introducing fading factor

$P_{{\tilde{z}}_{k+1}}^{\left(l\right)}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^{\left(c\right)}({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}-{\hat{z}}_{k+1|k}){({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}-{\hat{z}}_{k+1|k})}^T+{\mathrm{<figure-callout\; id="1"\; label="R\; k\; +"\; filenames="HDA0000468760650000023.png,HDA0000468760650000024.png"\; state="\{\{state\}\}">R</figure-callout>}}_{k+}$

R _k+1be observation noise matrix, calculate the cross covariance of not introducing fading factor

$P_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}^{\left(l\right)}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^{\left(c\right)}({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}-{\hat{x}}_{k+1|k}){({\mathrm{\&zeta;}}_{i,k+1|k}^{\left(l\right)}-{\hat{z}}_{k+1|k})}^T$

Step 4:

Calculate fading factor λ _k+1, the method that adopts Zhou Donghua to propose.If theoretical output residual sequence is

In reality

${\mathrm{\&lambda;}}_{k+1}=\left\{\begin{array}{c}{\mathrm{\&lambda;}}_0,{\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1,{\mathrm{\&lambda;}}_0<1\end{array}\right.,{\mathrm{\&lambda;}}_0=\mathrm{tr}\left[N_{k+1}\right]/\mathrm{tr}\left[M_{k+1}\right]$

Wherein N _k+1, M _k+1by V _k+1calculate V in reality _k+1for the covariance matrix of reality output residual sequence, in reality, be unknown,

$V_{k+1}=\left\{\begin{array}{c}{\mathrm{\&epsiv;}}_1{\mathrm{\&epsiv;}}_1^T,k=1;\\ \frac{\mathrm{\&rho;}{V}_k+{\mathrm{\&epsiv;}}_{k+1}{\mathrm{\&epsiv;}}_{k+1}^{\mathrm{<figure-callout\; id="1"\; label="T"\; filenames="HDA0000468760650000023.png,HDA0000468760650000024.png"\; state="\{\{state\}\}">T</figure-callout>}}}{1+\mathrm{\&rho;}},k\&GreaterEqual;1\end{array}\right.$

In formula, ρ is forgetting factor, and 0< ρ≤1 is got ρ=0.95 conventionally.

$N_{k+1}=V_{k+1}-R_{k+1}-{\left[P_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}^{\left(l\right)}\right]}^T{\left[P_{k+1|k}^{\left(l\right)}\right]}^{-1}{Q}_k{\left[P_{k+1|k}^{\left(l\right)}\right]}^{-1}{P}_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}^{\left(l\right)}$

$M_{k+1}=P_{{\tilde{z}}_{k+1}}^{\left(l\right)}-V_{k+1}+N_{k+1}$

Step 5:

Try to achieve fading factor λ _k+1after, can try to achieve new status predication error covariance matrix P _k+1|k:

$P_{k+1|k}={\mathrm{\&lambda;}}_{k+1}\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^c[{\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_k]{[{\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_k]}^T+Q_k$

Q _kfor system noise matrix, adopt the sampling policy in step 1 to calculate Sigma point, propagate as ζ by nonlinear measurement function h () _{i, k+1|k}, again system is measured to renewal.

${\mathrm{\&chi;}}_{i,k+1|k}={\hat{x}}_{k+1|k}+\sqrt{P_{k+1|k}}\mathrm{\&gamma;}$

ζ _i,k+1|k=h(χ _i,k+1|k)+r _k+1

Calculate the prediction of output

${\hat{z}}_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^m{\mathrm{\&zeta;}}_{i,k+1|k}$

Calculate autocovariance

$P_{{\tilde{z}}_{k+1}}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^c({\mathrm{\&chi;}}_{i,k+1|k}-{\hat{z}}_{k+1|k}){({\mathrm{\&chi;}}_{i,k+1|k}-{\hat{z}}_{k+1|k})}^T+{\mathrm{<figure-callout\; id="1"\; label="R\; k\; +"\; filenames="HDA0000468760650000023.png,HDA0000468760650000024.png"\; state="\{\{state\}\}">R</figure-callout>}}_{k+}$

Calculate cross covariance

$P_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^c({\mathrm{\&chi;}}_{i,k+1|k}-{\hat{x}}_{k+1|k}){({\mathrm{\&zeta;}}_{i,k+1}-{\hat{z}}_{k+1|k})}^T$

Step 6:

Filtering is upgraded.Calculate the scale factor K in k+1 moment _k+1, the estimated value in k+1 moment

with evaluated error covariance P _k+1:

$K_{k+1}=P_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}{P}_{{\tilde{z}}_{k+1}}^{-1}$

${\hat{x}}_{k+1}={\hat{x}}_{k+1|k}+K_{k+1}(z_{k+1}-{\hat{z}}_{k+1|k})$

$P_{k+1}=P_{k+1|k}-K_{k+1}{P}_{{\tilde{z}}_{k+1}}{K}_{k+1}^T$

Return to step 1, carry out the estimation in next moment.

For the estimated accuracy of explanation algorithm when the stable state, the validity of evaluation algorithms, the present invention adopts root-mean-square error (root-mean-square error, RMSE) to describe the quality of estimation.

Table 1

Above data are to be resolved and drawn by processing MEMS/GPS integrated navigation system raw data, can find out, TUKF is introduced strong tracking filter by ST-TUKF, in the estimation of speed, position, attitude, reduce its root-mean-square error amount, compare directly UKF is introduced to wave filter, solve its problem producing due to non local sampling in High Dimensional Systems, for low precision inertial navigation system provides position, speed and attitude information more accurately.Expanded the range of application of strong tracking filter simultaneously.Can find out to Fig. 8 at Fig. 3, in the situation that having dynamic disturbances, compare traditional UKF, ST-TUKF also can well follow the tracks of system, and after disturbance finishes rapid regression system state.

Claims (1)

1. a filtering method of the strong tracking UKF based on transformed samples point, is characterized in that:

Step 1:

According to transformed samples strategy, determine number, position and the corresponding weight value of sampled point,

for one-step prediction estimated value, the P of the state in k moment _kfor maintaining system, estimation variance, n there is 2n sampled point, sampled point χ _i,kbe expressed as follows

${\mathrm{\&chi;}}_{i,k}={\hat{x}}_k+\sqrt{P_k}\mathrm{\&gamma;}$

γ=(γ ₁, γ ₂... γ _2n), wherein γ _k=(γ _{k, 1}, γ _{k, 2}... γ _k,n) ^t, k=1,2 ... 2n

${\mathrm{\&gamma;}}_{k,2r-1}=\sqrt{2}\cos ((2r-1)\mathrm{k\&pi;}/n)$

${\mathrm{\&gamma;}}_{k,2r}=\sqrt{2}\sin ((2r-1)\mathrm{k\&pi;}/n)$

R=1,2 ..., [n/2], [n/2] represents to be not more than the maximum integer of n/2, if n is odd number,

corresponding weight value is specifically expressed as follows:

$\left\{\begin{array}{c}{W}_0=\mathrm{\&kappa;}/(n+\mathrm{\&kappa;})\\ W_i=1/2(n+\mathrm{\&kappa;})\\ W_{i+n}=1/2(n+\mathrm{\&kappa;})\end{array}\right.,i=1,\&CenterDot;\&CenterDot;\&CenterDot;,n$

W _irepresent the corresponding weight value of i point, i=0,1 ..., 2n, and weights meet

κ is a scale factor, selects κ=3-n;

Step 2:

Time upgrades, according to the selected sampled point χ of step 1 _i,k, propagate as ξ by nonlinear state function f _{i, k+1|k}, and then obtain the step status predication value after weighting

$\left\{\begin{array}{c}{\mathrm{\&xi;}}_{i,k+1|k}=f[{\mathrm{\&chi;}}_{i,k},u_k]+q_k,i=\mathrm{0,1},\&CenterDot;\&CenterDot;\&CenterDot;,2n\\ {\hat{x}}_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^{\left(m\right)}{\mathrm{\&xi;}}_{i,k+1|k}\end{array}\right.$

$P_{k+1|k}^{\left(l\right)}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}[{\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_k]{[{\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_k]}^T+Q_k$

status predication error covariance matrix while representing not introduce fading factor;

Step 3:

Measure and upgrade, utilize the step status predication value and the error covariance matrix that in step 2, draw to adopt the sampling policy in step 1 to calculate Sigma point, by speed, position measurement function the h () propagation of GPS be

that is:

${\mathrm{\&zeta;}}_{i,k+1|k}^{\left(l\right)}=h\left({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}\right)+r_{k+1}$

R _k+1for k+1 observation noise is calculated the prediction of output of not introducing fading factor

${\hat{z}}_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^m{\mathrm{\&zeta;}}_{i,k+1|k}^{\left(l\right)}$

Calculate the autocovariance of not introducing fading factor

$P_{{\tilde{z}}_{k+1}}^{\left(l\right)}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^{\left(c\right)}({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}-{\hat{z}}_{k+1|k}){({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}-{\hat{z}}_{k+1|k})}^T+R_{k+1}$

R _k+1be observation noise matrix, calculate the cross covariance of not introducing fading factor

$P_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}^{\left(l\right)}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^{\left(c\right)}({\mathrm{\&chi;}}_{i,k+1|k}^{\left(l\right)}-{\hat{x}}_{k+1|k}){({\mathrm{\&zeta;}}_{i,k+1|k}^{\left(l\right)}-{\hat{z}}_{k+1|k})}^T;$

Step 4:

Calculate fading factor λ _k+1, establish theoretical output residual sequence and be

${\mathrm{\&lambda;}}_{k+1}=\left\{\begin{array}{c}{\mathrm{\&lambda;}}_0,{\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1,{\mathrm{\&lambda;}}_0<1\end{array}\right.,{\mathrm{\&lambda;}}_0=\mathrm{tr}\left[N_{k+1}\right]/\mathrm{tr}\left[M_{k+1}\right]$

Wherein N _k+1, M _k+1by V _k+1calculate V _k+1for the covariance matrix of reality output residual sequence, be unknown,

$V_{k+1}=\left\{\begin{array}{c}{\mathrm{\&epsiv;}}_1{\mathrm{\&epsiv;}}_1^T,k=1;\\ \frac{\mathrm{\&rho;}{V}_k+{\mathrm{\&epsiv;}}_{k+1}{\mathrm{\&epsiv;}}_{k+1}^T}{1+\mathrm{\&rho;}},k\&GreaterEqual;1\end{array}\right.$

In formula, ρ is forgetting factor, and 0< ρ≤1, gets ρ=0.95,

$N_{k+1}=V_{k+1}-R_{k+1}-{\left[P_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}^{\left(l\right)}\right]}^T{\left[P_{k+1|k}^{\left(l\right)}\right]}^{-1}{Q}_k{\left[P_{k+1|k}^{\left(l\right)}\right]}^{-1}{P}_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}^{\left(l\right)}$

$M_{k+1}=P_{{\tilde{z}}_{k+1}}^{\left(l\right)}-V_{k+1}+N_{k+1};$

Step 5:

Try to achieve fading factor λ _k+1after; Try to achieve new status predication error covariance matrix P _k+1|k:

$P_{k+1|k}={\mathrm{\&lambda;}}_{k+1}\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^c[{\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_k]{[{\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_k]}^T+Q_k$

Q _kfor system noise matrix, adopt the sampling policy in step 1 to calculate Sigma point, propagate as ζ by nonlinear measurement function h () _{i, k+1|k}, again system is measured to renewal,

${\mathrm{\&chi;}}_{i,k+1|k}={\hat{x}}_{k+1|k}+\sqrt{P_{k+1|k}}\mathrm{\&gamma;}$

${\mathrm{\&zeta;}}_{i,k+1|k}=h\left({\mathrm{\&chi;}}_{i,k+1|k}\right)+r_{k+1}$

Calculate the prediction of output

${\hat{z}}_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^m{\mathrm{\&zeta;}}_{i,k+1|k}$

Calculate autocovariance

$P_{{\tilde{z}}_{k+1}}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^c({\mathrm{\&chi;}}_{i,k+1|k}-{\hat{z}}_{k+1|k}){({\mathrm{\&chi;}}_{i,k+1|k}-{\hat{z}}_{k+1|k})}^T+R_{k+1}$

Calculate cross covariance

$P_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W_i}^c({\mathrm{\&chi;}}_{i,k+1|k}-{\hat{x}}_{k+1|k}){({\mathrm{\&zeta;}}_{i,k+1}-{\hat{z}}_{k+1|k})}^T;$

Step 6:

Filtering is upgraded, and calculates the scale factor K in k+1 moment _k+1, the estimated value in k+1 moment

with evaluated error covariance P _k+1:

$K_{k+1}=P_{{\tilde{x}}_{k+1}{\tilde{z}}_{k+1}}{P}_{{\tilde{z}}_{k+1}}^{-1}$

${\hat{x}}_{k+1}={\hat{x}}_{k+1|k}+K_{k+1}(z_{k+1}-{\hat{z}}_{k+1|k})$

$P_{k+1}=P_{k+1|k}-K_{k+1}{P}_{{\tilde{z}}_{k+1}}{K}_{k+1}^T$

Return to step 1, carry out the estimation in next moment.

Images