CN102706360A - Method utilizing optical flow sensors and rate gyroscope to estimate state of air vehicle - Google Patents

Abstract

The invention relates to a method utilizing optical flow sensors and a rate gyroscope to estimate the state of an air vehicle. The method comprises the following four steps of: step 1, aiming at the air vehicle needing to install the optical flow sensors, establishing a linearized-perturbation kinematical equation; step 2, arranging five optical flow sensors onto the air vehicle in a multipoint mode, causing the distance among the sensors to be far as much as possible, causing the sensors to direct different directions; step 3, establishing a measurement equation of the optical flow sensors, establishing a measurement equation of the rate gyroscope, and forming a measurement equation of multiple sensors of optical flow and inertia navigation of a system; and step 4, estimating the flight state of the air vehicle with UKF, namely unscented kalman filters. The optical flow sensor has small size, light weight, small power consumption and low cost, is convenient to be installed on small air vehicles, and does not externally radiate electromagnetic signals. The stealthiness of the air vehicle is increased.

Description

A kind of method of utilizing light stream sensor and rate gyro to the aircraft state estimation

(1) technical field:

(Miniature aerial vehicles, MAVs) measurement and the estimation technique field of attitude, flying speed and height are specifically related to a kind of method of utilizing light stream sensor and rate gyro to the MAV state estimation to the invention belongs to small aircraft.

(2) background technology:

Aircraft mainly relies on forward-looking radar, radar altimeter and climb meter to measure terrain clearance and rising or falling speed, and for small aircraft, (Laser Rangefinders LRF) seems too heavy with radar to laser range finder.SICKLMS291 is a typical laser range finder, generally is used for the robot field, and its quality approximately is 4.5 kilograms.Be used for push-button aircraft (Unmanned Aerial Vehicle; The synthetic-aperture radar of the minimum UAV) possibly be the miniSAR that make in inferior laboratory, the U.S. Holy Land (Sandia National Labs); Its quality is about 4～5 kilograms; So heavy equipment has increased the weight and volume of unmanned plane, has reduced its flying power and load capacity.

The light stream sensor mass is little; Have only about 10 grams, receive extraneous light fully passively, radiationless; Have with respect to radar altimeter that quality is little, the advantage of good concealment; Except that the flying speed of aircraft, flying height, rate of pitch, this method can also estimate other flight informations such as the angle of attack, the angle of pitch, rising or falling speed of aircraft, and these information help helping MAV to accomplish particular flight tasks such as detection, the disaster relief.

(3) summary of the invention:

1, purpose: the purpose of this invention is to provide a kind of method of utilizing light stream sensor and rate gyro to the aircraft state estimation; It uses 5 light stream sensors and three rate gyros of 1 cover; Volume is little, in light weight, power consumption is little, cost is low; Be convenient to mounting arrangements on small aircraft, external electromagnetic radiation signal has improved the disguise of aircraft.

2, technical scheme: insect is when mobile; The luminance patterns of surrounding environment forms a series of continually varying images on retina; These a series of continually varying information are " flowing through " retina constantly, seems a kind of " stream " of light, so claim that the apparent motion of this brightness of image pattern is light stream.Some external laboratory; Developed the physical prototyping of light stream sensor; And utilize the light stream sensor to realize the automatic obstacle avoiding of unmanned vehicle, constant-level flight, landing automatically, wind speed estimation, target detection and hovering, these technology will have very important using value at aspects such as detection, the disaster relief.

According to the definition of light stream and the geometric relationship shown in Fig. 1, the expression formula that can draw light stream is:

$f=\frac{\mathrm{<figure-callout\; id="2"\; label="v\; cos"\; filenames="HDA00001750515800031.png,HDA00001750515800032.png"\; state="\{\{state\}\}">v</figure-callout>}{\cos }^{}{}^2\mathrm{\&theta;}}{h}+\mathrm{\&omega;}---\left(1\right)$

In the formula, f is light stream (1/s), and v is the horizontal velocity (m/s) of light stream sensor, and h is the height (m) on light stream sensor distance ground, and θ is the angle (rad) of optical axis and vertical, and ω is the rotational speed (rad/s) of light stream sensor.

Can find out by formula (1); The measured value of light stream is relevant with the speed coupling with attitude, the height of light stream sensor; Simultaneously the light stream sensor has that but volume is little, in light weight, power consumption is little, the characteristics of the low networking of cost, then consider a plurality of light stream sensors are connected firmly on aircraft, in conjunction with inertial navigation device---rate gyro; Utilize the information fusion technology of multisensor, realize estimation attitude of flight vehicle information.The light stream transducer arrangements is as shown in Figure 2.

The present invention is a kind of to utilize light stream sensor and the rate gyro method to the aircraft state estimation, and these method concrete steps are following:

Step 1:, set up its linearize disturbance motion equation to the aircraft that needs to install the light stream sensor;

$\stackrel{\&CenterDot;}{X}=\mathrm{AX}+\mathrm{Bu}+w\left(t\right)---\left(2\right)$

Here, X=[V _Tα

Q h β θ ψ c p r ψ γ] ^T, $u={\left[\begin{array}{ccc}{\mathrm{\&delta;}}_{\mathrm{Th}}& {\mathrm{\&delta;}}_{\mathrm{Ev}}^R& {\mathrm{\&delta;}}_{\mathrm{Ev}}^L\end{array}\right]}^T$

V _T: flight speed (m/s)

α, β: the angle of attack and yaw angle (rad)

γ,

ψ: roll angle, the angle of pitch and crab angle (rad)

R, q, p: angular velocity in roll, rate of pitch and yaw rate (rad/sec)

H: flying height (m)

θ, ψ _c: trajectory tilt angle and trajectory deflection angle (rad)

δ _Th: throttle push rod angle (rad)

left rudder drift angle and right standard rudder drift angle (rad)

W (t): white-noise process, E [w (t)]=0, E [w (t) w ^T(τ)]=q _wδ (t-τ), q _wVariance intensity battle array for w (t).

Step 2: 5 light stream sensor multiple spots are arranged on the aircraft, and under the situation of space permission, the distance between each sensor will be tried one's best far, and points to different directions, does like this to help improving follow-up estimated accuracy;

Wherein, " multiple spot layout " is meant that the light stream sensor will be installed in the diverse location of aircraft, and exemplary position is head, centre, afterbody and wingtip; " distance will be tried one's best far away " is meant; Be installed in the light stream sensor of head, afterbody or wingtip; Under the situation that does not influence other airborne equipment; Will be as far as possible near body foremost, rearmost end or side, so just guaranteed that the distance between head, afterbody and wingtip light stream sensor is big as far as possible.

Step 3: according to each light stream sensor in carry-on installation site and direction; Set up the measurement equation of light stream sensor; Utilize inertial navigation device---the rate gyro that carries on the aircraft, three rate gyros perhaps are installed on aircraft in addition, set up the measurement equation of rate gyro; With the measurement equation of light stream sensor, the light stream of construction system and inertial navigation multisensor measurement equation;

In fact, a light stream sensor can be measured two light stream components on the orthogonal directions simultaneously, and it measures output and can be designated as

$f=\left(\begin{array}{c}{f}_x\\ f_y\end{array}\right)---\left(3\right)$

And aircraft rate of pitch

can be measured by rate gyro, so the system measurements equation is:

$Z={\left(\begin{array}{ccccc}{f}_1^T& f_2^T& ...& f_n^T& {\mathrm{\&omega;}}^T\end{array}\right)}^T+v\left(t\right)---\left(4\right)$

In the formula, v (t) is a measurement noise, supposes that it is that average is 0 white noise, i.e. E [v (t)]=0, and E [v (t) v ^T(τ)]=r _vδ (t-τ), r _vVariance intensity battle array for v (t).

Step 4: the state of flight of aircraft is estimated with UKF (Unscented Kalman Filter, Unscented kalman filtering).

3, advantage and effect: the present invention is a kind of method of utilizing light stream sensor and rate gyro to the aircraft state estimation; Its advantage is: (1) measuring sensor volume is little, in light weight, power consumption is little, cost is low, is convenient on aircraft, arrange, installs and uses; (2) the not external electromagnetic radiation signal of measuring sensor helps aircraft and accomplishes disguised task; (3) except that the flying speed of aircraft, flying height, this method can also estimate other flight informations such as attitude angle and the attitude angular velocity of aircraft.

(4) description of drawings:

Fig. 1 is a light stream sensor measurement graph of a relation

Among Fig. 1, v is the horizontal velocity (rad/s) of light stream sensor, and h is the height (m) on light stream sensor distance ground, and θ is the angle (rad) of optical axis and vertical, and ω is the rotational speed (rad/s) of light stream sensor.

Fig. 2 is that the light stream sensor is at carry-on arrangement synoptic diagram

Fig. 3 is a FB(flow block) of the present invention

Fig. 4 is the graph of a relation of each coordinate system

Among Fig. 4, S _uRepresent local coordinate system, S _bThe expression body coordinate system, S _fExpression light stream sensor coordinate system.r _UbBe S _bWith respect to S _uPosition vector, r _BfBe S _fWith respect to S _bPosition vector, r _UfBe S _fWith respect to S _uPosition vector.

Fig. 5 is the simulation process block diagram

Among Fig. 5; U is a controlled quentity controlled variable; X is a quantity of state; Z is a measuring value, and is the state estimation value.

Fig. 6 is the effect comparison of measuring value before and after UKF filtering of light stream sensor

Fig. 7 is the effect comparison of measuring value before and after UKF filtering of three-axis gyroscope

Fig. 8 is that UKF is to MAV speed, height and azimuthal estimation

V among Fig. 8 _TThe velocity magnitude (m/s) of expression MAV, h represents the flying height (m) of MAV, ψ _cRepresent MAV flight azimuth (°)

Fig. 9 is the estimation of UKF to the MAV aerodynamic condition

Among Fig. 9, α represent the angle of attack (°), β represent yaw angle (°)

Figure 10 is the estimation of UKF to the MAV flight attitude

Among Figure 10;

expression angle of pitch (°); ψ represent crab angle (°), γ represent roll angle (°).

(5) embodiment:

According to light stream sensor measurement graph of a relation shown in Figure 1 and the allocation plan synoptic diagram of light stream sensor on MAV shown in Figure 2, we have proposed a kind of method of utilizing light stream sensor and rate gyro to the aircraft state estimation.The light stream sensor can record the light stream information at aircraft the place ahead, below, side even rear, and these information provide foundation for estimate the residing surrounding enviroment of aircraft comprehensively.

In order to reduce the complexity of problem, the simplified system mathematical model, make following hypothesis:

1) the quality texture of aircraft surrounding environment is mixed and disorderly, and light stream can be surveyed;

2) each light stream sensor can both operate as normal, and their output contains measurement noise, but does not have full of prunes wild value;

3) field angle of light stream sensor is very little, and the information that records is the light stream information on the camera lens axis;

Based on above hypothesis, see Fig. 3, the present invention is a kind of method of utilizing light stream sensor and rate gyro to the aircraft state estimation, these method concrete steps are following:

Step 1:, set up its linearize disturbance motion equation to the aircraft that needs to install the light stream sensor;

The linearize disturbance motion equation of certain type MAV is:

$\stackrel{\&CenterDot;}{X}=\mathrm{AX}+\mathrm{Bu}+w\left(t\right)---\left(5\right)$

In the formula, α is the angle of attack (rad),

Be the angle of pitch (rad), δ _zBe angle of rudder reflection (rad) that h is body height of center of mass (m), w (t) is a white-noise process, E [w (t)]=0, E [w (t) w ^T(τ)]=q _wδ (t-τ), q _wVariance intensity battle array for w (t).

$A=\begin{array}{c}\left[\begin{array}{cccccccccccc}-0.239& 1.513& -9.81& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0.055& -5.94& 0& 0.922& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0.000& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0.017& -2.978& 0& -7.853& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& -12& 12& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& -0.072& 0.807& 0& 0.159& -0.987& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1& 0.161& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 1.013& 0& 0\\ 0& 0& 0& 0& 0& -115.511& 0& 0& -10.254& 1.445& 0& 0\\ 0& 0& 0& 0& 0& -15.179& 0& 0& -0.023& -0.233& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0\end{array}\right]\end{array}$

$B={\left[\begin{array}{cccccccccccc}6.741& -0.09& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ -0.006& -0.008& 0& -1.328& 0& 0& 0& 0& -0.93& -0.132& 0& 0\\ -0.006& -0.008& 0& -1.328& 0& 0& 0& 0& 0.93& 0.132& 0& 0\end{array}\right]}^T$

Step 2: 5 light stream sensor multiple spots are arranged on the aircraft, and point to different directions;

Step 3: according to each light stream sensor in carry-on installation site and direction; Set up the measurement equation of light stream sensor; Utilize inertial navigation device---the rate gyro that carries on the aircraft, a rate gyro perhaps is installed on aircraft in addition, set up the measurement equation of rate gyro; With the measurement equation of light stream sensor, the light stream of construction system and inertial navigation multisensor measurement equation;

Before the measurement equation of derivation light stream sensor, the several coordinate systems of definition are as shown in Figure 4: local coordinate system (S earlier _u): the local coordinate system and the earth are connected, and are used for representing the absolute status of MAV.Select NED (North-East-Down, east northeast ground) coordinate system here for use.This coordinate system and earth surface are connected, and the x axle refers to north, and the y axle refers to east, and the z axle refers to ground.Ignore earth rotation, local coordinate system can be regarded inertial coordinates system as.It mainly is used for representing the absolute status of MAV, such as absolute velocity, absolute attitude etc.

Body coordinate system (S _b): body coordinate system is connected on the MAV, and its initial point is at the barycenter place of MAV, and the x axle points to the place ahead of MAV, the z axle along the vertical plane of symmetry of MAV down, the y axle is confirmed by the right-hand rule.

Light stream sensor coordinate system (S _f): the light stream sensor coordinate system is connected on the light stream sensor, and its initial point is in the along of camera lens, and the z axle is outside optical axis direction points to, and x axle and y axle overlap with the light stream of two orthogonal directionss that record respectively.

So the transition matrix that is tied to body coordinate system by local coordinate is:

$L_{\mathrm{bu}}=L_x\left(\mathrm{\&gamma;}\right)L_y\left(\mathrm{\&theta;}\right)L_z\left(\mathrm{\&psi;}\right)$

$=\left(\begin{array}{ccc}\cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\sin \mathrm{\&theta;}\\ \sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}+\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}\\ \cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}-\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right)---\left(6\right)$

By body coordinate system be to the transition matrix of light stream sensor coordinate system:

$L_{\mathrm{fb}}=L_y\left(\mathrm{\&eta;}\right)L_x\left(\mathrm{\&mu;}\right)$

$=\left(\begin{array}{ccc}\cos \mathrm{\&eta;}& \sin \mathrm{\&eta;}\sin \mathrm{\&mu;}& -\sin \mathrm{\&eta;}\cos \mathrm{\&mu;}\\ 0& \cos \mathrm{\&mu;}& \sin \mathrm{\&mu;}\\ \sin \mathrm{\&mu;}& -\cos \mathrm{\&eta;}\sin \mathrm{\&mu;}& \cos \mathrm{\&eta;}\cos \mathrm{\&mu;}\end{array}\right)---\left(7\right)$

Here, μ and η are the established angles of light stream sensor, and they are light stream sensor coordinate system Eulerian angle with respect to body coordinate system, that is to say, with body coordinate system along x _bAxle rotational angle μ, and then along y _bAxle rotational angle η can obtain the light stream sensor coordinate system.

Therefore, the transition matrix that is tied to the light stream sensor coordinate system by local coordinate is:

L _fu=L _fbL _bu?(8)

Definition f _xAnd f _yThe light stream component of two orthogonal directionss that the light stream sensor records, then they are respectively along x _fAnd y _fDirection, so measurement equation can be write as:

$f=\left(\begin{array}{c}{f}_x\\ f_y\end{array}\right)=\left(\begin{array}{c}\frac{{\left(V_f\right)}_{f,x}}{d_{\mathrm{fg}}}+{\left({\mathrm{\&omega;}}_f\right)}_{f,y}\\ \frac{{\left(V_f\right)}_{f,y}}{d_{\mathrm{fg}}}+{\left({\mathrm{\&omega;}}_f\right)}_{f,x}\end{array}\right)---\left(9\right)$

Here, V _fAnd ω _fBe respectively the velocity and the angular velocity vector of light stream sensor, subscript _{F, x}With _{F, y}Represent x component and y component in the light stream sensor coordinate system respectively.d _FgFor the focus of light stream sensor along z _fDistance to ground.Make r _UbBe S _bWith respect to S _uPosition vector, r _BfBe S _fWith respect to S _bPosition vector, so the velocity of light stream sensor can be expressed as:

$V_f=\frac{{\mathrm{dr}}_{\mathrm{uf}}}{\mathrm{dt}}=\frac{d}{\mathrm{dt}}(r_{\mathrm{ub}}+r_{\mathrm{bf}})=\frac{{\mathrm{dr}}_{\mathrm{ub}}}{\mathrm{dt}}+\frac{{\mathrm{dr}}_{\mathrm{bf}}}{\mathrm{dt}}---\left(10\right)$

Here, r _UfBe S _fWith respect to S _uPosition vector.

With velocity V _fTo S _fProjection:

${\left(V_f\right)}_f=L_{\mathrm{fu}}\frac{d{\left(r_{\mathrm{ub}}\right)}_u}{\mathrm{dt}}+L_{\mathrm{fb}}(\frac{{d\left(r_{\mathrm{bf}}\right)}_b}{\mathrm{dt}}+{\left({\mathrm{\&omega;}}_b\right)}_b\&times;{\left(r_{\mathrm{bf}}\right)}_b)---\left(11\right)$

$=L_{\mathrm{fu}}\frac{d{\left(r_{\mathrm{ub}}\right)}_u}{\mathrm{dt}}+L_{\mathrm{fb}}{\left({\mathrm{\&omega;}}_b\right)}_b\&times;{\left(r_{\mathrm{bf}}\right)}_b$

It is pointed out that (ω _b) _bCan record through three rate gyros that are installed on the MAV.

If z _fDirection vector be k _f, (k then _f) _f=(0 0 1) ^T, with k _fTo S _uProjection gets:

(k _f) _u=L _uf(k _f) _f (12)

z _fWith z _uBetween the cosine value of angle be (k _f) _{U, z}, so from light stream sensor focus to ground along z _fThe distance of direction is:

$d_{\mathrm{fg}}=\left|\frac{{\left(r_{\mathrm{uf}}\right)}_{u,z}}{{\left(k_f\right)}_{u,z}}\right|$

$=-\frac{{(r_{\mathrm{ub}}+r_{\mathrm{bf}})}_{u,z}}{{\left(k_f\right)}_{u,z}}---\left(13\right)$

$=-\frac{{\left(r_{\mathrm{ub}}\right)}_{u,z}{\left[L_{\mathrm{ub}}{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}{{\left(k_f\right)}_{u,z}}$

And

(ω _f) _f＝(ω _b) _f

(14)

=L _fb(ω _b) _b

Formula (11), (13) and (14) substitutions (9) can be obtained the measurement equation of i light stream sensor, and system always measures equation and does.

$Z={\left(\begin{array}{ccccc}{f}_1^T& f_2^T& ...& f_n^T& {\left({\mathrm{\&omega;}}_b\right)}_b^T\end{array}\right)}^T+v\left(t\right)---\left(15\right)$

Here, f _i(i=1,2 ..., n) represent the measured value of i light stream sensor.In the formula, v (t) is a measurement noise, supposes that it is that average is 0 white noise, i.e. E [v (t)]=0, and E [v (t) v ^T(τ)]=r _vδ (t-τ), r _vVariance intensity battle array for v (t)., because the light stream number of sensors is 5, can survey 10 light stream components here, rate gyro can be surveyed 3 angular velocity components, so total dimension that measures equation is 13.

Step 4: select for use UKF that the state of flight of aircraft is estimated.

According to the UKF algorithmic rule, because system state dimension and system noise dimension are 12, the dimension of measurement equation is 13, so the dimension of augmented state vector is L=12+12+13=37, the sampling policy that Sigma is ordered is selected symmetric sampling for use, and its number is 2L+1=75.Other some starting condition have:

Installation site and the direction of 5 light stream sensors on MAV is with (x _by _bz _bμ η) form provides, and forms matrix M _{5 * 5}:

${\mathrm{<figure-callout\; id="5"\; label="M"\; filenames="HDA00001750515800042.png"\; state="\{\{state\}\}">M</figure-callout>}}_{5\&times;5}=\left(\begin{array}{ccccc}0& 0& 0.1& 0& 0\\ 0.2& 0& 0.1& 0& 30/57.3\\ -0.2& 0& 0.1& 0& -30/57.3\\ 0& 0.5& 0.1& -30/57.3& 0\\ 0& -0.5& 0.1& 30/57.3& 0\end{array}\right)---\left(16\right)$

MAV kinetic model initial value:

x ₀=(11?0?0?0?13?5/57.3?0?5/57.3?0?0?5/57.3?5/57.3) ^T；

The initial value that UKF estimates:

${\mathrm{\&chi;}}_0^x={\left(\begin{array}{cccccccccccc}10& 10/57.3& 10/57.3& 0& 15& 0& 0& 0& 0& 0& 0& 0\end{array}\right)}^T.$

By simulation process block diagram shown in Figure 5, through numerical simulation, the metric data filter effect that obtains is as shown in Figure 6.The output of light stream sensor contains noise, and these noise parts are directly from measurement noise, and another part secondary source is in process noise.As can be seen from Figure 6, UKF can suppress noise effectively, and Fig. 7 has explained this conclusion equally.

UKF is as shown in Figure 8 to the state estimation effect of MAV.At initial time, estimated value is that actual value exists certain deviation, but passing in time; This dwindles partially rapidly, and is final, and estimated value is consistent with the actual value trend; And all converging on expectation value, this explanation UKF can estimate speed, height and the orientation of MAV quickly and effectively.Fig. 9 and Figure 10 have proved the validity that UKF estimates other state parameter of MAV.

Claims (3)

1. one kind is utilized light stream sensor and rate gyro to the method for aircraft state estimation, and it is characterized in that: these method concrete steps are following:

Step 1:, set up its linearize disturbance motion equation to the aircraft that needs to install the light stream sensor;

$\stackrel{\&CenterDot;}{X}=\mathrm{AX}+\mathrm{Bu}+w\left(t\right)---\left(1\right)$

Here, X=[V _Tα Q h β θ ψ c p r ψ γ] ^T, $u={\left[\begin{array}{ccc}{\mathrm{\&delta;}}_{\mathrm{Th}}& {\mathrm{\&delta;}}_{\mathrm{Ev}}^R& {\mathrm{\&delta;}}_{\mathrm{Ev}}^L\end{array}\right]}^T;$

V _T: flight speed (m/s)

α, β: the angle of attack and yaw angle (rad)

γ, ψ: roll angle, the angle of pitch and crab angle (rad)

R, q, p: angular velocity in roll, rate of pitch and yaw rate (rad/sec)

H: flying height (m)

θ, ψ _c: trajectory tilt angle and trajectory deflection angle (rad)

δ _Th: throttle push rod angle (rad)

left rudder drift angle and right standard rudder drift angle (rad)

W (t): white-noise process, E [w (t)]=0, E [w (t) w ^T(τ)]=q _wδ (t-τ), q _wVariance intensity battle array for w (t);

Step 2: 5 light stream sensor multiple spots are arranged on the aircraft, and under the situation of space permission, the distance between each sensor is far away as far as possible, and points to different directions, does like this to help improving follow-up estimated accuracy;

Step 3: according to each light stream sensor in carry-on installation site and direction; Set up the measurement equation of light stream sensor; Utilize inertial navigation device---the rate gyro that carries on the aircraft, three rate gyros perhaps are installed on aircraft in addition, set up the measurement equation of rate gyro; With the measurement equation of light stream sensor, the light stream of construction system and inertial navigation multisensor measurement equation;

In fact, a light stream sensor can be measured two light stream components on the orthogonal directions simultaneously, and it measures output and can be designated as

$f=\left(\begin{array}{c}{f}_x\\ f_y\end{array}\right)---\left(2\right)$

And aircraft rate of pitch

can be measured by rate gyro, so the system measurements equation is:

$Z={\left(\begin{array}{ccccc}{f}_1^T& f_2^T& ...& f_n^T& {\mathrm{\&omega;}}^T\end{array}\right)}^T+v\left(t\right)---\left(3\right)$

In the formula, v (t) is a measurement noise, supposes that it is that average is 0 white noise, i.e. E [v (t)]=0, and E [v (t) v ^T(τ)]=r _vδ (t-τ), r _vVariance intensity battle array for v (t);

Step 4: with UKF is that Unscented kalman filtering is estimated the state of flight of aircraft.

2. a kind of method of utilizing light stream sensor and rate gyro to the aircraft state estimation according to claim 1; It is characterized in that: " multiple spot layout " described in the step 2 is meant that the light stream sensor will be installed in the diverse location of aircraft, and exemplary position is head, centre, afterbody and wingtip.

3. a kind of method of utilizing light stream sensor and rate gyro to the aircraft state estimation according to claim 1; It is characterized in that: " distance is far away as far as possible " described in the step 2 is meant the light stream sensor that is installed in head, afterbody or wingtip; Under the situation that does not influence other airborne equipment; Will near body foremost, rearmost end or side, so just guaranteed that the distance between head, afterbody and wingtip light stream sensor is big as far as possible.

Images