CN103712621B - Polarised light and infrared sensor are assisted inertial navigation system method for determining posture - Google Patents

Abstract

The auxiliary inertial navigation system method for determining posture of polarised light of the present invention and infrared sensor belongs to aircraft attitude measurement and estimation technique field, relates to the auxiliary inertial navigation system method for determining posture of a kind of polarised light and infrared sensor. The three-dimension altitude angle that the method adopts polarized light sensor, infrared sensor to measure gyroscope survey in course angle, roll angle, the angle of pitch and the inertial navigation system of aircraft carries out Kalman filtering, gained optimal estimation attitude angle is fed back in inertial navigation system, improve the certainty of measurement of inertial navigation system, the strapdown matrix that the present invention measures two kinds of modes carries out optimization fusion, result feedback to inertial navigation system after fusion is corrected it, exported position, the speed of aircraft by inertial navigation system to user. In the present invention, sensor used all belongs to independent navigation device, is not subject to external interference, compared with the navigate mode such as GPS, the Big Dipper, has certain anti-duplicity and disguise.

Description

Polarised light and infrared sensor are assisted inertial navigation system method for determining posture

Technical field

The invention belongs to aircraft attitude measurement and estimation technique field, relate to a kind of polarised light and infrared sensingDevice is assisted inertial navigation system method for determining posture.

Background technology

Inertial navigation system (InertialNavigationSystem, INS) is called for short: inertial navigation system, it is a kind of completeAutonomous navigation system, can provide continuously, in real time position, speed and attitude (roll angle, the angle of pitch andCourse angle) information, precision is very high in short-term for it, and has good concealment, the not advantage such as climate condition restriction,Thereby be widely used in the field such as Aeronautics and Astronautics, navigation. But the error of inertial navigation system increases in time,Therefore need to form integrated navigation system with other navigation system. In the most frequently used INS/GPS integrated navigation systemIn system, utilize Kalman Filter Technology can effectively reduce site error and the speed mistake of integrated navigation systemPoor, but very little on the impact of attitude error, be especially difficult to suppress the accumulation of course angle error, so forThe research that improves inertial navigation attitude measurement accuracy becomes current a lot of scholars' research emphasis.

In conventional art for the attitude error correction of inertial navigation system mostly adopt differential GPS, magnetometer orObliquity sensor is realized, but because above method all exists shortcoming in various degree, such as differential GPSMany antennas need to be installed, be subject to volume restrictions, be not suitable for micro air vehicle, and wireless signal is easily disturbed,Do not there is disguise; Magnetometer adopts ground magnetic principle work, affected by surrounding magnetic field; Obliquity sensorRely on weight component to measure attitude, be not suitable for the aircraft of accelerated motion.

Summary of the invention

The object of the invention is to overcome the defect of prior art, invent that a kind of polarised light and infrared sensor are auxiliary to be used toLead method for determining posture, polarized light sensor is measured course information according to optical principle, and infrared sensor is according to infraredHotness principle is measured roll, pitch attitude information, both combinations can accurately be measured to the three-dimensional appearance of aircraftState angle. Because it is based on optics and the work of infra-red heat principle of induction, be not subject to Electromagnetic Interference, have certainIndependence and disguise, and not temporal evolution of precision have outstanding behaviours in motion in the time of long boat. ThisBright polarised light, infrared sensor and INS are combined, not only can suppress inertial navigation system attitude errorDisperse, certain correcting action has also been played in position and velocity error simultaneously. The method can be effectivelyThe gyroscope error that accumulation produces in time in integral process in compensation inertial navigation system, thereby the high accuracy of obtaining,Complete autonomous position and attitude information.

The technical solution used in the present invention is that the auxiliary inertial navigation system of polarised light and infrared sensor is determined appearance, its featureBe, adopt polarized light sensor, infrared sensor measure the course angle of aircraft, roll angle, the angle of pitch andIn inertial navigation system, the three-dimension altitude angle of gyroscope survey carries out Kalman filtering, by gained optimal estimation appearanceState angle feeds back in inertial navigation system, improves the certainty of measurement of inertial navigation system.

The concrete steps of the method are as follows:

Step 1: gather the data of infrared sensor and polarized light sensor output, determine the roll of aircraftAngle φ_IR, pitching angle theta_IR, course angle ψ_P, set up the initial strapdown matrix of combined system

$C_b^n=\begin{array}{c}\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P& -\cos {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P+\sin {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P& \sin {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P+\cos {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P\\ \cos {\mathrm{\θ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P& \cos {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P+\sin {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_{\mathrm{IR}}& -\sin {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P+\cos {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P---\left(1\right)\\ -\sin {\mathrm{\θ}}_{\mathrm{IR}}& \sin {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\θ}}_{\mathrm{IR}}& \cos {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\θ}}_{\mathrm{IR}}\end{array}\right]\end{array}$

Wherein, φ_INFθ_INFBe respectively roll angle, the angle of pitch of infrared sensor, ψ_PFor polarized light sensorCourse angle.

Step 2: gather accelerometer output, will export f^bObtain to navigation system (n) by strapdown matrix projectionTo fⁿ, as formula (2), through revising Coriolis impact, integral operation obtains the speed V under navigation systemⁿ, fromBe the projection under n system of the angular speed of relative e system and obtain n

$f^n=C_b^n{f}^b---\left(2\right)$

Step 3: utilizeUpgrade the transformation matrix of coordinates of n system and e systemAnd according to latitude and longitude information and seatRelation between mark transformation matrix obtains real-time latitude and longitude information;

Step 4: gather gyrostatic outputRemoveProjection under carrier system (b)ObtainAs formula (3), utilizeUpgrade strapdown matrixObtain the roll angle φ of inertial navigation system output_g, pitching angle theta_g、Course angle ψ_g；

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-{\mathrm{\ω}}_{\mathrm{in}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b{\mathrm{\ω}}_{\mathrm{in}}^n---\left(3\right)$

WhereinFor the projection of the angular velocity of rotation between navigation system and inertial system under navigation system;For system victoryConnection matrixTransposed form;For gyrostatic output.

Strapdown matrix update can be realized by resolving differential equation (4):

${\stackrel{\·}{C}}_b^n=C_b^n{\·\mathrm{\Ω}}_{\mathrm{nb}}^b---\left(4\right)$

Wherein,For vectorAntisymmetric matrix, ${\mathrm{\Ω}}_{\mathrm{nb}}^b=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_{\mathrm{nbZ}}^b& {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbZ}}^b& 0& -{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ -{\mathrm{\ω}}_{\mathrm{nby}}^b& {\mathrm{\ω}}_{\mathrm{nbx}}^b& 0\end{array}\right]$

Step 5: gather the data of infrared sensor and polarized light sensor output, determine aircraft 3 d poseAngle φ_IRθ_IRψ_P, the attitude angle obtaining with gyroscope in step 4 is carried out Kalman filtering and is obtained optimal estimation three-dimensionalAttitude angle

Step 6: by optimal estimation attitude angle composition strapdown matrix feedback to correction position and appearance in inertia systemThe operational formula of state, its makeover process divides three parts:

(1) the system strapdown matrix of formula (2) in correction step 2Obtain the output f by accelerometer^bProjectionTo the new vector f of navigation systemⁿ；

(2) revise formula (3) system strapdown matrix in step 4Transposed form, obtainIn carrier system (b)Under new projection

(3) the system strapdown matrix of formula (4) in correction step 4Obtain new victory by resolving the differential equationConnection matrix.

Step 7: repeating step two is to step 6 process, and the system that realizes is exported the position of aircraft, speed in real timeDegree, attitude information.

The present invention's advantage is compared with prior art: (1) sensor used all belongs to independent navigation device,Be not subject to external interference, compared with the navigate mode such as GPS, the Big Dipper, there is certain anti-duplicity and disguise.(2) not accumulation in time of the measure error of polarised light, infrared sensor, with precision is in short-term high but error is dispersedAngular velocity gyro forms has complementary advantages, after Kalman filtering is carried out data fusion, and the precision of combined systemCan keep long-time stable.

Brief description of the drawings

Fig. 1 is the calculation flow chart of data fusion method of the present invention

Fig. 2 is theory diagram of the present invention

Detailed description of the invention

Describe specific embodiment of the invention, the coordinate system the present invention relates in detail below in conjunction with technical scheme and accompanying drawingHave: carrier coordinate system (b); Navigation coordinate system (n); Terrestrial coordinate system (e); Inertial coodinate system (i). Aircraft withCarrier coordinate system is connected, and carrier coordinate system is to the conversion strapdown matrix between navigation coordinate systemRepresent. ?In the present invention, the gyroscope in inertial navigation system can be determined strapdown matrix, simultaneously infrared sensor and polarised lightSensor also can be determined strapdown matrix, and the strapdown matrix that the present invention measures two kinds of modes carries out optimization and meltsClose, result feedback to inertial navigation system after fusion is corrected it, export aircraft by inertial navigation system to userPosition, speed.

In accompanying drawing 1, represent the calculation flow chart of data fusion method of the present invention.

1. gather the data of infrared sensor and polarized light sensor output, determine the roll angle φ of aircraft_IR, bowElevation angle φ_IR, course angle ψ_P, set up the initial strapdown matrix of combined system

$C_b^n=\begin{array}{c}\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P& -\cos {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P+\sin {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P& \sin {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P+\cos {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P\\ \cos {\mathrm{\θ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P& \cos {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P+\sin {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_{\mathrm{IR}}& -\sin {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\Ψ}}_P+\cos {\mathrm{\φ}}_{\mathrm{IR}}\sin {\mathrm{\θ}}_{\mathrm{IR}}\sin {\mathrm{\Ψ}}_P\\ -\sin {\mathrm{\θ}}_{\mathrm{IR}}& \sin {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\θ}}_{\mathrm{IR}}& \cos {\mathrm{\φ}}_{\mathrm{IR}}\cos {\mathrm{\θ}}_{\mathrm{IR}}\end{array}\right]\end{array}$

$\left(5\right)$

Wherein, φ_INFθ_INFBe respectively roll angle, the angle of pitch of infrared sensor, ψ_PFor polarized light sensorCourse angle.

2. gather accelerometer output, will export f^bObtain f by strapdown matrix projection to navigation system (n)ⁿ，As formula (6), through revising Coriolis impact, integral operation obtains the speed V under navigation systemⁿThereby, obtainN is the projection under n system of the angular speed of relative e systemAs formula (7).

$f^n=C_b^n{f}^b---\left(6\right)$

${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{ccc}\frac{V_E}{r_p+h}& \frac{-V_N}{r_m+h}& \frac{V_E\tan \mathrm{Lat}}{r_p+h}\end{array}\right]---\left(7\right)$

Wherein V_E、V_N, Lat, h be respectively the lower east orientation speed of navigation system, north orientation speed, latitude, highly;r_p、r_mFor the long and short radius of the earth.

3. utilizeUpgrade the transformation matrix of coordinates of n system and e systemAnd according to latitude and longitude information and coordinate transformRelation between matrix obtains real-time latitude and longitude information, and matrix update can be realized by resolving the differential equation (8):

${\stackrel{\·}{C}}_b^n=C_b^n\·{\mathrm{\Ω}}_{\mathrm{nb}}^b---\left(8\right)$

Wherein,For vectorAntisymmetric matrix, ${\mathrm{\Ω}}_{\mathrm{nb}}^b=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_{\mathrm{nbZ}}^b& {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbZ}}^b& 0& -{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ -{\mathrm{\ω}}_{\mathrm{nby}}^b& {\mathrm{\ω}}_{\mathrm{nbx}}^b& 0\end{array}\right]$

$C_e^n=\left[\begin{array}{ccc}\sin \mathrm{Lat}\·\cos \mathrm{long}& \sin \mathrm{Latlong}& \cos \mathrm{Lat}\\ -\sin \mathrm{long}& \cos \mathrm{long}& 0\\ -\cos \mathrm{Lat}\cos \mathrm{long}& -\cos \mathrm{Lat}\·\sin \log & \sin \mathrm{Lat}\end{array}\right]---\left(9\right)$

Latitude: $\mathrm{Lat}=\arcsin C_e^n\left(\mathrm{3,3}\right)---\left(10\right)$

Longitude: $\mathrm{long}=\arctan \frac{C_e^n\left(\mathrm{3,2}\right)}{C_e^n\left(\mathrm{3,1}\right)}---\left(11\right)$

4. gather gyrostatic outputRemoveProjection under carrier system (b)ObtainUtilizeMoreNew strapdown matrixRenewal process and step 3 matrix update similar process, with reference to three-dimensional appearance in formula (5)The relation of state angle and strapdown matrix, obtains the roll angle φ that inertial navigation system is exported_g, pitching angle theta_g, course angle ψ_g。

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-{\mathrm{\ω}}_{\mathrm{in}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b{\mathrm{\ω}}_{\mathrm{in}}^n---\left(12\right)$

WhereinFor the projection of the angular velocity of rotation between navigation system and inertial system under navigation system;For system victoryConnection matrixTransposed form;For gyrostatic output.

Strapdown matrix update can be realized by resolving the differential equation (13):

${\stackrel{\·}{C}}_b^n=C_b^n\·{\mathrm{\Ω}}_{\mathrm{nb}}^b---\left(13\right)$

Wherein,For vectorAntisymmetric matrix, ${\mathrm{\Ω}}_{\mathrm{nb}}^b=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_{\mathrm{nbZ}}^b& {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbZ}}^b& 0& -{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ -{\mathrm{\ω}}_{\mathrm{nby}}^b& {\mathrm{\ω}}_{\mathrm{nbx}}^b& 0\end{array}\right]$

5. gather the data of infrared sensor and polarized light sensor output, determine aircraft 3 d poseAngle φ_IRθ_IRψ_P, the attitude angle obtaining with gyroscope in step 4 is carried out Kalman filtering and is obtained optimal estimation three-dimensionalAttitude angle

Concrete Kalman filtering computational process is as follows:

1. the system equation of infrared sensor, polarized light sensor and inertial navigation combination comprises state equation and measurement sideJourney, is respectively:

System state equation

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BU}+W---\left(14\right)$

Wherein X=[φ θ ψ ε_φε_θε_ψ]^TFor system state vector; $U={\left[\begin{array}{cccccc}{\stackrel{\·}{\mathrm{\φ}}}_g& {\stackrel{\·}{\mathrm{\θ}}}_g& {\stackrel{\·}{\mathrm{\Ψ}}}_g& 0& 0& 0\end{array}\right]}^T$ For being used toGuiding systems records the spread vector of three axis angular rates; W=[ω_φω_θω_ψ000]^TFor system noise vector;A, B are system transfer matrix:

$A=\left[\begin{array}{cccccc}0& 0& 0& -1& 0& 0\\ 0& 0& 0& 0& -1& 0\\ 0& 0& 0& 0& 0& -1\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right],B=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right];$

The measurement equation of system:

Z＝HX+V(15)

Wherein Z=[φ_INFθ_INFψ_P000]^TFor measurement vector, φ_INFθ_INFBe respectively infrared sensorRoll angle, the angle of pitch, ψ_PFor the course angle of polarized light sensor; V=[υ_φυ_θυ_ψ000]^TFor amountSurvey noise vector; H is observing matrix.

$\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]$

If the sampling time is Δ T, the discrete form of system equation:

X_K=ΦX_K-1+CU_K+W_K-1(16)

Wherein X_k-1For k-1 moment state vector; X_kFor k moment state vector; Φ, C are respectively discretizationAfter transfer matrix:

$\mathrm{\Φ}=\left[\begin{array}{cccccc}1& 0& 0& -\mathrm{\ΔT}& 0& 0\\ 0& 1& 0& 0& -\mathrm{\ΔT}& 0\\ 0& 0& 1& 0& 0& -\mathrm{\ΔT}\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],C=\left[\begin{array}{cccccc}\mathrm{\ΔT}& 0& 0& 0& 0& 0\\ 0& \mathrm{\ΔT}& 0& 0& 0& 0\\ 0& 0& \mathrm{\ΔT}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right];$

The discrete form of measurement equation:

Z_K＝HX_K+V_K(17)

Wherein, $H_K=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& \mathrm{\ΔT}& 0& 0\\ 0& 0& 0& 0& \mathrm{\ΔT}& 0\\ 0& 0& 0& 0& 0& \mathrm{\ΔT}\end{array}\right]$

2. Kalman filtering rudimentary algorithm layout, this algorithm flow is as follows:

State one-step prediction equation:

${\hat{X}}_{k/k-1}{\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+C_{k,k-1}{U}_k---\left(18\right)$

State Estimation accounting equation:

${\hat{X}}_k={\hat{X}}_{K/K-1}+{\hat{K}}_k(Z_k-H_K{\hat{X}}_{k/k-1})---\left(19\right)$

Filtering gain matrix equation:

${\hat{K}}_k={\hat{P}}_{k/k-1}{H}_k^T{(H_k{\hat{P}}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(20\right)$

One-step prediction mean square error equation:

${\hat{P}}_{k\backslash k-1}={\mathrm{\Φ}}_{k\backslash k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(21\right)$

Estimate mean square error equation

${\hat{P}}_k=(I-{\hat{K}}_k{H}_k){\hat{P}}_{k\backslash k-1}{(I-{\hat{K}}_k{H}_k)}^T+{\hat{K}}_k{R}_k{\hat{K}}_k^T---\left(22\right)$

6. the fortune to correction position in inertia system and attitude by optimal estimation attitude angle composition strapdown matrix feedbackCalculate formula, its makeover process divides three parts:

(1) the system strapdown matrix of formula (6) in correction step 2Obtain the output f by accelerometer^bThrowShadow is to the new vector f of navigation systemⁿ；

(2) revise formula (12) system strapdown matrix in step 4Transposed form, obtainIn carrier system (b)Under new projection

(3) the system strapdown matrix of formula (13) in correction step 4Obtain new by resolving the differential equationStrapdown matrix.

7. repeating step 2 is to step 6 process, and the system that realizes is exported the position of aircraft, speed, attitude in real timeInformation.

The strapdown matrix that the present invention measures two kinds of modes carries out optimization fusion, merging rear result feedback to being used toGuiding systems is corrected it, is exported position, the speed of aircraft by inertial navigation system to user. Used in the present inventionSensor all belongs to independent navigation device, is not subject to external interference, compared with the navigate mode such as GPS, the Big Dipper, toolThere are certain anti-duplicity and disguise.

Claims (1)

1. the auxiliary inertial navigation system method for determining posture of polarised light and infrared sensor, is characterized in that, the method is adoptedMeasure course angle, roll angle, the angle of pitch and the inertia of aircraft leads with polarized light sensor, infrared sensorIn boat system, the three-dimension altitude angle of gyroscope survey carries out Kalman filtering, by anti-gained optimal estimation attitude angleBe fed in inertial navigation system, improve the certainty of measurement of inertial navigation system, method concrete steps are as follows:

Step 1: gather the data of infrared sensor and polarized light sensor output, determine the roll of aircraftAngle φ_IR, pitching angle theta_IR, course angle ψ_P, set up the initial strapdown matrix of combined system

$C_b^n=\left[\begin{array}{ccc}{\mathrm{cos\θ}}_{IR}{\mathrm{cos\ψ}}_P& -{\mathrm{cos\φ}}_{IR}{\mathrm{sin\ψ}}_P+{\mathrm{sin\φ}}_{IR}{\mathrm{sin\θ}}_{IR}{\mathrm{cos\ψ}}_P& {\mathrm{sin\φ}}_{IR}{\mathrm{sin\ψ}}_P+{\mathrm{cos\φ}}_{IR}{\mathrm{sin\θ}}_{IR}{\mathrm{cos\ψ}}_P\\ {\mathrm{cos\θ}}_{IR}{\mathrm{sin\ψ}}_P& {\mathrm{cos\φ}}_{IR}{\mathrm{cos\ψ}}_P+{\mathrm{sin\φ}}_{IR}{\mathrm{sin\θ}}_{IR}{\mathrm{sin\ψ}}_P& -{\mathrm{sin\φ}}_{IR}{\mathrm{cos\ψ}}_P+{\mathrm{cos\φ}}_{IR}{\mathrm{sin\θ}}_{IR}{\mathrm{sin\ψ}}_P\\ -{\mathrm{sin\θ}}_{IR}& {\mathrm{sin\φ}}_{IR}{\mathrm{cos\θ}}_{IR}& {\mathrm{cos\φ}}_{IR}{\mathrm{cos\θ}}_{IR}\end{array}\right]---\left(1\right)$

Wherein, φ_INFθ_INFBe respectively roll angle, the angle of pitch of infrared sensor, ψ_PFor the course of polarized light sensorAngle;

Step 2: gather accelerometer output, will export f^bObtain to navigation system (n) by strapdown matrix projectionTo fⁿ, as formula (2), through revising Coriolis impact, integral operation obtains the speed V under navigation systemⁿ, fromBe the projection under n system of the angular speed of relative e system and obtain n

$f^n=C_b^n{f}^b---\left(2\right)$

Step 3: utilizeUpgrade the transformation matrix of coordinates of n system and e systemAnd according to latitude and longitude information and seatRelation between mark transformation matrix obtains real-time latitude and longitude information, and matrix update is real by resolving the differential equation (8)Existing:

Step 4: gather gyrostatic outputRemoveProjection under carrier system (b)ObtainAsFormula (3), utilizesUpgrade strapdown matrixObtain the roll angle φ of inertial navigation system output_g, pitching angle theta_g、Course angle ψ_g；

${\mathrm{\ω}}_{nb}^b={\mathrm{\ω}}_{ib}^b-{\mathrm{\ω}}_{in}^b={\mathrm{\ω}}_{ib}^b-C_n^b{\mathrm{\ω}}_{in}^n---\left(3\right)$

WhereinFor the projection of the angular velocity of rotation between navigation system and inertial system under navigation system;For system victoryConnection matrixTransposed form;For gyrostatic output;

Strapdown matrix update can be realized by resolving differential equation (4):

Step 5: gather the data of infrared sensor and polarized light sensor output, determine aircraft 3 d poseAngle φ_IRθ_IRψ_P, the attitude angle obtaining with gyroscope in step 4 is carried out Kalman filtering and is obtained optimal estimation three-dimensionalAttitude angle

Step 6: by optimal estimation attitude angle composition strapdown matrix feedback to correction position and appearance in inertia systemThe operational formula of state, its makeover process divides three parts:

(1) the system strapdown matrix of formula (2) in correction step 2Obtain the output f by accelerometer^bProject toThe new vector f of navigation systemⁿ；

(2) revise formula (3) system strapdown matrix in step 4Transposed form, obtainUnder carrier system (b)New projection

(3) the system strapdown matrix of formula (4) in correction step 4Obtain new strapdown by resolving the differential equationMatrix;

Step 7: repeating step two is to step 6 process, and the system that realizes is exported the position of aircraft, speed in real timeDegree, attitude information.