Chapter 4 State Estimation

Chapter 4 State Estimation Navigation of an unmanned vehicle, always depends on a good estimation of the vehicle states. Especially if no external sensors or marers are available, more or less complex algorithms have to be used. For this project, the orientation estimation is done by the widely used Kalman filter. However, position estimation is still an object of ongoing research. The here described approach uses the company own software RECONSTRUCTME. 4.1 Orientation With using Euler angles (Z Y X ) between π and π, it should also be taen care of potential problems referring to the transition between maxima. For this project, Φ and Θ do not need special treatment, as the robot is not expected to exceed the maxima, since this would mean that the thrust vector is pointing to the ground. For the same reason the singularity at Θ = 90 of the used Euler angle representation of the orientation is irrelevant. However, the Ψ angle can and will mae this mentioned maxima transition. This demands special treatment considering the calculation of e in the PID controller in Fig. 3.2b. To compensate this overflow, a simple algorithm can be used: 1 i f (e Ψ > π ) 2 Ψ = 2π ; 3 e l s e i f (e Ψ < π ) 4 Ψ += 2π ; 4.1.1 Kalman Filter A good introduction to the topic can be found in [52]. A more detailed description of the concept, derivation and properties is given in [50]. The Kalman filter, also nown as linear quadratic estimator, is an optimal estimator for discrete linear system of the form with the statex R n, and a measurement x +1 = A x +w (4.1) z = H x +v (4.2) withz R m, if process noisew and the measurement noisev are assumed to be independent random variables with Gaussian probability density functions and zero mean value. The normal probability distributionspare then p(w) N(0,Q), withq = diag{σ 2 w1,σ2 w2,...} p(v) N(0,R), withr = diag{σ 2 v1,σ2 v2,...} (4.3) withσ 2 being the variance of the corresponding noise distribution. Then n matixa in (4.1) describes the transition of the system between time step and + 1, in the absence of neither input nor process 27

4 State Estimation 4.1.1 Kalman Filter 28 noise. Them n matrixhin the measurement equation (4.2) relates the statex to the measurementz. The filter (in one popular algebraic form) consists of two stages. First, a prediction of the state ˆx at time, using the system dynamic matrixa 1 and state ˆx 1 in (4.1) from the last iteration1. This will be referred to as a priori estimation 1. Second, a correction of this estimation, using the measurements z. Using the a priori estimation ˆx in (4.2), an innovation variable (or residual)y can be defined as y = z H ˆx. (4.4) The innovation variable is used to correct the a priori prediction using then m gain matrixk ˆx = ˆx +Ky. (4.5) This estimation is referred to as a posteriori estimation ˆx. The gain matrix K describes a blending factor between the model based prediction, and the measurement. To achieve a readable description of the optimal choice fork, the a priori and a posteriori estimate errors are defined as e x ˆx (4.6) e x ˆx and the corresponding error covariances are P = E [ (x ˆx )(x ˆx )T] P = E [ (x ˆx )(x ˆx ) T]. (4.7) The core tas of the Kalman filter, is to choose the gain matrixk so that the a posterioiri estimate error covariance P is minimized with respect of the mean square error. For more details on that derivation see [50]. One popular form of the resultingk that minimizes the a posteriori estimate error covariance is the so called Kalman gain K = P HT ( H P HT +R ) 1. (4.8) It can be seen, that the covariance R of the measurement noise defined in (4.3) and the a priori error covariance P defined in (4.7) have direct influence on the optimal gain matrix K. To point out the extreme cases, ifr is zero meaning that the measurement is very trustworthy, the gain matrix becomes lim K = H 1 R 0 (4.9) and thus (4.5) degenerates to ˆx = H 1 z. (4.10) The estimation then is only based on the measurement. On the other hand, if the error covariance P approaches zero, meaning that the model based estimation is accurate, the gain matrix K does not consider the measurement at all lim P 0 K = 0 ˆx (K = 0) = ˆx. (4.11) The summary of the whole Kalman filter algorithm is shown in Fig. 4.1. 1 note the super script in ˆx and later inp

4 State Estimation 4.1.2 Extended Kalman Filter 29 Initial estimates for ˆx andp a priori prediction (1) Project the state ahead ˆx +1 = A ˆx (2) Project the error covariance ahead P +1 = A P A T +Q a posteriori correction (1) Compute the Kalman gain K = P ( HT H P HT +R 1 ) (2) Update estimate with measurement z ˆx = ˆx +K ( z H ˆx ) (3) Update the error covariance P = (IK H )P Figure 4.1: Data flow of the Kalman filter operation 4.1.2 Extended Kalman Filter For estimation of a non-linear system, several extended versions of the Kalman filter exist. A widely used approach is to linearize the system dynamics in every step around the a priori estimation ˆx, and proceed as for a linear system. This approach is nown as the EKF. The fundamental imperfection of the EKF, as also pointed out in [52], is that the distributions of the various random variables are no longer normal, after undergoing their respective non-linear transformations. Thus, the optimality of the estimation is only approximated by linearization. The stochastic system equations from (4.1) and (4.2) are now generalized to the non-linear case again with the state vector x R n and x +1 = f(x,w ) (4.12) z = h(x,v ) (4.13) with the measurement vector z R m. By deriving the Jacobian matrix of the partial derivatives of f andhwith respect to the state vector x and the noise vectors w and v f(x,w ) T A lin, = x f(x,w ) T W lin, = w h(x,v ) T H lin, = x h(x,v ) T V lin, = v (4.14a) (4.14b) (4.14c), (4.14d) the EKF is implemented as shown in Fig. 4.2. A detailed derivation can again be found in [52] and [50].

4 State Estimation 4.1.3 Orientation Estimation in Use 30 Initial estimates for ˆx andp a priori prediction (1) Project the state ahead ˆx +1 = f(ˆx,0) (2) Project the error covariance ahead P +1 = A lin,p A T lin, +W lin,q W lin, a posteriori correction (1) Compute the Kalman gain ) 1 K = P HT lin, (H lin, P HT lin, +V lin,r V lin, (2) Update estimate with measurement z ˆx = ˆx +K ( z h(ˆx,0)) (3) Update the error covariance P = (IK H lin, )P Figure 4.2: Data flow of the extended Kalman filter operation 4.1.3 Orientation Estimation in Use The orientation estimator in use, as already implemented in the open-source code [18] in the module attitude_estimator_ef, is an EKF. As there did not exist a documentation of the module, the code was analyzed and is described in this section. The state vector and measurement vector are x = Bω IB B ω IB Br g Br m,z = B ω IB B r g B r m (4.15) with the angular velocity of the quadcopter B ω IB = [ω x,ω y,ω z ] T, the estimated angular acceleration B ω IB = [ ω x, ω y, ω z ] T, the vector of the earth s gravitation field B r g = [ B r g,x, B r g,y, B r g,z ] T and the magnetic field vector B r m = [ B r m,x, B r m,y, B r m,z ] T. The available measurements are the angular velocities B ω IB from the gyroscopic sensors, the vector of gravitation B r g from the acceleration sensors 2 and the vector of the magnetic field of the earth B r m directly from the magnetic sensors on the IMU. Note that all variables are described in the body fixed coordinate system (compare chapter 2.2.1). To simplify notation, the left side index B will be omitted in the following explanation and if no other coordinate index is given, the variable shall be referred with respect to the body fixed system. 4.1.3.1 Prediction Step Under the assumption of the angular acceleration staying constant during the time step t, the angular velocity B ω IB for the next time+1 can be predicted withω +1 = ω + ω t. The prediction of the two vector fields can be derived similarly with r +1 = r +v t. However, the velocity vectors B v a, and B v m, are not directly measured. They can be derived from the relation of velocity and position in 2 neglecting the actual translational accelerations of the quadcopter, as they are assumed to be smaller by factor 10

4 State Estimation 4.1.3 Orientation Estimation in Use 31 the inertial frame with Bv = A BI d dt Ir Iv = d dt I r (4.16) = A d BI (A dt IBB r) = A BI A }{{ IB } Bṙ+A BI Ȧ }{{ IB } Br I ω B IB = B ṙ+ B ω IBB r. (4.17) As for the gravitation and magnetic vector field the translational velocities B ṙ are irrelevant, because the acting force is invariant to translational movement, only the rotational terms B ω IBB r have to be taen into account. Thus, the non-linear system dynamics are described with x +1 = f(x,w) = ω + ω t+wω, ω +w ω, r a, + ω r a, t+w ra, r m, + ω r m, t+w rm,. (4.18) For the two equations needed for the algorithm shown in Fig. 4.2, the matrices A lin, and W lin,, as shown in (4.14a) and (4.14b), are the calculated Jacobi matrices 3 A lin, = f(x,w ) x = W lin, = f(x,w ) w I I t 0 0 0 I 0 0 r a, t 0 I+ ω t 0 r m, t 0 0 I+ ω t = I 0 0 0 0 I 0 0 0 0 I 0 0 0 0 I The implemented formulas for the a priori prediction step at time +1 are with neglected process noisew, and. (4.19a) (4.19b) ˆx +1 = f(ˆx,0) (4.20) P +1 = A lin,p A T lin, +Q (4.21) where the measurement noise matrix Q holds the variances σ 2 of the states as diagonal entries. They represent the uncertainty of the prediction. As this variances are unnown and can not be measured, they act as tuning variables for the filter. 4.1.3.2 Correction Step Unlie the state prediction, the measurement of the system can be described with trivial linear functions I 0 0 0 σ 2 ω z = 0 0 I 0 x + σ 2 r a. (4.22) 0 0 0 I } {{ } H σ 2 r m }{{} R 3 Note the cross product is anticommutative: ãb = ba in line3and 4 of matrix definition A lin,

4 State Estimation 4.1.3 Orientation Estimation in Use 32 Thus, the linear observation matrix H is already given, and also the Jacobian matrix from (4.14d), linearizing the influence of the measurement noiser, degenerates to a identity matrixi. The implemented functions for the correction step as listed in Fig. 4.2 are directly given with the Kalman gain K = P HT ( H P HT +R ) 1, (4.23) the a posteriori estimation ˆx = ˆx +K ( z H ˆx ) and the propagation of the error covariance during the correction step (4.24) P = (IK H )P. (4.25) For the first iteration, initial values for the first a posteriori have to be chosen. The first estimation ˆx is initialized with measurements Bω IB B ω IB ˆx,init = B ω IB Br a = 0 B r g. (4.26) Br m B r m,init The first error covariance is initialized 4 with P,init = 100I. Over time, the chosen initial value of the error covariance matrix does not have any influence, as it will converge, as long as it is not initialized with equal to0. 4.1.3.3 Incomplete measurements As the three 3D sensors (gyro, acceleration, magnetometer) deliver measurements at independent sample rates, the measurement vector from (4.22) will not always be fully available. To avoid using old measurements multiple times, the observation matrix H and noise matrix R vary in their size, depending on which sensor has new measurements available. If all sensors have new measurements, the vectors are as in (4.22). As the gyroscope sensor has the fastest sampling rate, and is the most crucial measurement for the estimation of the angular acceleration ω, the correction step will only be calculated, once a new ω measurement is available. For an incomplete measurement, the matrices are given with,init z = [ I 0 0 0 ] x + ( σ 2 ω ) (4.27) }{{}}{{} H R if only gyroscope measurements are available, [ ] ) I 0 0 0 σ z = x 0 0 I 0 +( 2 ω σ 2 r }{{}}{{ a } H R (4.28) for gyroscope and acceleration measurements and [ ] ) I 0 0 0 σ z = x 0 0 0 I +( 2 ω σ 2 r. (4.29) }{{}}{{ m } H R in case of gyroscope and a magnetometer measurements. The remaining formalism (4.23) to (4.25) remains the same. Subsequently, only the states for which measurements are available will be corrected. 4 I is the identity matrix

4 State Estimation 4.2 Position 33 4.1.3.4 Extraction of the Orientation In every iteration step of the Kalman filter, the angular velocity B ω IB and acceleration B ω IB are estimated. The orientation which is also from interest has to be extracted. To avoid any singularity problems, the orientation is directly represented with the 3 3 rotation matrix A BI by describing the three unit base vectors of the initial coordinate system in the body fixed coordinate system A BI = [ Be Ix Be Iy Be Iz ]. (4.30) The three base vectors are extracted from the estimated acceleration vector Bˆr a and the estimated magnetic field vector Bˆr m. Note, that a NED inertial coordinate system is used. As mentioned above, by neglecting the translational acceleration of the robot over the gravity field, the base vector in z-direction is given by 5 Be Iz = B r a B r a 2. (4.31) This estimation is especially accurate if the copter is hovering. Furthermore, the inertial system can always be chosen in a way that the base vector in x direction corresponds with the magnetic field of the earth. However, as the estimation will never be accurate, this vector is not directly taen to derive Br Ix. Rather to preserve orthogonality of the inertial system, it is used to calculate the base vector in y-direction Be Iy = B e Iz B r m B e Iz B r m 2. (4.32) Given two base vectors, the third one can always be calculated for an orthogonal system: 4.2 Position Be Ix = B e Iy B e Iz B e Iy B e Iz 2. (4.33) Initial experiments were done using the PX4FLOW module [6]. It is a flow sensor module based on low-cost components. See chapter 5.3.3 for a more detailed description of the module.to gather velocity information in x- and y direction, an optical flow estimation is done based on the sum of absolute differences (SAD) bloc matching algorithm with angular rate compensation using the gyroscope measurement [33]. The sensor module further delivers height measurement using a sonar sensor. However, the results of using the PX4FLOW as a standalone position estimation by integrating the measurements, were not very satisfying due to an unacceptable drift. For now, the position estimation process is only based on the software RECONSTRUCTME [37]. Unlie common SLAM algorihms such as Davison s MonoSLAM [30] or Klein and Murray s PTAM [36], the here used approach does create real surfaces instead of than only dense maps. 4.2.1 ReconstructMe The software RECONSTRUCTME that is used for position estimation, is developed by the company PROFACTOR in Steyr (Austria). It is a portable, low-cost 3D body scanning system [37]. It wors with publicly available low-cost active technology, such as the here used PRIMESENSE CARMINE 1.09 [5]. The algorithms are processed on decent standard computer hardware, which is designed for accelerated processing (e.g. general purpose computation on graphics processing unit (GPGPU)). The sensor uses the approach of structured light for depth calculation [35]. To do so, a constant pattern of specles is projected onto the scene by splitting up a laser beam via a diffraction grid. This pattern is captured by a 5 To provide the normalized unit base vectors, the 2-Norm v 2 = vi 2 is used. i