Wang Menglong,Wang Hua

School of Astronautics,Beihang University,Beijing 100083,China

An adaptive attitude algorithm based on a current statistical model for maneuvering acceleration

Wang Menglong,Wang Hua*

School of Astronautics,Beihang University,Beijing 100083,China

Attitude and heading reference system;Current statistical model;Kalman filter;Loitering vehicle;Maneuvering acceleration;Membership function

A current statistical model for maneuvering acceleration using an adaptive extended Kalman filter(CS-MAEKF)algorithm is proposed to solve problems existing in conventional extended Kalman filters such as large estimation error and divergent tendencies in the presence of continuous maneuvering acceleration.A membership function is introduced in this algorithm to adaptively modify the upper and lower limits of loitering vehicles’maneuvering acceleration and for realtime adjustment of maneuvering acceleration variance.This allows the algorithm to have superior static and dynamic performance for loitering vehicles undergoing different maneuvers.Digital simulations and dynamic flight testing show that the yaw angle accuracy of the algorithm is 30%better than conventional algorithms,and pitch and roll angle calculation precision is improved by 60%.The mean square deviation of heading and attitude angle error during dynamic flight is less than 3.05°.Experimental results show that CS-MAEKF meets the application requirements of miniature loitering vehicles.

1.Introduction

Extended Kalman filters(EKF)have been widely used in attitude and heading reference systems(AHRS)for error estimation and error correction.However,this kind of approximate linear filtering method based on Taylor expansion requires the establishment of an accurate system model and a priori knowledge of statistical properties for system noise and observation noise.This results in inferior robustness in terms of model error and observation noise.1–5During flight,a loitering vehicle often needs to perform maneuvers,such as acceleration,deceleration,and turning,during which lasting and continuous maneuvering acceleration will be generated.Since the state equation of the conventional EKF filter does not consider the impact of maneuvering acceleration,it gives large estimation errors in the presence of acceleration disturbance,which can lead to divergence.6–8To solve this problem,the system must effectively identify the amount of loitering vehicle maneuvering acceleration and eliminate its impact on the filtering process.In Ref.9,GPS acceleration was introduced into the observation equation to compensate for maneuveringacceleration,but due to the low GPS data update rate,it was easily disturbed by electromagnetic interference.In Ref.10,a 6-state EKF based on gravity vectors and geomagnetic field observations was proposed;the result of this algorithm validated feasibility in static conditions,but with limited ability to deal with dynamic conditions.Gao et al.11presented a real-time quaternion-based EKF that has good performance without disturbances from carrier acceleration;however,its attitude accuracy is unstable with maneuvering.Guo et al.12proposed a 9-state EKF maneuvering acceleration model that requires filtering model parameters to be set in advance.This model works well in quasi static conditions,but,due to diff iculty dealing with abrupt maneuvering acceleration interference and low convergence,it is not suitable for loitering vehicles and other wide-range dynamic applications.Wang et al.13proved mathematically that a modified Rayleigh distribution can have a blind region during estimation.A membership function was therefore adopted in that literature to adjust extreme acceleration values.A current statistical model’s estimation blind region was compressed significantly in this way.It thus provides a new method to obtain independent heading and attitude angle measurements in the presence of maneuvering acceleration interference.A current statistical model for maneuvering acceleration using an adaptive extended Kalman filter(CS-MAEKF)algorithm is proposed in this paper based on analysis of current statistical models.It can be used to improve the measurement precision of AHRS undergoing strong or weak maneuvering.The validity of this algorithm is verified by simulation and experimental results in this paper.

2.Current statistical model based on maneuvering acceleration

When a loitering vehicle performs variable acceleration and turning maneuvers for a long period of time,it is better to use a nonzero mean time correlation model to describe the dynamics of carrier maneuvering acceleration.The current statistical model is a more practical nonzero mean time correlation motion model and ideal to describe maneuvering acceleration of loitering vehicles.The basic concept is depicted below:When the carrier maneuvers with certain acceleration at the current moment,the range of acceleration for the next instant will be limited and confined in the neighborhood of the current acceleration.14A modified Rayleigh distribution is adopted to describe the statistical properties of the carrier acceleration.When the carrier maneuvering acceleration at the current moment is positive,the probability density function is

whereamax＞0 denotes the upper limit of carrier acceleration;arepresents maneuvering acceleration of the carrier;μ＞0 is a constant.At this point,mean and variance of carrier maneuvering acceleration can be expressed as

Similarly,when the carrier’s current maneuvering acceleration is negative,the probability density function is

Mean and variance of carrier maneuvering acceleration are

Here,a-max＜0 denotes the lower limit of carrier acceleration.

When the maneuvering acceleration of carrier at current time is zero,the probability density function is:P(a)=δ(a),where δ(a)is the Dirac function.

Once the current maneuvering acceleration is given,the probability density function of maneuvering acceleration is determined completely.Based on the basic concepts of the currentstatisticalmodel,afirstordertimecorrelationmodelisused to describe the carrier maneuvering acceleration as follows:

wheretis current time,ˉa(t)is the maneuvering acceleration mean,which is constant in each sampling period;Δa(t)is the maneuvering acceleration error,which is zero mean colored acceleration noise;α is the carrier maneuvering frequency;wa(t)is white noise with a variance of σ2wa=2ασ2a.

Estimation value and variance of the carrier maneuvering acceleration have the following mathematical relationships:13

When the maneuvering acceleration of carrier at current time is positive:

When the maneuvering acceleration of carrier at current time is negative:

Ref.13mathematically proves that the current statistical model is unable to provide accurate distribution of the acceleration when the maneuvering acceleration mean is in[(4- π)a-max/4，(4- π)amax/4],which means an estimation blind region exists.

Although some researchers present adaptive algorithms based on fuzzy membership functions to modify the limits of carrier acceleration,the triangular membership function is extensively applied.15–17A triangular membership function makes system noise estimation jump when the membership grade changes,which leads to decreasing algorithm accuracy.13Ref.13proposes modifying the extreme value of acceleration by using the exponential function as a membership function to compress the range of estimation blind region,which turns out to be quite effective.It is also points out that the membership functionMshould meet the following conditions:

where[a-up，aup]is the probable value range of carrier maneuvering acceleration;athanda-thare two set acceleration thresholds,withath＞aupanda-th＜a-up.

The algorithm from Ref.13adjusts the process noise variance under certain acceleration mean values.This algorithm provides a lower filter stable gain and,consequently,a higher overshoot that will impact estimate accuracy if the maneuvering acceleration of the carrier jumps and especially when the moving state changes from uniform motion to acceleration.

In this paper,a Gaussian membership functionMis introduced to adjust the upper and lower limits of carrier maneuvering acceleration:

wherek1andk2are two constants.

Fig.1 shows the curve of the membership function.

A Gaussian membership function smoothly changes in the whole domain of discourse and will not cause a jump in system noise estimate characteristic of a triangle membership function.The value of Gaussian membership function is tiny for a small independent variable and increases rapidly when the independent variable is larger.Therefore,the scheme of applying a Gaussian membership function to correct current maneuvering acceleration limits of the carrier effectively adjusts maneuvering acceleration limits in weak maneuvering and non-maneuvering states and guarantees favorable characteristics of the current statistical model in high maneuvering state.

In the Gaussian membership function,the values ofk1andk2represent width of the function shape,which has a major impact on performance of the fuzzy controller.Smallerk1andk2indicate narrower shape of the membership function and more responsive control,which means more effective correction of acceleration limits.Conversely,largerk1andk2indicate wider membership function shape,more gentle curves,and less sensitive and responsive control,which correspond to less effective correction of acceleration limits.In weak maneuvering and non-maneuvering states,for a smallerk1andk2,the membership function provides a more obvious correction for the acceleration limits and contributes significantly to accuracy.However,ask1andk2become smaller,the membership function also provides a more obvious correction in high maneuvering mode,which leads to slower responses,a major overshoot and subsequent impaired algorithm estimate effect.

The specific value range ofk1andk2is difficult to determine from Eq.(8)alone,so we also use simulation tests.The algorithm estimate accuracy is more favorable whenk1andk2are in the[20,25]range.

Fig.1 Gaussian membership function.

In practical applications,the upper and lower limit values of carrier maneuvering acceleration are adapted withM.

wheretkis current time,tk-1is previous time.

On this basis,the estimation variance of maneuvering acceleration can be adapted according to the value of the current carrier maneuvering acceleration,thus improving estimation capacity of the algorithm for strong maneuvering,weak maneuvering and static maneuvering acceleration.

3.CS-MAEKF algorithm description

Because the miniature HARS discussed uses micro inertial devices with high measurement noise,long operation time,and various disturbances caused by electromagnetic and carrier maneuvering acceleration,an indirect feedback correction Kalman filter is designed to fuse the sensor data to obtain carrier attitude information.

3.1.Differential equation of attitude and heading angle error

In this paper,the micro HARS is based on two sets of coordinates:a navigation coordinate system(Oxnynzn)and a carrier coordinate system(Oxbybzb).The navigation coordinate system adopts a ‘north-east-ground” rule.The carrier coordinate system can be obtained by rotating the navigation system by a corresponding attitude angle(yaw angle,pitch angle and roll angle)along thezn,ynandxnaxes in turn.

The actual heading and attitude angle quaternion q(t)can be decomposed into an estimation heading attitude angle quaternion^q(t)and an estimation error quaternion qe(t).18,19

where?represents multiplication of quaternion.After differentiation for both sides of Eq.(11):

Substituting Eq.(15)into Eq.(14)and ignoring the high order small items results in

3.2.Differential equation of micro electro mechanical gyro drift error

The estimation value of loitering vehicle movement angular velocity is22–24

Based on Eq.(17),we have

where wp(t)is a zero mean white noise process with weak strength.

3.3.Differential equation of maneuvering acceleration error

Based on the current statistical model concept,maneuvering acceleration error of a loitering vehicle can be represented by Eq.(5):

where Δa(t)is maneuvering acceleration error and zero mean colored acceleration noise;α represents the maneuvering frequency vector of carrier;wa(t)is the white noise.

3.4.State equation and observation equation

Take the following filtering state vector:

Based on Eqs.(5),(16)and(19),the structural system state equation is

W(t)acts for zero mean white noise process with covariance matrix white noise process with covariance matrix E｛W(t)WT(t)｝ =Q(t)δ(t- τ),where τ represents the time constant.

The system noise variance strength matrix Q(t)can be represented by the following equation:

where the diagonal elements of Q1are determined by the gyro noise,those in Q2are associated with gyroscope drift,and those in Q3are associated with flight dynamics characteristics.According to the current statistical model theory,Q3is calculated as14

where σ2ais maneuvering acceleration variance of the carrier,which can be adapted with Eqs.(6)and(7)based on the current maneuvering acceleration value.

As the filtering period for miniature attitude and heading reference system is short in this paper,a single-step state transition matrix and equivalent discrete noise variance matrix can be simplified as follows:

whereTis filtering period.

Select as observations the difference between the accelerometer output and the acceleration estimate as well as the difference between the magnetic sensors and the projection of geomagnetic vectors in the carrier coordinate system.The acceleration estimate includes estimated carrier maneuvering acceleration and projection of gravitational acceleration vectors in the carrier coordinate system.The system observation equation is then expressed as

Together,Eqs.(21),(26)and(5)–(10)form the complete CS-MAEKF filter algorithm described in this paper.In each sampling period,the heading and attitude quaternion is updated with the rotation vector algorithm.The process for updating system error state and covariance time is described below:

Fig.2 Structure of CS-MAEKF.

where Γ(tk-1)is system noise matrix;P(tk-1)is the covariance matrix of state estimation error;K(tk)is the Kalman gain matrix.

The structure diagram of CS-MAEKF is shown in Fig.2.In Fig.2,EKF can be worked out through Eqs.(26)and(27);ˉωb(tk),mb(tk)and ab(tk)represent the actual movement angular velocity of carrier,the geomagnetic field strength and the carrier maneuvering acceleration at thetkmoment in carrier coordinate,respectively;mn(tk)and gn(tk)represent the geomagnetic field strength and the gravity acceleration at thetkmoment in navigation coordinate,respectively;^a(tk)is the carrier maneuvering acceleration estimation at thetkmoment;^Cbn(tk)is the attitude transfer matrix between navigation coordinate and carrier coordinate at thetkmoment.The dashed lines indicate the process of real-time correction for attitude quaternion and error compensation after Kalman filtering.

4.Test results

In order to verify the effectiveness of the CS-MAEKF filtering algorithm in engineering practices,digital simulation tests and dynamic flight tests are performed in presence of disturbances due to maneuvering acceleration.

4.1.Digital simulation testing

Loitering vehicles are mainly in the flight state of variable velocity maneuvering when performing tasks,and such variable speed maneuvering flight will generate continuous maneuvering acceleration.This set of digital simulations validates the effectiveness of the CS-MAEKF algorithm in the presence of interference caused by sustained maneuvering acceleration by examining the variable velocity circular flight of a loitering vehicle.The simulation’s circular radiusr=600 m,height is 200 m,initial velocity is 20 m/s,roll angle is 20°,and pitch angle is 0°.The vehicle completes a series of variable velocity circling movement in the velocity plane in the following order:constant speed→ acceleration→ constant speed→ acceleration→ constant speed→ deceleration →constant speed→deceleration→constant speed.Each constant speed cycle lasts for 30 s,and each variable velocity cycle,where acceleration and deceleration velocity is 2 m/s2,lasts for 10 s.Total flight simulation time is 190 s.

Heading and attitude angle error during the loitering vehicle’s variable velocity circular flight is estimated with the conventional 9-state EKF algorithm,a modified 9-state EKF algorithm suggested in Ref.12,and the CS-MAEKF algorithm proposed in this paper.The filter parameter settings are listed below:

The system noise variance matrix of conventional 9-state EKF is

Q=diag(0.01，0.01，0.01，0.01，0.01，0.01，0.03，0.03，0.03)

The system noise variance matrix of modified 9-state EKF is

Q=diag(0.01，0.01，0.01，0.01，0.01，0.01，0.03，0.03，0.03)

The system noise variance matrix of CS-MAEKF is

Q=diag(0.01，0.01，0.01，0.01，0.01，0.01，q1，q2，q3)

The observation noise variance matrix is

R=diag(0.02，0.02，0.02，0.02，0.02，0.02)

The initial state vector of filter is

X0=diag(0，0，0，0，0，0，0，0，0)

The initial estimate covariance is

P0=diag(0.01，0.01，0.01，0.01，0.01，0.01，0.01，0.01，0.01)

In the CS-MAEKF system noise variance matrix,q1,q2andq3are adjusted with changes of maneuvering acceleration.The other parameters are:a-th=-10 m/s2,ath=10 m/s2,k1=k2=23,α=1/60.The estimation errors in simulation for heading and attitude angles are shown in Fig.3,and the maximum estimated errors are shown in Table 1.

According to Fig.3 and the data in Table 1,the estimation error in heading and attitude angles is bigger since the conventional 9-state EKF doesn’t consider the influence of maneuvering acceleration.The maximum estimation errors of roll angle,pitch angle and yaw angle reach 32.23°,14.23°and 50.22°,respectively.Although the influence of maneuvering acceleration is considered by the modified 9-state EKF,the system noise variance matrix cannot change with the current maneuvering acceleration.When abrupt changes in maneuvering acceleration abrupt appear, state estimation accuracy decreases significantly.The maximum estimate error in the roll angle,pitch angle,and yaw angle are 3.94°,3.31°,and 6.82°,respectively.The influence of maneuvering acceleration is fully considered by the CS-MAEKF algorithm.Corresponding parameters in the system noise variance matrix are adapted in real time based on maneuvering acceleration of the loitering vehicle,which enhances stability of estimation and signif icantly decreases error in heading and attitude angle estimation.The maximum estimate errors of the roll angle,pitch angle and yaw angle are 1.13°,1.41°and 4.43°,respectively.

Statistics in Table 1 indicate that the maximum roll angle error and pitch angle error calculated by CS-MAEKF algorithm are only about 1/3 that of the modified 9-state EKF,and the maximum yaw angle error is just about 2/3 that of the modified 9-state EKF.

Fig.3 Variation curves of estimation errors for heading and attitude angles with different algorithms.

Fig.4 Test flight platform.

Fig.5 Flight path of dynamic flight test.

4.2.Dynamic flight test

In order to verify the dynamic flight performance of the CSMAEKF algorithm,the MTI of XSENS Technologies from Holland is selected as this paper’s reference benchmark for heading and attitude angle.MTI performance parameters are as follows:sampling frequency is 100 Hz;static measurement accuracy of roll angle and pitch angle is less than 0.5°,and yaw angle less than 1°;dynamic measurement accuracy is 2°RMS.

Table 1 Maximum estimation errors for heading and attitude angles.

Table 2 Maximum estimated errors and mean square deviation for heading and attitude angles.

Fig.6 Variation curves of roll angle,pitch angle and yaw angle in dynamic flight test.

A test flight platform is used to carry MTI and simulate a reconnaissance mission.During this simulation,the aircraft first performs maneuvering flight in a wide area and begins circling in a small area after its target is locked.The test flight platform and the flight path are shown in Figs.4 and 5,respectively.By using the raw data from the MTI accelerometer,gyroscope and geomagnetic sensor,attitude can be calculated with the conventional 9-state EKF,modified 9-state EKF and CS-MAEKF algorithms.Calculation results are compared with the MTI output for heading and attitude angle.Flight trajectory is designed as sigmoid evasive maneuvering flight and circular maneuvering flight.During the whole flight,there is constant maneuvering acceleration with abrupt changes of amplitude and direction to test the dynamic performance of the CS-MAEKF algorithm.Dynamic flight test results are shown in Table 2 and Figs.6 and 7.

Fig.7 Variation curves of estimated errors of roll angle,pitch angle and yaw angle in dynamic flight test.

From Fig.6,it can be seen that good consistency exists between the heading and attitude angles calculated by the CS-MAEKF algorithm and the reference output from MTI.Fig.7 and Table 2 show that the maximum heading and attitude angle error calculated by the CS-MAEKF algorithm is just about 1/3 that of the modified 9-state EKF and only about 1/10 that of the conventional 9-state EKF.The maximum roll angle error of CS-MAEKF is 4.54°,the maximum pitch angle error of CS-MAEKF is 3.48°,the maximum value of the yaw angle error of CS-MAEKF is 8.73°,and the heading and attitude angle has a mean square deviation of CS-MAEKF not exceeding 3.05°.The dynamic flight test results verify the stability and effectiveness of the CS-MAEKF algorithm proposed in this paper,which demonstrates good calculation accuracy in terms of heading and attitude angle in dynamic flight.

5.Conclusions

(1)This paper proposed a CS-MAEKF algorithm to reduce the influence on the estimation error of attitude angle under maneuvering acceleration.In this algorithm,carrier maneuvering acceleration is modeled with current statisticalmodeling,and introduced to the CSMAEKF algorithm as a state variable of the system.Considering estimation precision and the real-time nature of system state for a loitering vehicle in different environments,a Gaussian function is adopted as a membership function to allow adaptive adjustment of estimation variance for carrier maneuvering acceleration,which can improve estimation precision of carrier maneuvering acceleration as well as heading and attitude angles.

(2)Three Kalman algorithms,including conventional 9-state EKF algorithm,modified 9-state EKF algorithm and CS-MAEKF algorithm are introduced and compared by digital simulation test and dynamic flight test in order to choose the best one for loitering vehicles under different flight conditions.Experimental results indicate that the CS-MAEKF can provide the best performance under continuous maneuvering acceleration.

(3)The CS-MAEKF algorithm’s dynamic performance can meet the requirements of loitering vehicles in dynamic flight.This algorithm can be used for the attitude and heading reference systems of low cost miniature loitering vehicles.

1.Kim KH,Lee JG,Park CG.Adaptive two-stage extended Kalman filter for a fault-tolerant INS-GPS loosely coupled system.IEEE Trans Aerospace Electron Syst2009;45(1):125–37.

2.Quan W,Xu L,Zhang HJ,Fang JC.Interlaced optimal-REQUEST and unscented Kalman filtering for attitude determination.Chin J Aeronaut2013;26(2):449–55.

3.Chen JZ,Yuan JP,Fang Q.Flight vehicle attitude determination using the modified Rodrigues parameters.Chin J Aeronaut2008;21(5):433–40.

4.Maley JM.Efficient attitude estimation for a Spin-stabilized projectile.J Guid Control Dyn2016;39(2):339–50.

5.Chang LB,Li JS,Chen SY.Initial alignment by attitude estimation for strapdown inertial navigation systems.IEEE Trans Instrum Measure2015;64(3):784–94.

6.Weibel D,Lawrence D,Palo S.Small unmanned aerial system attitude estimation for flight in wind.J Guid Control Dyn2015;38(7):1300–5.

7.Martin P,Salau¨n E.Design and implementation of a low-cost observer-based attitude and heading reference system.Control Eng Pract2010;18(7):712–22.

8.Sheng HL,Zhang TH.MEMS-based low-cost strap-down AHRS research.Measurement2015;59:63–72.

9.Psiaki ML,Powell SP,Kintner Jr PM.The accuracy of the GPS-derived acceleration vector,a novel attitude reference.Proceedings of AIAA 1999 guidance,navigation,and control conference;1999 Aug 9–11;Poland.Reston:AIAA;1999.p.1–10.

10.Zhu R,Sun D,Zhou ZY,Wang DQ.A linear fusion algorithm for attitude determination using low cost MEMS-based sensors.Measurement2007;40(3):322–8.

11.Gao ZY,Niu XJ,Guo MF.Quaternion-based Kalman filter for micro-machined strapdown attitude heading reference system.Chin J Aeronaut2002;15(3):171–5.

12.Guo PF,Ren Z,Qiu HT,Yang YC.Maneuvering acceleration assisted extended Kalman filter for attitude and heading reference system.Syst Eng Electron2009;31(3):625–37[Chinese].

13.Wang XH,Qin Z,Yang HJ,Yang XY.A fuzzy adaptive algorithm based on current statistical model for maneuvering target tracking.Acta Armament2009;30(8):1089–93[Chinese].

14.Zhou HR,Jing ZL,Wang PD.Tracking of maneuvering targets.Beijing:National Defense Industry Press;1991.p.56–75[Chinese].

15.Tafti AD,Sadati N.Novel adaptive Kalman filtering and fuzzy track fusion approach for real time applications.Proceedings of 2008 the 3rd IEEE conference on industrial electronics and applications;2008 Jun 3–5;Singapore.Piscataway(NJ):IEEE Press;2008.p.120–125.

16.Wang QH,Huang JJ,Huang JX.Fuzzy logic-based multi-factor aided multiple-model filter for general aviation target tracking.J Intell Fuzzy Syst2015;29(6):2603–9.

17.Jwo DJ,Wang SH.Adaptive fuzzy strong tracking extended Kalman filtering for GPS navigation.IEEE Sensors J2007;7(5):778–89.

18.Hemerly EM,Maciel BCO,Milhan AD,Schad VR.Attitude and heading reference system with acceleration compensation.Aircraft Eng Aerospace Technol2012;84(2):87–93.

19.Weimer F,Frangenberg M,Fichter W.Pipelined particle filter with non-observability measure for attitude and velocity estimation.J Guid Control Dyn2015;38(3):506–18.

20.Guerrero-Castellanos JF,Madrigal-Sastre H,Durand S,Marchand N,Guerrero-Sa′nchez WF,Salmero′n BB,et al.Design and implementation of an attitude and heading reference system(AHRS).Proceedings of the 8th international conference on electrical engineering,computing science and automatic control(CCE);2011 Oct 26–28;Merida City,Mexico.Piscataway(NJ):IEEE Press;2011.p.1–5.

21.Gebre-Egziabher D,Hayward RC,Powell JD.Design of multisensor attitude determination systems.IEEE Trans Aerospace Electron Syst2004;40(2):627–49.

22.Reinstein M,Sipos M,Rohac J.Error analyses of attitude and heading reference systems.Przegla■d Elektrotech2009;85(8):114–8.

23.Han SL,Wang JL.A novel method to integrate IMU and magnetometers in attitude and heading reference systems.J Navigat2011;64(4):727–38.

24.Crassidis JL,Lai KL,Harman RR.Real-time attitude-independent three-axis magnetometer calibration.J Guid Control Dyn2005;28(1):115–20.

29 March 2016;revised 24 June 2016;accepted 28 September 2016

Available online 22 December 2016

?2016 Chinese Society of Aeronautics and Astronautics.Production and hosting by Elsevier Ltd.This is anopenaccessarticleundertheCCBY-NC-NDlicense(http://creativecommons.org/licenses/by-nc-nd/4.0/).

*Corresponding author.

E-mailaddresses:wangmenglongtiger@163.com (M.Wang),Whua402@163.com(H.Wang).

Peer review under responsibility of Editorial Committee of CJA.