CN104764451A - Target posture tracking method based on inertia and geomagnetic sensor - Google Patents

Abstract

The invention discloses a target posture tracking method based on inertia and a geomagnetic sensor. According to the target posture tracking method, posture tracking can be realized without any reference marker and a specific tracking environment, and the method is simple and feasible; a gyroscope, an accelerometer and the geomagnetic sensor are used for respectively acquiring components of angular speeds, accelerated speeds and magnetic intensity data corresponding to a current posture of a target in three sensitive axes; a posture tracking result of the gyroscope is corrected by using the accelerometer and the geomagnetic sensor and drift errors are eliminated so that the precision of a tracking result is improved; and the target posture tracking method utilizes an efficient offline operated Kalman filtering algorithm and fuses a multi-sensor tracking result, so that the real-time online posture tracking is realized.

Description

A Target Attitude Tracking Method Based on Inertial and Geomagnetic Sensors

technical field

The invention relates to the technical field of intelligent sensing and human-computer interaction, in particular to a target posture tracking method based on inertial and geomagnetic sensors.

Background technique

Augmented reality technology based on attitude tracking superimposes and displays computer-generated information such as virtual three-dimensional objects, videos, texts, and pictures into real scenes in real time, and realizes natural human-computer interaction through attitude tracking. , education and entertainment industries have broad application prospects. The purpose of attitude tracking in augmented reality is to identify moving targets and obtain information about their attitude changes. The acquired attitude information is usually applied in the following five aspects:

1) Field of view control, using attitude information to control the position and direction of the virtual camera;

2) Navigation, assisting users to move in the virtual environment;

3) Virtual-real interaction, by tracking the user's limbs, interactive functions such as grasping and moving are realized;

4) Instrument tracking, tracking the instruments and equipment in the system, such as surgical tools, maintenance tools, etc.;

5) Gesture transmission, which transmits the user's posture information to the follower device, such as a surgical robot or a virtual character in a movie.

Commonly used inertial sensors for attitude tracking include inertial gyroscopes and accelerometers. In the measurement, the strapdown inertial navigation principle is used to measure the target attitude. The inertial gyro is mainly used to measure the three-axis rotational angular velocity or angular acceleration, and the incremental integration method is used to obtain the direction information of the tracker, but it will produce drift errors that increase with time. The accelerometer is mainly used to measure the gravitational acceleration components on the three sensitive axes, and then use the cosine algorithm to obtain the pitch angle and roll angle of the tracker and overcome the drift error on the two attitude angles caused by the gyroscope. The inertial tracker has the advantages of small size, light weight, no occlusion problem, unlimited working range, and high refresh rate. At the same time, the feature of using the magnetic field sensor to obtain the heading angle can realize the three-dimensional attitude tracking of the target.

The method of human body posture tracking using inertial sensors was proposed in 1999 (see Jihong Lee, Insoo Ha. Sensor fusion and calibration for motion captures using accelerometers. Robotics and Automation, Volume 3, Page 1954–1959.1999). This method uses the components of the earth's gravity on the three sensitive axes of the accelerometer to calculate the pitch angle and roll angle of the target, and can obtain the attitude information of the target in two degrees of freedom. The disadvantage is that the complete three-freedom cannot be obtained only through the gravity component. Attitude information and accelerometer data are not suitable for Kalman filter which can improve system accuracy.

Attitude tracking methods using gyroscopes, accelerometers and magnetic sensors such as 2007, Rehbinder, H, etc. (see Rehbinder, H., Xiaoming Hu. Drift-free attitude estimation for accelerated rigid bodies. Robotics and Automation, Volume 4, Page 4244– 4249.2001) proposed the use of multi-sensor fusion method, by combining angular velocity, gravitational acceleration and magnetic field components and using Kalman filter for error correction, the real-time tracking of the three-dimensional attitude of the target was realized, but this method failed to solve the linear acceleration tracking of the target attitude. Impact.

Contents of the invention

In view of this, the present invention provides a target posture tracking method based on inertial and geomagnetic sensors, without the need for complex tracking equipment and specific tracking environments, the real-time three-dimensional posture tracking of the target can be completed using inertial and geomagnetic sensors, and The tracking results are characterized by high precision and high refresh rate.

A target attitude tracking method based on inertial and geomagnetic sensors of the present invention uses gyroscopes, accelerometers and geomagnetic sensors to respectively collect the components of angular velocity, acceleration and magnetic intensity data corresponding to the current attitude of the target on three sensitive axes, and The Kalman filter is used to fuse the above three sensor data and calculate the attitude information of the target. The specific method is as follows:

Step 1. Time update the target attitude information through the Kalman filter, specifically:

S10, the state vector of the target in the time update equation Make an estimate:

Wherein, A is the filter gain matrix formed by the angular velocity measurement values of the target measured by the gyroscope on the three sensitive axes; is the estimated value of the target state vector at the current moment, is the target state vector output by the Kalman filter at the last moment;

S11. Estimated value of the transfer function of the Kalman filter Make an update:

${\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; AP</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; A</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; Q</span>}$

Among them, Q is the system noise covariance matrix; AT is the transpose matrix of the gain matrix A; Pk-1 is the transfer function output by the Kalman filter at the previous moment;

Step 2. Measure and update the attitude information of the target through the Kalman filter, specifically:

S20, based on the estimated value of the transfer function updated in step 1 Obtain the measurement confidence parameter K _k of the Kalman filter:

${\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}{\mathrm{<span\; class="notranslate">\; h</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; HP</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}{\mathrm{<span\; class="notranslate">\; h</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; R</span>}<span\; class="notranslate">\; )</span>}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}$

Among them, H is the conversion matrix between the measurement model and the estimated model:

[q ₁ q ₂ q ₃ q ₄ ] represents the estimated value of the state vector of the constituent target The rotation quaternion of R is the measurement error model: R=ACC _x ² +ACC _y ² +ACC _z ² -g ² ; where ACC _x , ACC _y and ACC _z are the acceleration components of the target on the three sensitive axes measured by the acceleration sensor, g is the acceleration due to gravity;

S21, the measurement result of the state vector of the target Make an update:

$Z_k=\left[\begin{array}{c}\mathrm{\&theta;}\\ \mathrm{\&gamma;}\\ \mathrm{\&psi;}\end{array}\right]$ Represents the attitude of the target, where θ=arcsin(ACC _x ) represents the pitch angle of the target, represents the roll angle of the target, Indicates the heading angle of the target, A=MAG _x cos(θ)+MAG _y sin(θ)+MAG _z cos(θ)sin(γ), B=MAG _y cos(γ)+MAG _z sin(γ), MAG _x , MAG _y and MAG _z represent the components of the earth's magnetic field at the position of the target obtained by the magnetic field sensor on the three sensitive axes of the body coordinate system;

S22. Update the measurement result P _k of the transfer function:

${\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}$

Step 3. Measurement results of the target state vector That is, the tracking result of the current target attitude, and the measurement result at the current moment The measurement result P _k of the sum transfer function is output and used as the input result of the filter at the next moment, and returns to step 1 to perform target attitude tracking at the next moment.

In the step 1, the value of the system noise covariance matrix Q is 0.01.

The present invention has following beneficial effect:

The target attitude tracking method of the present invention can realize attitude tracking without any reference markers and specific tracking environment, and the method is simple and easy; the angular velocity, acceleration and magnetic intensity corresponding to the current attitude of the target are collected respectively by using a gyroscope, an accelerometer and a geomagnetic sensor The components of the data on the three sensitive axes use the accelerometer and the magnetic sensor to correct the attitude tracking results of the gyroscope, eliminating the drift error and thus improving the accuracy of the tracking results; The result of multi-sensor tracking realizes real-time online attitude tracking.

Description of drawings

Fig. 1 is the method flow chart of target posture tracking of the present invention;

Fig. 2 is a diagram of the spatial coordinate system of the posture tracking module in the embodiment of the present invention.

Detailed ways

The present invention will be described in detail below with reference to the accompanying drawings and examples.

Figure 1 is a flow chart of the method of the present invention. Gyroscopes, accelerometers, and geomagnetic sensors are used to collect the components of the angular velocity, acceleration, and magnetic intensity data corresponding to the current attitude of the target on the three sensitive axes, and the Kalman filter is used to fuse the above three sensor data and calculate the target. The attitude information of , the sensor body coordinate system represented by b and the space coordinate system represented by r are defined as shown in Figure 2, and the specific methods are as follows:

Step 1. Time update the target attitude information through the Kalman filter:

The time update equation of the discrete Kalman filter is as follows:

where the state vector in the time update equation consists of a rotation quaternion:

A in the formula (1) is the filter gain matrix formed by the angular velocity measurements on the three sensitive axes of the target measured by the gyroscope; its function is based on the state vector of the target at the previous moment Estimate the current state vector Its composition is as follows:

$\mathrm{<span\; class="notranslate">\; A</span>}<span\; class="notranslate">\; =</span>\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; dGyro</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; dGyro</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; dGyro</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; dGyro</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; dGyro</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; dGyro</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; dGyro</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; dGyro</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 1</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; dGyro</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; dGyro</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; dGyro</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; dGyro</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

Among them, d represents the time interval between two adjacent states of the target; Gyro(0), Gyro(1) and Gyro(2) are the distances of the target measured by the gyroscope on the three sensitive axes (x, y and z axes) Rotational angular velocity, its measurement result will not be affected by the motion acceleration and is linear, so the present invention uses the gyroscope measurement value to form the gain matrix of the system. And using the gyroscope in the Kalman filter to estimate the target attitude angle is beneficial to improve the computing speed of the system. An estimate of the transfer function of the Kalman filter Update with the following formula:

${\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; AP</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; A</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; Q</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

Where Q is the system noise covariance matrix. The Q value is determined according to the moving speed of the target. When the moving speed of the target is slow, set the Q value to a lower value, and vice versa. In this example, the Q value is selected as 0.001.

Step 2. Measure and update the target attitude information through the Kalman filter:

The measurement confidence parameter of the discrete Kalman filter is defined as follows:

${\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}{\mathrm{<span\; class="notranslate">\; h</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; HP</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}{\mathrm{<span\; class="notranslate">\; h</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; R</span>}<span\; class="notranslate">\; )</span>}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span>$

The numerical value of the confidence parameter of the measured value is related to the measurement noise and the filter transfer function. H is the conversion matrix between the measurement model and the estimated model, which is defined according to formula (6):

The measurement error model in the measurement update equation is defined according to the following equation:

R＝ACC _x ² +ACC _y ² +ACC _z ² -g ² (7)

Among them, ACC _x , ACC _y and ACC _z are the acceleration components of the target on the x, y and z axes measured by the acceleration sensor, and g is the acceleration of gravity; in order to extract the motion acceleration as noise from the measured value of the system, We define the motion acceleration error model of the inertial measurement system based on the difference between the sum of the squares of the acceleration measurements and the square of the gravitational acceleration. In an ideal state, the error value R should be 0, and as the target motion acceleration increases or decreases, the error value R will also change accordingly. Adding this model to the Kalman filter will effectively eliminate the estimation error caused by motion acceleration. Bring the parameters calculated above into the Kalman filter measurement update equation to get the measurement result of this Kalman filter And update the measurement result P _k from the transfer function:

${\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 9</span>}<span\; class="notranslate">\; )</span>$

The real-time attitude Z _k of the target in formula (8) is obtained by cosine algorithm. For a three-axis measurement system, the pitch angle of the system can be calculated by the component of the acceleration of gravity on the x-axis of the coordinate system of the tracking module:

θ＝arcsin(ACC _x ) (10)

In the case of obtaining the pitch angle, we can obtain the roll angle of the system through the gravitational acceleration component on the other axis:

$\mathrm{<span\; class="notranslate">\; \&gamma;</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; arcsin</span>}<span\; class="notranslate">\; (</span>\frac{{\mathrm{<span\; class="notranslate">\; ACC</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}{\mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; cos</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 11</span>}<span\; class="notranslate">\; )</span>$

Although the pitch angle and roll angle are known, the heading angle of the system cannot be determined only by measuring values in the same coordinate system, such as the acceleration of gravity. The direction and magnitude of the geomagnetic field are constant and different from the direction of the geomagnetic field, which can be used as another reference measurement value for calculating the target attitude angle, so a magnetic sensor is used to assist in measuring the heading angle. Through the following formula, we can use the acceleration of gravity and the geomagnetic field to calculate the heading angle of the target:

$\mathrm{<span\; class="notranslate">\; \&psi;</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; arctan</span>}\frac{\mathrm{<span\; class="notranslate">\; A</span>}}{\mathrm{<span\; class="notranslate">\; B</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 12</span>}<span\; class="notranslate">\; )</span>$

A＝MAG _x cos(θ)+MAG _y sin(θ)+MAG _z cos(θ)sin(γ) (13)

B＝MAG _y cos(γ)+MAG _z sin(γ) (14)

MAG _x , MAG _y and MAG _z in the formula represent the components of the earth's magnetic field at the target's location obtained by the magnetic field sensor on the x, y and z axes of the body coordinate system, respectively.

Through the above calculation method, the real-time attitude of the target can be calculated by the components of the acceleration of gravity and the earth's magnetic field on the three axes of the target body coordinate system:

${\mathrm{<span\; class="notranslate">\; Z</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&theta;</span>}\\ \mathrm{<span\; class="notranslate">\; \&gamma;</span>}\\ \mathrm{<span\; class="notranslate">\; \&psi;</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 15</span>}<span\; class="notranslate">\; )</span>$

Step 3. The measurement result of the state vector of the target That is, the tracking result of the current target attitude, and the measurement result at the current moment The measurement result P _k of the sum transfer function is output and used as the input result of the filter at the next moment, and returns to step 1 to perform attitude tracking at the next moment.

To sum up, the above are only preferred embodiments of the present invention, and are not intended to limit the protection scope of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims (2)

1. the target attitude tracking method based on inertia and geomagnetic sensor, it is characterized in that, employing gyroscope, accelerometer and geomagnetic sensor gather angular velocity, acceleration and the magnetic intensity data component in three sensitive axes corresponding to target current pose respectively, and adopt Kalman filter to melt the attitude information of joint account target to above-mentioned three sensing datas, concrete grammar is as follows:

Step 1, by Kalman filter, time renewal is carried out to targeted attitude information, is specially:

S10, in time update equation to the state vector of target estimate:

Wherein, A is the target filter gain matrix that angular velocity measurement value is formed in three sensitive axes that described gyroscope records; for current target state vector estimated value, for the dbjective state vector that a moment in Kalman filter exports;

S11, estimated value to the transport function of Kalman filter upgrade:

$p_k^{-}={\mathrm{Ap}}_{k-1}{A}^T+Q$

Wherein, Q value is system noise covariance matrix; A ^tfor the transposed matrix of gain matrix A; P _k-1for the transport function that a moment in Kalman filter exports;

Step 2, by Kalman filter, measurement updaue is carried out to targeted attitude information, is specially:

S20, based on the estimated value upgrading the transport function obtained in step 1 obtain the measurement confidence parameter K of Kalman filter _k:

$K_k=P_k^{-}{H}^T{({\mathrm{HR}}_k^{-}{H}^T+R)}^{-1}$

Wherein, H is transition matrix between measurement model and estimation model:

[q ₁q ₂q ₃q ₄] represent the state vector estimated value forming target rotation hypercomplex number, namely r is Measuring error model: wherein ACC _x, ACC _yand ACC _zbe respectively the component of acceleration of target in three sensitive axes that acceleration transducer records, g is acceleration of gravity;

S21, measurement result to the state vector of target upgrade:

$Z_k=\left[\begin{array}{c}\mathrm{\&theta;}\\ \mathrm{\&gamma;}\\ \mathrm{\&psi;}\end{array}\right]$ Represent the attitude of target, wherein, θ=arcsin (ACC _x) represent the angle of pitch of target, represent the roll angle of target, represent the course angle of target, A=MAG _xcos (θ)+MAG _ysin (θ)+MAG _zcos (θ) sin (γ), B=MAG _ycos (γ)+MAG _zsin (γ), MAG _x, MAG _yand MAG _zrepresent the component of magnetic field of the earth in three sensitive axes of body coordinate system of the target position that magnetic field sensor obtains respectively;

S22, measurement result P to transport function _kupgrade:

$P_k=(1-K_{l}H)P_k^{-}$

The measurement result of step 3, dbjective state vector be the tracking results of current goal attitude, by current time measurement result with the measurement result P of transport function _kexport and as the input results of subsequent time wave filter, return step 1, carry out the target attitude tracking of subsequent time.

2. a kind of target attitude tracking method based on inertia and geomagnetic sensor as claimed in claim 1, it is characterized in that, in described step 1, system noise covariance matrix Q value is 0.01.