CN1916567A - Method based on filter of self-adapting closed loop for modifying navigator combined between big dipper double star and strapping inertial guidance - Google Patents

Abstract

A method for revising integrated navigation system of Plough double star/ strap down inertial navigation based on adaptive closed loop H filter includes utilizing inertial measuring component in SINS / BDNS integrated navigation system to measure angular speed signal and linear acceleration signal of carrier, obtaining speed and position of carrier by Plough receiver in utilizing Plough satellite information, carrying out strap down inertial operation by navigation computer according to obtained information to let adaptive closed loop H filter carry out revision on integrated navigation system.

Description

Based on self-adapting closed loop H ∞Wave filter to method that double satellite receiver/the inertial navigation integrated navigation system is revised

One, technical field

The present invention relates to a kind of to the method that double satellite receiver/the inertial navigation integrated navigation system is revised, relate in particular to a kind of in satellite/strap-down inertial combined system, adopt the robust filtering combination to its method of revising.

Two, background technology

Big Dipper navigation [double satellite receiver navigation] system (BeiDou Naigation System is called for short BDNS) is the first generation satellite navigation system that China independently develops, and obtains the position and the speed of carrier by the mode of radio distance-measuring.Strapdown inertial navigation system (Strapdown Inertial Navigation System is called for short SINS) is the device that obtains the carrier navigation information according to the mechanics law in the relative inertness space of newton's proposition.It utilizes inertance elements such as gyroscope, accelerometer to experience the moving body movable information, carries out the navigational parameters such as attitude, speed and position that integral operation obtains movable body by computing machine then.

These two kinds of navigational system respectively have characteristics: the SINS abundant information, and the signal frequency height, working method is hidden, but is subjected to the influence of gyroscopic drift error accumulation, and As time goes on system accuracy can reduce; BDNS measuring accuracy height, the not free accumulative total of positioning error effect, but be subjected to influence such as electric wave blocks, multipath effect, electromagnetic interference (EMI) during work easily and reduced reliability, and signal frequency is lower.Therefore BDNS and SINS have very strong complementarity, and both combinations can obtain a performance and be better than individual integrated navigation system.

Because it is short that triones navigation system builds up the time, add the reason of system own, therefore matured products such as the Big Dipper receiver of present stage development and foreign GPS are compared, problem such as exist the coloured noise of output big, the data trip point is many, and the error model of receiver is perfect not enough.Therefore the BDNS/SINS integrated navigation system then faces the inaccurate difficult problem of Filtering Model as adopting Kalman filtering commonly used, thereby has influenced the final effect of combination.Problem such as inaccurate or modeling difficulty adopts H usually at this multisensor combined system Filtering Model _∞Filtering (sees that ten thousand shake just Zhou Bailing, Ma Yunfeng, " adaptive combined H _∞Wave filter and the application in integrated navigation system thereof ", " Southeast China University's journal ", 2004,34 (5): 623-626; Duan Zhiyong, Yuan's letter, " H _∞The applied research of filtering in the GPS/INS integrated navigation system ", " Nanjing Aero-Space University's journal ", 2000,32 (2): 189-193), but because H _∞The uncertainty that filtering itself is separated (Zhang Ming, the Shi Dinghan, " H of parameter uncertain system _∞Wave filter---based on the criterion and the design proposal of algebraically Riccati inequality ", " control theory and application ", 1999,16 (5): 711-714), often cause its filtering accuracy poor, influenced the filtering result, therefore limited its application in engineering practice.

The present invention is to traditional H _∞Filtering improves, and proposes a kind of new filtering modification method---self-adapting closed loop H _∞Filtering has great importance to the application of promoting the BDNS/SINS integrated navigation system.

Three, summary of the invention

1, goal of the invention: the purpose of this invention is to provide a kind of improved H that utilizes _∞The method of navigation accuracy revised, improves by wave filter to the navigation results of BDNS/SINS integrated navigation system.

2, summary of the invention:

In order to reach above-mentioned goal of the invention, of the present invention based on self-adapting closed loop H _∞Wave filter to the method that double satellite receiver/the inertial navigation integrated navigation system is revised, comprise the following steps:

(1) utilize inertial measurement cluster in the SINS/BDNS integrated navigation system to measure the angular velocity signal and the linear acceleration signal of carrier, export these two kinds of signals again and be transferred to navigational computer, the SINS/BDNS integrated navigation system is double satellite receiver/inertial navigation integrated navigation system;

(2) Big Dipper receiver utilizes big-dipper satellite information, obtains the speed and the position of carrier, outputs to navigational computer again;

(3) signal that obtained according to step (1), (2) of navigational computer carries out strap down inertial navigation and calculates: at first utilize the angular velocity signal of step (1) gained to obtain attitude matrix, and therefrom extract attitude angle and position angle; Utilize the acceleration signal of step (2) gained to obtain speed and the position of carrier in geographic coordinate system then; Export attitude, orientation, speed and the position of carrier at last;

Also comprise the following steps:

(4) set up the combined filter model:

Constitute state vector X with three platform error angles of inertial navigation, three velocity errors, these 9 navigation error amounts the most basic of three site errors, position, speed difference formation measuring value Z with SINS and BDNS obtain filtering dynamic equation group:

In the formula (1), A (t), G (t) are respectively system state matrix and state-noise battle array; V _I(t), V _ρ(t) be system noise vector sum measurement noise vector; H (t) is the system measurements matrix;

System state vector is expressed as:

X＝[φ _E φ _N φ _U δv _E δv _N δv _U δL δλ δh] ^T

The system noise vector representation is:

V _I＝[ω _gx ω _gy ω _gz ω _ax ω _ay ω _az] ^T

The measurement vector representation is:

Z＝[L _B-L _I λ _B-λ _I h _B-h _I v _EB-v _EI v _NB-v _NI v _UB-v _UI] ^T

The measurement matrix representation is:

$H=\left[\begin{array}{ccc}{0}_{3\&times;3}& 0_{3\&times;3}& \mathrm{diag}\left[\begin{array}{ccc}{R}_M& R_{\mathrm{<figure-callout\; id="1"\; label="N\; cos\; L"\; filenames="A20061008614200184.png,A20061008614200192.png"\; state="\{\{state\}\}">N</figure-callout>}}\cos L& 1\end{array}\right]\\ 0_{3\&times;3}& {\mathrm{<figure-callout\; id="3"\; label="I"\; filenames="A20061008614200223.png"\; state="\{\{state\}\}">I</figure-callout>}}_{3\&times;3}& 0_{3\&times;3}\end{array}\right]$

Measurement noise is expressed as:

$V_p={\left[\begin{array}{cccccc}{N}_N& N_E& N_h& {\mathrm{\&epsiv;}}_{v_E}& {\mathrm{\&epsiv;}}_{v_N}& {\mathrm{\&epsiv;}}_{v_U}\end{array}\right]}^T$

φ wherein _E, φ _N, φ _UBe respectively strapdown system mathematical platform error angle; δ v _E, δ v _N, δ v _UVelocity error for inertial navigation; δ L, δ λ, δ h are the site error of inertial navigation; Subscript E, N, U represent sky, northeast coordinate system.ω _Gx, ω _Gy, ω _GzRepresent gyro noise respectively; ω _Ax, ω _Ay, ω _AzExpression accelerometer noise; R _M, R _NBe respectively the radius-of-curvature on ellipsoid meridian circle of living in and the prime vertical; L _B, λ _B, h _BLongitude and latitude height for the output of Big Dipper receiver; L _I, λ _I, h _ILongitude and latitude height for inertial navigation output; v _EB, v _NB, v _UBSpeed for the output of Big Dipper receiver; v _EI, v _NI, v _UISpeed for inertial navigation output; N _E, N _N, N _hPosition noise for the output of Big Dipper receiver; ε _VE, ε _VNAnd ε _VUSpeed noise for Big Dipper receiver.

(5) make up conventional H _∞Wave filter:

After (1) formula discretize, be shown below:

$\left.\begin{array}{c}{\hat{x}}_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{\hat{x}}_{k-1,k-1}\\ P_{k,k-1}=P_{k-1,k-1}^{-1}-{\mathrm{\&gamma;}}^{-2}I\\ {\hat{x}}_{k,k}={\hat{x}}_{k,k-1}+K_k[y_k-C_k{\hat{x}}_{k,k-1}]\\ K_k=[{\mathrm{\&Gamma;}}_k{D}_k^T+{\mathrm{\&Phi;}}_{k,k-1}{P}_{k,k-1}^{-1}{C}_k^T]{[I+C_k{P}_{k,k-1}^{-1}{C}_k^T]}^{-1}\\ P_{k,k}=({\mathrm{\&Phi;}}_{k,k-1}-K_k{C}_k)P_{k,k-1}^{-1}{({\mathrm{\&Phi;}}_{k,k-1}-K_k{C}_k)}^{-1}+({\mathrm{\&Gamma;}}_k-K_k{D}_k){({\mathrm{\&Gamma;}}_k-K_k{D}_k)}^{-1}\end{array}\right\}---\left(2\right)$

Wherein

$\left.\begin{array}{c}{x}_{k+1}={\mathrm{\&Phi;}}_{k+1,k}{x}_k+{\mathrm{\&Gamma;}}_k{w}_k\\ y_k=C_k{x}_k+D_k{w}_k\end{array}\right\}$

${\mathrm{\&Phi;}}_{k+1,k}=\underset{i=0}{\overset{n}{\mathrm{\&Sigma;}}}\frac{A_k^n}{n!}{(t_k-t_{k-1})}^n,{\mathrm{\&Gamma;}}_k=T\underset{i=0}{\overset{n}{\mathrm{\&Sigma;}}}\frac{A_k^{n-1}}{n!}{(t_k-t_{k-1})}^{n-1}{B}_k$

$B_k=\left[\begin{array}{cc}{\mathrm{<figure-callout\; id="0"\; label="G\; k"\; filenames="A20061008614200184.png,A20061008614200191.png"\; state="\{\{state\}\}">G</figure-callout>}}_k& 0_{9\&times;4}\end{array}\right],C_k=H_k,D_k=\left[\begin{array}{cc}{0}_{9\&times;4}& I\end{array}\right],w_k=\left[\begin{array}{c}{V}_{\mathrm{Ik}}\\ V_{\mathrm{\&rho;k}}\end{array}\right]$

The γ here is H _∞The adjusting threshold values of wave filter is got the number greater than 1.The γ value in theory can be infinitely great, but in fact greatly after certain value, system has not just been influenced.

(6) conventional H of improvement step (5) gained _∞Wave filter obtains self-adapting closed loop H _∞Wave filter: in (2), introduce closed loop correction and forgetting factor λ respectively, constitute self-adapting closed loop H _∞Wave filter, as follows:

$\left.\begin{array}{c}{\hat{x}}_{k,k-1}\&equiv;0\\ P_{k,k-1}=P_{k-1,k-1}^{-1}-{\mathrm{\&gamma;}}^{-2}I\\ {\hat{x}}_{k,k}=K_k{y}_k\\ {\hat{x}}_{k,k}^{*}=\mathrm{\&lambda;}{\hat{x}}_{k,k}=\mathrm{\&lambda;}{K}_k{y}_k=\left({\mathrm{\&lambda;K}}_k\right)y_k=K_k^{*}{y}_k\\ K_k=[{\mathrm{\&Gamma;}}_k{D}_k^T+{\mathrm{\&Phi;}}_{k,k-1}{P}_{k,k-1}^{-1}{C}_k^T]{[I+C_k{P}_{k,k-1}^{-1}{C}_k^T]}^{-1}\\ P_{k,k}=({\mathrm{\&Phi;}}_{k,k-1}-K_k{C}_k)P_{k,k-1}^{-1}{({\mathrm{\&Phi;}}_{k,k-1}-K_k{C}_k)}^{-1}({\mathrm{\&Gamma;}}_k-K_k{D}_k){({\mathrm{\&Gamma;}}_k-K_k{D}_k)}^{-1}\end{array}\right\}---\left(3\right)$

Find out thus, after the closed loop correction, the one-step prediction valuation of error Be changed to 0, this is the result that brings of feedback modifiers just, is also showing that error has been lowered to 0 (theoretic, as still to have error in the reality) by after the feedback simultaneously; Regulate K by the forgetting factor λ that introduces in addition _k, strengthened the weight of utilizing of current data, strengthened the susceptibility of wave filter to current data, reduced filtering error; Because the forgetting factor that adds does not influence P _{K, k}Iteration, therefore guaranteed the original robustness of filtering.Self-adapting closed loop H _∞The process of resolving of wave filter as shown in Figure 4.

Forgetting factor λ (or λ _k) utilize filtering system adjacent moment measuring value (y _K-1, y _k) residual error change and calculate, as follows:

${\mathrm{\&lambda;}}_k=(1-\frac{{\left|\right|y_{k-1}\left|\right|}_2}{{\left|\right|y_k\left|\right|}_2})\mathrm{\&sigma;}---\left(4\right)$

Wherein σ adjusts coefficient, plays restriction λ _kPeaked effect; Can obtain the error amount of inertial navigation through above process

Step (4) to step (6) is actually the self-adapting closed loop H that makes up in the inventive method _∞Why the process of wave filter will make up such H _∞Wave filter is to estimate error and eliminate for more accurate.Common H _∞The wave filter advantage is that stability is strong, but low precision by such improvement, can improve filtering accuracy under the prerequisite that does not influence stability.

(7) double satellite receiver/inertial navigation integrated navigation system is revised, obtained revised integrated navigation result, this step process is as follows:

Utilize the feedback modifiers passage, the error that Filtering Estimation is gone out

Go to revise the corresponding parameter of strapdown inertial navitation system (SINS), obtain final integrated navigation result.The correction of its medium velocity, position is fairly simple, and is as follows, directly deducts error and gets final product:

$\left.\begin{array}{c}{v}_E^{\&prime;}=v_E-{\mathrm{\&delta;v}}_E\\ v_N^{\&prime;}=v_N-{\mathrm{\&delta;v}}_N\\ v_U^{\&prime;}=v_U-{\mathrm{\&delta;v}}_U\\ L^{\&prime;}=L-\mathrm{\&delta;L}\\ {\mathrm{\&lambda;}}^{\&prime;}=\mathrm{\&lambda;}-\mathrm{\&delta;\&lambda;}\\ h^{\&prime;}=h-\mathrm{\&delta;h}\end{array}\right\}---\left(5\right)$

V in the formula _E', v _N', v _U' be revised speed, L ', λ ', h ' they are revised position, v _E, v _N, v _U, L, λ, the h correspondence be the speed and the position of strapdown inertial navitation system (SINS).

The correction relative complex of attitude angle: at first with the platform error angle φ that estimates _E, φ _N, φ _UTo original attitude matrix C _n ^cRevise renewal (shown in (6) formula), obtain new attitude matrix C _n ^b, and then from C _n ^bSolve roll angle (γ), the angle of pitch (θ) and course angle (ψ), shown in (7) formula:

$C_n^b=\left[\begin{array}{ccc}1& {\mathrm{\&phi;}}_U& -{\mathrm{\&phi;}}_N\\ -{\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="1"\; label="U"\; filenames="A20061008614200184.png,A20061008614200192.png"\; state="\{\{state\}\}">U</figure-callout>}}& 1& {\mathrm{\&phi;}}_E\\ {\mathrm{\&phi;}}_N& -{\mathrm{\&phi;}}_E& 1\end{array}\right]C_n^c---\left(6\right)$

$\left.\begin{array}{c}\mathrm{\&gamma;}={\mathrm{tg}}^{-1}(-C_n^b\left(\mathrm{1,3}\right)/C_n^b\left(\mathrm{3,3}\right))\\ \mathrm{\&theta;}={\sin }^{-1}\left(C_n^b\left(\mathrm{2,3}\right)\right)\\ \mathrm{\&psi;}={\mathrm{tg}}^{-1}(C_n^b\left(\mathrm{2,1}\right)/C_n^b\left(\mathrm{2,2}\right))\end{array}\right\}---\left(7\right)$

C wherein _n ^b(i, j) element of (i=1,2,3, j=1,2,3) expression attitude matrix.

At last, revised integrated navigation result is comprised that attitude, speed and position output to supervisory system, both finished correction double satellite receiver/inertial navigation integrated navigation system.

3, beneficial effect

Method of the present invention has following advantage: (1) integrated navigation system filtering parameter is provided with simply, and variable parameter has only wave filter threshold values γ and self-adaptation to adjust factor sigma; (2) this method is insensitive to system noise, the filtering system strong robustness; (3) the experiment proved that, this method filtering accuracy height, revised integrated navigation result often is better than the integrated navigation result of conventional Kalman filtering under the equal conditions.

The above beneficial effect of the invention is described as follows:

But generate gyro, the acceleration information of speed, positional information and the IMU of Big Dipper receiver according to the emulation of table 1 flight path parameter, and utilize these data to carry out integrated navigation and resolve.The flight path of emulation such as Fig. 5～shown in Figure 8.

Table 1 simulated flight flight path parameter

Order	State of flight	Time period	Angular speed			Longitudinal acceleration
							(s)	ψ′	θ′	γ′	(m/s 2)
		1.	The sliding race	0～20	0	0						0	4
2.	Pull-up						20～24	0	7.5	0	5
		3.	Climb	24～64	0	0						0	5
4.	Level off						64～68	0	-7.5	0	0
		5.	Flat flying	68～668	0	0						0	0
6.	Roll						668～671	0	0	10	0
		7.	Turn	671～731	1.5	0						0	0
8.	Level off						731～734	0	0	-10	0
		9.	Flat flying	731～1334	0	0						10	0
10.	Pull-up						1334～1342	0	7.5	0	2
		11.	Climb	1342～1374	0	0						0	2
12.	Level off						1374～1382	0	-7.5	0	0
		13.	Flat flying	1382～1862	0	0						0	0
14.	Slow down						1862～1902	0	0	0	-2.5
		15.	Roll	1902～1905	0	0						10	0
16.	Turn						1905～1965	1.5	0	0	0
		17.	Level off	1965～1968	0	0						-10	0
18.	Flat flying						1968～2568	0	0	0	0
		19.	Bow	2568～2574	0	-7.5						0	0
20.	Dive						2574～2594	0	0	0	0
		21.	Level off	2594～2600	0	7.5						0	0
22.	Roll						2600～2603	0	0	10	0
		23.	Turn	2603～2663	1.5	0						0	0
24.	Level off						2663～2666	0	0	-10	0
		25.	Flat flying	2666～3600	0	0						0	0

Here in order to prove absolutely the correcting action of the inventive method, also adopt open loop Kalman filtering, conventional H respectively to navigation results _∞Filtering (is open loop H _∞Filtering) and not be with adaptive closed loop H _∞Filtering is carried out error correction to the Big Dipper/SINS integrated navigation, and the integrated navigation error that obtains is as follows respectively:

A) the integrated navigation graph of errors under the open loop Kalman filtering as shown in Figure 9;

B) conventional H _∞Filtering (open loop H _∞Filtering) the integrated navigation graph of errors under as shown in figure 10;

C) closed loop H _∞Integrated navigation graph of errors under the filtering as shown in figure 11;

D) have the closed loop H of forgetting factor (adjust parameter σ=2) _∞Integrated navigation graph of errors under the filtering as shown in figure 12;

At first contrast a), b) result under two kinds of filtering corrections, as can be seen:

1. open loop H _∞The attitude of filtering, speed and site error are significantly less than open loop Kalman filtering.

2. open loop H _∞It is stable that the site error of filtering keeps, and the site error of open loop Kalman filtering is dispersed.The reason that causes open loop Kalman filtering divergence is that system noise is coloured noise rather than white noise.Can contrast H thus _∞The advantage of filtering promptly has good robustness to system noise.

Divided ring H _∞Filtering (Fig. 9), closed loop H _∞The situation of change of filtering accuracy (1 σ) is analyzed between filtering (Figure 10) and modification method of the present invention (Figure 11) three, and the filtering accuracy statistics is as shown in table 2, and can clearly find out from table: the navigation accuracy under the inventive method is the highest, conventional H _∞Filtering (open loop H _∞Filtering) precision is the poorest.Thereby verified the validity of the inventive method.

Table 2 Big Dipper/SINS position and speed is combined in different H _∞Navigation accuracy statistics under the filtering correcting mode

H ∞Filtering	Attitude angle precision (')			Velocity accuracy (m/s)			Positional precision (m)
										Roll	Pitching	The course	East orientation	North orientation	It to	Longitude	Latitude	Highly
	Open loop	3.43	5.55	2.18	2.33	3.18	3.88	4.67	7.5										49.01
Closed loop										3.19	4.89	2.09	1.06	1.30	1.20	1.94	3.95	10.18
	Self-adapting closed loop	2.68	3.68	2.05	0.99	1.25	1.02	1.80	2.55										13.87

Comprehensive above the analysis as can be seen, the inventive method can effectively be revised the error of the Big Dipper/inertial navigation integrated navigation system, improves the overall performance of system.

Four, description of drawings

Fig. 1 Big Dipper/inertial navigation integrated navigation system hardware block diagram;

Fig. 2 integrated navigation system algorithm block diagram;

Fig. 3 strapdown inertial navigation system schematic diagram;

Fig. 4 self-adapting closed loop H of the present invention _∞The filtering process flow diagram;

Fig. 5 emulation track synoptic diagram;

Fig. 6 emulation track attitude change curve;

Fig. 7 emulation path velocity change curve;

Fig. 8 emulation track position change curve;

Integrated navigation graph of errors under Fig. 9 open loop Kalman filtering correction;

Figure 10 conventional H _∞Integrated navigation graph of errors under the filtering correction;

Figure 11 closed loop H _∞Integrated navigation graph of errors under the filtering correction;

Figure 12 self-adapting closed loop H _∞Integrated navigation graph of errors under the filtering correction;

Figure 13 integrated navigation system sport car route synoptic diagram;

Figure 14 closes/open loop H _∞The latitude error contrast synoptic diagram of filtering integrated navigation;

Figure 15 closes/open loop H _∞The velocity error of integrated navigation contrast synoptic diagram under the filtering;

Figure 16 H _∞Forgetting factor is to the synoptic diagram that influences of navigation error under the correction of filtering closed loop.

Five, embodiment

Embodiment 1:

This example is to adopt adaptive H _∞The preventing test of the Big Dipper of filtering correction/inertial navigation integrated navigation system.The sport car time: the morning on November 19th, 2005; The sport car route: from the southern suburbs in Xi'an, arrive Hu County at a high speed through western family, return initial point along western Feng highway again, last about more than 90 minutes, the sport car route as shown in figure 13.In this example the IMU of strapdown inertial navigation system by in the flexible gyroscope and the quartzy piezoelectric accelerometer of low precision constitute, nominal accuracy is: gyro 0.2 degree/hour, accelerometer 10 ^-4G.

For the correcting action of this method to navigation error effectively is described, write down the raw data of the Big Dipper and IMU during sport car, repeatedly reading file at the dynamic data that just can utilize sport car afterwards like this resolves, algorithms of different is verified on same group of data, thereby guaranteed algorithm rationality relatively.

This example comprises the steps:

(1) utilizes inertial measurement cluster in the SINS/BDNS integrated navigation system to measure the angular velocity signal and the linear acceleration signal of carrier, export these two kinds of signals again and be transferred to navigational computer; BDNS/SINS integrated navigation system hardware configuration comprises inertial measurement cluster (IMU), Big Dipper receiver, navigational computer, control display system as shown in Figure 1;

(2) Big Dipper receiver utilizes big-dipper satellite information, obtains the speed and the position of carrier, outputs to navigational computer again;

(3) signal that obtained according to step (1), (2) of navigational computer carries out strap down inertial navigation and calculates: at first utilize the angular velocity signal of step (1) gained to obtain attitude matrix, and therefrom extract attitude angle and position angle; Utilize the acceleration signal of step (2) gained to obtain speed and the position of carrier in geographic coordinate system then; Export attitude, orientation, speed and the position of carrier at last;

(4) set up the combined filter model: constitute state vector X with three platform error angles of inertial navigation, three velocity errors, these 9 navigation error amounts the most basic of three site errors, position, speed difference with SINS and BDNS constitute measuring value Z, obtain filtering dynamic equation group:

In the formula (1), A (t), G (t) are respectively system state matrix and state-noise battle array; V _I(t), V _ρ(t) be system noise vector sum measurement noise vector; H (t) is the system measurements matrix;

System state vector is expressed as:

X＝[φ _E φ _N φ _U δv _E δv _N δv _U δL δλ δh] ^T

The system noise vector representation is:

V _I＝[ω _gx ω _gy ω _gz ω _ax ω _ay ω _az] ^T

The measurement vector representation is:

Z＝[L _B-L _I λ _B-λ _I h _B-h _I v _EB-v _EI v _NB-v _NI v _UB-v _UI] ^T

The measurement matrix representation is:

$H=\left[\begin{array}{ccc}{0}_{3\&times;3}& 0_{3\&times;3}& \mathrm{diag}\left[\begin{array}{ccc}{R}_M& R_{\mathrm{<figure-callout\; id="1"\; label="N\; cos\; L"\; filenames="A20061008614200184.png,A20061008614200192.png"\; state="\{\{state\}\}">N</figure-callout>}}\cos L& 1\end{array}\right]\\ 0_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;3}\end{array}\right]$

Measurement noise is expressed as:

$V_p={\left[\begin{array}{cccccc}{N}_N& N_E& N_h& {\mathrm{\&epsiv;}}_{v_E}& {\mathrm{\&epsiv;}}_{v_N}& {\mathrm{\&epsiv;}}_{v_U}\end{array}\right]}^T$

φ wherein _E, φ _N, φ _UBe respectively strapdown system mathematical platform error angle; δ v _E, δ v _N, δ v _UVelocity error for inertial navigation; δ L, δ λ, δ h are the site error of inertial navigation; Subscript E, N, U represent sky, northeast coordinate system.ω _Gx, ω _Gy, ω _GzRepresent gyro noise respectively; ω _Ax, ω _Ay, ω _AzExpression accelerometer noise; R _M, R _NBe respectively the radius-of-curvature on ellipsoid meridian circle of living in and the prime vertical; L _B, λ _B, h _BLongitude and latitude height for the output of Big Dipper receiver: L _I, λ _I, h _ILongitude and latitude height for inertial navigation output; v _EB, v _NB, v _UBSpeed for the output of Big Dipper receiver; v _EI, v _NI, v _UISpeed for inertial navigation output; N _E, N _N, N _hPosition noise for the output of Big Dipper receiver; ε _VE, ε _VNAnd ε _VUSpeed noise for Big Dipper receiver.

(5) make up conventional H _∞Wave filter:

After (1) formula discretize, be shown below:

$\left.\begin{array}{c}{\hat{x}}_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{\hat{x}}_{k-1,k-1}\\ P_{k,k-1}=P_{k-1,k-1}^{-1}-{\mathrm{\&gamma;}}^{-2}I\\ {\hat{x}}_{k,k}={\hat{x}}_{k,k-1}+K_k[y_k-C_k{\hat{x}}_{k,k-1}]\\ K_k=[{\mathrm{\&Gamma;}}_k{D}_k^T+{\mathrm{\&Phi;}}_{k,k-1}{P}_{k,k-1}^{-1}{C}_k^T]{[I+C_k{P}_{k,k-1}^{-1}{C}_k^T]}^{-1}\\ P_{k,k}=({\mathrm{\&Phi;}}_{k,k-1}-K_k{C}_k)P_{k,k-1}^{-1}{({\mathrm{\&Phi;}}_{k,k-1}-K_k{C}_k)}^{-1}+({\mathrm{\&Gamma;}}_k-K_k{D}_k){({\mathrm{\&Gamma;}}_k-K_k{D}_k)}^{-1}\end{array}\right\}---\left(2\right)$

Wherein

$\left.\begin{array}{c}{x}_{k+1}={\mathrm{\&Phi;}}_{k+1,k}{x}_k+{\mathrm{\&Gamma;}}_k{w}_k\\ y_k=C_k{x}_k+D_k{w}_k\end{array}\right\}$

${\mathrm{\&Phi;}}_{k+1,k}=\underset{i=0}{\overset{n}{\mathrm{\&Sigma;}}}\frac{A_k^n}{n!}{(t_k-t_{k-1})}^n,{\mathrm{\&Gamma;}}_k=T\underset{i=0}{\overset{n}{\mathrm{\&Sigma;}}}\frac{A_k^{n-1}}{n!}{(t_k-t_{k-1})}^{n-1}{B}_k$

$B_k=\left[\begin{array}{cc}{\mathrm{<figure-callout\; id="0"\; label="G\; k"\; filenames="A20061008614200184.png,A20061008614200191.png"\; state="\{\{state\}\}">G</figure-callout>}}_k& 0_{9\&times;4}\end{array}\right],C_k=H_k,D_k=\left[\begin{array}{cc}{0}_{9\&times;4}& I\end{array}\right],w_k=\left[\begin{array}{c}{V}_{\mathrm{Ik}}\\ V_{\mathrm{\&rho;k}}\end{array}\right]$

The γ here is H _∞The adjusting threshold values of wave filter is got the number greater than 1.The γ value in theory can be infinitely great, but in fact greatly after certain value, system has not just been influenced.

(6) conventional H of improvement step (5) gained _∞Wave filter obtains self-adapting closed loop H _∞Wave filter:

In (2), introduce closed loop correction and forgetting factor λ respectively, constitute self-adapting closed loop H _∞Wave filter, as follows:

$\left.\begin{array}{c}{\hat{x}}_{k,k-1}\&equiv;0\\ P_{k,k-1}=P_{k-1,k-1}^{-1}-{\mathrm{\&gamma;}}^{-2}I\\ {\hat{x}}_{k,k}=K_k{y}_k\\ {\hat{x}}_{k,k}^{*}=\mathrm{\&lambda;}{\hat{x}}_{k,k}=\mathrm{\&lambda;}{K}_k{y}_k=\left({\mathrm{\&lambda;K}}_k\right)y_k=K_k^{*}{y}_k\\ K_k=[{\mathrm{\&Gamma;}}_k{D}_k^T+{\mathrm{\&Phi;}}_{k,k-1}{P}_{k,k-1}^{-1}{C}_k^T]{[I+C_k{P}_{k,k-1}^{-1}{C}_k^T]}^{-1}\\ P_{k,k}=({\mathrm{\&Phi;}}_{k,k-1}-K_k{C}_k)P_{k,k-1}^{-1}{({\mathrm{\&Phi;}}_{k,k-1}-K_k{C}_k)}^{-1}({\mathrm{\&Gamma;}}_k-K_k{D}_k){({\mathrm{\&Gamma;}}_k-K_k{D}_k)}^{-1}\end{array}\right\}---\left(3\right)$

Find out thus, after the closed loop correction, the one-step prediction valuation of error

Be changed to 0, this is the result that brings of feedback modifiers just, is also showing that error has been lowered to 0 (theoretic, as still to have error in the reality) by after the feedback simultaneously; Regulate K by the forgetting factor λ that introduces in addition _k, strengthened the weight of utilizing of current data, strengthened the susceptibility of wave filter to current data, reduced filtering error; Because the forgetting factor that adds does not influence _{Pk, k}Iteration, therefore guaranteed the original robustness of filtering.Self-adapting closed loop H _∞The process of resolving of wave filter as shown in Figure 4.

Forgetting factor λ (or λ _k) utilize filtering system adjacent moment measuring value (y _K-1, y _k) residual error change and calculate, as follows:

${\mathrm{\&lambda;}}_k=(1-\frac{{\left|\right|y_{k-1}\left|\right|}_2}{{\left|\right|y_k\left|\right|}_2})\mathrm{\&sigma;}---\left(4\right)$

Wherein σ adjusts coefficient, plays restriction λ _kPeaked effect; Can obtain the error amount of inertial navigation through above process

(7) obtain revised integrated navigation result:

Utilize the feedback modifiers passage, the error that Filtering Estimation is gone out Go to revise the corresponding parameter of strapdown inertial navitation system (SINS), obtain final integrated navigation result.The correction of its medium velocity, position is fairly simple, and is as follows, directly deducts error and gets final product:

$\left.\begin{array}{c}{v}_E^{\&prime;}=v_E-{\mathrm{\&delta;v}}_E\\ v_N^{\&prime;}=v_N-{\mathrm{\&delta;v}}_N\\ v_U^{\&prime;}=v_U-{\mathrm{\&delta;v}}_U\\ L^{\&prime;}=L-\mathrm{\&delta;L}\\ {\mathrm{\&lambda;}}^{\&prime;}=\mathrm{\&lambda;}-\mathrm{\&delta;\&lambda;}\\ h^{\&prime;}=h-\mathrm{\&delta;h}\end{array}\right\}---\left(5\right)$

V in the formula _E', v _N', v _U' be revised speed, L ', λ ', h ' they are revised position, v _E, v _N, v _U, L, λ, the h correspondence be the speed and the position of strapdown inertial navitation system (SINS).

The correction relative complex of attitude angle: at first with the platform error angle φ that estimates _E, φ _N, φ _UTo original attitude matrix C _n ^cRevise renewal (shown in (6) formula), obtain new attitude matrix C _n ^b, and then from C _n ^bSolve roll angle (γ), the angle of pitch (θ) and course angle (ψ), shown in (7) formula:

$C_n^b=\left[\begin{array}{ccc}1& {\mathrm{\&phi;}}_U& -{\mathrm{\&phi;}}_N\\ -{\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="1"\; label="U"\; filenames="A20061008614200184.png,A20061008614200192.png"\; state="\{\{state\}\}">U</figure-callout>}}& 1& {\mathrm{\&phi;}}_E\\ {\mathrm{\&phi;}}_N& -{\mathrm{\&phi;}}_E& 1\end{array}\right]C_n^c---\left(6\right)$

$\left.\begin{array}{c}\mathrm{\&gamma;}={\mathrm{tg}}^{-1}(-C_n^b\left(\mathrm{1,3}\right)/C_n^b\left(\mathrm{3,3}\right))\\ \mathrm{\&theta;}={\sin }^{-1}\left(C_n^b\left(\mathrm{2,3}\right)\right)\\ \mathrm{\&psi;}={\mathrm{tg}}^{-1}(C_n^b\left(\mathrm{2,1}\right)/C_n^b\left(\mathrm{2,2}\right))\end{array}\right\}---\left(7\right)$

C wherein _n ^b(i, j) element of (i=1,2,3, j=1,2,3) expression attitude matrix.

At last, revised integrated navigation result is comprised that attitude, speed and position output to supervisory system.

Self-adapting closed loop H of the present invention _∞The filtering modification method has adopted closed loop and two kinds of means of self-adaptation to improve the correction precision, therefore respectively this means role is described here.

Open loop and closed loop H _∞To such as Figure 14, shown in Figure 15, the error here is meant the difference of the corresponding output with passive big dipper of integrated navigation result to the correction effect of integrated navigation error in filtering.Can know and find out by contrast: Figure 14 open loop H _∞Latitude error has significantly and disperses-restrain-stabilization process under the filtering, and closed loop H _∞Latitude error curve under the filtering is then very steady, and the contrast situation of longitude error is also like this; The clearer closed loop H that demonstrates of velocity error contrast of Figure 15 _∞The characteristics that filtering error is little.The error statistics of Figure 14～15 correspondences is as shown in table 3, and data show that the inventive method is than conventional H in the table _∞Filtering can better be revised the integrated navigation error.

Table 3 Big Dipper/SINS integrated navigation system sport car experimental error statistics

Integrated navigation filtering type	Speed variance (1 σ)		Position variance (1 σ)
					East orientation (m/s)	North orientation (m/s)	Longitude (10 -5deg)	Latitude (10 -5deg)
	Open loop H ∞Filtering	4.81	4.55	21.79					30.11
Closed loop H ∞Filtering					1.04	1.16	17.25	16.26

Annotate: 10 of longitude and latitude ^-5The position distance of degree approximate representation 1.1m

Forgetting factor is to closed loop H in the present embodiment _∞The curve of filtering accuracy influence as shown in figure 16, some correspondences are once read the emulation of file sport car among the figure, the adjustment coefficient of filtering employing when the x coordinate of point is corresponding sport car emulation.As can be seen from Figure, along with the increase of adjusting factor sigma, i.e. the enhancing of forgetting factor effect, the speed of integrated navigation system, site error all obviously descend, and trend is stable, so σ can choose suitable size according to actual conditions.Figure 16 shows when σ=7, in the site error 5m, in the velocity error 0.88m/s.What be better than obviously as can be seen that table 3 lists does not have a closed loop H under the forgetting factor _∞Filtering accuracy, thus the validity of forgetting factor verified.

Claims (1)

1, a kind of based on self-adapting closed loop H _∞Wave filter to the method that double satellite receiver/the inertial navigation integrated navigation system is revised, comprise the following steps:

(1) utilizes inertial measurement cluster in the SINS/BDNS integrated navigation system to measure the angular velocity signal and the linear acceleration signal of carrier, export these two kinds of signals again and be transferred to navigational computer;

(2) Big Dipper receiver utilizes big-dipper satellite information, obtains the speed and the position of carrier, outputs to navigational computer again;

(3) signal that obtained according to step (1), (2) of navigational computer carries out strap down inertial navigation and calculates: at first utilize the angular velocity signal of step (1) gained to obtain attitude matrix, and therefrom extract attitude angle and position angle; Utilize the acceleration signal of step (2) gained to obtain speed and the position of carrier in geographic coordinate system then; Export attitude, orientation, speed and the position of carrier at last,

It is characterized in that, also comprise the following steps:

(4) set up the combined filter model: constitute state vector X with three platform error angles of inertial navigation, three velocity errors, these 9 navigation error amounts the most basic of three site errors, position, speed difference with SINS and BDNS constitute measuring value Z, obtain filtering dynamic equation group:

In the formula (1), A (t), G (t) are respectively system state matrix and state-noise battle array, V _I(t), V _ρ(t) be system noise vector sum measurement noise vector, H (t) is the system measurements matrix;

System state vector is expressed as:

X＝[φ _E φ _N φ _U δv _E δv _N δv _U δL δλ δh] ^T

The system noise vector representation is:

V _I＝[ω _gx ω _gy ω _gz ω _ax ω _ay ω _az] ^T

The measurement vector representation is:

Z＝[L _B-L _I λ _B-λ _I h _B-h _I v _EB-v _EI v _NB-v _NI v _UB-v _UI] ^T

The measurement matrix representation is:

$H=\left[\begin{array}{ccc}{0}_{3\&times;3}& 0_{3\&times;3}& \mathrm{diag}\left[\begin{array}{ccc}{R}_M& R_N\cos L& 1\end{array}\right]\\ 0_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;3}\end{array}\right]$

Measurement noise is expressed as:

$V_p={\left[\begin{array}{cccccc}{N}_N& N_E& N_h& {\mathrm{\&epsiv;}}_{v_E}& {\mathrm{\&epsiv;}}_{v_N}& {\mathrm{\&epsiv;}}_{v_U}\end{array}\right]}^T$

φ wherein _E, φ _N, φ _UBe respectively strapdown system mathematical platform error angle; δ v _E, δ v _N, δ v _UVelocity error for inertial navigation; δ L, δ λ, δ h are the site error of inertial navigation; Subscript E, N, U represent sky, northeast coordinate system.ω _Gx, ω _Gy, ω _GzRepresent gyro noise respectively; ω _Ax, ω _Ay, ω _AzExpression accelerometer noise; R _M, R _NBe respectively the radius-of-curvature on ellipsoid meridian circle of living in and the prime vertical; I _B, λ _B, h _BLongitude and latitude height for the output of Big Dipper receiver; L _I, λ _I, h _ILongitude and latitude height for inertial navigation output; v _EB, v _NB, v _UBSpeed for the output of Big Dipper receiver; v _EI, v _NI, v _UISpeed for inertial navigation output; N _E, N _N, N _hPosition noise for the output of Big Dipper receiver; ε _VE, ε _VNAnd ε _VUSpeed noise for Big Dipper receiver;

(5) make up conventional H _∞Wave filter:

After (1) formula discretize, be shown below:

$\left.\begin{array}{c}{\hat{x}}_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{\hat{x}}_{k-1,k-1}\\ P_{k,k-1}=P_{k-1,k-1}^{-1}-{\mathrm{\&gamma;}}^{-2}I\\ {\hat{x}}_{k,k}={\hat{x}}_{k,k-1}+K_k[y_k-C_k{\hat{x}}_{k,k-1}]\\ K_k=[{\mathrm{\&Gamma;}}_k{D}_k^T+{\mathrm{\&Phi;}}_{k,k-1}{P}_{k,k-1}^{-1}{C}_k^T]{[I+C_k{P}_{k,k-1}^{-1}{C}_k^T]}^{-1}\\ P_{k,k}=({\mathrm{\&Phi;}}_{k,k-1}-K_k{C}_k)P_{k,k-1}^{-1}{({\mathrm{\&Phi;}}_{k,k-1}-K_k{C}_k)}^{-1}+({\mathrm{\&Gamma;}}_k-K_k{D}_k){({\mathrm{\&Gamma;}}_k-K_k{D}_k)}^{-1}\end{array}\right\}---\left(2\right)$

Wherein

$\left.\begin{array}{c}{x}_{k+1}={\mathrm{\&Phi;}}_{k+1,k}{x}_k+{\mathrm{\&Gamma;}}_k{w}_k\\ y_k=C_k{x}_k+D_k{w}_k\end{array}\right\}$

${\mathrm{\&Phi;}}_{k+1,k}=\underset{i=0}{\overset{n}{\mathrm{\&Sigma;}}}\frac{A_k^n}{n!}{(t_k-t_{k-1})}^n,{\mathrm{\&Gamma;}}_k=T\underset{i=0}{\overset{n}{\mathrm{\&Sigma;}}}\frac{A_k^{n-1}}{n!}{(t_k-t_{k-1})}^{n-1}{B}_k$

B _k＝[G _k 0 _9×4]，C _k＝H _k，D _k＝[0 _9×4 I]， $w_k=\left[\begin{array}{c}{V}_{\mathrm{Ik}}\\ V_{\mathrm{\&rho;k}}\end{array}\right]$

Wherein, γ is H _∞The adjusting threshold values of wave filter is got the number greater than 1;

(6) conventional H of improvement step (5) gained _∞Wave filter obtains self-adapting closed loop H _∞Wave filter:

In formula (2), introduce closed loop correction and forgetting factor λ respectively, constitute self-adapting closed loop H _∞Wave filter, as follows:

$\left.\begin{array}{c}{\hat{x}}_{k,k-1}\&equiv;0\\ P_{k,k-1}=P_{k-1,k-1}^{-1}-{\mathrm{\&gamma;}}^{-2}I\\ {\hat{x}}_{k,k}=K_k{y}_k\\ {\hat{x}}_{k,k}^{*}=\mathrm{\&lambda;}{\hat{x}}_{k,k}=\mathrm{\&lambda;}{K}_k{y}_k=\left(\mathrm{\&lambda;}{K}_k\right)y_k=K_k^{*}{y}_k\\ K_k=[{\mathrm{\&Gamma;}}_k{D}_k^T+{\mathrm{\&Phi;}}_{k,k-1}{P}_{k,k-1}^{-1}{C}_k^T]{[I+C_k{P}_{k,k-1}^{-1}{C}_k^T]}^{-1}\\ P_{k,k}=({\mathrm{\&Phi;}}_{k,k-1}-K_k{C}_k)P_{k,k-1}^{-1}{({\mathrm{\&Phi;}}_{k,k-1}-K_k{C}_k)}^{-1}+({\mathrm{\&Gamma;}}_k-K_k{D}_k){({\mathrm{\&Gamma;}}_k-K_k{D}_k)}^{-1}\end{array}\right\}---\left(3\right)$

Wherein, forgetting factor λ _kUtilize filtering system adjacent moment measuring value (y _K-1, y _k) residual error change and calculate, as follows:

${\mathrm{\&lambda;}}_k=(1-\frac{{\left|\right|y_{k-1}\left|\right|}_2}{{\left|\right|y_k\left|\right|}_2})\mathrm{\&sigma;}---\left(4\right)$

Wherein σ adjusts coefficient;

To the end of this step, can obtain the error amount of inertial navigation

(7) double satellite receiver/inertial navigation integrated navigation system is revised, obtained revised integrated navigation result, this step process is as follows:

Utilize the feedback modifiers passage, the error that Filtering Estimation is gone out Go to revise the corresponding parameter of strapdown inertial navitation system (SINS), obtain final integrated navigation result, wherein, shown in the amendment type of speed, position (5):

$\left.\begin{array}{c}{v}_E^{\&prime;}=v_E-{\mathrm{\&delta;v}}_E\\ v_N^{\&prime;}=v_N-{\mathrm{\&delta;v}}_N\\ v_U^{\&prime;}=v_U-{\mathrm{\&delta;v}}_U\\ L^{\&prime;}=L-\mathrm{\&delta;L}\\ {\mathrm{\&lambda;}}^{\&prime;}=\mathrm{\&lambda;}-\mathrm{\&delta;\&lambda;}\\ h^{\&prime;}=h-\mathrm{\&delta;h}\end{array}\right\}---\left(5\right)$

V ＇ in the following formula _E, v ＇ _N, v ＇ _UBe revised speed, L ', λ ', h ＇ are revised position, v _E, v _N, v _U, L, λ, the h correspondence be the speed and the position of strapdown inertial navitation system (SINS);

Correction to attitude angle is as follows: at first according to formula (6) the platform error angle φ that estimates _E, φ _N, φ _UTo original attitude matrix C _n ^cRevise renewal, obtain new attitude matrix C _n ^b, and then from C _n ^bSolve roll angle (γ), the angle of pitch (θ) and course angle (ψ), shown in (7) formula:

$C_n^b=\left[\begin{array}{ccc}1& {\mathrm{\&phi;}}_U& -{\mathrm{\&phi;}}_N\\ -{\mathrm{\&phi;}}_U& 1& {\mathrm{\&phi;}}_E\\ {\mathrm{\&phi;}}_N& -{\mathrm{\&phi;}}_E& 1\end{array}\right]C_n^c---\left(6\right)$

$\left.\begin{array}{c}\mathrm{\&gamma;}={\mathrm{tg}}^{-1}(-C_n^b\left(\mathrm{1,3}\right)/C_n^b\left(\mathrm{3,3}\right))\\ \mathrm{\&theta;}={\sin }^{-1}\left(C_n^b\left(\mathrm{2,3}\right)\right)\\ \mathrm{\&psi;}={\mathrm{tg}}^{-1}(C_n^b\left(\mathrm{2,1}\right)/C_n^b\left(\mathrm{2,2}\right))\end{array}\right\}---\left(7\right)$

C wherein _n ^b(i, j) element of (i=1,2,3, j=1,2,3) expression attitude matrix;

At last,, comprise that attitude, speed and position output to supervisory system, both finished correction double satellite receiver/inertial navigation integrated navigation system with revised integrated navigation result.

Images