An attitude calculation algorithm based on WNN-EKF

CHEN Guangwu, FAN Ziyan, WEI Zongshou, LI Wenyuan, ZHANG Linjing

(1. Automatic Control Research Institute, Lanzhou Jiaotong University, Lanzhou 730070， China；2. Gansu Provincial Key Laboratory of Traffic Information Engineering and Control, Lanzhou 730070， China)

Abstract： In the strapdown inertial navigation system, the attitude information is obtained through an inertial measurement unit (IMU) device, which mainly includes a triaxial gyroscope, a triaxial accelerometer and a triaxial magnetometer. However, IMU sensors have system noise and drift errors, and these errors can accumulate over time, which makes it difficult to control the attitude accuracy. In order to solve the problems of gyro drift over time and random errors generated by the surrounding environment, this paper presents an attitude calculation algorithm based on wavelet neural network-extended Kalman filter (WNN-EKF). The wavelet neural network (WNN) is used to optimize the model and compensate the extended Kalman filter’s own model error. Through the semi-physical simulation experiment, the results show that the algorithm improves the accuracy of attitude calculation and enhances the self-adaptability to the environment.

Key words： inertial measurement unit (IMU); quaternion; attitude calculation; wavelet neural network (WNN); extended Kalman filter (EKF)

0 Introduction

Inertial navigation system (INS) is a self-contained positioning and attitude device that continuously measures three orthogonal linear accelerations and three angular rates. The principle of inertial navigation is based on Newton’s first and second laws of motion. By measuring vehicle acceleration and angular velocity in an inertial frame of reference, integrating it with respect to time and transforming it to the navigation frame, the components of velocity, attitude and position can be obtained. Sensors used to implement such a system are accelerometers for the measurement of the vehicle’s linear acceleration (specific force) and gyroscopes for monitoring the vehicle’s rotation (angular velocity) with respect to an inertial frame of reference[1]. Since specific force measurements contain the effect of the earth’s gravity field, a gravity model is needed to extract the vehicle’s motion acceleration from the measurements. Because they employ three translational (accelerometers) and three rotational (gyroscopes) sensors, inertial measuring units (IMU) can be used as positioning and attitude-monitoring devices[2].

Due to its low cost, small size, easy integration and low power consumption, micro-electro-mechanical systems (MEMS) has become the first choice of inertial navigation system in recent years. The MEMS-based IMU can measure the three-axis gyro information and acceleration information of the carrier motion, besides, the carrier attitude can be obtained through the attitude calculation, and the inertial navigation can be realized. With the continuous development of MEMS technology, computer technology and consideration of low-cost factors, gyroscopes, accelerometers and magnetic sensors have been applied in more and more fields. However, the gyroscope has temperature drift characteristics so that the long-running drift is severe, and the integrated operation generates cumulative error. The accelerometer is susceptible to carrier vibration and motion acceleration, and the magnetic sensor is susceptible to ferromagnetic materials. Therefore, it is worth studying how to combine the data of three sensors to filter out external interference and get high reliability and high-precision attitude data.

The discrete simple Kalman filter (KF) is a predictor-corrector type estimator for the linear system when an unknown variable cannot be directly measured. It should be estimated optimally from the output measurements. It provides an efficient recursive means to estimate the state of a process in a way that minimizes the mean of the squared error. The extended KF (EKF) is the nonlinear version of KF that linearizes about the estimation of the covariance and the current mean value[3]. The EKF uses a nonlinear high-gain feedback calculated in every time-step[4-6]. The EKF is one of the variants of KF, which is only valid in case of linear systems. The EKF is proposed through a linearization procedure in order to apply a KF algorithm to the nonlinear battery system. The EKF can be used to calculate the attitude[3]. Along with the rapid development of neural network technology, due to its good nonlinear mapping ability and strong self-learning ability, the application field is continuously expanded[7]. For the same learning task, the wavelet neural network (WNN) is compared to the forward neural network. Firstly, the primitives and the whole structure of the wavelet neural network are determined according to the wavelet analysis theory, which can avoid the blindness of BP neural network and other structural design. Secondly, the wavelet neural network has stronger learning ability and higher precision. The WNN has obvious advantages of a simpler structure and higher precision. For the attitude calculation algorithm and the defects of the above sensors, the wavelet neural network is combined with the Kalman filter model to perform the attitude calculation.

1 Quaternion principle

For the attitude, the paper describes the attitude with the pitch angle, the roll angle and the yaw angle. “North East Sky” is selected as the navigation coordinate system and “Left Front Up” as the carrier coordinate system. Since the measurement system and the carrier are fixed, the system coordinate system and the carrier coordinate system are considered to be consistent. During the movement of the system, the carrier coordinate system at each moment can coincide with the navigation coordinate system after one to three rotations. Because the result measured by the inertial measurement unit inside the measurement system is a value relative to the carrier coordinate system, it is necessary to convert the measured data to the navigation coordinate system. This process uses a pose transformation matrix from the carrier coordinate system to the navigation coordinate system.

The attitude transformation matrix is expressed as

(1)

(2)

The desired attitude angle is expressed as

(3)

whereγis the roll angle;φis the yaw angle;θis the pitch angle.

The navigation coordinate system and the carrier coordinate system are regarded as a cartesian coordinate system (the axes of the coordinate system are perpendicular to each other). So the coordinate system is considered as a rigid body. When studying the angular position relationship between the two coordinate systems, one coordinate can be translated so that its origin coincides with the origin of another coordinate system, thus the spatial angular position relationship between the two sets of marks can be used as the fixed point rotation of the rigid body. According to the idea, the quaternion algorithm for attitude update can be obtained.

The pose quaternion defined from the carrier coordinate system to the navigation coordinate system is expressed as

Q(q0,q1,q2,q3)=q0·1+q1i+q2j+q3k,

(4)

whereq0,q1,q2andq3are four real numbers;i,jandkare unit vectors and are orthogonal.

After a one-time equivalent rotation, the attitude transformation matrix can be represented by a quaternion and is expressed as

(5)

The quaternion pose matrix differential equation has smaller calculation because it only needs to solve four first-order differential equations, and it can meet the real-time requirements in engineering practice. However, the drift of the algorithm is more serious, so this paper introduces the WNN-EKF model to solve the attitude information based on the quaternion method.

2 WNN-EKF algorithm

2.1 EKF algorithm

The classical Kalman filter algorithm is only suitable for systems that are linear and satisfy Gaussian distribution. But it is not so simple in actual engineering, such as sometimes the system is not linear, so EKF are needed. The probability distribution of the current state is a binary function with respect to the previous state and the amount of control to be performed, and superimposes a Gaussian noise, and the measured value is also a superposition of Gaussian noise on the current state. The specific expression is expressed as

(6)

whereg(ut,xt-1) andh(xt) can be a nonlinear function;εtis process noise;δtis observation noise.

In order to solve the state estimation problem in nonlinear systems with the idea of classical Kalman filter,g(ut,xt-1) andh(xt) are developed with the Taylor series and are linearized, only taking their one term as first-order EKF. Subsequent work is similar to standard Kalman filtering under linear system approximation. EKF algorithm in the proposed modeled is expressed as

X(k)=(q0(k)q1(k)q2(k)q3(k)bx(k)by(k)bz(k))T,

(7)

whereq0(k),q1(k),q2(k) andq3(k) are attitude quaternion;bx(k),by(k) andbz(k) are respectively gyroscope random drift vectors for the roll axis, pitch axis and yaw axis.

The equation of state of the system is expressed as

(8)

whereq(k) is attitude quaternion;b(k) is respectively gyroscope random drift vectors for the roll axis, pitch axis and yaw axis;wqandwbare process noise

The Jacobi matrix forf(X(k-1),k-1) is expressed as

(9)

(10)

(11)

whereIis identity matrix.

The system observation is expressed as

Z(k)=(abx(k)aby(k)abz(k)φm(k))T,

(12)

whereabx(k),aby(k) andabz(k) are the value of a triaxial acceleration measurement in the carrier coordinate system;φm(k) is the yaw angle obtained by projecting the output value of the magnetometer on a horizontal plane. Eq.(11) is refered as

υ(k)=h(x(k),k)+υ(k).

(13)

After finding the Jacobi matrix forh(x(k),k), the system measurement matrix is expressed as

(14)

(15)

Measurement noise covariance matrix is expressed as

(16)

whereR(k) is positive definite constant diagonal matrix.

Kalman gain is expressed as

K(k)=

(17)

State posterior estimate is expressed as

(18)

Variance posterior estimate is expressed as

P(k)=

[I-K(k)H(k)]P(k,k-1)[I-K(k)H(k)]T+

K(k)R(k)KT(k).

(19)

2.2 WNN-EKF algorithm

2.2.1 WNN structure

WNN is a neural network model based on wavelet analysis theory. It makes full use of the better localization property of wavelet transform and combines the self-learning ability of neural network, so it has strong approximation ability. Compared with the BP network, the basis function used by the network is the orthogonal wavelet base, and the correlation redundancy between the weights is small so that the training of a certain weight does not affect other weights, so the convergence speed is high. In addition, the wavelet network is a local approximation network that is easy to adapt to new data and can avoid large extrapolation errors, moreover, its structure is also simple, and the adjustable parameters are the least, which is beneficial to shorten the training time.

According to the characteristics of the signal, this paper uses a fusion wavelet neural network. The network body is a 3-layer network circle with a single input and single output structure[8]. The input layer is parallel input, and the hidden layer and the output layer contain neurons. The structure of the wavelet neural network is shown in Fig.1[10].

The scaling of the wavelet function and the parameters of the neural network can be trained by network learning. In the Fig.1, the excitation functionfh(x) of the hidden layer neuron is a Morlet wavelet function and is expressed as

fh(x)=-xcos(1.75x)exp(-x2/2)-

1.75sin(1.75x)exp(-x2/2),

(20)

wherexis the input of the input layer. The excitation functionfo(x) of the output layer is expressed as

fo(x)=cos(1.75x)exp(-x2/2),

(21)

wherexis the output of the hidden layer.

Fig.1 Structure of wavelet neural network

2.2.2 WNN-EKF algorithm

The model uses gyroscopes and accelerometers to measure the angular velocity and acceleration on the three axes of the body, while using a magnetometer to correct the drift of the gyroscope and accelerometer. Information from the gyroscope, accelerometer and magnetometer is fused by an EKF to obtain attitude information before calibration. At the same time, the information from the three sensors and the attitude information before calibration can also be input into the WNN system for training and prediction, and calibrating attitude angle to obtain attitude information after calibration. To avoid cross-coupling of the training process, three parallel WNNs are respectively used to correspond to three attitude angles. Fig.2 shows the structure of WNN-EKF.

Fig.2 Structure of WNN-EKF

3 Experimental verification

In order to verify the superiority of the WNN-EKF algorithm described in this paper, the 3DM-AHRS300A attitude reference system with integrated gyroscope, accelerometer and magnetometer was selected as the experimental platform, and static experiments and dynamic experiments were carried out, respectively. The specific performance parameters of the sensor of the azimuth reference system are shown in Table 1.

Table 1 Performance parameters of 3DM-AHRS300A

3.1 Static experiment of 3DM-AHRS300A attitude reference system

The navigation attitude reference system was in a horizontal state, and was connected to the computer, then the data had been collected for 30 minutes, and the update frequency is 100 Hz. After the data’s collection was completed, the collected attitude data of the attitude reference system (the data of accelerometer, gyroscope and the magnetometer) was used to calculate the static attitude by using the traditional EKF, the BP-EKF and the WNN-EKF methods, respectively. The comparison of the data of roll angle, pitch angle and yaw angle calculated by the two methods with the reference data is shown in Fig.3.

In the Fig.3, it can be seen that the volatility of the calculation results of the proposed method is smaller than that of the traditional EKF and the BP-EKF, and the result curve is closest to the reference curve. In order to further compare the error of the attitude calculation by the three methods, the three methods are compared with the reference values to draw the attitude error curve as shown in the Figs.4-6.

Fig.4 shows the attitude error curve drawn by comparing the value of the traditional EKF calculation with the reference value. Fig.5 shows the attitude error curve drawn by comparing the value of the BP-EKF calculation with the reference value. Fig.6 shows the attitude error curve drawn by comparing the value of the WNN-EKF calculation with the reference value. Table 2 shows the mean of error based on Figs.4-6. It can be seen from the results that using the WNN-EKF improves the accuracy of roll angle, pitch angle and yaw angle compared with the traditional EKF and the BP-EKF.

Fig.3 Comparison of static experimental attitude angle

Fig.4 Error curves of traditional EKF calculation

Fig.5 Error curves of BP-EKF calculation

Fig.6 Error curves of WNN-EKF calculation

Table 2 Mean of static experimental error

3.2 Dynamic experiment of 3DM-AHRS300A attitude reference system

In order to verify the performance of the proposed method in the dynamic attitude calculating, the dynamic experiment was designed. The attitude reference system was installed on the trolley, and the trolley moved around a 6 m×8 m rectangular field to collect the output data of the attitude reference system, and the trolley made three turns in the trolley movement.

After the experiment, the collected data (accelerometer data, gyroscope data and magnetometer data) were simulated by the traditional EKF, the BP-EKF and WNN-EKF methods, and the data is shown in Fig.7.

Fig.7 The comparison of dynamic experimental attitude angle

It can be seen from the Fig.7 that the result of the WNN-EKF calculation has less volatility and the best denoising effect. In the experiment, the car made three turning processes, however, the traditional EKF algorithm is greatly affected by the external environment and its adaptive ability is poor, and the BP-EKF algorithm is closer to the reference value than the traditional EKF algorithm and has higher precision than the traditional EKF algorithm, but through the WNN-EKF algorithm to calculate, it can be seen that the attitude angle curve is relatively stable without abnormal fluctuations, and the process of three turns is tracked, and the algorithm has strong environmental adaptability. Moreover, after comparing the three algorithms, the curve representing the result of the WNN-EKF algorithm is closest to the reference curve, so the accuracy of the WNN-EKF algorithm is the highest.

Fig.8 shows an attitude error curve drawn by comparing the value of traditional EKF calculation with a reference value. Fig.9 shows an attitude error curve drawn by comparing the value of the BP-EKF calculation with a reference value.Fig.10 shows an attitude error curve drawn by comparing the value of the WNN-EKF calculation with a reference value. Table 3 shows the mean of error based on Figs.7-9. It can be seen from the results that the WNN-EKF algorithm has smaller error and is relatively stable, which indicates that the algorithm can effectively improve the solution accuracy.

Fig.8 Error curves of traditional EKF calculation

Fig.9 Error curves of BP-EKF calculation

Fig.10 Error curves of WNN-EKF calculation

Table 3 Mean of dynamic experimental error

4 Conclusions

In this paper, the WNN-EKF algorithm is proposed for attitude calculation. The wavelet neural network compensates the model error of EKF itself, which effectively improves the accuracy of attitude calculation and the ability of environment adaptation. Finally, the simulation results verified the effectiveness of this method, which provides a new idea for attitude calculating and navigation positioning problems. Although wavelet neural network combined with Kalman filtering method improves the accuracy and stability of attitude calculation, with the increase of test data, the training time of neural network will be larger. Therefore, the problem of attitude calculation speed in the method described in this paper needs to be further studied and improved.