Extended Kalman Filter Based Quadrotor State Estimation Based on ...

Proceedings of the ASME 2015 Dynamic Systems and Control Conference DSCC2015 October 28-30, 2015, Columbus, Ohio, USA

DSCC2015-9929

EXTENDED KALMAN FILTER BASED QUADROTOR STATE ESTIMATION BASED ON ASYNCHRONOUS MULTISENSOR DATA

Mohammad Sarim∗ Research Assistant Department of Mechanical and Materials Engineering University of Cincinnati Cincinnati, Ohio 45221 [email protected]

Alireza Nemati† PhD Candidate Department of Electrical Engineering and Computing Systems University of Cincinnati Cincinnati, Ohio 45221 [email protected]

Manish Kumar‡ Associate Professor Department of Mechanical and Materials Engineering University of Cincinnati Cincinnati, Ohio 45221 [email protected]

Kelly Cohen Professor Department of Aerospace Engineering and Engineering Mechanics University of Cincinnati Cincinnati, Ohio, 45221 [email protected]

ABSTRACT For effective navigation and tracking applications involving Unmanned Aerial Vehicles (UAVs), data fusion from multiple sensors is utilized. However, asynchronous nature of the sensors, coupled with loss of data and communication delays, makes this process not very reliable. For a better estimation of the data, some sort of filtering scheme is needed. This paper presents an Extended Kalman Filter (EKF) based quadrotor state estimation by exploiting the dynamic model of the UAV. The data coming from the sensors is noisy and intermittent. The EKF filters and provides estimated data for the missing timestamps. An indoor flight test establishes the accuracy of the EKF, and another outdoor flight test validates the developed scheme for the real world

scenario.

INTRODUCTION The UAVs are increasingly being used in both military and civilian applications, particularly in the area of surveillance and exploration of disasters, such as fire, earthquake, and flood, search and rescue operations, monitoring of forest fires and other such targets, HazMat spills, and mobile sensor networks. For most of these applications, multiple sensors are used to get information about the environment. For instance, in monitoring of forest fires, it is required to get a picture of the location of the fire and also have the corresponding GPS data to accurately track it. Further, if the camera is mounted on a gimbal, the attitude data is also needed to calculate the actual location of the spot in world coordinates. The problem of sensor fusion is more pronounced here since the data from sensors arrive at different rates. Hence, at a given

∗ Past affiliation: PhD Candidate, Department of Mechanical, Industrial, and Manufacturing Engineering, The University of Toledo, Toledo, Ohio 43606. † Past affiliation: PhD Candidate, Department of Electrical Engineering and Computer Science, The University of Toledo, Toledo, Ohio 43606. ‡ Past affiliation: Associate Professor, Department of Mechanical, Industrial, and Manufacturing Engineering, The University of Toledo, Toledo, Ohio 43606.

1

Copyright © 2015 by ASME

time instant, due to added effect of communication delays, some data points might be missing. In other words, synchronizing the data coming from multiple sensors, each operating at their respective frequencies, needs attention. Researchers have worked on non-linear control of quadrotors such as [1]. Also, for helicopters, control aspect is studied in [2] using feedback linearization and [3] based on reduced order observer. Further, for fixed winged UAVs, [4] talks about stability in longitudinal axis. A lot of work has been done in the area of Kalman filtering for intermittent observations. Sinopoli et al. [5] have generalized a discrete-time Kalman filter, modeling the arrival of an observation as a random process. The general case of time-varying Kalman filter is studied by Fortmann et al. [6] and Zhou [7] has proposed a Kalman filter based registration approach for multiple asynchronous sensors. Emran et al [8] also tried to use single low cost Inertial Measurement Unit (IMU) sensor in order to estimate quadrotor’s attitude by using two extended Kalman filters along with a Direction Cosine Matrix (DCM) algorithm. In their approach, an EKF is being used to filter a 3-axis gyro sensor which gives the angular rates, and then Euler angles are being computed through DCM algorithm using the filtered gyro and magnetometer sensor. Another EKF is used to enhance the estimation of the Euler angles. The fusion of GPS and IMU data for state estimation of UAVs becomes an interesting problem because these data are asynchronous in nature and the data rate of GPS is quite low as compared to IMU. The GPS/IMU data fusion is done in [9–12]. While Guo [10] have used a constant acceleration model, Caron et al. [9] are using a Weiner process acceleration model [13]. They have introduced contextual variables in order to increase the reliability of the position information. Bad data brought by GPS sensor is detected and rejected, instead the filtered IMU data is used to compensate the GPS rejected data. Though, these two separate data have been fused, the results show that in the case of bad GPS data for more than 30 seconds, the IMU data can not be perfectly replaced and positioning errors quickly drift. In order to have accurate positioning data when there is no GPS information for a long period of time, absolute or dead-reckoning sensor can be used. Detection of any possible fault both before and during the fusion process is also another way to increase the accuracy. For instance, Salah et al [14] implemented the fault detection methodology applied for both high frequency faults from GPS receiver and low frequency fault in IMU caused by misalignment of the unit and bias in the sensor reading. They have shown that if the bias is not removed from the gyro, the position error would boost with the cube of time. Sasiadek and Wang [15] have modified EKF using Fuzzy logic adaptive system not only to demonstrate better performance but also to show that their approach can also be applied to fusion process with sensors different than GPS or IMU.

In [16] also, a Kalman filter based registration approach is applied for multiple asynchronous sensor fusion. A modified two-stage Kalman estimator is used to estimate the sensor registration errors. It is shown in their result that the generalization error by the Kalman filter approach is only half of the other method. Other works include [14, 17, 18] who have also used Kalman filter for the GPS/IMU data fusion. Hence, there is a need to develop a robust filtering algorithm that can precisely estimate the state of a UAV at every timestep. This work focuses on this and an EKF based estimation algorithm is developed to estimate and fuse the IMU and GPS data. This is further improved by using the quadrotor dynamic model.

QUADROTOR DYNAMICS Dynamic Model The dynamic model of a quadrotor UAV is presented in this section. The coordinate systems and the free body diagram of the UAV are shown in Figure 1. The world frame (W ) denotes the fixed reference frame with respect to which all motion can be referred to and the body-frame (B) is a frame attached to the center of mass of the vehicle. There is a vertical force for each rotor that comes from the rotation of the rotor. In addition to the forces, each rotor produces a moment perpendicular to the plane of propeller rotation.

Figure 1: The coordinate systems and the free body diagram

The Z-X-Y Euler angle convention is used to model the rotation of the quadrotor in the W frame. To get to the B frame, we first rotate through zW by yaw angle, ψ, then rotate about the

2

Copyright © 2015 by ASME

intermediate x-axis by the roll angle, φ, and then about the yB by the pitch angle, θ. Hence, the rotation matrix R from W to B frame can be written as:

and the motor speeds are given by r ωi,0 = ωH =

  cψcθ − sφsψsθ −cφsψ cψsθ + cθsφsψ   R = cθsψ + cψsφsθ cφcψ sψsθ − cφcθsφ  −cφsθ sφ cφcθ

(7)

(1) Attitude Control The controller inputs are four independent speeds of propellers. The dynamic model of the quadcopter, described in (2) and (4), is used to design a PD controller. Substituting (3) and (5) in (4) and assuming that the component of angular velocity in zB direction is small compared to other terms, we get the vector of desired rotor speeds as linear combination of four terms:

where cψ and sψ denote cos(ψ) and sin(ψ) respectively, and similarly for other angles. The equations of motion governing the acceleration of the center of mass can be written as follows:

     0 0 X¨       m  Y¨  =  0  + R  0  −mg Z¨ ∑ Fi

mg 4kF



     ωdes 1 0 −1 1 ωH + ∆ωF 1  des     ω2  1 1 0 −1  ∆ωφ   des  =    ω3  1 0 1 1   ∆ωθ  1 −1 0 −1 ∆ωψ ωdes 4

(2)

where m is the mass of the quadrotor and g is the acceleration due to gravity.

where ωdes i , (i = 1, 2, 3, 4) are the desired angular velocities of the respective rotors, and ωH is hovering speed. The proportionalderivative control laws are used to control ∆ωφ , ∆ωθ , ∆ωψ , and ∆ωF , which are deviations that result into forces/moments causing roll, pitch, yaw, and a net force along the zB axis, respectively, and are calculated as:

Motor Control Each rotor produces a vertical force Fi due to the rotation, given by: Fi = kF ω2i

(3)

∆ωφ = k p, φ (φdes − φ) + kd, φ (pdes − p)

where ωi is the rotational speed of rotor i, and kF is a constant whose value is given in [19] as kF = 6.11 × 10−8 N/rpm2 . In addition, Euler equations are written in order to obtain angular accelerations of the vehicle given by:         p p L(F2 − F4 ) p˙         L(F1 − F3 ) I  q˙  =  − q ×I q  r r M1 − M2 + M3 − M4 r˙

∆ωθ = k p, θ (θdes − θ) + kd, θ (qdes − q) ∆ωψ = k p, ψ (ψdes − ψ) + kd, ψ (rdes − r) m ¨ des Z ∆ωF = 8kF ωH

where p, q, and r are the components of angular velocities of the vehicle in the body frame. The relationship between these components and the pitch, roll, and yaw are provided in [20]. Position Control In order to have the quad-rotor track a desired trajectory ri,T , the command acceleration, r¨ides is calculated from proportionalderivative controller based on position error, as [21]:

(5)

where ωi is the angular velocity of ith rotor and kM is the constant, given in [19] as kM = 1.5 × 10−9 Nm/rpm2 . For hover state, the thrusts from the propellers must satisfy Fi,0 =

mg 4

(9)

(4)

where L is distance of each rotor from the vehicle’s center of gravity. I is the matrix for moment of inertia along xB , yB , and zB directions respectively. Mi , (i = 1, 2, 3, 4) are rotor moments produced by angular velocity of rotors and are given by: Mi = kM ω2i

(8)

(¨ri,T − r¨ides ) + kd,i (˙ri,T − r˙i ) + k p,i (ri,T − ri ) = 0

(10)

where ri and ri,T (i = 1, 2, 3) are the 3-dimensional position of the quad-rotor and desired trajectory respectively. It may be noted that r˙i,T = r¨i,T = 0 for hover.

(6)

3

Copyright © 2015 by ASME

During the flight, the orientation of the vehicle needs to be set close to zero. This can be obtained by linearizing the equation of motion that correspond to the nominal hover states. The nominal hover state is (r = r0 , φ = θ = 0, ψ = ψT , r˙ = 0 and ˙ = φ˙ = 0) . The change of the pitch and roll angles are θ˙ = ψ supposed to be small during flight. By linearizing Equation (2) about these nominal hovering states, desired pitch and roll angles to cause the motion can be derived as given by the following equations :

 1  ¨ des X sinψT − Y¨ des cosψT g  1  ¨ des θdes = X cosψT + Y¨ des sinψT g

From (2), (4), (12), and (13), we obtain after simplification:  φ˙   θ˙     ˙ ψ    a θ˙ ψ  1 ˙ + b1U2    a φ˙ ψ  2 ˙ + b2U3     b3U4   f (X,U) =   x˙     y˙     z˙     U1 ux m     uy Um1   −g + cos θ cos φ Um1 

φdes =

(11)

(15)

where, where X¨ des and Y¨ des are the desired acceleration in X and Y directions respectively . ψT is the yaw angle to be tracked which is the same as the desired yaw angle ψdes .

a1 a2 b1 b2 b3 ux uy

EXTENDED KALMAN FILTER AND STATE ESTIMATION Eqs. (2) and (4) are used to write the system in state-space form X˙ = f (X,U). The state vector X and input vector, U are chosen as follows:

h iT ˙ x y z x˙ y˙ z˙ X = φ θ ψ φ˙ θ˙ ψ

= (Iyy − Izz )/Ixx = (Izz − Ixx )/Iyy = L/Ixx = L/Iyy = 1/Izz = cos ψ sin θ + cos θ sin φ sin ψ = sin ψ sin θ − cos θ sin φ cos ψ

A simplification to the Eqn. (15) can be made by realizing ˙ and a2 φ˙ ψ ˙ respectively) are that the first terms in φ¨ and θ¨ (a1 θ˙ ψ very small and can be neglected. An extended Kalman filter [22] is then employed for state estimation. For the linearized state space model X = AX + BU, the prediction and update equations for EKF are as follows: EKF Time Update Equations:

(12)

− xˆk− = Axˆk−1 + Buk−1

h iT U = U1 U2 U3 U4

Pk− = Ak Pk−1 ATk +Wk Qk−1WkT (13) EKF Measurement Update Equations: Kk = Pk− H T (HPk− H T +Vk RkVkT )−1 xˆk = xˆk− + Kk (zk − H xˆk− ) Pk = (I − Kk Hk )Pk−

where the inputs are mapped by:

U1 U2 U3 U4

= F1 + F2 + F3 + F4 = F2 − F4 = F3 − F1 = M1 − M2 + M3 − M4

A and B matrices are determined from Jacobians of f (X,U) and B = ∂ f (X,U) as A = ∂ f (X,U) ∂X ∂U . U is the input vector and is assumed to be known from the rotor speeds. The measurement

(14)

4

Copyright © 2015 by ASME

matrix is ZI = HI X and ZG = HG X, where HI and HG are based on the availability of the IMU and GPS data respectively, and are given as follows:

  100000000000   HI = 0 1 0 0 0 0 0 0 0 0 0 0 001000000000

(16)

  000000100000   HG = 0 0 0 0 0 0 0 1 0 0 0 0 000000001000

(17)

zero. Further, the roll, pitch, and yaw angles are also zero. The h iT input vector is Ue = U1e U2e U3e U4e , where, U1e = mg and U2e = U3e = U4e = 0. For the three cases considered, the rank of the observability matrix is calculated as (a) 6, (b) 10, and (c) 12. Since the total number of state variables is 12, the system is observable for case (c) only, i.e. when all the data are available. This is expected, since it is not possible to completely observe the state without the complete knowledge of all the states. The question remains as to observability of the system when the IMU and GPS data are available intermittently. Experiments were carried out to verify that the filter was still stable in the presence of intermittent data.

EXPERIMENTAL RESULTS To test the accuracy of the developed data estimation and fusion scheme, flight tests are conducted both indoors and outdoors. For indoor tests, a test area equipped with OptiTrack motion detector camera system is used. This gives the position and orientation data via Tracking Tools software provided by Natural Point, Inc. to complement the OptiTrack system. This data can be exported to a CSV file, and then read using MATLAB or any other programming software. The outdoor tests are conducted in an open area where GPS is present.

If both IMU and GPS data are present, then Z = HX and H changes to:

 1  0  0 H = 0  0 0

0 1 0 0 0 0

0 0 1 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

0 0 0 0 0 0

0 0 0 0 0 0

 0  0  0  0  0 0

(18)

Indoor Flight Tests The Parrot AR.Drone 2.0 quadrotor is used for the indoor flight test. Once the data is extracted from the CSV file, a sensor noise is added to the data to simulate measurements from noisy sensor such as GPS and IMU. The noisy sensor data is used by EKF to provide the estimated state of the UAV. The acquired data is considered as ground truth and those are considered to be the actual positions and orientations of the UAV. The inertial properties of the Parrot AR.Drone 2.0 quadrotor used in the indoor flight test simulations are taken from [23] as:

Further, if no data is available at a given timestamp, or is severely corrupted, then only time update equations are invoked and measurement update is not done. The observability for the above system can be determined from the observability matrix, considering that there are twelve state variables, given by:



 HA    HA2    3 HA  Θ=  .   .   .  HA11

(19)

m = 0.7428kg Ixx = 4.50 × 10−3 kg.m2 Iyy = 5.10 × 10−3 kg.m2 Izz = 9.50 × 10−3 kg.m2 L = 0.175m

where H is the measurement matrix for the three cases - (a) When only IMU data is available, (b) When only GPS data is available, and (c) When all data are available. The observability of the system is calculated at the equi ∂ f (X,U) librium position. At this point, A = ∂X and (X=xe,U=ue) . The equilibrium position is the hovB = ∂ f (X,U) ∂U

The results are presented for two cases - (1) when the position and orientation data are available alternately, and (2) when the rate of position and orientation data is one-half of that in the previous case. For case 1, the estimated position and attitude data is plotted with actual data as shown in Figures 2 - 5. For case 2,

(X=xe,U=ue)

ering state and at this point, all linear and angular velocities are

5

Copyright © 2015 by ASME

the errors in estimation of the position and orientation are compared with the case when the data is available at all instants of time. These errors are plotted in Figures 6 - 11.

Figure 4: Estimated pitch of the quadrotor (indoor flight, case 1).

Figure 2: 3D trajectory followed by the quadrotor (indoor flight, case 1).

Figure 5: Estimated yaw of the quadrotor (indoor flight, case 1).

the orientation was measured using the IMU on 3DR PixHawk flight controller. This establishes the accuracy of the EKF in real world scenario where communication delays and data loss are inevitable. The estimated data is plotted in Figures 12 - 15 with the intermittent sensor data. Figure 3: Estimated roll of the quadrotor (indoor flight, case 1). CONCLUSIONS AND FUTURE WORK An Extended Kalman Filter employing quadrotor dynamics is developed to estimate the quadrotor state based on multisensor data. The data fusion is achieved by combining the position and orientation data. The problem becomes challenging because the position and orientation data, measured by different sensors, are available at different rates. Since, all the data is not available

Outdoor Flight Tests Another flight test was made using a 3DRobotics quadrotor UAV flown outdoors manually. In this case the position data was measured using the onboard 3DR GPS Module, and

6

Copyright © 2015 by ASME

Figure 6: Error in roll estimation (indoor flight, case 2).

Figure 8: Error in yaw estimation (indoor flight, case 2).

Figure 7: Error in pitch estimation (indoor flight, case 2).

Figure 9: Error in x estimation (indoor flight, case 2).

at all times, it is required to be estimated at those time instants. The observability test of the system establishes the fact that all the state data is needed for the system to be observable. The results show the variation of the estimated data with actual data, and the indoor and outdoor flight experiments establish the accuracy of the EKF. A large variation in the estimated data could be attributed to the non-linearities in the employed dynamic model. As an extension to this work, geo-tagging of on-board video feed can be accomplished once a reliable state estimate is available at all instants when the video feed is available. This can be crucial for geo-locating a target on ground while the UAV is flying at an altitude and camera is mounted on a gimbal with a known orientation. Applications of these include monitoring of forest fires, target tracking, and mapping of agricultural areas.

REFERENCES [1] Nemati, A., and Kumar, M., 2014. “Non-linear control of tilting-quadcopter using feedback linearization based motion control”. In ASME 2014 Dynamic Systems and Control Conference, American Society of Mechanical Engineers, pp. V003T48A005–V003T48A005. [2] Nemati, A., Haddad Zarif, M., and Fateh, M., 2007. “Helicopter adaptive control with parameter estimation based on feedback linearization”. World Academy of Science, Engineering and Technology, 29, p. 2007. [3] Nemati, A., and Kumar, M., 2015. “Control of micro coaxial helicopter based on a reduced order observer”. Journal of Aerospace Engineering. [4] Radmanesh, M., Nematollahi, O., Nili-Ahmadabadi, M.,

7

Copyright © 2015 by ASME

Figure 10: Error in y estimation (indoor flight, case 2).

Figure 12: Estimated roll (outdoor flight).

Figure 11: Error in z estimation (indoor flight, case 2).

Figure 13: Estimated pitch (outdoor flight).

and Hassanalian, M., 2014. “A novel strategy for designing and manufacturing a fixed wing mav for the purpose of increasing maneuverability and stability in longitudinal axis”. Journal of Applied Fluid Mechanics, 7(3), pp. 435–446. [5] Sinopoli, B., Schenato, L., Franceschetti, M., Poolla, K., Jordan, M., and Sastry, S. S., 2003. Kalman filtering with intermittent observations. Tech. Rep. UCB/ERL M03/15, EECS Department, University of California, Berkeley. [6] Fortmann, T., Bar-Shalom, Y., Scheffe, M., and Gelfand, S., 1985. “Detection thresholds for tracking in clutter–a connection between estimation and signal processing”. Automatic Control, IEEE Transactions on, 30(3), Mar, pp. 221– 229. [7] Zhou, Y., 2004. “A kalman filter based registration ap-

proach for asynchronous sensors in multiple sensor fusion applications”. In Acoustics, Speech, and Signal Processing, 2004. Proceedings. (ICASSP ’04). IEEE International Conference on, Vol. 2, pp. ii–293–6 vol.2. [8] Emran, B. J., Al-Omari, M., Abdel-Hafez, M. F., and Jaradat, M. A., 2014. “A cascaded approach for quadrotor’s attitude estimation”. Procedia Technology, 15, pp. 268– 277. [9] Caron, F., Duflos, E., Pomorski, D., and Vanheeghe, P., 2006. “Gps/imu data fusion using multisensor kalman filtering: introduction of contextual aspects”. Information Fusion, 7(2), pp. 221 – 230. [10] Guo, H., Yu, M., Zou, C., and Huang, W., 2010. “Kalman filtering for gps/magnetometer integrated navigation sys-

8

Copyright © 2015 by ASME

[15]

[16]

[17] [18]

Figure 14: Estimated yaw (outdoor flight).

[19]

[20]

[21]

[22] [23]

tial navigation systems for autonomous land vehicles”. PhD thesis, The University of Sydney. Xu, H., Wang, C., Yang, R., and Yang, M., 2006. “Extended kalman filter based magnetic guidance for intelligent vehicles”. In Intelligent Vehicles Symposium, 2006 IEEE, IEEE, pp. 169–175. Zhou, Y., 2004. “A kalman filter based registration approach for asynchronous sensors in multiple sensor fusion applications”. In Acoustics, Speech, and Signal Processing, 2004. Proceedings.(ICASSP’04). IEEE International Conference on, Vol. 2, IEEE, pp. ii–293. Stephen, J., 2000. “Development of a multi-sensor gnss based vehicle navigation system”. UCGE Reports(20140). Shin, E.-H., and El-Sheimy, N., 2001. Accuracy improvement of low cost INS/GPS for land applications. University of Calgary, Department of Geomatics Engineering. Powers, C., Mellinger, D., and Kumar, V., 2014. “Quadrotor kinematics and dynamics”. In Handbook of Unmanned Aerial Vehicles, K. P. Valavanis and G. J. Vachtsevanos, eds. Springer Netherlands, pp. 307–328. Nemati, A., and Kumar, M., 2014. “Modeling and control of a single axis tilting quadcopter”. In American Control Conference (ACC), 2014, IEEE, pp. 3077–3082. Tan, R., and Kumar, M., 2013. “Proportional navigation (PN) based tracking of ground targets by quadrotor uavs”. In ASME 2013 Dynamic Systems and Control Conference, American Society of Mechanical Engineers, pp. V001T01A004–V001T01A004. Welch, G., and Bishop, G., 2001. “An Introduction to the Kalman Filter”. Pestana, J., Sanchez-Lopez, J., Mellado-Bataller, I., Fu, C., and Campoy, P., 2012. “Ar drone identification and navigation control at cvg-upm”. XXXIII Jornadas Nacionales de Automatica.

Figure 15: Estimated altitude (outdoor flight).

[11]

[12]

[13]

[14]

tem”. Advances in Space Research, 45(11), pp. 1350 – 1357. Gao, S., Zhong, Y., Zhang, X., and Shirinzadeh, B., 2009. “Multi-sensor optimal data fusion for ins/gps/sar integrated navigation system”. Aerospace Science and Technology, 13(45), pp. 232 – 237. Wendel, J., Meister, O., Schlaile, C., and Trommer, G. F., 2006. “An integrated gps/mems-imu navigation system for an autonomous helicopter”. Aerospace Science and Technology, 10(6), pp. 527 – 533. Bar-Shalom, Y., Li, X. R., and Kirubarajan, T., 2004. Estimation with applications to tracking and navigation: theory algorithms and software. John Wiley & Sons. Sukkarieh, S., 2000. “Low cost, high integrity, aided iner-

9

Copyright © 2015 by ASME