CN104913781A - Unequal interval federated filter method based on dynamic information distribution - Google Patents

Abstract

The invention discloses an unequal interval federated filter method based on dynamic information distribution, belonging to the technical field of aircraft integrated navigation. The method comprises the following steps: firstly, establishing a fused reset mode federated filter of an aviation vehicle-loaded inertial navigation system and other navigation systems, then constructing an unequal interval federated filter by the combination of the different characteristics of data updating frequency of a practical sensor, and finally, establishing the dynamic information distribution-based unequal interval federated filter based on the number of effective local sub filters, a system covariance matrix, and the information distribution factors of a dynamic observable degree calculating filter. According to the method, under the condition of hardly increasing the extra calculating burden, the sub filters are reset according the number of the effective local sub filters, the system covariance matrix and the information distribution factors of the dynamic observable degree calculating filter, the information of different measuring periods is optimally fused, the information distribution efficiency and information distribution performance of the federated filter calculation method can be improved, and the method has a certain practical value.

Description

A kind of unequal interval federated filter method of distributing based on multidate information

Technical field

The present invention discloses a kind of unequal interval federated filter method of distributing based on multidate information, belongs to integrated navigation technology field.

Background technology

In the commonwealth filter technique of integrated navigation system, the computation period of local filter and senior filter is all generally fixing.But the Data Update frequency on the one hand due to various assisting navigation equipment is different, cause the measuring period of each local filter different, likely synchronously can not provide partial estimation to senior filter, this causes the unequal interval problem of filtering.General treating method carries out filtering again after adopting the method for extrapolation interpolation that unequal interval sample is changed at equal intervals, but this method destroys the primitiveness of information, the sample estimated value sometime obtained also is not equal to this moment possible measuring value, thus introduce personal error, affect filtering accuracy, and increase computation burden.

Under Chinese scholars proposition innovative approach has been included in reconfiguration structure, prove its unequal interval algorithm and concentrated Kalman Algorithm equivalence; Subfilter is sampled simultaneously and is observed target with identical sampling rate; Extrapolation interpolation shifts each subfilter moment the various methods such as onto senior filter time of fusion, but restriction too much limits practical application.Be not difficult by analysis to find, existing related conclusions, also needs to carry out more deep research.

Summary of the invention

Goal of the invention: for above-mentioned prior art, a kind of unequal interval federated filter method of distributing based on multidate information is proposed, improve unequal interval measurement information fusion accuracy, be suitable for the realization of the INS/CNS/SAR/TER combination unequal interval federated filter that high-altitude long-endurance unmanned plane uses.

Technical scheme: a kind of unequal interval federated filter method of distributing based on multidate information, comprises the following steps:

Step (1), choose sky, northeast geographic coordinate system, INS errors quantity of state is defined as:

$X={[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δv}}_E,{\mathrm{\δv}}_N,{\mathrm{\δv}}_U,\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\mathrm{\ϵ}}_{\mathrm{rx}},{\mathrm{\ϵ}}_{\mathrm{ry}},{\mathrm{\ϵ}}_{\mathrm{rz}},{\▿}_x,{\▿}_y,{\▿}_z]}^T---\left(25\right)$

Formula (1) φ _e, φ _n, φ _ueast orientation platform error angle quantity of state, north orientation platform error angle quantity of state and sky respectively in expression INS errors quantity of state are to platform error angle quantity of state; δ v _e, δ v _n, δ v _ueast orientation velocity error quantity of state, north orientation velocity error quantity of state and sky respectively in expression INS errors quantity of state are to velocity error quantity of state; δ L, δ λ, δ h represent latitude error quantity of state, longitude error quantity of state and height error quantity of state in airborne INS errors quantity of state respectively; ε _bx, ε _by, ε _bz, ε _rx, ε _ry, ε _rzrepresent X-axis, Y-axis, Z-direction gyroscope constant value drift error state amount and X-axis in INS errors quantity of state, Y-axis, Z-direction gyro first order Markov drift error quantity of state respectively; represent X-axis, Y-axis and the Z-direction accelerometer bias in INS errors quantity of state respectively, subscript T is transposition;

Step (2), the measurement equation of each subsystem under setting up Department of Geography, comprises INS/CNS attitude measurement equation, INS/SAR images match horizontal level measurement equation, INS/TER horizontal level measurement equation;

Step (3), build unequal interval and measure federated filter subfilter, the fusion cycle of integrated navigation system is set to consistent with computation period, KF filtering is carried out to the sub-system error quantity of state in each subsystem measurement equation described in step (2), the Kalman filtering in the filtering cycle is divided into two information updating processes: the time upgrades and measures and upgrades;

In the computation period having at least 1 sensor to have new measurement information to arrive, it is as follows that unequal interval measures federated filter process:

A1), Kalman filtering is carried out to the subfilter with measurement information;

B1), to not having the subfilter of measurement information only to carry out time renewal;

C1), the effective subfilter information with measurement information merges by senior filter;

In the computation period not having new sensor measurement information to arrive:

A2), any subfilter all utilizes the information of systematic state transfer battle array to carry out time renewal;

B2), senior filter utilizes the information of systematic state transfer battle array to carry out time renewal;

Step (4), according to the eigenwert of the covariance matrix of each navigation subsystem and the conditional number of Observable matrix, calculates dynamic federated filter information sharing scheme, sets up the distribution principle of procedural information between each subfilter of each navigation subsystem;

Step (5), the filter result that Federated Filters sub-system is sent here carries out data fusion, exports global optimum's estimated value.

As preferred version of the present invention, integrated navigation system state equation is such as formula shown in (2):

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(2\right)$

In formula, F (t) represents the one step state transition matrix of INS/CNS/SAR/TER integrated navigation system state equation; G (t) represents the system white noise error matrix of INS/CNS/SAR/TER integrated navigation system state equation; W (t) is the systematic error white noise vector of inertia/satellite combined guidance system state equation;

According to described integrated navigation system state equation, in described step (2), the measurement equation of each subsystem is:

1), described INS/CNS attitude measurement equation is:

$\begin{array}{c}Z{\left(t\right)}_{\mathrm{CNS}}=\left[\begin{array}{c}{\mathrm{\γ}}_{\mathrm{rINS}}-{\mathrm{\γ}}_{\mathrm{rCNS}}\\ {\mathrm{\θ}}_{\mathrm{pINS}}-{\mathrm{\θ}}_{\mathrm{pCNS}}\\ {\mathrm{\ψ}}_{\mathrm{hINS}}-{\mathrm{\ψ}}_{\mathrm{hCNS}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\δ\γ}}_r+Q_{\mathrm{rCNS}}\\ {\mathrm{\δ\θ}}_p+Q_{\mathrm{pCNS}}\\ {\mathrm{\δ\ψ}}_h+Q_{\mathrm{hCNS}}\end{array}\right]\\ =H_a\left(t\right)X\left(t\right)+N_{\mathrm{CNS}}\left(t\right)\end{array}---\left(26\right)$

Wherein, γ _rINS, θ _pINSand ψ _hINSbe respectively the roll angle of inertial navigation system, the angle of pitch and course angle, γ _rCNS, θ _pCNS, ψ _hCNSbe respectively the roll angle of celestial navigation system, the angle of pitch and course angle, δ γ _r, δ θ _pwith δ ψ _hbe respectively the difference of roll angle, the angle of pitch and course angle in inertial navigation system and celestial navigation system, O _rCNS, O _pCNSand O _hCNSfor subtracting each other the error of generation in a small amount, H _at measurement battle array that () is celestial navigation subsystem, N _cNSt measurement noise matrix that () is astronomical subsystem;

2), described INS/SAR images match horizontal level measurement equation is:

$\begin{array}{c}{Z}_{\mathrm{SAR}}\left(t\right)=\left[\begin{array}{c}(L_{\mathrm{INS}}-L_{\mathrm{SAR}})R_M\\ ({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{SAR}})R_N\cos L\end{array}\begin{array}{c}\\ \end{array}\right]=\left[\begin{array}{c}{R}_M\mathrm{\δL}+N_{M^{\mathrm{SAR}}}\\ R_N\cos \mathrm{L\δ\λ}+N_{N^{\mathrm{SAR}}}\end{array}\right]\\ =H_{\mathrm{hp}}\left(t\right)X\left(t\right)+N_{\mathrm{SAR}}\left(t\right)\end{array}---\left(27\right)$

Wherein, L _iNS, λ _iNSbe respectively latitude and the longitude of inertial navigation system, L _sAR, λ _sARbe respectively latitude and the longitude of scene navigational system, R _mfor radius of curvature of the earth meridian circle, R _nradius of curvature of the earth prime vertical, δ L, γ λ are respectively the difference of dimension and longitude in inertial navigation system and scene navigational system, for measurement noise matrix under meridian circle, for measurement noise matrix under prime vertical, H _hpt measurement battle array that () is scene navigation subsystem, N _sARt measurement noise matrix that () is scene subsystem;

3), described INS/TER horizontal level measurement equation is:

$\begin{array}{c}{Z}_{\mathrm{TER}}\left(t\right)=\left[\begin{array}{c}(L_{\mathrm{INS}}-L_{\mathrm{TER}})R_M\\ ({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{TER}})R_N\cos L\end{array}\begin{array}{c}\\ \end{array}\right]=\left[\begin{array}{c}{R}_M\mathrm{\δL}+N_{M^{\mathrm{TER}}}\\ R_N\cos \mathrm{L\δ\λ}+N_{N^{\mathrm{TER}}}\end{array}\right]\\ =H_{\mathrm{hp}}\left(t\right)X\left(t\right)+N_{\mathrm{TER}}\left(t\right)\end{array}---\left(28\right)$

Wherein, L _tER, λ _tERbe respectively latitude and the longitude of topographical navigation system, H _tpt measurement battle array that () is topographical navigation subsystem, N _tERt measurement noise matrix that () is landform subsystem.

As preferred version of the present invention, obtain after the integrated navigation system model in step (2) is carried out discretize:

$\left\{\begin{array}{c}X\left(k\right)=\mathrm{\Φ}(k-1)X(k-1)+\mathrm{\Γ}(k-1)W(k-1)\\ Z\left(k\right)=H\left(k\right)X\left(k\right)+V\left(k\right)\end{array}\right.---\left(29\right)$

Wherein, W (k-1) and V (k) is respectively mutual incoherent system noise and measurement noise, and has:

E{W(k-1)W ^T(k-1)}＝Q(k-1)

(30)

E{V(k)V ^T(k)}＝R(k)

In formula, the state-transition matrix that Φ (k-1) is system, Γ (k-1) is system noise factor matrix, and E{} is the symbol { } being got to average, Q (k-1) is system noise variance matrix, and R (k) is for measuring white noise vector variance matrix;

Suppose that globalstate estimation is in described step (3), the subfilter with measurement information is carried out to the step a1 of Kalman filtering) be specially:

${\hat{X}}_i(k+1/k)=\mathrm{\Φ}(k+1/k){\hat{X}}_i\left(k\right)---\left(31\right)$

P _i(k+1/k)＝Φ(k+1/k)P _i(k)Φ ^T(k+1/k)+Γ(k+1/k)Q _i(k)Γ ^T(k+1/k) (32)

$K_i(k+2)=P_i(k+1/k)H_i^T(k+1){[H_i(k+1)P_i(k+1/k)H_i^T(k+1)+R_i\left(k\right)]}^{-1}---\left(33\right)$

${\hat{X}}_i(k+1)={\hat{X}}_i(k+1/k)+K_i(k+1)[Z_i(k+1)-H_i(k+1){\hat{X}}_i(k+1/k)]---\left(34\right)$

P _i(k+1)＝[I-K _i(k+1)H _i(k+1)]P _i(k+1/k) (35)

Wherein, Z _i(k+1) be the measurement information matrix in the i-th subfilter kth moment, state X _ithe Kalman filtering valuation of (k), utilize calculate to X _i(k+1) one-step prediction, at one-step prediction basis on according to measuring value Z _i(k+1) the calculating valuation calculated, K _i(k+1) be according to making valuation the filter gain that the minimum criterion of mean square deviation battle array is chosen, P _iand P (k+1/k) _i(k+1) being is a prediction respectively and valuation mean squared error matrix;

The described step b1 to not having the subfilter of measurement information only to carry out time renewal) be specially:

${\hat{X}}_i(k+1/k)=\mathrm{\Φ}(k+1/k){\hat{X}}_i\left(k\right)---\left(36\right)$

P _i(k+1/k)＝Φ(k+1/k)P _i(k)Φ ^T(k+1/k)+Γ(k+1/k)Q _i(k)Γ ^T(k+1/k) (37)

The effective subfilter information with measurement information is carried out the step c1 merged by described senior filter) be specially:

$P_f^{-1}(k+1)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_i^{-1}(k+1)---\left(38\right)$

${\hat{X}}_f(k+1)=P_f(k+1)\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_i^{-1}(k+1){\hat{X}}_i(k+1)---\left(39\right)$

Wherein, for the mean squared error matrix of overall estimated state amount;

Described any subfilter all utilizes the information of systematic state transfer battle array to carry out the step a2 of time renewal) be specially:

${\hat{X}}_i(k+1/k)=\mathrm{\Φ}(k+1/k){\hat{X}}_i\left(k\right)---\left(40\right)$

P _i(k+1/k)＝Φ(k+1/k)P _i(k)Φ ^T(k+1/k)+Γ(k+1/k)Q _i(k)Γ ^T(k+1/k) (41)

Described senior filter utilizes the information of systematic state transfer battle array to carry out the step b2 of time renewal) be specially:

${\hat{X}}_f(k+1/k)=\mathrm{\Φ}(k+1/k){\hat{X}}_f\left(k\right)---\left(42\right)$

P _f(k+1/k)＝Φ(k+1/k)P _f(k)Φ ^T(k+1/k)+Γ(k+1/k)Q _f(k)Γ ^T(k+1/k) (43)。

Step 4), according to the eigenwert of the covariance matrix of each navigation subsystem and the conditional number of Observable matrix, calculate dynamic federated filter information sharing scheme, set up the distribution principle of procedural information between each subfilter of each navigation subsystem, concrete steps are:

As preferred version of the present invention, described step (4) is specially:

Definition unequal interval federated filter motion vector form information partition factor is B _i:

$B_i=\frac{1}{2}\×(A_i+{\mathrm{\γ}}_i)---\left(44\right)$

Wherein, A _ifor the partition factor calculated according to the eigenwert of subsystem covariance matrix, γ _ifor the partition factor calculated according to the conditional number of subsystem Observable matrix;

By system covariance matrix P _ican be expressed as by Eigenvalues Decomposition:

$P_i=L_i{\mathrm{\Λ}}_i{L}_i^T---\left(45\right)$

Λ in formula _i=diag{ λ _i1, λ _i2..., λ _in, λ _i1, λ _i2..., λ _infor P _ieigenwert; N is P _ithe exponent number of battle array;

To X _ieach component x _ijindependently carry out information sharing scheme calculating, x _ijrepresent the jth component in i-th local filter state estimation, information sharing scheme is:

$\begin{array}{c}{\mathrm{\α}}_{\mathrm{ij}}=\frac{1/{\mathrm{\λ}}_{\mathrm{ij}}}{1/{\mathrm{\λ}}_{2j}+1/{\mathrm{\λ}}_{2j}+\·\·\·+1/{\mathrm{\λ}}_{\mathrm{Nj}}}\\ i=\mathrm{1,2},\·\·\·,N;j=\mathrm{1,2},\·\·\·,n\end{array}---\left(46\right)$

Then X _icorresponding information sharing scheme is matrix form:

If when certain subfilter current time does not have a measuring value, by the eigenvalue λ of its system covariance matrix ₁, λ ₂..., λ _nbe set to 0;

If the observability matrix of certain time period dynamic system is Q _i, Q _i∈ R ^{p × q}, σ _ifor the Observable matrix Q of subsystems _iconditional number, if certain subfilter of current time does not have measuring value, the conditional number σ of its Observable matrix is set to 0, that is:

σ _i＝conv(Q _i) (25)

If σ _ivalue comparatively large, then corresponding system state variables has good observation, can obtain the estimation of degree of precision; If σ _ivalue less, then corresponding system state variables may occur unusual, falls into unobservable interval;

Then γ _icomputing formula be:

$\begin{array}{c}{\mathrm{\γ}}_i=\left\{\begin{array}{cc}\frac{{\mathrm{\σ}}_i}{{\mathrm{\σ}}_1+{\mathrm{\σ}}_2+\·\·\·{\mathrm{\σ}}_3}& {\mathrm{\σ}}_i\≠0\\ 0& {\mathrm{\σ}}_i=0\end{array}\right.\\ i=\mathrm{1,2},\·\·\·,N\end{array}---\left(26\right)$

Be B according to described unequal interval federated filter motion vector form information partition factor _iduring assigning process information, the procedural information Q of system ^-1(k) and P ^-1k () distributes between each subfilter and senior filter by following information sharing principle:

$\left\{\begin{array}{c}{P}_i^{-1}\left(k\right)=\sqrt{B_i}{P}_g^{-1}\left(k\right)\sqrt{B_i}\\ Q_i^{-1}\left(k\right)=\sqrt{B_{\mathrm{iQ}}}{Q}_g^{-1}\left(k\right)\sqrt{B_{\mathrm{iQ}}}\\ {\hat{X}}_i\left(k\right)={\hat{X}}_g\left(k\right)\end{array}\right.(i=\mathrm{1,2},\·\·\·,N)---\left(48\right)$

In formula, represent the state estimation quantity of information of k moment i subfilter; represent total state estimation quantity of information of k moment Federated Filters; represent the process noise quantity of information of k moment i subfilter, represent total process noise quantity of information of k moment Federated Filters; represent the state estimation of k moment i subfilter; represent the state estimation of k moment Federated Filters; B _irepresent total state estimation quantity of information information sharing scheme; B _iQfor B _iin rear 9 diagonal entries, represent total process noise quantity of information partition factor.

Beneficial effect: the present invention adopts above technical scheme compared with prior art, has following technique effect:

The invention solves the nonsynchronous problem of various kinds of sensors information in unequal interval Federated Filters structure, construct a kind of unequal interval federated filter method of distributing based on multidate information, it has the following advantages:

(1) problem that measuring equipment Data Update frequencies different in integrated navigation process is different is solved, process measurement information is asynchronous, the fusion cycle of system is set to consistent with computation period, in each computation period, if there is measurement information to arrive, then carry out fusion calculation simultaneously, if there is no measurement information, then do not carry out fusion calculation.

(2) unequal interval federated filter is divided into has at least 1 sensor to have new measurement information and do not have sensor to have measurement information two kinds of situations, be convenient to the number of Federated Filters according to subfilter effective in system, the information sharing scheme of dynamic conditioning federated filter, make the local filter with measurement information to distribute the information of obtaining, and do not have the local filter of measurement information to refuse assignment information.

(3) according to the covariance matrix of effective local filter and the information sharing scheme of observability dynamic calculation Federated Filters, because the reflection of system covariance matrix energy real-time follow-up can reflect the Observable characteristic of system in real time to the estimated accuracy of state, the conditional number of mission observability matrix.

(4) inconsistent according to different subfilter precision, and the uncertain situation of effective subfilter number in incoordinate interval filtering, the weight allocation of the redundant subsystems information in the design of unequal interval Federated Filters during information fusion final by dynamic conditioning, the filtering estimated accuracy of wave filter is improved, thus it is all higher than any one subsystem to obtain comprehensive navigation results.

Accompanying drawing explanation

Fig. 1 is the structural drawing of the unequal interval federated filter method based on multidate information distribution of the present invention;

Fig. 2 is INS/CNS/TER/SAR integrated navigation unequal interval federated filter roll angle graph of errors of the present invention;

Fig. 3 is INS/CNS/TER/SAR integrated navigation unequal interval federated filter angle of pitch graph of errors of the present invention;

Fig. 4 is INS/CNS/TER/SAR integrated navigation unequal interval federated filter longitude error curve of the present invention;

Fig. 5 is INS/CNS/TER/SAR integrated navigation unequal interval federated filter latitude error curve of the present invention.

Embodiment

Below in conjunction with accompanying drawing the present invention done and further explain.

As shown in Figure 1, a kind of unequal interval federated filter method of distributing based on multidate information, starts with from the angle of Department of Geography's navigation, and the linearization measurement equation of foundation system state equation and each subsystem, forms filtering subsystem.The cycle is exported different according to the measurement of each sensor, build unequal interval federated filter framework, utilize the covariance matrix of effective local filter number, local filter and the information sharing scheme of observability dynamic calculation Federated Filters, the navigation information of each effective subsystem of optimum fusion, thus complete the step such as information distribution, optimum fusion further, realize the optimal estimation to integrated navigation error state amount, make the navigation accuracy of system be better than the precision of each subsystem.Specific implementation method is as follows:

Step 1), choose sky, northeast geographic coordinate system, adopt linear kalman filter to combine, the state equation of system is the error state amount equation of inertial navigation system, by to the performance of inertial navigation system and the analysis of error source, the error state amount equation that can obtain inertial navigation system is

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(49\right)$

Wherein, the coefficient of regime matrix of F (t) corresponding to inertial navigation system error equation, the white noise error matrix of coefficients of G (t) corresponding to inertial navigation system error equation, the white noise stochastic error vector of W (t) corresponding to inertial navigation system error equation, INS errors quantity of state is defined as:

$X={[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δv}}_E,{\mathrm{\δv}}_N,{\mathrm{\δv}}_U,\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\mathrm{\ϵ}}_{\mathrm{rx}},{\mathrm{\ϵ}}_{\mathrm{ry}},{\mathrm{\ϵ}}_{\mathrm{rz}},{\▿}_x,{\▿}_y,{\▿}_z]}^T---\left(2\right)$

Formula (2) φ _e, φ _n, φ _ueast orientation platform error angle quantity of state, north orientation platform error angle quantity of state and sky respectively in expression INS errors quantity of state are to platform error angle quantity of state; δ v _e, δ v _n, δ v _ueast orientation velocity error quantity of state, north orientation velocity error quantity of state and sky respectively in expression INS errors quantity of state are to velocity error quantity of state; δ L, δ λ, δ h represent latitude error quantity of state, longitude error quantity of state and height error quantity of state in airborne INS errors quantity of state respectively; ε _bx, ε _by, ε _bz, ε _rx, ε _ry, ε _rzrepresent X-axis, Y-axis, Z-direction gyroscope constant value drift error state amount and X-axis in INS errors quantity of state, Y-axis, Z-direction gyro first order Markov drift error quantity of state respectively; represent X-axis, Y-axis and the Z-direction accelerometer bias in INS errors quantity of state respectively, subscript T is transposition;

Step 2), adopt Department of Geography's upper/lower positions, speed, attitude linearization Observation principle, according to the different operating characteristic of navigation sensor each in federated filter system as landform, scene and astronomy, the measurement equation of each subsystem under setting up Department of Geography, comprises INS/CNS attitude measurement equation, INS/SAR images match horizontal level measurement equation, INS/TER horizontal level measurement equation; Wherein:

1), CNS adopts attitude type celestial navigation system, and the difference of roll angle, the angle of pitch and course angle that the 3 d pose information that the measurement information in measurement equation is exported by INS exports with astronomy is respectively formed, and described INS/CNS attitude measurement equation is:

$\begin{array}{c}Z{\left(t\right)}_{\mathrm{CNS}}=\left[\begin{array}{c}{\mathrm{\γ}}_{\mathrm{rINS}}-{\mathrm{\γ}}_{\mathrm{rCNS}}\\ {\mathrm{\θ}}_{\mathrm{pINS}}-{\mathrm{\θ}}_{\mathrm{pCNS}}\\ {\mathrm{\ψ}}_{\mathrm{hINS}}-{\mathrm{\ψ}}_{\mathrm{hCNS}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\δ\γ}}_r+Q_{\mathrm{rCNS}}\\ {\mathrm{\δ\θ}}_p+Q_{\mathrm{pCNS}}\\ {\mathrm{\δ\ψ}}_h+Q_{\mathrm{hCNS}}\end{array}\right]\\ =H_a\left(t\right)X\left(t\right)+N_{\mathrm{CNS}}\left(t\right)\end{array}---\left(50\right)$

Wherein, γ _rINS, θ _pINSand ψ _hINSbe respectively the roll angle of inertial navigation system, the angle of pitch and course angle, γ _rCNS, θ _pCNS, ψ _hCNSbe respectively the roll angle of celestial navigation system, the angle of pitch and course angle, δ γ _r, δ θ _pwith δ ψ _hbe respectively the difference of roll angle, the angle of pitch and course angle in inertial navigation system and celestial navigation system, O _rCNS, O _pCNSand O _hCNSfor subtracting each other the error of generation in a small amount, H _at measurement battle array that () is celestial navigation subsystem, N _cNSt measurement noise matrix that () is astronomical subsystem;

2), the latitude that measurement information is exported by INS and SAR image Matching Navigation System, longitude difference are formed, and described INS/SAR images match horizontal level measurement equation is:

$\begin{array}{c}{Z}_{\mathrm{SAR}}\left(t\right)=\left[\begin{array}{c}(L_{\mathrm{INS}}-L_{\mathrm{SAR}})R_M\\ ({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{SAR}})R_N\cos L\end{array}\begin{array}{c}\\ \end{array}\right]=\left[\begin{array}{c}{R}_M\mathrm{\δL}+N_{M^{\mathrm{SAR}}}\\ R_N\cos \mathrm{L\δ\λ}+N_{N^{\mathrm{SAR}}}\end{array}\right]\\ =H_{\mathrm{hp}}\left(t\right)X\left(t\right)+N_{\mathrm{SAR}}\left(t\right)\end{array}---\left(51\right)$

Wherein, L _iNS, λ _iNSbe respectively latitude and the longitude of inertial navigation system, L _sAR, λ _sARbe respectively latitude and the longitude of scene navigational system, R _mfor radius of curvature of the earth meridian circle, R _nradius of curvature of the earth prime vertical, δ L, δ λ are respectively the difference of dimension and longitude in inertial navigation system and scene navigational system, for measurement noise matrix under meridian circle, for measurement noise matrix under prime vertical, H _hpt measurement battle array that () is scene navigation subsystem, N _sARt measurement noise matrix that () is scene subsystem;

3), the latitude that measurement information is exported by INS and TER Terrain Contour Matching navigation system, longitude difference are formed, and described INS/TER horizontal level measurement equation is:

$\begin{array}{c}{Z}_{\mathrm{TER}}\left(t\right)=\left[\begin{array}{c}(L_{\mathrm{INS}}-L_{\mathrm{TER}})R_M\\ ({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{TER}})R_N\cos L\end{array}\begin{array}{c}\\ \end{array}\right]=\left[\begin{array}{c}{R}_M\mathrm{\δL}+N_{M^{\mathrm{TER}}}\\ R_N\cos \mathrm{L\δ\λ}+N_{N^{\mathrm{TER}}}\end{array}\right]\\ =H_{\mathrm{hp}}\left(t\right)X\left(t\right)+N_{\mathrm{TER}}\left(t\right)\end{array}---\left(52\right)$

Wherein, L _tER, λ _tERbe respectively latitude and the longitude of topographical navigation system, H _tpt measurement battle array that () is topographical navigation subsystem, N _tERt measurement noise matrix that () is landform subsystem;

Step 3), build unequal interval and measure federated filter subfilter, the fusion cycle of integrated navigation system is set to consistent with computation period, KF filtering is carried out to the sub-system error quantity of state in each subsystem measurement equation described in step (2), different for different measuring equipment Data Update frequencies, process measurement information is asynchronous; Specifically, be exactly because the filtering cycle of each local filter is different, and then there is the time irreversibility problem between senior filter and each local filter.Adopt the process of unequal interval transformed measurement, design and employing solve unequal interval targetedly and measure federated filter subfilter method.Kalman filtering in the filtering cycle is divided into two information updating processes: the time upgrades and measures and upgrades; Wherein, described unequal interval measurement federated filter subfilter building method is:

Obtain after integrated navigation system model in step (2) is carried out discretize:

$\left\{\begin{array}{c}X\left(k\right)=\mathrm{\Φ}(k-1)X(k-1)+\mathrm{\Γ}(k-1)W(k-1)\\ Z\left(k\right)=H\left(k\right)X\left(k\right)+V\left(k\right)\end{array}\right.---\left(53\right)$

Wherein, W (k-1) and V (k) is respectively mutual incoherent system noise and measurement noise, and has:

E{W(k-1)W ^T(k-1)}＝Q(k-1)

(54)

E{V(k)V ^T(k)}＝R(k)

In formula, the state-transition matrix that Φ (k-1) is system, Γ (k-1) is system noise factor matrix, and E{} is the symbol { } being got to average, Q (k-1) is system noise variance matrix, and R (k) is for measuring white noise vector variance matrix;

Suppose that globalstate estimation is in the computation period having at least 1 sensor to have new measurement information to arrive, it is as follows that unequal interval measures federated filter process:

A1), carry out Kalman filtering to the subfilter with measurement information, process is as follows:

${\hat{X}}_i(k+1/k)=\mathrm{\Φ}(k+1/k){\hat{X}}_i\left(k\right)---\left(55\right)$

P _i(k+1/k)＝Φ(k+1/k)P _i(k)Φ ^T(k+1/k)+Γ(k+1/k)Q _i(k)Γ ^T(k+1/k) (56)

$K_i(k+2)=P_i(k+1/k)H_i^T(k+1){[H_i(k+1)P_i(k+1/k)H_i^T(k+1)+R_i\left(k\right)]}^{-1}---\left(57\right)$

${\hat{X}}_i(k+1)={\hat{X}}_i(k+1/k)+K_i(k+1)[Z_i(k+1)-H_i(k+1){\hat{X}}_i(k+1/k)]---\left(58\right)$

P _i(k+1)＝[I-K _i(k+1)H _i(k+1)]P _i(k+1/k) (59)

Wherein, Z _i(k+1) be the measurement information matrix in the i-th subfilter kth moment, state X _ithe Kalman filtering valuation of (k), utilize calculate to X _i(k+1) one-step prediction, at one-step prediction basis on according to measuring value Z _i(k+1) the calculating valuation calculated, K _i(k+1) be according to making valuation the filter gain that the minimum criterion of mean square deviation battle array is chosen, P _iand P (k+1/k) _i(k+1) being is a prediction respectively and valuation mean squared error matrix;

B1), to not having the subfilter of measurement information only to carry out time renewal, process is as follows:

${\hat{X}}_i(k+1/k)=\mathrm{\Φ}(k+1/k){\hat{X}}_i\left(k\right)---\left(60\right)$

P _i(k+1/k)＝Φ(k+1/k)P _i(k)Φ ^T(k+1/k)+Γ(k+1/k)Q _i(k)Γ ^T(k+1/k) (61)

C1), the effective subfilter information with measurement information merges by senior filter:

$P_f^{-1}(k+1)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_i^{-1}(k+1)---\left(62\right)$

${\hat{X}}_f(k+1)=P_f(k+1)\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_i^{-1}(k+1){\hat{X}}_i(k+1)---\left(63\right)$

Wherein, for the mean squared error matrix of overall estimated state amount;

In the computation period not having new sensor measurement information to arrive:

A2), any subfilter all utilizes the information of systematic state transfer battle array to carry out time renewal:

${\hat{X}}_i(k+1/k)=\mathrm{\Φ}(k+1/k){\hat{X}}_i\left(k\right)---\left(64\right)$

P _i(k+1/k)＝Φ(k+1/k)P _i(k)Φ ^T(k+1/k)+Γ(k+1/k)Q _i(k)Γ ^T(k+1/k) (65)

B2), senior filter utilizes the information of systematic state transfer battle array to carry out time renewal:

${\hat{X}}_f(k+1/k)=\mathrm{\Φ}(k+1/k){\hat{X}}_f\left(k\right)---\left(66\right)$

P _f(k+1/k)＝Φ(k+1/k)P _f(k)Φ ^T(k+1/k)+Γ(k+1/k)Q _f(k)Γ ^T(k+1/k) (67)

Step 4), according to the eigenwert of the covariance matrix of each navigation subsystem and the conditional number of Observable matrix, calculate dynamic federated filter information sharing scheme, set up the distribution principle of procedural information between each subfilter of each navigation subsystem, consider the estimated accuracy of system covariance matrix energy real-time follow-up reflection to state, the conditional number of mission observability matrix can reflect the Observable characteristic of system in real time, the eigenwert of emerging system covariance matrix and the conditional number of system Observable matrix, in incoordinate interval filtering, the subfilter number in each moment is uncertain, therefore, the dynamic assignment of information is carried out according to subfilter number effective in system, concrete steps are:

Definition unequal interval federated filter motion vector form information partition factor is B _i:

$B_i=\frac{1}{2}\×(A_i+{\mathrm{\γ}}_i)---\left(68\right)$

Wherein, A _ifor the partition factor calculated according to the eigenwert of subsystem covariance matrix, γ _ifor the partition factor calculated according to the conditional number of subsystem Observable matrix;

By system covariance matrix P _ican be expressed as by Eigenvalues Decomposition:

$P_i=L_i{\mathrm{\Λ}}_i{L}_i^T---\left(69\right)$

Λ in formula _i=diag{ λ _i1, λ _i2..., λ _in, λ _i1, λ _i2..., λ _infor P _ieigenwert; N is P _ithe exponent number of battle array;

To X _ieach component x _ij(x _ijrepresent the jth component in i-th local filter state estimation) independently carry out information sharing scheme calculating, information sharing scheme is:

$\begin{array}{c}{\mathrm{\α}}_{\mathrm{ij}}=\frac{1/{\mathrm{\λ}}_{\mathrm{ij}}}{1/{\mathrm{\λ}}_{2j}+1/{\mathrm{\λ}}_{2j}+\·\·\·+1/{\mathrm{\λ}}_{\mathrm{Nj}}}\\ i=\mathrm{1,2},\·\·\·,N;j=\mathrm{1,2},\·\·\·,n\end{array}---\left(70\right)$

Then X _icorresponding information sharing scheme is matrix form:

If when certain subfilter current time does not have a measuring value, by the eigenvalue λ of its system covariance matrix ₁, λ ₂..., λ _nbe set to 0;

If the observability matrix of certain time period dynamic system is Q _i, Q _i∈ R ^{p × q}, ρ _ifor the Observable matrix Q of subsystems _iconditional number, if certain subfilter of current time does not have measuring value, the conditional number σ of its Observable matrix is set to 0, that is:

σ _i＝conv(Q _i) (25)

If σ _ivalue comparatively large, then corresponding system state variables has good observation, can obtain the estimation of degree of precision; If σ _ivalue less, then corresponding system state variables may occur unusual, falls into unobservable interval;

Then γ _icomputing formula be:

$\begin{array}{c}{\mathrm{\γ}}_i=\left\{\begin{array}{cc}\frac{{\mathrm{\σ}}_i}{{\mathrm{\σ}}_1+{\mathrm{\σ}}_2+\·\·\·{\mathrm{\σ}}_3}& {\mathrm{\σ}}_i\≠0\\ 0& {\mathrm{\σ}}_i=0\end{array}\right.\\ i=\mathrm{1,2},\·\·\·,N\end{array}---\left(26\right)$

Be B according to described unequal interval federated filter motion vector form information partition factor _iduring assigning process information, the procedural information Q of system ^-1(k) and P ^-1k () distributes between each subfilter and senior filter by following information sharing principle:

$\left\{\begin{array}{c}{P}_i^{-1}\left(k\right)=\sqrt{B_i}{P}_g^{-1}\left(k\right)\sqrt{B_i}\\ Q_i^{-1}\left(k\right)=\sqrt{B_{\mathrm{iQ}}}{Q}_g^{-1}\left(k\right)\sqrt{B_{\mathrm{iQ}}}\\ {\hat{X}}_i\left(k\right)={\hat{X}}_g\left(k\right)\end{array}\right.(i=\mathrm{1,2},\·\·\·,N)---\left(72\right)$

In formula, represent the state estimation quantity of information of k moment i subfilter; represent total state estimation quantity of information of k moment Federated Filters; represent the process noise quantity of information of k moment i subfilter, represent total process noise quantity of information of k moment Federated Filters; represent the state estimation of k moment i subfilter; represent the state estimation of k moment Federated Filters; B _irepresent total state estimation quantity of information information sharing scheme; B _iQfor B _iin rear 9 diagonal entries, represent total process noise quantity of information partition factor;

Step 5), the filter result that Federated Filters sub-system is sent here carries out data fusion, exports global optimum's estimated value.

In the present embodiment, the attitude angle error in measurement arranging CNS output is 6 rads, and the horizontal level error in measurement of TER and SAR is respectively 100 meters and 10 meters; The data of CNS, TER, SAR subsystem measure output gap and are respectively 2 seconds, 3 seconds and 4 seconds, by the combination of CNS, TER, SAR tri-subsystem whole process.Because the output frequency of inertial sensor is higher, be generally 50Hz, can arrange unequal interval federated filter update cycle time and discrete periodic is 1 second, the information fusion cycle is also 1 second.

The simulation result of Fig. 2 ~ Fig. 5 shows, within adopting the posture angle error of the method can reach 2 rads, is less than astronomical attitude error 6 rads; Site error can reach within 5 meters, is less than the site error of scene (10 meters) and landform (100 meters).Therefore the unequal interval Federated Filtering based on multidate information distribution invented is effective, improves the efficiency of information distribution, and can ensure the precision of system, have certain practical value.

The above is only the preferred embodiment of the present invention; it should be pointed out that for those skilled in the art, under the premise without departing from the principles of the invention; can also make some improvements and modifications, these improvements and modifications also should be considered as protection scope of the present invention.

Claims (4)

1., based on the unequal interval federated filter method that multidate information distributes, it is characterized in that, comprise the following steps:

Step (1), choose sky, northeast geographic coordinate system, INS errors quantity of state is defined as:

Formula (1) φ _e, φ _n, φ _ueast orientation platform error angle quantity of state, north orientation platform error angle quantity of state and sky respectively in expression INS errors quantity of state are to platform error angle quantity of state; δ v _e, δ v _n, δ v _ueast orientation velocity error quantity of state, north orientation velocity error quantity of state and sky respectively in expression INS errors quantity of state are to velocity error quantity of state; δ L, δ λ, δ h represent latitude error quantity of state, longitude error quantity of state and height error quantity of state in airborne INS errors quantity of state respectively; ε _bx, ε _by, ε _bz, ε _rx, ε _ry, ε _rzrepresent X-axis, Y-axis, Z-direction gyroscope constant value drift error state amount and X-axis in INS errors quantity of state, Y-axis, Z-direction gyro first order Markov drift error quantity of state respectively; represent X-axis, Y-axis and the Z-direction accelerometer bias in INS errors quantity of state respectively, subscript T is transposition;

Step (2), the measurement equation of each subsystem under setting up Department of Geography, comprises INS/CNS attitude measurement equation, INS/SAR images match horizontal level measurement equation, INS/TER horizontal level measurement equation;

Step (3), build unequal interval and measure federated filter subfilter, the fusion cycle of integrated navigation system is set to consistent with computation period, KF filtering is carried out to the sub-system error quantity of state in each subsystem measurement equation described in step (2), the Kalman filtering in the filtering cycle is divided into two information updating processes: the time upgrades and measures and upgrades;

In the computation period having at least 1 sensor to have new measurement information to arrive, it is as follows that unequal interval measures federated filter process:

A1), Kalman filtering is carried out to the subfilter with measurement information;

B1), to not having the subfilter of measurement information only to carry out time renewal;

C1), the effective subfilter information with measurement information merges by senior filter;

In the computation period not having new sensor measurement information to arrive:

A2), any subfilter all utilizes the information of systematic state transfer battle array to carry out time renewal;

B2), senior filter utilizes the information of systematic state transfer battle array to carry out time renewal;

Step (4), according to the eigenwert of the covariance matrix of each navigation subsystem and the conditional number of Observable matrix, calculates dynamic federated filter information sharing scheme, sets up the distribution principle of procedural information between each subfilter of each navigation subsystem;

Step (5), the filter result that Federated Filters sub-system is sent here carries out data fusion, exports global optimum's estimated value.

2. a kind of unequal interval federated filter method of distributing based on multidate information according to claim 1, is characterized in that: integrated navigation system state equation is such as formula shown in (2):

In formula, F (t) represents the one step state transition matrix of INS/CNS/SAR/TER integrated navigation system state equation; G (t) represents the system white noise error matrix of INS/CNS/SAR/TER integrated navigation system state equation; W (t) is the systematic error white noise vector of inertia/satellite combined guidance system state equation;

According to described integrated navigation system state equation, in described step (2), the measurement equation of each subsystem is:

1), described INS/CNS attitude measurement equation is:

Wherein, γ _rINS, θ _pINSand ψ _hINSbe respectively the roll angle of inertial navigation system, the angle of pitch and course angle, γ _rCNS, θ _pCNS, ψ _hCNSbe respectively the roll angle of celestial navigation system, the angle of pitch and course angle, δ γ _r, δ θ _pwith δ ψ _hbe respectively the difference of roll angle, the angle of pitch and course angle in inertial navigation system and celestial navigation system, O _rCNS, O _pCNSand O _hCNSfor subtracting each other the error of generation in a small amount, H _at measurement battle array that () is celestial navigation subsystem, N _cNSt measurement noise matrix that () is astronomical subsystem;

2), described INS/SAR images match horizontal level measurement equation is:

Wherein, L _iNS, λ _iNSbe respectively latitude and the longitude of inertial navigation system, L _sAR, λ _sARbe respectively latitude and the longitude of scene navigational system, R _mfor radius of curvature of the earth meridian circle, R _nradius of curvature of the earth prime vertical, δ L, δ λ are respectively the difference of dimension and longitude in inertial navigation system and scene navigational system, for measurement noise matrix under meridian circle, for measurement noise matrix under prime vertical, H _hpt measurement battle array that () is scene navigation subsystem, N _sARt measurement noise matrix that () is scene subsystem;

3), described INS/TER horizontal level measurement equation is:

Wherein, L _tER, λ _tERbe respectively latitude and the longitude of topographical navigation system, H _tpt measurement battle array that () is topographical navigation subsystem, N _tERt measurement noise matrix that () is landform subsystem.

3. a kind of unequal interval federated filter method of distributing based on multidate information according to claim 2, is characterized in that: obtain after the integrated navigation system model in step (2) is carried out discretize:

Wherein, W (k-1) and V (k) is respectively mutual incoherent system noise and measurement noise, and has:

E{W(k-1)W ^T(k-1)}＝Q(k-1)

(6)

E{V(k)V ^T(k)}＝R(k)

In formula, the state-transition matrix that Φ (k-1) is system, Γ (k-1) is system noise factor matrix, and E{} is the symbol { } being got to average, Q (k-1) is system noise variance matrix, and R (k) is for measuring white noise vector variance matrix;

Suppose that globalstate estimation is in described step (3), the subfilter with measurement information is carried out to the step a1 of Kalman filtering) be specially:

P _i(k+1/k)＝Φ(k+1/k)P _i(k)Φ ^T(k+1/k)+Γ(k+1/k)Q _i(k)Γ ^T(k+1/k) (8)

P _i(k+1)＝[I-K _i(k+1)H _i(k+1)]P _i(k+1/k) (11)

Wherein, Z _i(k+1) be the measurement information matrix in the i-th subfilter kth moment, state X _ithe Kalman filtering valuation of (k), utilize calculate to X _i(k+1) one-step prediction, at one-step prediction basis on according to measuring value Z _i(k+1) the calculating valuation calculated, K _i(k+1) be according to making valuation the filter gain that the minimum criterion of mean square deviation battle array is chosen, P _iand P (k+1/k) _i(k+1) being is a prediction respectively and valuation mean squared error matrix;

The described step b1 to not having the subfilter of measurement information only to carry out time renewal) be specially:

P _i(k+1/k)＝Φ(k+1/k)P _i(k)Φ ^T(k+1/k)+Γ(k+1/k)Q _i(k)Γ ^T(k+1/k) (13)

The effective subfilter information with measurement information is carried out the step c1 merged by described senior filter) be specially:

Wherein, for the mean squared error matrix of overall estimated state amount;

Described any subfilter all utilizes the information of systematic state transfer battle array to carry out the step a2 of time renewal) be specially:

P _i(k+1/k)＝Φ(k+1/k)P _i(k)Φ ^T(k+1/k)+Γ(k+1/k)Q _i(k)Γ ^T(k+1/k) (17)

Described senior filter utilizes the information of systematic state transfer battle array to carry out the step b2 of time renewal) be specially:

P _f(k+1/k)＝Φ(k+1/k)P _f(k)Φ ^T(k+1/k)+Γ(k+1/k)Q _f(k)Γ ^T(k+1/k) (19)。

Step 4), according to the eigenwert of the covariance matrix of each navigation subsystem and the conditional number of Observable matrix, calculate dynamic federated filter information sharing scheme, set up the distribution principle of procedural information between each subfilter of each navigation subsystem.

4. a kind of unequal interval federated filter method of distributing based on multidate information according to claim 2, is characterized in that: described step (4) is specially:

Definition unequal interval federated filter motion vector form information partition factor is B _i:

Wherein, A _ifor the partition factor calculated according to the eigenwert of subsystem covariance matrix, for the partition factor calculated according to the conditional number of subsystem Observable matrix;

By system covariance matrix P _ican be expressed as by Eigenvalues Decomposition:

Λ in formula _i=diag{ λ _i1, λ _i2..., λ _in, λ _i1, λ _i2..., λ _infor P _ieigenwert; N is P _ithe exponent number of battle array;

To X _ieach component x _ijindependently carry out information sharing scheme calculating, x _ijrepresent the jth component in i-th local filter state estimation, information sharing scheme is:

Then X _icorresponding information sharing scheme is matrix form:

If when certain subfilter current time does not have a measuring value, by the eigenvalue λ of its system covariance matrix ₁, λ ₂..., λ _nbe set to 0;

If the observability matrix of certain time period dynamic system is Q _i, Q _i∈ R ^{p × q}, σ _ifor the Observable matrix Q of subsystems _iconditional number, if certain subfilter of current time does not have measuring value, the conditional number σ of its Observable matrix is set to 0, that is:

σ _i＝conv(Q _i) (25)

If σ _ivalue comparatively large, then corresponding system state variables has good observation, can obtain the estimation of degree of precision; If σ _ivalue less, then corresponding system state variables may occur unusual, falls into unobservable interval;

Then computing formula be:

Be B according to described unequal interval federated filter motion vector form information partition factor _iduring assigning process information, the procedural information Q of system ^-1(k) and P ^-1k () distributes between each subfilter and senior filter by following information sharing principle:

In formula, represent the state estimation quantity of information of k moment i subfilter; represent total state estimation quantity of information of k moment Federated Filters; represent the process noise quantity of information of k moment i subfilter, represent total process noise quantity of information of k moment Federated Filters; represent the state estimation of k moment i subfilter; represent the state estimation of k moment Federated Filters; B _irepresent total state estimation quantity of information information sharing scheme; B _iQfor B _iin rear 9 diagonal entries, represent total process noise quantity of information partition factor.