CN102654404B - Method for improving resolving precision and anti-jamming capability of attitude heading reference system - Google Patents

Abstract

The invention discloses a method for improving resolving precision and anti-jamming capability of an attitude heading reference system. The attitude heading reference system mainly comprises a micro mechanical gyro, a micro mechanical accelerometer and a geomagnetic sensor, and is used for measuring attitude information of a movement carrier in real time. The method specifically comprises the following steps of: firstly, initializing the system according to output of the accelerometer and the geomagnetic sensor for completing a coarse alignment process; secondly, according to output of the gyro, determining an updating equation of a quaternion, calculating a quaternion of an updating system in real time, and realizing an updating process of the system; thirdly, with the output of the accelerometer and the geomagnetic sensor as the reference, realizing a correction process of the system by applying a kalman filtering technology; and fourthly, by combining with a fuzzy control theory, adding a fuzzy control module in the system, estimating the measurement noise of the system in real time, and realizing self-adaptive kalman filtration. According to the invention, the calculating complexity of the system is greatly lowered while the precision of the attitude heading reference system is improved; and especially, when the system is interfered by larger noise, the method can be used for rightly estimating and measuring the characteristic of the noise, thereby improving the attitude precision and enhancing the stability and reliability of the system.

Description

A kind of method improving attitude heading reference system calculation accuracy and system rejection to disturbance ability

Technical field

What the present invention relates to is a kind of method improving attitude heading reference system calculation accuracy and system rejection to disturbance ability based on fuzzy control Adaptive Kalman Filtering Algorithm.

Technical background

Attitude heading reference system (Attitude and Heading Reference System, AHRS), primarily of the device such as micro-mechanical gyroscope, micro-mechanical accelerometer and geomagnetic sensor composition, is used for measuring in real time the attitude information of motion carrier.Attitude heading reference system relies on inertance element to measure, without the need to outwards launching or accepting electromagnetic wave, completely independently can determine the attitude information of carrier, therefore it is very well disguised, militarily have and apply very widely, as: unmanned aerial vehicle, Navigation And Guidance, Aero-Space etc.; Meanwhile, because attitude heading reference system adopts micro mechanical device, the integrated level of system is high, compact, cost are low, and therefore it also has a wide range of applications at civil area, as: automobile navigation and control, platform courses, attitude of ship control, robot etc.

In attitude heading reference system, the course information of carrier mainly relies on geomagnetic sensor to determine, but in current social life, the widespread use of the electronic product such as electrical equipment, Wireless Telecom Equipment, makes carrier often be among the environment of electromagnetic interference (EMI).Kalman filtration module in attitude heading reference system, require to system noise and measurement noise priori known, system noise can obtain through the repetitive measurement to system, but, complicated electromagnetic environment makes measurement noise uncertain, this will have a strong impact on the precision of conventional kalman filter result, and system filter even can be caused time serious to disperse.Therefore, how to estimate measurement noise in real time, become the matter of utmost importance improving system rejection to disturbance ability and calculation accuracy.

J.Z.Sasiadek proposes the self-adaptation kalman filtering algorithm based on fuzzy control in paper " Sensor Fusion Based on Fuzzy Kalman Filtering forAutonomous Robot Vehicle ", and it has successfully been applied in GPS/INS integrated navigation system.At present, self-adaptation kalman filtering algorithm based on fuzzy control has been widely used in GPS/INS integrated navigation system, but its application in AHRS is little, Qin Wei at paper " Fuzzy Adaptive Extended Kalman Filter for Miniature Attitude and HeadingReference System " though in by this algorithm application in attitude heading reference system, but it adopts expansion kalman filtering and adjusts in real time system noise and measurement noise simultaneously simultaneously, this increases the computation complexity of system undoubtedly greatly, have a strong impact on the real-time of system.

Summary of the invention

The object of the invention is to the deficiency overcoming prior art existence, and provide a kind of based on conventional Kalman filter, in conjunction with fuzzy control theory, real-time estimation measurement noise, realize auto adapted filtering, with the raising attitude heading reference system calculation accuracy of stability and reliability improving system and the method for system rejection to disturbance ability.

The method that the present invention proposes is filtered into basis with conventional kalman, in conjunction with fuzzy control theory, real-time estimation measurement noise, realize auto adapted filtering, the calculation accuracy of raising system and antijamming capability, described attitude heading reference system, primarily of micro-mechanical gyroscope, micro-mechanical accelerometer and geomagnetic sensor composition, is used for measuring in real time the attitude information of motion carrier; Described method specifically comprises the following steps:

First carries out the initialization of system according to the output of accelerometer and Magnetic Sensor, completes coarse alignment process;

Second according to gyrostatic output, determines the renewal equation of hypercomplex number, calculates the hypercomplex number of renewal system in real time, realize the renewal process of system;

3rd with the output of accelerometer and Magnetic Sensor as a reference, uses kalman filtering technique, realize the makeover process of system;

4th, in conjunction with fuzzy control theory, adds fuzzy control model in systems in which, and the measurement noise of real-time estimating system realizes self-adaptation kalman filtering.

The present invention in described coarse alignment process, according to the output of accelerometer, the initial pitch angle of certainty annuity and roll angle; In conjunction with the output of Magnetic Sensor, determine angle, initial heading.According to initial attitude angle, determine initial strap-down matrix and initial hypercomplex number;

In described system update process, the renewal of hypercomplex number adopts Runge-Kutta algorithm, according to upgrading the hypercomplex number obtained, upgrades strap-down matrix and attitude angle;

In described system makeover process, hypercomplex number error vector and gyrostatic zero inclined error vector are as state vector, using acceleration error vector earth magnetism error vector as observation vector, determine state equation and the observation equation of kalman filtering, real-time estimating system error, and by Error Feedback in system, revise hypercomplex number, strap-down matrix and attitude angle;

Two fuzzy controllers are had in described fuzzy control model, in kalman filtering, calculate respectively in real time theory, the realized variance of the theory of acceleration part in residual vector, realized variance and magnetic part, respectively using the ratio of the theoretical variance of the two and realized variance as the input of two fuzzy controllers.

Fuzzy controller of the present invention, according to input, exports the measurement noise of correction factor correction acceleration and the measurement noise of magnetic respectively, realizes the effect estimating measurement noise in real time.

The present invention while raising attitude heading reference system precision, can greatly reduce the computation complexity of system; Especially when system is by larger noise, this algorithm can correctly be estimated to measure noisiness, improves attitude accuracy, improves stability and the reliability of system.

Accompanying drawing explanation

Fig. 1 is of the present invention is the attitude heading reference system figure based on fuzzy control self-adaptation kalman filtering.

Fig. 2 is the input for fuzzy control model of the present invention, exports membership function figure.

Embodiment

Below in conjunction with accompanying drawing, the present invention will be described in detail: the present invention is a kind of method improving attitude heading reference system calculation accuracy and system rejection to disturbance ability.

The method that the present invention proposes is filtered into basis with conventional kalman, in conjunction with fuzzy control theory, real-time estimation measurement noise, realize auto adapted filtering, the calculation accuracy of raising system and antijamming capability, described attitude heading reference system, primarily of micro-mechanical gyroscope, micro-mechanical accelerometer and geomagnetic sensor composition, is used for measuring in real time the attitude information of motion carrier; Described method specifically comprises the following steps:

The first, carry out the initialization of system according to the output of accelerometer and Magnetic Sensor, complete coarse alignment process;

The second, according to gyrostatic output, determine the renewal equation of hypercomplex number, calculate the hypercomplex number of renewal system in real time, realize the renewal process of system;

Three, with the output of accelerometer and Magnetic Sensor as a reference, use kalman filtering technique, realize the makeover process of system;

Four, in conjunction with fuzzy control theory, add fuzzy control model in systems in which, the measurement noise of real-time estimating system, realize self-adaptation kalman filtering.

The present invention in described coarse alignment process, according to the output of accelerometer, the initial pitch angle of certainty annuity and roll angle; In conjunction with the output of Magnetic Sensor, determine angle, initial heading.According to initial attitude angle, determine initial strap-down matrix and initial hypercomplex number;

Due to the change of course angle, do not change the output of accelerometer, therefore make course angle y=0, then the acceleration transformational relation under carrier coordinate system and navigational coordinate system can be expressed as:

$\left(\begin{array}{c}{a}_x^b\\ a_y^b\\ a_z^b\end{array}\right)=C_n^b\left(\begin{array}{c}{a}_x^n\\ a_y^n\\ a_z^n\end{array}\right)\left(\begin{array}{ccc}\cos r& 0& \sin r\\ \sin r\sin p& \cos p& -\cos r\sin p\\ -\sin r\cos p& \sin p& \cos r\cos p\end{array}\right)\left(\begin{array}{c}0\\ 0\\ -g\end{array}\right)$

Thus, angle of pitch p can be calculated and roll angle r is:

$\left\{\begin{array}{c}p=\arcsin \left(\frac{a_y^b}{-g}\right)\\ r=\arctan \left(\frac{a_x^b}{{-a}_z^b}\right)\end{array}\right.$

According to the angle of pitch p calculated and roll angle r, the transformational relation of ground magnetic vector can be expressed as:

$\left(\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right)=\left(\begin{array}{ccc}\cos r& \sin r\sin p& -\sin r\cos p\\ 0& \cos p& \sin p\\ \sin r& -\cos r\sin p& \cos r\cos p\end{array}\right)\left(\begin{array}{c}{m}_x^b\\ m_y^b\\ m_z^b\end{array}\right)$

Then, course angle is expressed as:

$y=\arctan \left(\frac{m_x}{m_y}\right)$

After determining carrier initial attitude angle, calculate initial strap-down matrix:

$T=\left(\begin{array}{ccc}\cos r\cos y-\sin r\sin p\sin y& -\cos p\sin y& \sin r\cos y+\cos r\sin p\sin y\\ \cos y\sin y+\sin r\sin p\cos y& \cos p\cos y& \sin r\sin y-\cos r\sin p\cos y\\ -\sin r\cos p& \sin p& \cos r\cos p\end{array}\right)$

Then, the initial value of hypercomplex number is calculated:

$\left\{\begin{array}{c}\left|q_0\right|=\frac{1}{2}\sqrt{1+T_{11}+T_{22}+T_{33}},\sin {\mathrm{gnq}}_0=+\\ \left|q_1\right|=\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}},{\mathrm{signq}}_1=\mathrm{sign}(T_{32}-T_{23})\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}},\mathrm{si}{\mathrm{gnq}}_2=\mathrm{sign}(T_{13}-T_{31})\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}},{\mathrm{signq}}_3=\mathrm{sign}(T_{21}-T_{12})\end{array}\right.$

In described system update process, the renewal of hypercomplex number adopts Runge-Kutta algorithm, according to upgrading the hypercomplex number obtained, upgrades strap-down matrix and attitude angle;

Hypercomplex number renewal equation is:

Wherein, for under carrier coordinate system, the angular velocity of navigational coordinate system opposite carrier coordinate system, is obtained by gyrostatic output.

In described system makeover process, hypercomplex number error vector and gyrostatic zero inclined error vector are as state vector, using acceleration error vector earth magnetism error vector as observation vector, determine state equation and the observation equation of kalman filtering, real-time estimating system error, and by Error Feedback in system, revise hypercomplex number, strap-down matrix and attitude angle;

In Kalman filter module, choose hypercomplex number error vector and gyrostatic zero inclined error vector as state vector:

X＝[q _e1q _e2q _e3ε _xε _yε _z] ^T

Then the state equation of Kalman filter is:

Wherein: for antisymmetric matrix.[W ₁(t) W ₂(t)] ^tfor system state noise, its for average be 0, variance is Gauss's self noise of Q (t).

Choose the output a of accelerometer and geomagnetic sensor ^b, m ^bthe projection in carrier coordinate system with gravitational acceleration vector and ground magnetic vector difference as observation vector:

$Z\left(t\right)=\left(\begin{array}{c}{a}^b-C_n^b{a}^n\\ m^b-C_n^b{m}^n\end{array}\right)=\left(\begin{array}{c}{\mathrm{\δ}\hat{a}}^b\\ {\mathrm{\δ}\hat{m}}^b\end{array}\right)$

Then the observation equation of Kalman filter is:

$Z\left(t\right)=\left(\begin{array}{cc}2*[{\hat{a}}^b\×]& 0_{3*3}\\ 2*\left[{\hat{m}}^b\right]& 0_{3*3}\end{array}\right)\left(\begin{array}{c}{q}_e\\ \mathrm{\Δ\ϵ}\end{array}\right)+\left(\begin{array}{c}{V}_1\left(t\right)\\ V_2\left(t\right)\end{array}\right)$

Wherein, [V ₁(t) V ₂(t)] ^tfor observation noise, average is 0, variance is R (t), and by continuous print state equation and observation equation discretize, discrete Kalman filter process is as follows:

$\left\{\begin{array}{c}{\hat{X}}_{k|k-1}=\begin{array}{c}\left(\begin{array}{c}{\hat{q}}_{e_{k|k-1}}\\ {\mathrm{\Δ}\widehat{\mathrm{\ϵ}}}_{k|k-1}\end{array}\right)=\left(\begin{array}{c}0\\ {\mathrm{\Δ}\widehat{\mathrm{\ϵ}}}_{k-1}\end{array}\right)\end{array}\\ P_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\\ K_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1}\\ {\mathrm{\α}}_k=Z_k-H_k{\hat{X}}_{k|k-1}\\ {\hat{X}}_k={\hat{X}}_{k|k-1}+K_k{\mathrm{\α}}_k\\ P_k=(I-K_k{H}_k)P_{k|k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T\end{array}\right.$

Wherein, for state one-step prediction value, P _k|k-1for square error one-step prediction value, K _kfor optimal filtering gain, α _kfor newly cease vector, for state estimation, P _kfor square error is estimated.

Two fuzzy controllers are had in described fuzzy control model, in Kalman filter process, calculate respectively in real time theory, the realized variance of the theory of acceleration part in residual vector, realized variance and magnetic part, respectively using the ratio of the theoretical variance of the two and realized variance as the input of two fuzzy controllers.

The vectorial α of new breath _kbe otherwise known as residual vector, and its actual variance matrix is:

$C_k={\mathrm{\α}}_k{\mathrm{\α}}_k^T$

Its theoretical variance matrix is:

$S_k=H_k({\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1})H_k^T+R_k$

If realized variance matrix is equal with theoretical variance matrix, then actual measurement noise is equal with the measurement noise that we estimate; If realized variance matrix is greater than theoretical variance matrix, then system is subject to noise, and actual measurement noise is greater than the measurement noise of estimation, needs to increase estimating noise; In like manner, if realized variance matrix is less than theoretical variance matrix, then needs to reduce estimating noise, realize adaptable Kalman filter.

Realized variance matrix and theoretical variance matrix can be expressed as respectively again:

$C_k=\left[\begin{array}{cc}{C}_A& C_{\mathrm{AM}}\\ C_{\mathrm{MA}}& C_M\end{array}\right]$

$S_k=\left[\begin{array}{cc}{S}_A& S_{\mathrm{AM}}\\ S_{\mathrm{MA}}& S_M\end{array}\right]$

Wherein, wherein C _a, S _afor measurement variance and the theoretical variance of acceleration information; C _aM, C _mA, S _aM, S _mAfor measurement covariance and theoretical covariance: the C of acceleration information and Geomagnetism Information _m, S _mfor measurement variance and the theoretical variance of Geomagnetism Information.For preventing mutually disturbing between accelerometer and Magnetic Sensor, therefore have employed two fuzzy control adaptation modules (FLAS), defining q respectively _a=tr (C _a)/tr (S _a), q _m=tr (C _m)/tr (S _m) as the input of two fuzzy control models, monitor the observation noise of accelerometer and Magnetic Sensor, wherein tr () is for getting matrix trace.

Fuzzy controller of the present invention, according to input, exports the measurement noise of correction factor correction acceleration and the measurement noise of magnetic respectively, realizes the effect estimating measurement noise in real time.

Two fuzzy control models have identical input, export membership function. as shown in Figure 2.The inference rule of fuzzy control is:

According to input membership function, judge that noise variation tendency is, and obtain being subordinate to blur level β:

If q ∈ [0.5,0.7], then ' noise reduces;

If q ∈ [0.7,1.3], then ' noise is constant ':

If q ∈ [1.3,1.5], then ' noise increases ';

According to output membership function, the weights that can obtain exporting are:

If ' noise reduces ', then W=0.3* β+1;

If ' noise is constant ', then

If ' noise increases ', then W=1-0.3* β;

Obtain respectively revising weights W from two fuzzy control models _a, W _m, observation noise is revised in real time:

$R_k=\left[\begin{array}{cc}{W}_A& 0\\ 0& W_M\end{array}\right]\left[\begin{array}{cc}{R}_{A_{k-1}}& 0\\ 0& R_{M_{k-1}}\end{array}\right]=W*R_{k-1}$

Through the real-time estimation of fuzzy control model to measurement noise, make Kalman filter can more stable, estimating system error that precision is higher. according to the error that filtering obtains. system is revised:

$Q_{\mathrm{real}}=\left(\begin{array}{cccc}1& -q_{e1}& {-q}_{e2}& {-q}_{e3}\\ q_{e1}& 1& q_{e3}& -q_{e2}\\ q_{e2}& -q_{e3}& 1& q_{e1}\\ q_{e3}& q_{e2}& {-q}_{e1}& 1\end{array}\right)*Q$

By revised hypercomplex number Q _realbe converted into strap-down matrix T _real:

$T_{\mathrm{real}}=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)$

Obtain attitude of carrier angle by strap-down matrix, and it can be used as result to export:

$\left\{\begin{array}{c}p=\arcsin \left(T_{32}\right)\\ r=\arctan \left(\frac{-T_{31}}{T_{33}}\right)\\ y=\arctan \left(\frac{-T_{12}}{{-T}_{22}}\right)\end{array}\right.$

Fig. 1 is the attitude heading reference system figure based on fuzzy control adaptable Kalman filter, shown in figure: carry out hypercomplex number renewal according to gyrostatic output and then realize posture renewal, with the output of accelerometer and Magnetic Sensor for reference, by Kalman filter, attitude angle and course angle are corrected; Fuzzy control model estimates observation noise in real time according to observed quantity, thus achieves the adaptable Kalman filter based on fuzzy control

Fig. 2 is input, the output membership function figure of fuzzy control model, exports after output obfuscation according to input, output membership function figure.

Claims (1)

1. improve a method for attitude heading reference system calculation accuracy and system rejection to disturbance ability, described attitude heading reference system, primarily of micro-mechanical gyroscope, micro-mechanical accelerometer and geomagnetic sensor composition, is used for measuring in real time the attitude information of motion carrier; It is characterized in that described method specifically comprises the following steps:

The first, carry out the initialization of system according to the output of accelerometer and Magnetic Sensor, complete coarse alignment process;

The second, according to gyrostatic output, determine the renewal equation of hypercomplex number, calculate the hypercomplex number of renewal system in real time, realize the renewal process of system;

Three, with the output of accelerometer and Magnetic Sensor as a reference, use kalman filtering technique, realize the makeover process of system;

Four, in conjunction with fuzzy control theory, add fuzzy control model in systems in which, the measurement noise of real-time estimating system, realize self-adaptation kalman filtering;

In described coarse alignment process, according to the output of accelerometer, the initial pitch angle of certainty annuity and roll angle; In conjunction with the output of Magnetic Sensor, determine angle, initial heading; According to initial attitude angle, determine initial strap-down matrix and initial hypercomplex number;

Due to the change of course angle, do not change the output of accelerometer, therefore make course angle y=0, then the acceleration transformational relation under carrier coordinate system and navigational coordinate system can be expressed as:

Thus, angle of pitch p can be calculated and roll angle r is:

According to the angle of pitch p calculated and roll angle r, the transformational relation of ground magnetic vector can be expressed as:

Then, course angle is expressed as:

After determining carrier initial attitude angle, calculate initial strap-down matrix:

Then, the initial value of hypercomplex number is calculated:

In described system update process, the renewal of hypercomplex number adopts Runge-Kutta algorithm, according to upgrading the hypercomplex number obtained, upgrades strap-down matrix and attitude angle;

Hypercomplex number renewal equation is:

Wherein, for under carrier coordinate system, the angular velocity of navigational coordinate system opposite carrier coordinate system, is obtained by gyrostatic output;

In described system makeover process, hypercomplex number error vector and gyrostatic zero inclined error vector are as state vector, using acceleration error vector earth magnetism error vector as observation vector, determine state equation and the observation equation of kalman filtering, real-time estimating system error, and by Error Feedback in system, revise hypercomplex number, strap-down matrix and attitude angle;

In Kalman filter module, choose hypercomplex number error vector and gyrostatic zero inclined error vector as state vector:

X＝[q _e1q _e2q _e3ε _xε _yε _z] ^T

Then the state equation of Kalman filter is:

Wherein: for antisymmetric matrix; [W ₁(t) W ₂(t)] ^tfor system state noise, its for average be 0, variance is Gauss's self noise of Q (t);

Choose the output a of accelerometer and geomagnetic sensor ^b, m ^bthe projection in carrier coordinate system with gravitational acceleration vector and ground magnetic vector difference as observation vector:

Then the observation equation of Kalman filter is:

Wherein, [V ₁(t) V ₂(t)] ^tfor observation noise, average is 0, variance is R (t), and by continuous print state equation and observation equation discretize, discrete Kalman filter process is as follows:

Wherein, for state one-step prediction value, P _k|k-1for square error one-step prediction value, K _kfor optimal filtering gain, α _kfor newly cease vector, for state estimation, P _kfor square error is estimated;

Two fuzzy controllers are had in described fuzzy control model, in Kalman filter process, calculate respectively in real time theory, the realized variance of the theory of acceleration part in residual vector, realized variance and magnetic part, respectively using the ratio of the theoretical variance of the two and realized variance as the input of two fuzzy controllers;

The vectorial α of new breath _kbe otherwise known as residual vector, and its actual variance matrix is:

Its theoretical variance matrix is:

If realized variance matrix is equal with theoretical variance matrix, then actual measurement noise is equal with the measurement noise that we estimate; If realized variance matrix is greater than theoretical variance matrix, then system is subject to noise, and actual measurement noise is greater than the measurement noise of estimation, needs to increase estimating noise; In like manner, if realized variance matrix is less than theoretical variance matrix, then needs to reduce estimating noise, realize adaptable Kalman filter;

Realized variance matrix and theoretical variance matrix can be expressed as respectively again:

Wherein, C _a, S _afor measurement variance and the theoretical variance of acceleration information; C _aM, C _mA, S _aM, S _mAfor measurement covariance and theoretical covariance: the C of acceleration information and Geomagnetism Information _m, S _mfor measurement variance and the theoretical variance of Geomagnetism Information; For preventing mutually disturbing between accelerometer and Magnetic Sensor, therefore have employed two fuzzy control adaptation modules (FLAS), defining q respectively _a=tr (C _a)/tr (S _a), q _m=tr (C _m)/tr (S _m) as the input of two fuzzy control models, monitor the observation noise of accelerometer and Magnetic Sensor, wherein tr () is for getting matrix trace;

Described fuzzy controller, according to input, exports the measurement noise of correction factor correction acceleration and the measurement noise of magnetic respectively, realizes the effect estimating measurement noise in real time;

Two fuzzy control models have identical input, export membership function; The inference rule of fuzzy control is:

If q ∈ [0.5,0.7], then ' noise reduces;

If q ∈ [0.7,1.3], then ' noise is constant ':

If q ∈ [1.3,1.5], then ' noise increases ';

According to output membership function, the weights that can obtain exporting are:

If ' noise reduces ', then W=0.3* β+1;

If ' noise is constant ', then

If ' noise increases ', then W=1-0.3* β;

Obtain respectively revising weights W from two fuzzy control models _a, W _m, observation noise is revised in real time:

Through the real-time estimation of fuzzy control model to measurement noise, make Kalman filter can more stable, estimating system error that precision is higher. according to the error that filtering obtains. system is revised:

By revised hypercomplex number Q _realbe converted into strap-down matrix T _real:

Obtain attitude of carrier angle by strap-down matrix, and it can be used as result to export: