CN1740746B - Micro-dynamic carrier attitude measuring apparatus and measuring method thereof - Google Patents

Abstract

The measuring equipment includes three-axis rate gyroscope, three-axis magnetic field meter, uniaxial accelerometer, temperature sensor, A/D conversion circuit, microprocessor and memory, serial communication interface and its correspondent software. Said invention also provides the concrete steps of its measurement method.

Description

Micro-dynamic carrier attitude measuring apparatus and measuring method thereof

Technical field

The present invention relates to a kind of microminiature carrier attitude measuring apparatus, be specially adapted to dynamic carrier attitude and measure, belong to signal Processing, measurement, navigation field.

The invention still further relates to a kind of measuring method according to this device.

Background technology

The measurement of attitude information has crucial meaning for motion carriers such as aircraft, vehicle, boats and ships.In the classic method, people use the attitude angle of gyroscope survey motion carrier.Good gyroscope has quite high precision, but simultaneously it also exist cost an arm and a leg, complex structure, volume are big, quality heavily waits shortcoming, this makes its application be subjected to considerable restraint.The rate gyro price is lower, volume is little, light weight, but its precision is lower, and drift is serious, therefore can not directly be used for measuring attitude angle.

3 components that utilize acceleration transducer and geomagnetic sensor to measure acceleration of gravity and terrestrial magnetic field also can calculate the attitude angle (as United States Patent (USP) 20020188416) of carrier.The volume of this class sensor can be accomplished very little, but because therefore also responsive acceleration of motion of acceleration transducer while is subjected to acceleration of motion easily and disturbs, can not be used under the nonequilibrium condition.

Another kind method is on the basis of acceleration and geomagnetic sensor, adds rate gyro, and structure is estimated (as the United States Patent (USP) 6647352 of having authorized) based on the Kalman filter of hypercomplex number in real time to attitude angle.The method is utilized 3 rate gyro output update mode amounts, and the measured value that adopts 3 axle accelerations and 3 magnetometers obtains the optimal estimation of state variable as observed reading under the least square meaning, and then obtains the estimated value of attitude angle.The advantage of the method is the random disturbance of filtering sensor signal preferably, and the drift of compensation gyro has improved precision effectively.But it requires the measured value of acceleration of gravity in long-time is no inclined to one side, that is to say, when carrier is in when being in acceleration (perhaps slow down, turn) state for a long time, the precision of the method will seriously reduce, and will be even not available.In order to address this problem, people have proposed to adjust automatically according to different conditions the method for wave filter noise.This method increases the degree of dependence to gyro signal when being in acceleration, deceleration or turn condition for a long time.Its prerequisite is that gyro signal drifts about in the period at this section and do not have significant change, and this performance to gyro has proposed higher requirement, but uses high accuracy gyroscope, the price and the weight of meeting increase system, and along with the growth of time, error is accumulation constantly, therefore still can not address this problem.

In addition, utilizing the method measurement attitude of carrier angle of differential GPS or GPS array also is a kind of common method, this method is subjected to the influence that satellite-signal is lost easily, and effect is bad under more situation of blocking, and the position of GPS receiving antenna and distance also can influence the precision of attitude measurement.

Summary of the invention

The present invention has introduced a kind of micro-dynamic carrier attitude measuring apparatus.This device comprises 3 rate gyros (101), 3 magnetometers (102), single-axis accelerometer (103), temperature sensor (104), individual axis velocity sensor (105), analog to digital conversion circuit (106), microprocessor and storer (107), serial communication interface (108), and the corresponding software measuring method.Wherein individual axis velocity sensor (105) is optional device, does not have its system still can operate as normal, helps to improve measuring accuracy but add individual axis velocity sensor (105).

3 rate gyros (101) are connected respectively to each road input end of analog to digital conversion circuit (106) to the output voltage signal of (105) 5 kinds of sensors of individual axis velocity sensor.The control signal of analog to digital conversion circuit (106) is connected with the corresponding port of microprocessor with storer (107) respectively with data-signal.The output port of microprocessor and storer (107) is connected with serial communication unit (108).Microprocessor and storer (107) are sampled to each road sensor signal according to default sampling rate control analog to digital conversion circuit (106), and read corresponding data, handle, and calculate attitude angle, store or output to serial communication unit (108).

Utilize above-mentioned measurement mechanism to measure attitude of carrier information, step is as follows:

The first step is connected measurement mechanism and tested carrier.Definition geographic coordinate system and carrier coordinate system are as shown in Figure 2.Fig. 2 (a) is a geographic coordinate system, three orthogonal axis of N, E, D respectively energized north, east, 3 directions.Fig. 2 (b) is a carrier coordinate system, x, y, three orthogonal axes of z respectively with N, E, when three of D align, each attitude angle is defined as zero, direction is as the criterion with the right-hand rule.The symbolic representation of attitude angle and scope can be defined as: the angle of pitch (θ) scope-90 °～90 °, roll angle (Φ) scope-180 °～180 °, 0 °～360 ° of course angle (ψ) scopes.

According to the order of course → pitching → lift-over, the direction cosine matrix R to the carrier coordinate can be expressed as by inertial coordinate:

$R=\left[\begin{array}{ccc}\mathrm{c\ψ}\·\mathrm{c\θ}& \mathrm{s\ψ}\·\mathrm{c\θ}& -\mathrm{s\θ}\\ \mathrm{c\ψ}\·\mathrm{s\θ}\·\mathrm{s\φ}-\mathrm{s\ψ}\·\mathrm{c\φ}& \mathrm{s\ψ}\·\mathrm{s\θ}\·\mathrm{s\φ}+\mathrm{c\ψ}\·\mathrm{c\φ}& \mathrm{c\θ}\·\mathrm{s\φ}\\ \mathrm{c\ψ}\·\mathrm{s\θ}\·\mathrm{c\φ}+\mathrm{s\ψ}\·\mathrm{s\φ}& \mathrm{s\ψ}\·\mathrm{s\θ}\·\mathrm{c\φ}-\mathrm{c\ψ}\·\mathrm{s\φ}& \mathrm{c\θ}\·\mathrm{c\φ}\end{array}\right]$

Wherein s and c are respectively writing a Chinese character in simplified form of function sin and cos.

The present invention constructs a vector v with first row and the 3rd row of direction cosine matrix R,

v＝[r ₁₁ r ₂₁ r ₃₁ r ₁₃ r ₂₃ r ₃₃]

R wherein _IjBe illustrated in the capable j row of the i element among the R.

Second step, system initialization.Set the sampling period, the initial value of set condition variable, the initial value of the covariance matrix of state estimation error is measured noise, process noise.With the static placement of measurement mechanism, set the zero point of 3 rate gyros.

The initial value of the covariance matrix of state estimation error can be given arbitrarily, do not influence the convergence of system.The size of measuring noise and process noise then will be according to concrete sensor and applied environment decision.The initial value of state variable need be determined by initial alignment, is zero position such as carrier being positioned over each attitude angle, and the initial value with state variable can be set to then

v ₀＝[1 0 0 0 0 1]。

The 3rd step, the pick-up transducers signal.Gather each road sensing data by microprocessor control analog to digital conversion circuit according to the sampling period, read analog-to-digital result in microprocessor.

In the 4th step, sensing data is compensated and demarcates.According to the sensor temperature family curve sensor sample result is carried out temperature compensation.Because the error that causes is installed also to be needed to demarcate.

The temperature characteristics of 3 rate gyros of sensor (101), 3 magnetometers (102), single-axis accelerometer (103), individual axis velocity sensor (105) will be through measuring in advance, and be kept in the storer.Temperature value during according to sampling, microprocessor reads the temperature characteristics respective value from storer, the sensor data are compensated.Be installed in the center of rotation of carrier if can not guarantee accelerometer (103), then according to current rotational angular velocity with go out to the distance calculation of center of rotation because the centripetal acceleration that rotation causes compensates.

$a_x=a_x^{\′}-({\mathrm{\ω}}_y^2+{\mathrm{\ω}}_z^2)X_{\mathrm{ax}}---\left(1\right)$

X wherein _AxBe the distance of x axis accelerometer to center of rotation, a _x' be the measured value of accelerometer, a _xFor compensating owing to the accekeration behind rotation and the eccentric acceleration that causes.

In the 5th step, utilize gyro data update mode variable.Utilize the estimated value of the state variable of 3 rate gyro measured values and previous moment can calculate the step forecast estimated value of current time state variable.

More new formula is as follows:

$\hat{v}(\mathrm{kh}+h|\mathrm{kh})=F\·\hat{v}\left(\mathrm{kh}\right)---\left(2\right)$

Wherein F is the function of the current angular speed of gyro, Be the estimated value of kh moment state variable v,

For the step forecast of state variable is estimated.

The 6th step is according to observed reading and constraint condition correction state variable.The constraint condition of 3 magnetometers, single-axis accelerometer and two state variables as measured value, is revised a step predicted value of state variable.

According to measured value and constraint condition, go on foot the quantity of state that forecast obtains to one

Revise:

P(kh+h|kh)＝F·P(kh)·F ^T+Φ _ev

K(kh+h)＝P(kh+h|kh)·C ^T[C·P(kh+h|kh)·C ^T+Φ _ey] ^-1

$\hat{v}(\mathrm{kh}+h)=\hat{v}(\mathrm{kh}+h|\mathrm{kh})+K(\mathrm{kh}+h)\·[y(\mathrm{kh}+h)-C\·\hat{v}(\mathrm{kh}+h|\mathrm{kh})]$

P(kh+h)＝[I-K(kh+h)·C]·P(kh+h|kh)。(3)

In the 7th step, calculate and export current attitude angle.Calculate current attitude angle information according to state variable, and storage or output as required.

V calculates attitude angle by state variable, and concrete formula is as follows:

The angle of pitch:

θ＝-arcsin(r ₁₃) (4)

Roll angle:

Course angle:

$\mathrm{\&psi;}=\left\{\begin{array}{cc}\arccos (r_{11}/\cos \left(\mathrm{\&theta;}\right)),& r_{31}\&times;r_{23}-r_{33}\&times;r_{21}\&GreaterEqual;0\\ 2\mathrm{\&pi;}-\arccos (r_{11}/\cos \left(\mathrm{\&theta;}\right)),& r_{31}\&times;r_{23}-r_{33}\&times;r_{21}<0\end{array}\right.---\left(6\right)$

In the 8th step, jump to the 3rd and go on foot or withdraw from.Jump to the continuation of the 3rd step and measure, perhaps stop to withdraw from.

The 5th step and the 6th goes on foot based on following discrete state spatial model:

v(kh+h)＝F·v(kh)+e _v(kh) (7)

y(kh+h)＝C·v(kh+h)+e _y(kh+h) (8)

Wherein

$F=I+h\&times;\left[\begin{array}{cc}\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)& 0\\ 0& \mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)\end{array}\right],$

y(kh+h)＝[m _x(kh+h)m _y(kh+h)m _z(kh+h)a _x(kh+h)1?1] ^T

$C=\left[\begin{array}{cccccc}\cos \mathrm{\&beta;}& 0& 0& \sin \mathrm{\&beta;}& 0& 0\\ 0& \cos \mathrm{\&beta;}& 0& 0& \sin \mathrm{\&beta;}& 0\\ 0& 0& \cos \mathrm{\&beta;}& 0& 0& \sin \mathrm{\&beta;}\\ 0& 0& 0& g& 0& 0\\ r_{11\mathrm{kh}}& r_{21\mathrm{kh}}& r_{31\mathrm{kh}}& 0& 0& 0\\ 0& 0& 0& r_{13\mathrm{kh}}& r_{23\mathrm{kh}}& r_{33\mathrm{kh}}\end{array}\right]$

e _vAnd e _yBe that power spectrum density is Φ _EvAnd Φ _EyThe zero-mean white noise.

With state equation (7), measure equation (8) structure Kalman Filtering for Discrete device, wherein measure Matrix C and constitute by a last moment state variable.Because the measured value y in the equation (8) only contains a _xSo, be not subjected to the influence of y, z direction of principal axis acceleration of motion item.As previously mentioned, if utilize a _x-﹠amp; Replace a _xThe influence of x direction of principal axis acceleration of motion will be eliminated, under high dynamic environment, still high-acruracy survey can be guaranteed like this.

The discrete state space equation is obtained according to sampling period h discretize by following continuity equation.

R gets differential to the direction cosine matrix, and it is as follows to obtain equation:

$\frac{\mathrm{dR}}{\mathrm{dt}}=\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)R---\left(9\right)$

Wherein $\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_z& -{\mathrm{\&omega;}}_y\\ -{\mathrm{\&omega;}}_z& 0& {\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_x& 0\end{array}\right]$

Because v is made up of first row and the 3rd row of R, so can draw by equation (9):

$\frac{d}{\mathrm{dt}}v=\left[\begin{array}{cc}\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)& 0\\ 0& \mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)\end{array}\right]v+e_v---\left(10\right)$

Wherein 0 is 3 * 3 null matrix, e _vBe approximately the zero-mean white noise, its power spectrum density is approximately Φ _Ev

The coordinate representation of acceleration of gravity vector in inertial coordinates system is a ₀=[0 0 g] ^TMeasured value is a=[a in carrier coordinate system _xa _ya _z] ^TThe acceleration of gravity vector is transformed into carrier coordinate system by inertial coordinates system:

a＝Ra ₀+e _a (11)

E wherein _aFor measuring noise.When carrier is in static state or low dynamic environment, e _aCan be approximated to be power spectrum density is Φ _aThe zero-mean white noise, but when carrier was in high dynamic environment, acceleration measurement and gravitational acceleration component differed greatly, when particularly having long period low frequency movement acceleration, as spiraling e for a long time _aCan not think the zero-mean white noise, but this acceleration is less relatively for the influence of x axle acceleration component.According to equation (6), acceleration at the axial component of carrier coordinate system x is:

a _x＝r ₁₃g+e _ax (12)

E wherein _AxCan be approximately power spectrum density is Φ _AxThe zero-mean white noise.If the x direction of principal axis adds speed pickup, use a _x-﹠amp; Replace a in the following formula _xWill help further to improve precision.

The coordinate representation of ground magnetic vector in inertial coordinates system is m ₀=[mcos β 0 M sin β] ^T, wherein M is the mould (in the statement of back M being omitted as the unit length amount) of ground magnetic vector, β is local earth's magnetic dip angle, revises after ignoring the geomagnetic declination or calculating course angle.Measured value is m=[m in carrier coordinate system _xm _ym _z] ^TThe ground magnetic vector is transformed into carrier coordinate system by inertial coordinates system:

m＝Rm ₀+e _m (13)

E wherein _mFor measuring noise, can be approximately independently zero-mean white noise.

Six elements noticing vector v are not separate, have following two constraint conditions:

$r_{11}^2+r_{21}^2+r_{31}^2=1,$ $r_{13}^2+r_{23}^2+r_{33}^2=1---\left(14\right)$

With " pseudo-measure equation " of constraint condition (14), constitute the measurement equation of vector v with equation (12), (13) as vector v.Equation contains nonlinear terms in (14), therefore need do linearization process.One in quadratic term in (14) is write in the output equation, is about to equation (12), (13), (14) and is write as following form:

$\left[\begin{array}{c}{m}_x\\ m_y\\ m_z\\ a_x\\ 1\\ 1\end{array}\right]=\left[\begin{array}{cccccc}\cos \mathrm{\&beta;}& 0& 0& \sin \mathrm{\&beta;}& 0& 0\\ 0& \cos \mathrm{\&beta;}& 0& 0& \sin \mathrm{\&beta;}& 0\\ 0& 0& \cos \mathrm{\&beta;}& 0& 0& \sin \mathrm{\&beta;}\\ 0& 0& 0& g& 0& 0\\ r_{11}& r_{21}& r_{31}& 0& 0& 0\\ 0& 0& 0& r_{13}& r_{23}& r_{33}\end{array}\right]\&times;\left[\begin{array}{c}{r}_{11}\\ r_{21}\\ r_{31}\\ r_{13}\\ r_{23}\\ r_{33}\end{array}\right]+e_y---\left(15\right)$

E wherein _y=[e _Mxe _Mye _Mze _Axe _C1e _C2] ^T, e _C1, e _C2Be the error of calculation of constraint condition, can be approximately white noise.

Equation (10) and equation (15) will be obtained the discrete system equation that constitutes by equation (7) and equation (8) by sampling period h discretize.

Description of drawings

Fig. 1 is the hardware composition frame chart of this attitude measuring.

Fig. 2 describes inertial coordinates system and carrier coordinate system, wherein (a) inertial coordinates system (NED), (b) carrier coordinate system (xyz).

Fig. 3 is the measuring method process flow diagram.

Fig. 4 is the aircraft coordinate system.

Embodiment

Be example to measure attitude of flight vehicle below, introduce the concrete real-time process of this attitude measuring and method.

Fig. 1 is the hardware composition frame chart of this attitude measuring.3 rate gyros (101), 3 magnetometers (102), single-axis accelerometer (103), temperature sensor (104), individual axis velocity sensor (105), analog to digital conversion circuit (106), microprocessor and storer (107), serial communication interface (108), wherein speed pickup (105) is optional device.3 rate gyros (101), 3 magnetometers (102), single-axis accelerometer devices such as (103) are selected the sensor based on the MEMS technology for use, and volume is little, in light weight.

The output voltage signal of 3 rate gyros (101), 3 magnetometers (102), single-axis accelerometer (103), temperature sensor (104), (105) 5 kinds of sensors of individual axis velocity sensor is connected respectively to each road input end of analog to digital conversion circuit (106).The control signal of analog to digital conversion circuit (106) is connected with the corresponding port of microprocessor with storer (107) respectively with data-signal.The output port of microprocessor and storer (107) is connected with serial communication unit (108).

Fig. 2 describes inertial coordinates system and carrier coordinate system, wherein (a) inertial coordinates system (NED), (b) carrier coordinate system (xyz).

Fig. 3 is the measuring method process flow diagram, and method and the step of utilizing said apparatus to measure attitude of flight vehicle have been described.

(301) this measurement mechanism and aircraft are connected, Fig. 4 illustrated coordinate axis x, y, z respectively with each corresponding axially aligning of aircraft.X axle and aircraft heading are to it, and y axle and aircraft wing direction are to it, and the z axle is vertical with the aircraft wing direction.

(302) system initialization.The setting sampling period is 25Hz; Carrier is positioned over each attitude angle is zero position, the initial value of state variable is set to then; The initial value of the covariance matrix of state estimation error is set at unit matrix arbitrarily; The setting measurement noise, the value of process noise.With the static placement of measurement mechanism, the measured value of setting 3 rate gyros is zero point.

(303) pick-up transducers signal.Gather each road sensing data by microprocessor control analog to digital conversion circuit according to the sampling period, read analog-to-digital result in microprocessor.

(304) temperature compensation and demarcation according to temperature sensor measurements, are read from storer and are demarcated good modified value in advance, and sensing data is carried out temperature compensation.Below used sensor signal be through the data after the temperature compensation.

Measure the distance X of x axis accelerometer to center of rotation _Ax, according to current rotational angular velocity and X _Ax, calculate owing to rotate the centripetal acceleration that causes, compensate.

$a_x=a_x^{\&prime;}-({\mathrm{\&omega;}}_y^2+{\mathrm{\&omega;}}_z^2)X_{\mathrm{ax}}$

A wherein _x' be the measured value of accelerometer, a _xFor compensating owing to the accekeration behind rotation and the eccentric acceleration that causes.

(305) utilize gyro data update mode variable.Utilize the estimated value of the state variable of 3 rate gyro measured values and previous moment can calculate the step forecast estimated value of current time state variable.

More new formula is as follows:

$\hat{v}(\mathrm{kh}+h|\mathrm{kh})=F\&CenterDot;\hat{v}\left(\mathrm{kh}\right)$

Wherein F is the function of the current angular speed of gyro,

Be the estimated value of kh moment state variable v,

For the step forecast of state variable is estimated.

(306) according to observed reading and constraint condition correction state variable.The constraint condition of 3 magnetometers, single-axis accelerometer and two state variables as measured value, is revised a step predicted value of state variable.

According to measured value and constraint condition, go on foot the quantity of state that forecast obtains to one

Revise:

P(kh+h|kh)＝F·P(kh)·F ^T+Φ _ev

K(kh+h)＝P(kh+h|kh)·C ^T[C·P(kh+h|kh)·C ^T+Φ _ey] ^-1

$\hat{v}(\mathrm{kh}+h)=\hat{v}(\mathrm{kh}+h|\mathrm{kh})+K(\mathrm{kh}+h)\&CenterDot;[y(\mathrm{kh}+h)-C\&CenterDot;\hat{v}(\mathrm{kh}+h|\mathrm{kh})]$

P(kh+h)＝[I-K(kh+h)·C]·P(kh+h|kh)；

(307) calculate and export current attitude angle.Calculate current attitude angle information according to state variable, and storage or output as required.

V calculates attitude angle by state variable, and concrete formula is as follows:

The angle of pitch:

θ＝-arcsin(r ₁₃)

Roll angle:

Course angle:

$\mathrm{\&psi;}=\left\{\begin{array}{cc}\arccos (r_{11}/\cos \left(\mathrm{\&theta;}\right)),& r_{31}\&times;r_{23}-r_{33}\&times;r_{21}\&GreaterEqual;0\\ 2\mathrm{\&pi;}-\arccos (r_{11}/\cos \left(\mathrm{\&theta;}\right)),& r_{31}\&times;r_{23}-r_{33}\&times;r_{21}<0\end{array}\right.$

(308) jumping to the 3rd goes on foot or withdraws from.Jump to the continuation of the 3rd step and measure, perhaps stop to withdraw from.

This device volume is little, in light weight, compare with the conventional inertia guider, less demanding to sensor performance, can select the very little MEMS sensor of volume, and pass through special algorithm, therefore effectively eliminated the interference of acceleration of motion, attitude measurement accuracy can be accomplished higher, is specially adapted in the high dynamic environment the dynamic attitude to aircraft, vehicle, boats and ships or other carriers and measures.

Claims (1)

1. the measuring method of an attitude of carrier information measurement is characterized in that, this measuring method comprises the steps: the first step: measurement mechanism and tested carrier are connected; Definition geographic coordinate system and carrier coordinate system;

According to the order of course → pitching → lift-over, construct by the direction cosine matrix R of inertial coordinate to the carrier coordinate:

$R=\left[\begin{array}{ccc}\mathrm{c\&psi;}\&CenterDot;\mathrm{c\&theta;}& \mathrm{s\&psi;}\&CenterDot;\mathrm{c\&theta;}& -\mathrm{s\&theta;}\\ \mathrm{c\&psi;}\&CenterDot;\mathrm{s\&theta;}\&CenterDot;\mathrm{s\&phi;}-\mathrm{s\&psi;}\&CenterDot;\mathrm{c\&phi;}& \mathrm{s\&psi;}\&CenterDot;\mathrm{s\&theta;}\&CenterDot;\mathrm{s\&phi;}+\mathrm{c\&psi;}\&CenterDot;\mathrm{c\&phi;}& \mathrm{c\&theta;}\&CenterDot;\mathrm{s\&phi;}\\ \mathrm{c\&psi;}\&CenterDot;\mathrm{s\&theta;}\&CenterDot;\mathrm{c\&phi;}+\mathrm{s\&psi;}\&CenterDot;\mathrm{s\&phi;}& \mathrm{s\&psi;}\&CenterDot;\mathrm{s\&theta;}\&CenterDot;\mathrm{c\&phi;}-\mathrm{c\&psi;}\&CenterDot;\mathrm{s\&phi;}& \mathrm{c\&theta;}\&CenterDot;\mathrm{c\&phi;}\end{array}\right]$

Wherein, s and c are respectively writing a Chinese character in simplified form of function sin and cos;

With first row and vector v of the 3rd row structure of direction cosine matrix R,

v＝[r ₁₁?r ₂₁?r ₃₁?r ₁₃?r ₂₃?r ₃₃]

R wherein _IjBe illustrated in the capable j row of the i element among the R;

Second step, system initialization

Set the sampling period, the initial value of set condition variable, the initial value of the covariance matrix of state estimation error is measured noise, process noise;

With the static placement of measurement mechanism, set the zero point of 3 rate gyros;

The 3rd step, the pick-up transducers signal

Microprocessor control analog to digital conversion circuit is gathered each road sensing data according to the sampling period, reads analog-to-digital result in microprocessor;

In the 4th step, sensing data is compensated and demarcates

According to the sensor temperature family curve sensor sample result is carried out temperature compensation;

Because the error that causes is installed also to be needed to demarcate;

In the 5th step, utilize gyro data update mode variable

Utilize the estimated value of the state variable of 3 rate gyro measured values and previous moment to calculate the step forecast estimated value of current time state variable; More new formula is:

$\hat{v}(\mathrm{kh}+h|\mathrm{kh})=F\&CenterDot;\hat{v}\left(\mathrm{kh}\right)$

Wherein F is the function of the current angular speed of gyro,

Be the estimated value of kh moment state variable v, For the step forecast of state variable is estimated;

The 6th step is according to observed reading and constraint condition correction state variable

The constraint condition of 3 magnetometers, single-axis accelerometer and two state variables as measured value, is revised a step predicted value of state variable;

According to measured value and constraint condition, go on foot the quantity of state that forecast obtains to one

Revise:

P(kh+h|kh)＝F·P(kh)·F ^T+Φ _ev

K(kh+h)＝P(kh+h|kh)·C ^T[C·P(kh+h|kh)·C ^T+Φ _ey] ^-1

$\hat{v}(\mathrm{kh}+h)=\hat{v}(\mathrm{kh}+h|\mathrm{kh})+K(\mathrm{kh}+h)\&CenterDot;[y(\mathrm{kh}+h)-C\&CenterDot;\hat{v}(\mathrm{kh}+h|\mathrm{kh})]$

P(kh+h)＝[I-K(kh+h)·C]·P(kh+h|kh)

In the 7th step, calculate and export current attitude angle

Calculate current attitude angle information according to state variable, and storage or output as required;

V calculates attitude angle by state variable, and concrete formula is as follows:

The angle of pitch:

θ＝-arcsin(r ₁₃)

Roll angle:

Course angle:

$\mathrm{\&psi;}=\left\{\begin{array}{cc}\arccos (r_{11}/\cos \mathrm{\&theta;}),& r_{31}\&times;r_{23}-r_{33}\&times;r_{21}\&GreaterEqual;0\\ 2\mathrm{\&pi;}-\arccos (r_{11}/\cos \left(\mathrm{\&theta;}\right)),& r_{31}\&times;r_{23}-r_{33}\&times;r_{21}<0\end{array}\right.$

In the 8th step, jump to the 3rd and go on foot or withdraw from;

Jump to the continuation of the 3rd step and measure, perhaps stop, withdrawing from, finish measurement.

Images