CN102692225B

CN102692225B - Attitude heading reference system for low-cost small unmanned aerial vehicle

Info

Publication number: CN102692225B
Application number: CN201110072606.2A
Authority: CN
Inventors: 杜小菁; 李蒙; 李怀建; 涂海峰; 郭康
Original assignee: Beijing Institute of Technology BIT
Current assignee: Beijing Institute of Technology BIT
Priority date: 2011-03-24
Filing date: 2011-03-24
Publication date: 2015-03-11
Anticipated expiration: 2031-03-24
Also published as: CN102692225A

Abstract

The invention relates to an attitude heading reference system for a low-cost small unmanned aerial vehicle. The attitude heading reference system comprises an angular rate gyro, an accelerometer, a global position system (GPS), an angular rate gyro operation module, an aiding module and a Kalman filter, wherein the angular rate gyro is used for measuring a roll angular rate, a pitch angular rate and a yaw angular rate of an aerial vehicle; the accelerometer is used for measuring a weight component of gravity in an aerial vehicle coordinate axis system; the GPS is used for measuring a track azimuth of an aerial vehicle; the angular rate gyro operation module is used for computing a pitch angle theta 1, a yaw angle psi 1 and a roll angle fai 1 with offsets; the aiding module is used for estimating an estimated roll angle fai 2 and an estimated pitch angle theta 2 and using the track azimuth measured by the GPS as an estimated yaw angle psi 2; and the Kalman filter is used for fusion of data produced by the angular rate gyro operation module and the aiding module to acquire a final attitude angle [fai theta psi]<T>. The attitude heading reference system reduces a cost of a small unmanned aerial vehicle system and a complex degree of a heading reference system and improves the precision of attitude estimation.

Description

Attitude heading reference system for low-cost small unmanned aerial vehicle

Technical Field

The invention relates to an attitude and heading reference system, in particular to an attitude and heading reference system for a low-cost small unmanned aerial vehicle based on a sensor fusion technology.

Background

An Attitude and Heading Reference System (AHRS), referred to as an Attitude and Heading Reference System for short, is used for determining the orientation of a moving carrier in space. The attitude information provided by the attitude heading reference system can be widely applied to navigation, guidance and control. For an unmanned aerial vehicle flying autonomously, a navigation attitude system is particularly important, and provides necessary attitude feedback for an inner loop of a control system. Generally, the determination of the attitude information depends on a high-precision angular rate gyro, however, the high-precision gyro has the defects of high cost, heavy weight, complex structure and the like, and obviously cannot be applied to a low-cost small unmanned aerial vehicle system. Therefore, under the constraint condition of a low-cost sensor, providing a set of attitude and heading reference system with small calculated amount and high reliability for a small unmanned aerial vehicle system is always a problem of extensive research in related fields at home and abroad.

Typically, the attitude of the motion carrier is described by the euler angles (phi, theta, psi). According to the relation between the angular rate of carrier motion and the Euler angle change rate, the measurement value of the angular rate gyroscope can be integrated to obtain the attitude information of the carrier, but the method has higher requirement on the gyroscope precision (the output error is less than 0.1 degree/h). The low-cost small unmanned aerial vehicle usually adopts an MEMS (micro electric mechanical System) sensor, although the sensor has the characteristics of low cost, small size and the like, the accuracy of the sensor is poor, the output error is between 10 degrees and 100 degrees/h, and the output value of an integral gyro can generate large drift, so the sensor cannot be directly used for attitude estimation.

The attitude information may also be determined using observation vectors independent of the angular rate gyro. The basic principle of determining the attitude using the observation vectors is: and (3) superposing the two coordinate systems by using two non-collinear observation vectors under the two coordinate systems through three times of rotation, and then calculating to obtain the Euler angle. Some literature has studied such gyroscopic-Free Systems (Gyro-Free Systems). For example, attitude information is obtained by solving the Wahba problem using an accelerometer, a magnetometer, and a gps (global Positioning system) as sensors; and the attitude estimation can also be carried out by adopting GPS carrier phase observed quantity through a multi-antenna GPS. However, in many practical cases, such gyroscopic attitude systems cannot be used alone. Firstly, a gyroscopic attitude and heading system cannot provide attitude information with high bandwidth; secondly, when the carrier makes a specific maneuver, the attitude information estimated by using the method has a large error. For example, when the unmanned aerial vehicle is in a banked turn, the accelerometer is sensitive to centripetal acceleration besides gravity acceleration, so that the calculated attitude is inaccurate; and finally, the attitude information given by the gyro-free attitude heading reference system contains relatively large noise.

For the above reasons, a Sensor Fusion (Sensor Fusion) technology may be used to fuse the angular rate gyroscope with a gyroscope-free system composed of an accelerometer, a magnetometer, a GPS, and the like, so that they complement each other to make up for the strong points and the weak points. Because the angular rate gyroscope can provide high-bandwidth biased attitude information, the method can be used for smoothing a gyro-free system; the gyroscopic System is used to correct the drift of the angular rate gyro, and therefore, the gyroscopic System is also called a correction System (Aiding System). Thus, by combining the two with appropriate filters, a high bandwidth and unbiased pose information solution can be obtained. The core of the sensor fusion technology is a filter for fusing different sensors, and a method for measuring the attitude by using the filter to fuse an angular rate gyroscope and a gyroscope-free system is widely applied to the attitude determination problem of satellites for decades.

For low-cost small drones, although sensor fusion technology has been widely applied to pose determination, there are still some problems: firstly, sensors for attitude estimation in a small unmanned aerial vehicle system are generally an angular rate gyro, an accelerometer and a magnetic heading meter (for measuring a heading angle), and the magnetic heading meter has high manufacturing cost and is not suitable for a low-cost unmanned aerial vehicle system; secondly, although some unmanned aerial vehicle attitude estimation systems adopt a GPS to replace a magnetic direction finder, the cost is reduced, because the GPS outputs angles, the attitude estimation algorithm is established on the basis of an Euler angle method, the method needs to solve a trigonometric function in real time in calculation, and singularities can occur in some times, so that the method is not suitable for a low-cost flight control computer with poor calculation capability. At present, a solution capable of well solving attitude estimation when the unmanned aerial vehicle is in maneuvering overload is not seen.

Disclosure of Invention

The invention aims to provide a set of attitude and heading reference system scheme aiming at the defects of the prior art, and the small unmanned aerial vehicle attitude information estimation is realized only by low-cost sensors including an angular rate gyroscope, a GPS and an accelerometer.

The invention provides an attitude and heading reference system for a low-cost small unmanned aerial vehicle, which comprises an angular rate gyro, an accelerometer, a GPS, an angular rate gyro operation module, a correction module and a Kalman filter, wherein:

the angular rate gyroscope measures the roll rate, the pitch angle rate and the yaw rate of the airplane;

the accelerometer measures the component of gravity under the coordinate axis of the body;

GPS measures the track azimuth of the aircraft;

the angular rate gyro operation module obtains a roll angle phi with offset through the roll angle rate, the pitch angle rate and the yaw angle rate measured by the angular rate gyro₁Angle of pitch theta₁And yaw angle psi₁；

The correction module obtains the roll angle phi estimated by the correction module through the body axis component of the gravity acceleration measured by the accelerometer₂Angle of pitch theta₂And taking the azimuth angle of the flight path measured by the GPS as the estimated yaw angle psi₂；

The Kalman filter fuses data generated by the angular rate gyro operation module and the correction module to obtain a final attitude angle phi theta psi]^T。

Advantageous effects

The invention has the advantages that: the cost of the small unmanned aerial vehicle system is reduced by using the low-cost sensor; the complexity of the traditional attitude heading reference system is reduced; the accuracy and reliability of the attitude and heading estimation of the system are improved by means of a sensor fusion technology; the problem of inaccurate attitude estimation when unmanned aerial vehicle appears transshipping is solved.

Drawings

FIG. 1 is a block diagram of an attitude and heading reference system;

FIG. 2 is a roll angle solution under static conditions;

FIG. 3 is a pitch angle solution under static conditions;

FIG. 4 is an angular rate gyro drift estimate;

FIG. 5 is a graph of roll angle error and its standard deviation;

FIG. 6 is a graph of roll rate gyro drift error and its standard deviation;

FIG. 7 is an estimated roll angleComparing the error with the error of the real rolling angle phi;

FIG. 8 is a roll angle estimation based on flight tests, wherein (a) the roll angle is estimated for a time window method and (b) the roll angle is estimated for conventional filtering and gyro integration;

FIG. 9 is a flight test based pitch estimation, where (a) the pitch is estimated for a time window method and (b) the pitch is estimated for conventional filtering and gyro integration;

FIG. 10 is a filter switch flag;

fig. 11 shows the filter switch trigger and threshold values.

Detailed Description

Preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

The embodiment realizes an attitude and heading reference system for a low-cost small unmanned aerial vehicle, and the system consists of an angular rate gyro subsystem, a correction subsystem and a Kalman filter. FIG. 1 shows a block diagram of an attitude heading reference system. The embodiment adopts an error quaternion method to establish the model, and relieves the operation load of the onboard processor.

1) Angular rate gyro operation module

The attitude of a rigid body in three-dimensional space is typically described by three euler angles, which are roll angle phi, pitch angle theta and yaw angle psi, respectively. The attitude angle describes the relative relationship between two different coordinate systems. In navigation, guidance and control, a body coordinate axis S is generally set for an aircraft moving near the ground_bthe-Oxyz system is selected as a moving coordinate system and a navigation coordinate axis system S_n–Ox_ny_nz_nSelected as a reference coordinate system, defined as the North-East-Down (NED) reference System, x_nIn the north direction, y_nIn east direction, z_nPointing to the earth's center.

Defining an arbitrary vector u, which is respectively expressed as u in a navigation coordinate axis system and a body coordinate axis systemⁿAnd u^b. The projection transformation of the vector u under the two different coordinate axes is realized by a direction cosine matrix:

u^{b} = C_{n}^{b} u^{n} - - - (1)

in the formulaIs a Direction Cosine Matrix (DCM), also called a pose Matrix, usually defined in the form of euler angles.

In practical applications, there are several problems in using euler angles to represent gestures. First, in some special cases, the individual attitude angle becomes uncertain, the kinematic equation becomes singular, for example, when the pitch angle θ is ± 90 °, the yaw angle ψ is uncertain, and the equation d ψ/dt is singular. Secondly, in actual solution, the euler angle method needs to solve a large number of trigonometric functions, which inevitably brings computational burden to a processor. If the attitude quaternion method is used, on one hand, the singularity of the motion equation can be avoided, on the other hand, the trigonometric function does not need to be calculated, the processor resource is saved, and the calculation efficiency is improved. In order to overcome the above disadvantages, the present embodiment uses quaternion modeling.

Defining an attitude quaternion q:

<math> <mrow> <mi>q</mi> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mover> <mi>q</mi> <mo>&RightArrow;</mo> </mover> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> </math>

wherein q is₀A scalar section called the quaternion q,

<math> <mrow> <mover> <mi>q</mi> <mo>&RightArrow;</mo> </mover> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mn>1</mn> </msub> </mtd> <mtd> <msub> <mi>q</mi> <mn>2</mn> </msub> </mtd> <mtd> <msub> <mi>q</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </math>

called the vector portion of the quaternion q.

The directional cosine array in quaternion is defined as follows:

C_{n}^{b} (q) = [\begin{matrix} 1 - 2 (q_{2}^{2} + q_{3}^{2}) & 2 (q_{1} q_{2} + q_{3} q_{0}) & 2 (q_{1} q_{3} - q_{2} q_{0}) \\ 2 (q_{1} q_{2} - q_{3} q_{0}) & 1 - 2 (q_{1}^{2} + q_{3}^{2}) & 2 (q_{2} q_{3} + q_{0} q_{1}) \\ 2 (q_{1} q_{3} + q_{2} q_{0}) & 2 (q_{2} q_{3} - q_{1} q_{0}) & 1 - 2 (q_{1}^{2} + q_{2}^{2}) \end{matrix}] - - - (3)

the quaternion differential equation is determined by:

wherein p is the roll rate, q is the pitch rate, and r is the yaw rate.

As can be known from the quaternion differential equation (4), the quaternion can be updated in real time through the measurement value of the angular rate gyro, and then the attitude angle of the carrier is obtained according to the direction cosine matrix (3) expressed by the attitude quaternion

<math> <mrow> <mfenced open='' close=''> <mtable> <mtr> <mtd> <mi>φ</mi> <mo>=</mo> <mi>arctan</mi> <mfrac> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <mn>1</mn> <mo>-</mo> <mn>2</mn> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> </mrow> </mfrac> </mtd> </mtr> <mtr> <mtd> <mi>θ</mi> <mo>=</mo> <mi>arcsin</mi> <mn>2</mn> <mrow> <mo>(</mo> <mo>-</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mi>ψ</mi> <mo>=</mo> <mi>arctan</mi> <mfrac> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>3</mn> </msub> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <mn>1</mn> <mo>-</mo> <mn>2</mn> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> </mrow> </mfrac> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow> </math>

2) Correction module

Although there is an unbounded error in the attitude angle integrated from the angular rate gyro measurements, most AHRS systems are indispensable because the angular rate gyro can provide a high bandwidth output. Drift resulting from gyro integration can be suppressed by the correction system because the sensors that make up the correction system have the characteristic of output error being bounded. The correction system may periodically "reset" the attitude information provided via the angular rate gyro for correction purposes. In the embodiment, the accelerometer and the GPS are selected to form a correction system together.

The attitude angle can be obtained by observing the projection of the gravity vector under the coordinate axis of the body through the accelerometer, and the output vector of three axes is assumed to be f according to the principle of the accelerometer^bCan represent

f^{b} = a_{ib}^{b} - g_{b} - - - (6)

Wherein,the component of the motion acceleration of the carrier relative to the inertial system under the coordinate axis of the body, g_bIs the component of gravity acceleration vector under the coordinate axis system of the body

Wherein, the projection of the gravity acceleration vector under the navigation coordinate axis system can be expressed as g_n＝[0 0 g]^TAnd g is a gravitational acceleration constant.

In thatWhen the carrier is stationary or moving at a constant linear speed, f^bBecome into

At this time, the pitch angle θ and the roll angle φ may be determined by the above equation

<math> <mrow> <mi>θ</mi> <mo>=</mo> <mi>arcsin</mi> <mfrac> <msub> <mi>f</mi> <mi>x</mi> </msub> <mi>g</mi> </mfrac> </mrow> </math>

(9)

<math> <mrow> <mi>φ</mi> <mo>=</mo> <mi>arcsin</mi> <mfrac> <msub> <mrow> <mo>-</mo> <mi>f</mi> </mrow> <mi>y</mi> </msub> <mrow> <mi>g</mi> <mi>cos</mi> <mi>θ</mi> </mrow> </mfrac> </mrow> </math>

Equation (8) does not contain yaw angle ψ, and thus only pitch angle θ and roll angle Φ can be determined by only an accelerometer. To ensureAnd determining the yaw angle psi and introducing a heading signal provided by a GPS. It should be noted that GPS can only provide track azimuthBut may be used instead of yaw angle psi in case of a small crosswind.

3) Kalman filter

In the embodiment, a kalman filter is selected as a sensor fusion algorithm that combines an angular rate gyro and a correction system.

The filter comprises the following working steps: after the initial conditions are determined, attitude estimation can be performed by using the measurement data integration of the angular rate gyro, and the step is called time updating, and high-bandwidth attitude information is provided. In the time updating stage, because the gyro data has noise, the error of the estimated attitude is accumulated along with the time, and the attitude cannot be determined by simple gyro data integration. In order to suppress the gyro error, a correction system needs to be introduced to periodically correct the gyro data, and the step is called "measurement update". The covariance matrix of the state errors and the gyro drift estimate are also corrected at this step. Next, a new round of time update is performed, and so on periodically.

The true attitude quaternion q may be expressed as an estimated quaternionAnd error quaternion q_eForm of multiplication, i.e. taking the error quaternion q as_eIs to estimate a quaternionRotation to true quaternion q. The error quaternion is a small non-zero quantity due to the error of the angular rate gyro. q. q.s_e，The relationship of q to q is determined by:

<math> <mrow> <mi>q</mi> <mo>=</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&CircleTimes;</mo> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow> </math>

the error quaternion can be approximated as:

<math> <mrow> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>&RightArrow;</mo> </mover> <mi>e</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow> </math>

the formula (11) is derived, and a quaternion error differential equation (4) is utilized to obtain a quaternion error differential equation through linearization

<math> <mrow> <msub> <mover> <mi>q</mi> <mover> <mo>&RightArrow;</mo> <mo>·</mo> </mover> </mover> <mi>e</mi> </msub> <mo>=</mo> <mo>-</mo> <mo>[</mo> <mover> <mi>ω</mi> <mo>&RightArrow;</mo> </mover> <mo>×</mo> <mo>]</mo> <msub> <mover> <mi>q</mi> <mo>&RightArrow;</mo> </mover> <mi>e</mi> </msub> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mrow> <mo>(</mo> <mrow> <mover> <mi>ω</mi> <mo>^</mo> </mover> <mo>-</mo> <mi>ω</mi> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow> </math>

Wherein,representing the difference between the angular rate gyro measurement and the true value,is determined by

<math> <mrow> <mo>[</mo> <mover> <mi>ω</mi> <mo>&RightArrow;</mo> </mover> <mo>×</mo> <mo>]</mo> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mi>r</mi> </mtd> <mtd> <mo>-</mo> <mi>q</mi> </mtd> </mtr> <mtr> <mtd> <mo>-</mo> <mi>r</mi> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mi>p</mi> </mtd> </mtr> <mtr> <mtd> <mi>q</mi> </mtd> <mtd> <mo>-</mo> <mi>p</mi> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow> </math>

The observation quantity of the correction subsystem is an error angle and can be obtained by the following method: firstly, the attitude angle of the carrier is calculated according to the formula (9) by utilizing the measurement information of the accelerometer and the GPS, and then the attitude angle is subtracted from the attitude angle estimated value calculated by the formula (5) to obtain the angle error

In the formula，[φ θ ψ]^TIs an error angle of phi_AS θ_AS ψ_AS]^TIn order to correct the attitude angle obtained by the system,

is the attitude angle calculated by the quaternion method.

The relationship between the angular error and the attitude quaternion is determined by

<math> <mrow> <msub> <mover> <mi>q</mi> <mo>&RightArrow;</mo> </mover> <mi>e</mi> </msub> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mfrac> <mi>δφ</mi> <mn>2</mn> </mfrac> </mtd> <mtd> <mfrac> <mi>δθ</mi> <mn>2</mn> </mfrac> </mtd> <mtd> <mfrac> <mi>δψ</mi> <mn>2</mn> </mfrac> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow> </math>

The state quantity of the correction subsystem is selected as a vector of error quaternionsIn partAnd drift of three angular rate gyros b_p，b_qAnd b_r

<math> <mrow> <mi>X</mi> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>&RightArrow;</mo> </mover> <mi>e</mi> </msub> </mtd> <mtd> <msub> <mi>δb</mi> <mi>p</mi> </msub> </mtd> <mtd> <msub> <mi>δb</mi> <mi>q</mi> </msub> </mtd> <mtd> <msub> <mi>δb</mi> <mi>r</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow> </math>

The state equation of the system can be constructed

Wherein A (t) is a system matrix defined as follows

<math> <mrow> <mi>A</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mo>-</mo> <mo>[</mo> <mover> <mi>ω</mi> <mo>&RightArrow;</mo> </mover> <mo>×</mo> <mo>]</mo> </mtd> <mtd> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>×</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>×</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <mo>-</mo> <mfrac> <mn>1</mn> <mi>τ</mi> </mfrac> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>×</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow> </math>

The input matrix B (t) and the noise matrix W (t) are defined as follows

The formula (17) can be discretized into the following form

X_k+1＝F_kX_k+G_kW_k (20)

Wherein, F_kBeing a state transition matrix, G_kDriving a matrix for system noise

F_k＝I+A(t)△t

(21)

G_k＝B(t)△t

Writing the observation equation into the following discretization form

Z_k＝H_kX_k+V_k (22)

Wherein Z [ phi ] theta ψ [ ]]^TFor the observed quantity, V is the observed noise, and H is the observed matrix, which can be determined by equation (15)

H＝[2I_3×3 0_3×3] (23)

And in the measurement updating stage of Kalman filtering, correcting attitude information provided by an accelerometer and a GPS angular rate gyro system. The precondition for correcting the gyro drift by using the accelerometer is that the carrier is in a static or uniform linear motion state, and because additional acceleration does not exist, the accelerometer senses only the component of the gravity acceleration on the coordinate axis of the body, and the pitch angle theta and the roll angle phi can be calculated by using the formula (9). However, when the vehicle is maneuvered, the attitude estimate is biased by the additional acceleration sensed by the accelerometer. Therefore, a certain compensation method must be adopted to reduce the attitude angle error occurring when the carrier maneuvers. Two solutions are presented below and analyzed.

a) Overload compensation

When the airplane performs a banked turn, the centripetal acceleration can cause the measurement error of the accelerometer, so that the attitude estimation is inaccurate. For ease of analysis, the coordinated turns of the aircraft may be considered circular motion.

The centripetal acceleration of the particles in circular motion is determined by

a＝ω·V (24)

Where ω represents the angular velocity of the particle about the center of the circle and V represents the particle velocity. Obviously, the velocity vector, the angular velocity rotation axis and the acceleration vector are orthogonal. Outside the transition, the velocity vector of the aircraft is considered to be along the x-axis of the airframe. Thus, since there is no velocity component in the y-axis and z-axis directions of the body, the accelerometer in the x-axis direction of the body will not sense centripetal acceleration. The angular rate gyro and the accelerometer are installed along the coordinate axis of the body, and the airspeed head measures the vacuum speed V_TASAnd is also along the x-axis direction of the body, so the measurement data of the angular rate gyro and the airspeed head can be directly used for correcting the accelerometer. Because the accelerometer a along the z-axis direction of the body is not used in the Kalman filtering algorithm_zHere, only the accelerometer a in the y-axis direction of the body needs to be considered_yAnd (4) correcting.

The angular velocities p and r of the body in the directions of the x-axis and the z-axis cause the acceleration a of the body in the direction of the y-axis_y. However, if the centripetal acceleration equation is written in the form of angular velocity around the center of a circle and the radius of the circle (as follows), it can be seen that if the radius is small, the resulting acceleration will also be small.

a＝ω²R (25)

The body roll due to aileron deflection is always along the body x-axis, while the accelerometer a_yCloser to the x-axis. Thus, the acceleration in the y-axis direction due to the angular velocity p around the x-axis is also negligible, and the additional acceleration due to the turning is entirely due to the yaw rate r and the vacuum rate V around the z-axis_TASDetermining

a_y＝r·V_TAS (26)

Therefore, the formula (9) after the correction can be written as follows

(27)

<math> <mrow> <mi>φ</mi> <mo>=</mo> <mi>arcsin</mi> <mfrac> <mrow> <msub> <mrow> <mo>-</mo> <mi>f</mi> </mrow> <mi>y</mi> </msub> <mo>+</mo> <msub> <mi>rV</mi> <mi>TAS</mi> </msub> </mrow> <mrow> <mi>g</mi> <mi>cos</mi> <mi>θ</mi> </mrow> </mfrac> </mrow> </math>

A small unmanned aerial vehicle for executing a detection task is in a cruising state most of the time, and performs tilt turning when the course is required to be changed.

b) Time window

When the airplane is flying, the airplane can be switched between a cruising state and a maneuvering state. During cruising, the aircraft can be approximately considered not to be overloaded, and the correction system can be used for correcting gyro drift. When the aircraft is maneuvering, the correction system is not available due to the disturbance of the additional acceleration. In general, the time for the aircraft to make a continuous maneuver is not very long, and the accumulation of gyro errors is small, so that the kalman filter can be considered to be turned off, and then turned on again when the maneuver is finished.

The angular rate may vary significantly as the aircraft turns, and therefore the norm of the angular rate is selectedAs a flag for triggering the Kalman filter, the opening and closing of the time window is determined by

Wherein, beta is a time window threshold value, and flag is a window mark.

When u (t) is less than or equal to beta, the flag is 1, a window is opened, and the attitude is determined by using a Kalman filter; when u (t) > beta, the flag is 0, the window is closed, and at this time, the filter is disconnected, and the correction of the correction system is stopped.

The specific implementation steps of the attitude heading reference system for estimating the attitude information in the embodiment are as follows:

the first step is as follows: calculation of attitude quaternion using angular rate gyro measurement data

The second step is that: predicting the state error covariance matrix from F and G in equation (21)

P_{k}^{-} = F_{k - 1} P_{k - 1}^{+} F_{k - 1}^{T} + G_{k - 1} Q_{k - 1} G_{k - 1}^{T} - - - (30)

In the formula, Q_k-1Is a process noise covariance matrix.

The third step: in the correction system, the heading angle psi measured by (9) and GPS_GPSObtaining attitude angle information [ phi ] estimated by the correction system_AS θ_AS ψ_AS]^T。

The fourth step: system attitude angle information estimated using last time step

Differencing with the attitude angle information obtained by the correction system to obtain the observed quantity of the Kalman filter

The fifth step: computing kalman gain matrix

K_{k} = P_{k - 1} H_{k}^{T} {(H_{k} P_{k - 1} H_{k}^{T} + R_{k})}^{- 1} - - - (32)

In the formula, R_kA measured noise covariance matrix.

And a sixth step: updating state error covariance matrix

P_{k}^{+} = (I - K_{k} H_{k}) P_{k}^{-} {(I - K_{k} H_{k})}^{T} + K_{k} R_{k} K_{k}^{T} - - - (33)

The seventh step: updating state vectors

<math> <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>+</mo> </msubsup> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mover> <mi>q</mi> <mover> <mo>&RightArrow;</mo> <mo>^</mo> </mover> </mover> <mi>ek</mi> <mo>+</mo> </msubsup> </mtd> <mtd> <msubsup> <mrow> <mi>δ</mi> <mover> <mi>b</mi> <mo>^</mo> </mover> </mrow> <mi>pk</mi> <mo>+</mo> </msubsup> </mtd> <mtd> <msubsup> <mrow> <mi>δ</mi> <mover> <mi>b</mi> <mo>^</mo> </mover> </mrow> <mi>qk</mi> <mo>+</mo> </msubsup> </mtd> <mtd> <msubsup> <mrow> <mi>δ</mi> <mover> <mi>b</mi> <mo>^</mo> </mover> </mrow> <mi>rk</mi> <mo>+</mo> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>=</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>34</mn> <mo>)</mo> </mrow> </mrow> </math>

Eighth step: updating the attitude quaternion and angular rate gyro measurements obtained in the step using the estimated quaternion and gyro drift correction times

<math> <mrow> <msubsup> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>+</mo> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>-</mo> </msubsup> <mo>&CircleTimes;</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <msubsup> <mover> <mi>q</mi> <mover> <mo>&RightArrow;</mo> <mo>^</mo> </mover> </mover> <mi>ek</mi> <mo>+</mo> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </math>

The ninth step: quaternion from attitudeThe attitude angle phi theta phi can be calculated]^T。

The comprehensive performance of the invention is verified by static tests and flight tests.

1) Static state experiment

In static tests, an AHRS-400CC Inertial Measurement Unit (IMUs) available from Crossbow Technology was used to assist in performing the algorithmic tests. The AHRS-400CC comprises a triaxial accelerometer, an angular rate gyro and a magnetometer, and can output three Euler angles of roll, pitch and yaw through a built-in attitude estimation algorithm. In the experiment, the attitude is calculated by using gyroscope and accelerometer data collected by the AHRS-400CC, and the calculated attitude angle is compared with the real attitude output by the AHRS-400 CC. Since the attitude algorithm proposed herein uses the heading information provided by GPS to correct the yaw angle, and AHRS-400CC does not have a GPS sensor, only the pitch and roll verification is performed in static experiments. The AHRS-400CC is horizontally and statically placed, sensor data and attitude angle information are collected, and the experimental time is 160 s.

Fig. 2 and 3 show the results of the calculation of the attitude angle under static conditions. The solid line is the attitude angle obtained by the Kalman filtering algorithm, and the dotted line is the attitude angle obtained by the gyro integral. It can be seen that the attitude angle, integrated solely using the angular rate gyro measurements, drifts with increasing time due to the presence of measurement noise. Roll angle induced drift (about 0.0156/s) is greater than pitch angle drift (about 0.0031/s) due to the body's x-axis gyro drift being greater than the z-axis gyro drift. The attitude angle estimated by the kalman filter algorithm does not drift, and has a small oscillation amplitude. Table 1 shows the Root Mean Square Error (RMSE) comparison of the three algorithms. Therefore, the attitude angle estimated by the Kalman filtering algorithm can effectively inhibit the gyro drift through the correction of the accelerometer.

TABLE 1 attitude angle estimation Algorithm RMSE comparison

Diagonal rate gyro drift b_p，b_qThe estimate of (c) is shown in fig. 4. Solid line is b_pThe dotted line is b_q. As can be seen, the drift of the roll rate gyro is slightly larger than that of the pitch rate gyro, which is consistent with the results shown in fig. 2 and 3. The gyro drift eventually settles to a constant value.

FIGS. 5 and 6 show the roll angle error (solid line) φ and the roll angle rate gyro drift error (solid line) b, respectively, from the Kalman filtering algorithm_pAnd a corresponding 1 sigma standard deviation bound (dashed line). As with the previous analysis, the two figures show that the error of the Kalman filtering system is bounded. The 1 sigma standard deviation of the steady-state roll angle error is smaller than the noise standard deviation (0.2636 ℃) of the correction system, and the fact that the attitude angle estimation by adopting the sensor fusion algorithm is better than that of a pure angular rate gyro system or a correction system is proved again. The pitch angle error is similar to the roll angle error and will not be described herein.

Estimating roll angleError e between the same true roll angle phi_φAs shown in fig. 7. Dashed line is e generated using only angular rate gyro estimation_φAlthough the noise is small, the drift is large. The solid line is generated by Kalman filtering algorithm estimation_φIt can be seen that the attitude angle estimated by the sensor fusion technique combines the advantage of the gyro system's low noise correction system not drifting.

2) Flight test

In order to further verify the performance of the attitude and heading reference system in a real flight environment, after the mathematical simulation and the ground experiment, a flight test is required. The test procedure was as follows: a commercial inertial measurement unit AHRS-400CC is placed on a carrier unmanned aerial vehicle, and flight data including triaxial acceleration, triaxial angular velocity and flight time are recorded after the start of flight. In order to verify the attitude estimation capability of the attitude heading reference system in the carrier maneuvering state, the aircraft is controlled by the control hand to carry out large-maneuvering flight, the variation range of the rolling angle phi is +/-80 degrees, and the variation range of the pitch angle theta is +/-40 degrees. After the flight is finished, offline filtering calculation and analysis are carried out on the data collected by the AHRS-400CC by using an attitude estimation algorithm provided in the chapter, and attitude angle information is obtained. And finally, comparing and analyzing the attitude angle obtained by filtering with the attitude angle calculated by the self-built-in algorithm of the AHRS-400 CC. Because the AHRS-400CC attitude estimation algorithm has a solution to the carrier maneuvering condition, the output attitude angle is considered as the real airplane attitude.

According to the conclusion of simulation analysis, the overload compensation method is not available under the condition of large dynamic flight, so that the flight data is only processed by adopting a time window method in the section. Fig. 8 and 9 show the comparison between the estimated attitude angle and the true attitude angle, and three different attitude angle estimation methods are respectively adopted for comparison, including a time window method (dotted line), a gyro integral (dashed line), and a conventional kalman filter (dotted line). Because a GPS module is not used in the flight test, the yaw angle psi cannot be corrected, and only the roll angle phi and the pitch angle theta of the unmanned aerial vehicle are estimated.

As can be seen from fig. 8(a) and 9 (a), the attitude angle estimated by the time windowing method can be well matched to the true value. In the graph (b), the attitude angle obtained by gyro integration has a certain drift. The attitude angle calculated by means of the conventional kalman filter is seriously distorted because the unmanned aerial vehicle is in an overload state most of the time in a flight test, and the measurement data of the accelerometer cannot reflect attitude information faithfully, so that the gyro integral value cannot be corrected correctly. Table 2 shows the RMSE of the pose estimation for the three methods. Therefore, the time window method better solves the attitude estimation problem under the condition of carrier maneuver.

TABLE 2 attitude estimate RMSE comparison based on flight tests

Fig. 10 shows the flag amount for controlling the filter switch in the attitude estimation process, and fig. 11 shows the trigger amount and the threshold value (shown by reference numeral 1 in the figure) of the filter switch, where the trigger amount is the yaw rate r. It can be seen that a greater yaw rate is caused by a more vigorous course motion. When the trigger quantity is larger than the threshold value, the mark quantity is zero, the filter is disconnected, and the attitude is estimated by completely depending on the angular rate gyro. When the trigger quantity is lower than the threshold value, the flag quantity is set to be one, the Kalman filter is started, and at the moment, because the maneuvering overload is very small, the accelerometer can be used for correcting the estimated value, so that the attitude determination is completed.

In summary, the invention provides an attitude and heading estimation algorithm based on low-cost sensors (an angular rate gyro, an accelerometer and a GPS module). Firstly, a mathematical model of a moving carrier attitude and heading reference system based on error quaternion is deduced, and a sensor fusion technology based on Kalman filtering is adopted to improve the accuracy of the attitude and heading system. For a drone moving in space, the correction system cannot accurately estimate the attitude when it is in a maneuvering condition due to the presence of additional accelerations. This presents two solutions to this problem, time windowing and overload compensation. After the attitude and heading reference system scheme is determined, a static experiment and a flight experiment are respectively carried out to verify the correctness and feasibility of an attitude algorithm. The result analysis shows that the method can accurately estimate the space attitude of the unmanned aerial vehicle in the flight process, and can be applied to a low-cost small unmanned aerial vehicle system.

Claims

1. The utility model provides an gesture course reference system for low-cost unmanned aerial vehicle which characterized in that, includes angular rate top, accelerometer, GPS, angular rate top operation module, correction module and kalman filter, wherein:

GPS measures the track azimuth of the aircraft;

angular rate gyro operation modelThe block obtains a roll angle phi with offset through the roll angle rate, the pitch angle rate and the yaw angle rate measured by the angular rate gyroscope₁Angle of pitch theta₁And yaw angle psi₁；

The correction module obtains the roll angle phi estimated by the correction module through the component of the gravity measured by the accelerometer under the coordinate axis system of the body₂Angle of pitch theta₂And taking the azimuth angle of the flight path measured by the GPS as the estimated yaw angle psi₂；

The Kalman filter fuses data generated by the angular rate gyro operation module and the correction module to obtain a final attitude angle phi theta psi]^T；

The angular rate gyro operation module is modeled by using a quaternion method, and firstly, an attitude quaternion q is defined:

<math> <mrow> <mi>q</mi> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mover> <mi>q</mi> <mo>&RightArrow;</mo> </mover> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

wherein q is₀A scalar section called the quaternion q,

the vector part, called quaternion q, updates the quaternion in real time by the measurements of the angular rate gyro:

wherein p is a roll angle rate, q is a pitch angle rate, and r is a yaw angle rate;

then the attitude angle of the carrier is obtained by the following formula

<math> <mrow> <mfenced open='' close=''> <mtable> <mtr> <mtd> <msub> <mi>φ</mi> <mn>1</mn> </msub> <mo>=</mo> <mi>arctan</mi> <mfrac> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <mn>1</mn> <mo>-</mo> <mn>2</mn> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> </mrow> </mfrac> </mtd> </mtr> <mtr> <mtd> <msub> <mi>θ</mi> <mn>1</mn> </msub> <mo>=</mo> <mi>arcsin</mi> <mn>2</mn> <mrow> <mo>(</mo> <mo>-</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>ψ</mi> <mn>1</mn> </msub> <mo>=</mo> <mi>arctan</mi> <mfrac> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>3</mn> </msub> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <mn>1</mn> <mo>-</mo> <mn>2</mn> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> </mrow> </mfrac> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow> </math>

The Kalman filter comprises the following working steps: after the initial condition is determined, attitude estimation is carried out by utilizing the measurement data integration of the angular rate gyroscope, the step is called as 'time updating', and high-bandwidth attitude information is provided; in the time updating stage, because the gyro data has noise, the error of the estimated attitude is accumulated along with time, so the attitude cannot be determined by simple gyro data integration, in order to inhibit the gyro error, a correction system needs to be introduced to periodically correct the gyro data, the step is called measurement updating, and the covariance matrix of the state error and the gyro drift estimation are also corrected in the step; next, a new round of time update is performed, and the process is repeated periodically;

the specific method for the Kalman filter to perform data fusion is as follows:

The second step is that: predicted state error covariance matrix P

P_{k}^{-} = F_{k - 1} P_{k - 1}^{+} F_{k - 1}^{T} + G_{k - 1} Q_{k - 1} G_{k - 1}^{T}

In the formula, Q_k-1Is a process noise covariance matrix, and F and G are Jacobian matrices;

the third step: in the correction system, attitude angle information [ phi ] estimated by the correction system is obtained using an accelerometer and a GPS₂ θ₂ ψ₂]^T；

The fifth step: computing kalman gain matrix

K_{k} = P_{k - 1} H_{k}^{T} {(H_{k} P_{k - 1} H_{k}^{T} + R_{k})}^{- 1}

In the formula, R_kMeasuring a noise covariance matrix, wherein H is a Jacobian matrix;

and a sixth step: updating state error covariance matrix

P_{k}^{+} = (I - K_{k} H_{k}) P_{k}^{-} {(I - K_{k} H_{k})}^{T} + K_{k} R_{k} K_{k}^{T}

The seventh step: updating state vectors

<math> <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>+</mo> </msubsup> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mover> <mi>q</mi> <mover> <mo>&RightArrow;</mo> <mo>^</mo> </mover> </mover> <mi>ek</mi> <mo>+</mo> </msubsup> </mtd> <mtd> <msubsup> <mrow> <mi>δ</mi> <mover> <mi>b</mi> <mo>^</mo> </mover> </mrow> <mi>pk</mi> <mo>+</mo> </msubsup> </mtd> <mtd> <msubsup> <mrow> <mi>δ</mi> <mover> <mi>b</mi> <mo>^</mo> </mover> </mrow> <mi>qk</mi> <mo>+</mo> </msubsup> </mtd> <mtd> <msubsup> <mrow> <mi>δ</mi> <mover> <mi>b</mi> <mo>^</mo> </mover> </mrow> <mi>rk</mi> <mo>+</mo> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>=</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>Z</mi> <mi>k</mi> </msub> </mrow> </math>

Eighth step: using estimated error quaternionsAnd gyro driftAndcorrecting the attitude quaternion obtained in the time updating stepAnd angular rate gyro measurements

2. The attitude heading reference system of claim 1, wherein the correction module determines the estimated roll angle φ from the equation₂And a pitch angle theta₂：

<math> <mrow> <msub> <mi>θ</mi> <mn>2</mn> </msub> <mo>=</mo> <mi>arcsin</mi> <mfrac> <msub> <mi>f</mi> <mi>x</mi> </msub> <mi>g</mi> </mfrac> </mrow> </math>

<math> <mrow> <msub> <mi>φ</mi> <mn>2</mn> </msub> <mo>=</mo> <mi>arcsin</mi> <mfrac> <msub> <mrow> <mo>-</mo> <mi>f</mi> </mrow> <mi>y</mi> </msub> <mrow> <mi>g</mi> <mi>cos</mi> <mi>θ</mi> </mrow> </mfrac> </mrow> </math>

Wherein g is gravityConstant of acceleration, f_x、f_yIs the accelerometer reading.

3. The attitude heading reference system of claim 1, wherein the correction module determines the estimated roll angle φ when the aircraft is making a banked turn by the following equation₂And a pitch angle theta₂

<math> <mrow> <msub> <mi>φ</mi> <mn>2</mn> </msub> <mo>=</mo> <mi>arcsin</mi> <mfrac> <mrow> <msub> <mrow> <mo>-</mo> <mi>f</mi> </mrow> <mi>y</mi> </msub> <mo>+</mo> <msub> <mi>rV</mi> <mi>TAS</mi> </msub> </mrow> <mrow> <mi>g</mi> <mi>cos</mi> <mi>θ</mi> </mrow> </mfrac> </mrow> </math>

Wherein g is a gravitational acceleration constant, f_x、f_yFor accelerometer readings, r is the yaw rate in the direction around the z-axis, V_TASThe vacuum rate is used.

4. An attitude heading reference system according to any of claims 1 to 3 wherein, when the aircraft is making a banked turn, the norm or absolute value of the angular rate is selected as the indicator to trigger the Kalman filter, the opening or closing of the time window being determined by

Wherein, beta is a time window threshold value, and flag is a window mark;