CN102628691A - Completely independent relative inertial navigation method - Google Patents

Abstract

The invention belongs to the technical field of inertial navigation and in particular relates to a completely independent relative inertial navigation method. The purpose is to independently determine an azimuth angle and a latitude under the special condition that an accurate initial position cannot be obtained. The method comprises the following steps of: setting an initial course angle and an attitude angle as 0 degree and setting initial longitude and latitude as 0 degree; by using angular velocity and specific force output by a gyroscope and an accelerometer, figuring out a navigation parameter by using a traditional navigation calculating method; estimating and revising an initial error in a closed loop; carrying out navigation calculation relative to an initial position; and after accurate outside position information is obtained, resetting longitude information as the accurate value, and keeping the course angle, the attitude angle and the latitude invariable. By using the completely independent relative inertial navigation method provided by the invention, relative navigation can be carried out with high accuracy and true latitude information with high accuracy can be provided; moreover, the initial longitude error does not affect the relative navigation accuracy; and the relative navigation accuracy is superior to 2 nmile/9h.

Description

A kind of autonomous relative inertness air navigation aid fully

Technical field

The invention belongs to the inertial navigation technology field, be specifically related to a kind of fully autonomous relative inertness air navigation aid, be used in satellite navigation system unavailable or autonomous Underwater Vehicle Navigation System fault and restart etc. and in particular cases to set up the emergency navigation ability fast.

Background technology

Under normal circumstances, the inertial navigation system workflow is: bind initial position, carry out initial alignment, carry out dead reckoning on this basis; Initial position is the essential information that inertial navigation system is aimed at, navigated.But, if can't obtain initial position accurately for a certain reason, as Field Operational GPS disturbed or submarine navigation device inertial navigation system fault after restart.How to navigate in this case becomes the problem that will solve of needing badly.

Summary of the invention

The objective of the invention is to propose a kind of autonomous relative inertness air navigation aid fully, can can't obtain accurate initial position in particular cases for a certain reason, independently confirm position angle and latitude.

The technical scheme that the present invention adopted is:

A kind of autonomous relative inertness air navigation aid fully comprises the steps:

Initial course angle and attitude angle are set are 0 °, initial longitude and latitude is 0 °; Adopt the angular velocity and the specific force of the output of gyro and accelerometer, carry out the step of navigation calculation; Estimate, revise the initial error step: with formula (1) is error model, is observed quantity with speed and position relative error, estimates and revise the error of angle, initial heading, attitude angle and latitude;

${\stackrel{\·}{\mathrm{\φ}}}_e=-\frac{{\mathrm{\δV}}_n}{R}+{\mathrm{\φ}}_n\mathrm{\Ω}\sin L-{\mathrm{\φ}}_u\mathrm{\Ω}\cos L+{\mathrm{\ϵ}}_e$

${\stackrel{\·}{\mathrm{\φ}}}_n=\frac{{\mathrm{\δV}}_e}{R}-\mathrm{\δL\Ω}\sin L-{\mathrm{\φ}}_e\mathrm{\Ω}\sin L+{\mathrm{\ϵ}}_n$

${\stackrel{\·}{\mathrm{\φ}}}_u=\frac{{\mathrm{\δV}}_e}{R}\tan L+\mathrm{\δL\Ω}\cos L+{\mathrm{\φ}}_e\mathrm{\Ω}\cos L+{\mathrm{\ϵ}}_u---\left(1\right)$

$\mathrm{\δ}{\stackrel{\·}{V}}_e=2{\mathrm{\δV}}_n\mathrm{\Ω}\sin L+{\mathrm{\φ}}_u{f}_n-{\mathrm{\φ}}_n{f}_u+{\▿}_e$

$\mathrm{\δ}{\stackrel{\·}{V}}_n=-2{\mathrm{\δV}}_n\mathrm{\Ω}\sin L-{\mathrm{\φ}}_u{f}_e+{\mathrm{\φ}}_e{f}_u+{\▿}_n$

Symbol is the expression under the geographic coordinate system in the formula,

L representes latitude, and δ L representes latitude error, and Ω representes rotational-angular velocity of the earth, and R representes earth radius;

φ _e, φ _n, φ _uRepresent that respectively east orientation, north orientation, sky are to error angle;

δ V _e, δ V _nRepresent east orientation, north orientation velocity error respectively;

f _e, f _n, f _uRepresent east orientation, north orientation, acceleration of gravity respectively;

representes east orientation, north orientation accelerometer bias respectively;

ε _e, ε _n, ε _u, respectively represent east orientation, north orientation, day to gyroscopic drift;

Be respectively φ _e, φ _n, φ _u, δ V _e, δ V _nDerivative.

Aforesaid a kind of autonomous relative inertness air navigation aid fully, wherein: after estimating, revising the initial error step, obtain before the extraneous accurately positional information, adopt gyro, accelerometer output carrying out navigation calculation; After obtaining accurate extraneous positional information, longitude and latitude information are reset to this exact value.

Aforesaid a kind of autonomous relative inertness air navigation aid fully, wherein: said estimation, correction initial error step adopt the closed loop Kalman filter, and system state variables does

$X={[\mathrm{\δL},\mathrm{\δ\λ},{\mathrm{\δV}}_N,{\mathrm{\δV}}_U,{\mathrm{\δV}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\φ}}_E,{\▿}_x,{\▿}_y,{\▿}_z,{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z]}^T$

δ L, δ λ, δ V _N, δ V _U, δ V _E, φ _N, φ _U, φ _E,

ε _x, ε _y, ε _zRepresent latitude error, longitude error, northern fast error, day fast error, eastern fast error, north orientation error angle, azimuthal error angle, thing error angle, three accelerometer bias, three gyroscopic drifts successively;

Filtering computation process is following, and symbol is the expression under the carrier coordinate system:

State one-step prediction: X _K/k-1=Φ _{K, k-1}X _K-1

State variance battle array one-step prediction: $P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

Filter gain: $K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}$

State estimation: X _k=X _K/k-1+ K _k(Z _k-K _kX _K/k-1)

The state variance battle array is upgraded: P _k=(I-K _kH _k) P _K/k-1

In the above-mentioned formula, k is the Kalman filtering umber of beats, X _K-1Be the estimated value of a last bat, X _K/k-1Be one-step prediction value, Φ _{K, k-1}, be state-transition matrix, H _kFor measuring matrix, K _kBe filter gain matrix, R _kBe observation noise battle array, P _K/k-1Be one-step prediction error variance battle array, P _k, P _K-1Be estimation error variance battle array, Γ _K-1For system noise drives battle array, Q _K-1Be the system noise acoustic matrix, I is a unit matrix, X _kBe state estimator, Z _kBe observed quantity, λ representes longitude, and subscript T representes transposition;

A filtering of every completion is calculated, and carries out a closed loop correction subsequently, and correction is following:

Position correction: L=L-X (1) λ=λ-X (2), L, λ represent latitude and longitude respectively;

Speed correction: V _N=V _N-X (3), V _U=V _U-X (4), V _E=V _E-X (5), V _N, V _U, V _ERepresent that respectively north orientation speed, sky are to speed, east orientation speed;

Attitude correction:

φ=[X (6) X (7) X (8)] '; representes that respectively attitude matrix, φ represent the vector that north orientation error angle, azimuthal error angle, thing error angle are formed;

Adding table zero revises partially: $\left\{\begin{array}{c}{f}_x=f_x-{\▿}_x,f_y=f_y-{\▿}_y,f_z=f_z-{\▿}_z\\ {\▿}_x={\▿}_x+X\left(9\right),{\▿}_y={\▿}_y+X\left(10\right),{\▿}_z={\▿}_z+X\left(11\right)\end{array}\right.$

f _x, f _y, f _zThe measured value of representing three accelerometers respectively,

Represent three accelerometer bias respectively;

The gyroscopic drift correction: $\left\{\begin{array}{c}{\mathrm{\ω}}_x={\mathrm{\ω}}_x-{\mathrm{\ϵ}}_x,{\mathrm{\ω}}_y={\mathrm{\ω}}_y-{\mathrm{\ϵ}}_y,{\mathrm{\ω}}_z={\mathrm{\ω}}_z-{\mathrm{\ϵ}}_z\\ {\mathrm{\ϵ}}_x={\mathrm{\ϵ}}_x+X\left(12\right),{\mathrm{\ϵ}}_y={\mathrm{\ϵ}}_y+X\left(13\right),{\mathrm{\ϵ}}_z={\mathrm{\ϵ}}_z+X\left(14\right)\end{array}\right.$

ω _x, ω _y, ω _zRepresent three gyrostatic measured values respectively, ε _x, ε _y, ε _zRepresent three gyroscopic drifts respectively;

State vector correction: X (i)=0, (i=1,2...14).

Aforesaid a kind of autonomous relative inertness air navigation aid fully, wherein: adopt rotation modulation method to come the enhanced navigation precision, the anglec of rotation of exporting with the rotating mechanism scrambler is rotated demodulation to course angle and attitude angle, eliminates the motion of rotation modulation.

The invention has the beneficial effects as follows:

Through setting up error model and utilizing Kalman filter to estimate, can can't obtain accurate initial position in particular cases for a certain reason, independently confirm position angle and latitude.

Further can the accurate navigation information with respect to initial position be provided down long-time, but and do not need to aim at again the normal homing capability of direct recovery in the extraneous positional information time spent.

The autonomous initial latitude precision of confirming is superior to 1n mile; Navigation accuracy is superior to 2nmile/9h relatively.

Description of drawings

Fig. 1 is the process flow diagram of a kind of fully autonomous relative inertness air navigation aid provided by the invention;

Fig. 2 is for test the latitude error convergence process in the set-up procedure for the third time;

Fig. 3 is a 9h navigation position error.

Embodiment

Below in conjunction with accompanying drawing and embodiment a kind of autonomous relative inertness air navigation aid fully provided by the invention is introduced:

(S1) system initialization

This step is mainly used at carrier and sets up the initial reference direction and the position of navigating relatively under the static or mooring condition; Concrete steps are following:

(S1.1) initial value setting:

Initial course angle and attitude angle are set are 0 °; Because do not have initial position message accurately, initial longitude and latitude be set be 0 °.

(S1.2) navigation calculation:

Adopt the angular velocity and the specific force of the output of gyro and accelerometer, utilize existing navigation calculation method to calculate navigational parameter.

(S1.3) estimate, revise initial error:

Adopting the closed loop Kalman filter, is error model with formula (1), is observed quantity with speed and position relative error, estimates and revise the error of angle, initial heading, attitude angle and latitude, estimates gyroscopic drift simultaneously, so as after navigation procedure in compensate.

Error model is:

${\stackrel{\·}{\mathrm{\φ}}}_e=-\frac{{\mathrm{\δV}}_n}{R}+{\mathrm{\φ}}_n\mathrm{\Ω}\sin L-{\mathrm{\φ}}_u\mathrm{\Ω}\cos L+{\mathrm{\ϵ}}_e$

${\stackrel{\·}{\mathrm{\φ}}}_n=\frac{{\mathrm{\δV}}_e}{R}-\mathrm{\δL\Ω}\sin L-{\mathrm{\φ}}_e\mathrm{\Ω}\sin L+{\mathrm{\ϵ}}_n$

${\stackrel{\·}{\mathrm{\φ}}}_u=\frac{{\mathrm{\δV}}_e}{R}\tan L+\mathrm{\δL\Ω}\cos L+{\mathrm{\φ}}_e\mathrm{\Ω}\cos L+{\mathrm{\ϵ}}_u---\left(1\right)$

$\mathrm{\δ}{\stackrel{\·}{V}}_e=2{\mathrm{\δV}}_n\mathrm{\Ω}\sin L+{\mathrm{\φ}}_u{f}_n-{\mathrm{\φ}}_n{f}_u+{\▿}_e$

$\mathrm{\δ}{\stackrel{\·}{V}}_n=-2{\mathrm{\δV}}_n\mathrm{\Ω}\sin L-{\mathrm{\φ}}_u{f}_e+{\mathrm{\φ}}_e{f}_u+{\▿}_n$

Symbol is the expression under the geographic coordinate system in the formula,

L representes latitude, and δ L representes latitude error, and Ω representes rotational-angular velocity of the earth, and R representes earth radius;

φ _e, φ _n, φ _uRepresent that respectively east orientation, north orientation, sky are to error angle;

δ V _e, δ V _nRepresent east orientation, north orientation velocity error respectively;

f _e, f _n, f _uRepresent east orientation, north orientation, acceleration of gravity respectively;

representes east orientation, north orientation accelerometer bias respectively;

ε _e, ε _n, ε _u, respectively represent east orientation under the geographic coordinate system, north orientation, day to gyroscopic drift;

Be respectively φ _e, φ _n, φ _u, δ V _e, δ V _nDerivative.

When aligning reached stable state, above-mentioned differential equation two ends all leveled off to 0; Visible by error equation, at this moment, east orientation gyroscopic drift causes the Azimuth Estimation error

φ _u＝-ε _e/ΩcosL (2)

The north gyro drift causes the latitude evaluated error

δL＝ε _n/ΩsinL (3)

When setting up the closed loop Kalman filter, system state variables does

$X={[\mathrm{\δL},\mathrm{\δ\λ},{\mathrm{\δV}}_N,{\mathrm{\δV}}_U,{\mathrm{\δV}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\φ}}_E,{\▿}_x,{\▿}_y,{\▿}_z,{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z]}^T$

δ L, δ λ, δ V _N, δ V _U, δ V _E, φ _N, φ _U, φ _E,

ε _x, ε _y, ε _zRepresent latitude error, longitude error, northern fast error, day fast error, eastern fast error, north orientation error angle, azimuthal error angle, thing error angle, three accelerometer bias, three gyroscopic drifts successively.

Filtering computation process is following, and symbol is the expression under the carrier coordinate system:

State one-step prediction: X _K/k-1=Φ _{K, k-1}X _K-1

State variance battle array one-step prediction: $P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

Filter gain: $K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}$

State estimation: X _k=X _K/k-1+ K _k(Z _k-K _kX _K/k-1)

The state variance battle array is upgraded: P _k=(I-K _kH _k) P _K/k-1

In the above-mentioned formula, k is the Kalman filtering umber of beats, X _K-1Be the estimated value of a last bat, X _K/k-1Be one-step prediction value, Φ _{K, k-1}, be state-transition matrix, H _kFor measuring matrix, K _kBe filter gain matrix, R _kBe observation noise battle array, P _K/k-1Be one-step prediction error variance battle array, P _k, P _K-1Be estimation error variance battle array, Γ _K-1For system noise drives battle array, Q _K-1Be the system noise acoustic matrix, I is a unit matrix, X _kBe state estimator, Z _kBe observed quantity, λ representes longitude, and subscript T representes transposition.

A filtering of every completion is calculated, and carries out a closed loop correction subsequently, and correction is following:

Position correction: L=L-X (1) λ=λ-X (2), L, λ represent latitude and longitude respectively;

Speed correction: V _N=V _N-X (3), V _U=V _U-X (4), V _E=V _E-X (5), V _N, V _U, V _ERepresent that respectively north orientation speed, sky are to speed, east orientation speed;

Attitude correction:

φ=[X; (6) X; (7) X; (8)] ';

representes attitude matrix respectively

φ representes the vector that north orientation error angle, azimuthal error angle, thing error angle are formed;

Adding table zero revises partially: $\left\{\begin{array}{c}{f}_x=f_x-{\▿}_x,f_y=f_y-{\▿}_y,f_z=f_z-{\▿}_z\\ {\▿}_x={\▿}_x+X\left(9\right),{\▿}_y={\▿}_y+X\left(10\right),{\▿}_z={\▿}_z+X\left(11\right)\end{array}\right.$

f _x, f _y, f _zThe measured value of representing three accelerometers respectively,

Represent three accelerometer bias respectively.

The gyroscopic drift correction: $\left\{\begin{array}{c}{\mathrm{\ω}}_x={\mathrm{\ω}}_x-{\mathrm{\ϵ}}_x,{\mathrm{\ω}}_y={\mathrm{\ω}}_y-{\mathrm{\ϵ}}_y,{\mathrm{\ω}}_z={\mathrm{\ω}}_z-{\mathrm{\ϵ}}_z\\ {\mathrm{\ϵ}}_x={\mathrm{\ϵ}}_x+X\left(12\right),{\mathrm{\ϵ}}_y={\mathrm{\ϵ}}_y+X\left(13\right),{\mathrm{\ϵ}}_z={\mathrm{\ϵ}}_z+X\left(14\right)\end{array}\right.$

ω _x, ω _y, ω _zRepresent three gyrostatic measured values respectively, ε _x, ε _y, ε _zRepresent three gyroscopic drifts respectively.

State vector correction: X (i)=0, (i=1,2...14).

Except that the closed loop Kalman filter, also can adopt other wave filter methods of estimation, error model is formula (1), observed quantity is minimum should to comprise speed and position relative error.

When autoregistration was carried out in inertial navigation, initial latitude error and azimuth angle error all can be estimated to come out, thereby realized independently seeking north and definite latitude; Because latitude information obtains, and initial longitude error remains unchanged in navigation procedure and does not influence other navigation error, thus system relatively initial position navigate; If in navigation procedure, obtain longitude, latitude information once more,, get into normal navigational state afterwards then to the alliance error correction.

Visible by formula (3), the north gyro drift can cause initial latitude evaluated error, is the precision of the aligning of further raising system, initial latitude and navigation, can be according to the demand of navigation accuracy, and the precision enhancement measures of employing.Two kinds of precision enhancement measures are generally arranged: improve the inertia device precision or adopt rotation modulation method; The latter is a kind of efficient ways; Under the rotation modulating action; The responsive axial constant value drift of gyro shows as the cyclical variation amount of zero-mean under Department of Geography, make the system alignment precision improve greatly.

Static or berthing time based on navigation accuracy needs and reality allows is confirmed the aligning time, and generally speaking, for the system of modulating without spin, 30min can accomplish the initialization of initial reference direction and position; For single shaft rotation modulating system, need 6h to accomplish the estimation of vertical gyroscopic drift; For twin shaft rotation modulating system, need 4h to accomplish the estimation of all gyroscopic drifts.

(S2) navigate relatively

Accomplish after the initial alignment, adopt gyro, accelerometer output carrying out navigation calculation, can carry out precise navigation by relative initial position.

In step (S1), can adopt rotation modulation method to come the enhanced navigation precision; For the system that has adopted rotation modulation method, course angle that it resolves and attitude angle contain the rotation modulation movement, can not represent the true angular motion of carrier; Therefore; Need to adopt the anglec of rotation of rotating mechanism scrambler output that course angle and attitude angle are rotated demodulation, eliminate the motion of rotation modulation, obtain the real motion parameter of carrier.

(S3) navigational state conversion

Before obtaining extraneous accurately positional information, system keeps the high precision navigation of relative initial position, and wherein, course angle, attitude angle, latitude are exact values, and longitude information is the skew of relative initial position.

After regaining accurate extraneous positional information when having ready conditions, only need longitude and latitude information be reset to actual value and get final product, and not need to carry out again long initial alignment.

After positional information was reset, system switched to normal inertial navigation mode by relative navigational state, can be provided at tellurian accurate navigational parameter.

During test, on certain inertial navigation system, test, setup time 15min, about 9h that navigates is provided with 45 ° of initial latitude errors, 50 ° of initial orientation errors.Three times the experimental data processing result adds up in table 1, has provided initial latitude evaluated error and positioning error.

Table 1 aligning/navigation results (unit: n mile)

Visible by table 1, the autonomous initial latitude precision of confirming is superior to 1n mile; Receive the influence of factor such as equivalent north orientation drift, still there is certain error in initial latitude information.

Visible by Fig. 1, initial latitude error can converge near the true value in 15min fast.

Visible by table 1 and Fig. 2, system can carry out high-precision relative navigation, and high-precision true latitude information can be provided, and initial longitude error does not influence relative navigation accuracy; Navigation accuracy is superior to 2nmile/9h relatively.

Claims (4)

1. a complete autonomous relative inertness air navigation aid comprises the steps:

Initial course angle and attitude angle are set are 0 °, initial longitude and latitude is 0 °; Adopt the angular velocity and the specific force of the output of gyro and accelerometer, carry out the step of navigation calculation; Estimate, revise the initial error step: with formula (1) is error model, is observed quantity with speed and position relative error, estimates and revise the error of angle, initial heading, attitude angle and latitude;

${\stackrel{\·}{\mathrm{\φ}}}_e=-\frac{{\mathrm{\δV}}_n}{R}+{\mathrm{\φ}}_n\mathrm{\Ω}\sin L-{\mathrm{\φ}}_u\mathrm{\Ω}\cos L+{\mathrm{\ϵ}}_e$

${\stackrel{\·}{\mathrm{\φ}}}_n=\frac{{\mathrm{\δV}}_e}{R}-\mathrm{\δL\Ω}\sin L-{\mathrm{\φ}}_e\mathrm{\Ω}\sin L+{\mathrm{\ϵ}}_n$

${\stackrel{\·}{\mathrm{\φ}}}_u=\frac{{\mathrm{\δV}}_e}{R}\tan L+\mathrm{\δL\Ω}\cos L+{\mathrm{\φ}}_e\mathrm{\Ω}\cos L+{\mathrm{\ϵ}}_u---\left(1\right)$

$\mathrm{\δ}{\stackrel{\·}{V}}_e=2{\mathrm{\δV}}_n\mathrm{\Ω}\sin L+{\mathrm{\φ}}_u{f}_n-{\mathrm{\φ}}_n{f}_u+{\▿}_e$

$\mathrm{\δ}{\stackrel{\·}{V}}_n=-2{\mathrm{\δV}}_n\mathrm{\Ω}\sin L-{\mathrm{\φ}}_u{f}_e+{\mathrm{\φ}}_e{f}_u+{\▿}_n$

Symbol is the expression under the geographic coordinate system in the formula,

L representes latitude, and δ L representes latitude error, and Ω representes rotational-angular velocity of the earth, and R representes earth radius;

φ _e, φ _n, φ _uRepresent that respectively east orientation, north orientation, sky are to error angle;

δ V _e, δ V _nRepresent east orientation, north orientation velocity error respectively;

f _e, f _n, f _uRepresent east orientation, north orientation, acceleration of gravity respectively;

representes east orientation, north orientation accelerometer bias respectively;

ε _e, ε _n, ε _u, respectively represent east orientation, north orientation, day to gyroscopic drift;

Be respectively φ _e, φ _n, φ _u, δ V _e, δ V _nDerivative.

2. a kind of autonomous relative inertness air navigation aid fully according to claim 1 is characterized in that: after estimating, revising the initial error step, obtain before the extraneous accurately positional information, adopt gyro, accelerometer output carrying out navigation calculation; After obtaining accurate extraneous positional information, longitude and latitude information are reset to this exact value.

3. a kind of autonomous relative inertness air navigation aid fully according to claim 1 is characterized in that: said estimation, correction initial error step adopt the closed loop Kalman filter, and system state variables does $X={[\mathrm{\δ\; L},\mathrm{\δ\; \λ},{\mathrm{\δ\; V}}_N,{\mathrm{\δ\; V}}_U,{\mathrm{\δ\; V}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\φ}}_E,{\▿}_x,{\▿}_y,{\▿}_z,{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z]}^T$

δ L, δ λ, δ V _N, δ V _U, δ V _E, φ _N, φ _U, φ _E, ε _x, ε _y, ε _zRepresent latitude error, longitude error, northern fast error, day fast error, eastern fast error, north orientation error angle, azimuthal error angle, thing error angle, three accelerometer bias, three gyroscopic drifts successively;

Filtering computation process is following, and symbol is the expression under the carrier coordinate system:

State one-step prediction: X _K/k-1=Φ _{K, k-1}X _K-1

State variance battle array one-step prediction: $P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

Filter gain: $K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}$

State estimation: X _k=X _K/k-1+ K _k(Z _k-K _kX _K/k-1)

The state variance battle array is upgraded: P _k=(I-K _kH _k) P _K/k-1

In the above-mentioned formula, k is the Kalman filtering umber of beats, X _K-1Be the estimated value of a last bat, X _K/k-1Be one-step prediction value, Φ _{K, k-1}, be state-transition matrix, H _kFor measuring matrix, K _kBe filter gain matrix, R _kBe observation noise battle array, P _K/k-1Be one-step prediction error variance battle array, P _k, P _K-1Be estimation error variance battle array, Γ _K-1For system noise drives battle array, Q _K-1Be the system noise acoustic matrix, I is a unit matrix, X _kBe state estimator, Z _kBe observed quantity, λ representes longitude, and subscript T representes transposition;

A filtering of every completion is calculated, and carries out a closed loop correction subsequently, and correction is following:

Position correction: L=L-X (1) λ=λ-X (2), L, λ represent latitude and longitude respectively;

Speed correction: V _N=V _N-X (3), V _U=V _U-X (4), V _E=V _E-X (5), V _N, V _U, V _ERepresent that respectively north orientation speed, sky are to speed, east orientation speed;

Attitude correction:

φ=[X (6) X (7) X (8)] '; representes that respectively attitude matrix, φ represent the vector that north orientation error angle, azimuthal error angle, thing error angle are formed;

Adding table zero revises partially: $\left\{\begin{array}{c}{f}_x=f_x-{\▿}_x,f_y=f_y-{\▿}_y,f_z=f_z-{\▿}_z\\ {\▿}_x={\▿}_x+X\left(9\right),{\▿}_y={\▿}_y+X\left(10\right),{\▿}_z={\▿}_z+X\left(11\right)\end{array}\right.$

f _x, f _y, f _zThe measured value of representing three accelerometers respectively,

Represent three accelerometer bias respectively;

The gyroscopic drift correction: $\left\{\begin{array}{c}{\mathrm{\ω}}_x={\mathrm{\ω}}_x-{\mathrm{\ϵ}}_x,{\mathrm{\ω}}_y={\mathrm{\ω}}_y-{\mathrm{\ϵ}}_y,{\mathrm{\ω}}_z={\mathrm{\ω}}_z-{\mathrm{\ϵ}}_z\\ {\mathrm{\ϵ}}_x={\mathrm{\ϵ}}_x+X\left(12\right),{\mathrm{\ϵ}}_y={\mathrm{\ϵ}}_y+X\left(13\right),{\mathrm{\ϵ}}_z={\mathrm{\ϵ}}_z+X\left(14\right)\end{array}\right.$

ω _x, ω _y, ω _zRepresent three gyrostatic measured values respectively, ε _x, ε _y, ε _zRepresent three gyroscopic drifts respectively;

State vector correction: X (i)=0, (i=1,2...14).

4. a kind of autonomous relative inertness air navigation aid fully according to claim 1; It is characterized in that: adopt rotation modulation method to come the enhanced navigation precision; The anglec of rotation with the output of rotating mechanism scrambler is rotated demodulation to course angle and attitude angle, eliminates the motion of rotation modulation.

Images