Quadruped Robot Roll and Pitch Estimation using an Unscented Kalman Filter*

Transcription

1 24th Mediterranean Conference on Control and Automation (MED) June 21-24, 2016, Athens, Greece Quadruped Root Roll and Pitch Estimation using an Unscented Kalman Filter* Sotirios Nousias and Evangelos Papadopoulos, Senior Memer, IEEE Astract We present a novel algorithm for estimating a uadruped root s pitch and roll angles. Assuming even terrain and an ideal ounding gait, we compute the roll and pitch angles to e fused with on-oard Inertial Measurement Unit (IMU) measurements in an unscented Kalman filter (UKF). Simulation results illustrate the validity of the methodology developed. It is shown that the error in the estimation of oth angles is much smaller compared to those in the literature. I. INRODUCION Quadruped roots have een proven to provide a promising locomotion system, capale of performing extraordinary tasks compared to conventional wheeled vehicles. heir advantage stems from the fact that they use isolated footholds that optimize support in contrast to the continuous path of support reuired y wheel vehicles. o achieve the desired performance, the controllers of such roots reuire oth precise and real-time knowledge of the controlled states. Of all the states that can e chosen, pitch and roll are critical for dynamically stale gaits. he most common techniue to otain precise estimation is sensor fusion. o this end, a numer of research groups use various kinds of availale information, from leg kinematics to vision ased techniues. One of the earliest navigation systems ased on legkinematics was presented y Roston et al. [1]. Several years later Lin et al., working with the RHex hexapod, and assuming that three of its feet are co-linear at every time, they implemented a leg-odometer techniue ased on the kinematics [2]. As their method was affected y drift, they fused that information with the data provided y an on-oard Inertial Measurement Unit (IMU) and the results were improved [3]. Along these lines, Bloesch et al. fused IMU feedack and leg kinematics information within an Extended Kalman Filter (EKF) [4]. heir algorithm estimated within 0.5 deg. the roll and pitch angles of a uadruped, as well as its Center of Mass (CoM) velocity. he same group proposed an improved estimator for handling rough terrain [5]. Reinstein and Hoffman focused on velocity-aided estimators [6]. heir novel leg odometer was ased on a comination of joint and pressure sensors, yielding the stride length in each gait period. he estimated velocity was used to update the Inertial Navigation System (INS) algorithm within an EKF. he main drawack of this method is that the estimator reuires training, as the legged odometer is ased on a data-driven model that relates sensory information to * his research has een financed y the European Union (European Social Fund-ESF) and Greek funds through the Operational Program "Education and Lifelong Learning" of the National Strategic Reference Framework (NSRF) - Research Program: ARISEIA: Reinforcement of the interdisciplinary and/or inter-institutional research and innovation. he authors are with the Department of Mechanical Engineering, National echnical University, Athens, Greece. ( sotirisno@gmail.com; egpapado@central.ntua.gr; ph: ). stride length. Chilian developed a techniue for the fusion of leg-odometry, visual odometry and IMU data [7]. he multisensor data fusion resulted in roust and accurate pose estimation of the DLR crawler, even in poor lighting conditions. Singh considered optical flow ased estimation, ut the pitch angle experimental error was aout one degree [8]. Additionally, optical flow adds significant delay, degrading control performance. Alternate feedack sources include Motion Capture systems (MoCap) and Gloal Positioning Systems (GPS). Despite the cost of the former, it is still limited to confined workspaces, while the latter lacks signal roustness, accuracy and availaility in interior spaces. In this paper, we develop a novel algorithm for estimating the orientation of a uadruped root in ounding. Inspired y [2], we employ the uadruped full stance phase to calculate its asolute pitch and roll angles. he resulting estimates are used to improve the predicted states estimated using the onoard IMU measurements. It is assumed that measurements are affected y Gaussian noise. While results on estimating the rotation around an axis using more complex proailistic distriutions are encouraging, still a recursive estimation algorithm of 3D rotations is missing [9]. Our implementation is achieved with an UKF. Although the resulting computational costs in UKF are slightly higher than those in an EKF, the minimal selection of the states yields fast and precise information when reuested y the controller. Our method results in an error of 0.03 deg. for oth roll and pitch, outperforming previous referenced estimation techniues. II. GYROSCOPE MODEL AND ROLL PICH ANGLES A. Gyroscope Model As part of the Inertial Measurement Unit (IMU), a three-axis gyro provides measurements of the angular velocity expressed in the IMU coordinate frame. We use a simple model to relate the gyro measurement ω to the real angular rate ω, oth expressed in the IMU ody coordinate frame: ω = ω ω η ω (1) where the tilde is used to denote measured uantities. he measurement noise η ω is modeled as additive white Gaussian noise and the term ω represents the non-static ias of the gyro, considered as a Brownian process, i.e. its derivative with respect to time is modeled as white noise: ω = η ω (2) Both white noise processes are assumed to e of zero-mean, E[η ω ] = E[η ω ] = 0 (3) he angular velocity and the corresponding iases covariance matrices are Q ω, Q ω. he covariance parameters can e derived y examining the measured IMU Allan plots [9] /16/$ IEEE 731

2 B. Roll and Pitch Estimation he attitude of the uadruped can e descried y means of the uaternion parameterization defined as: ĵsin( ρ /2) = cos( ρ /2 ) = [ ] (4) 4 where ĵ is a unit vector corresponding to the rotation axis and ρ is the angle of rotation. A rotation uaternion must always satisfy the following constraint: = = 1 (5) In addition, the inverse uaternion is: 1 = [ ] (6) Due to the complexity of the prolem, a few assumptions are made. Firstly, the ground is considered even, which is realistic since high-speed gaits are can e achieved on even terrain. he slope of the ground can e nonzero; in such a case, it is assumed that this slope is known via some other system, such as vision. Additionally, the ounding gait is assumed to include a full-stance phase. We now focus our attention to the uadruped shown in Fig. 1. We denote y I and B the inertial and ody frame respectively, while the suscripts ( ) r and ( ) fl refer to ack right leg and front left leg respectively. Moreover, a presuperscript refers to the frame in which a vector is expressed. Z I Y X l r r r r z B Fig. 1. 3D model of the uadruped at doule stance during a ounding gait. Assuming that a leg pair such as the (r, fl) is in contact with the ground, then vectors l r and l fl can e expressed as functions of the encoder angles α r and α fl as follows: B l r = Rot y (α r )l r ẑ (7) B l fl = Rot y (α fl )l fl (ẑ) (8) where l i is the length of the foot i, which is provided y the leg spring encoders. he term Rot y ( α) is a rotation matrix corresponding to a rotation of angle α around a y-axis: cosα 0 sinα Rot y ( α)= (9) sinα 0 cosα Since the vectors r r and r fl, which express the position of the i-th hip of the uadruped in the ody frame, are constant in the ody frame, the computation of the vector B l 1 is trivial: B l 1 = B l r B l fl B r fl B r r (10) Additionally, the vector l 1 in the inertial frame is given y: I l 1 = I C B (ψ,θ,ϕ) B l 1 (11) y x r fl l 1 fl l fl where I C B represents the rotation matrix from B to I, parameterized y the yaw, ψ, pitch, θ, and roll, φ, angles. In the case of ideal ounding, the yaw angle is zero while due to the assumption of even terrain, the third component of I l 1 lies in the X-Y plane. he assumption of zero yaw is also realistic on a treadmill, where we can constrain the rotation of the platform with respect to the gravity vector. he mathematical expression of the aove oservations is: I C B ( 0,θ,ϕ) B l 1 Ẑ=0 (12) Considering the full stance phase, (12) holds for any set of two feet. Denoting B l 2 the distance etween the ack right leg and front right leg, pitch and roll can e computed as: B l ϕ =tan 1 2,z B l 1,x B l 2,x B l 1,z B l 2,x B l 1,y B l 2,y B (13) l 1,x sin( ϕ) B l θ =tan 1 1,y cos( ϕ) B l 1,z B (14) l 1,x Finally, given the roll and pitch, we compute the Direction Cosine Matrix (DCM) and the corresponding uaternion. he uaternion that expresses the rotation from the inertial to the ody frame is a function, namely m, of roll and pitch: ( ) (15) BI,m = m θ,ϕ Next, an UKF is developed to improve the IMU feedack. III. UNSCENED KALMAN FILER We use the following notation: a suscript k is added to discretized uantities, while a hat represents estimated uantities. wo kind of errors are used, δ and d. he former represents a rotation error in the Euclidean space, while the latter represents a rotation error in the uaternion manifold. Furthermore, the superscripts ( ) and ( ) refer to a priori and a posteriori estimates. A. State Definition he state vector x consists of the uaternion descriing the rotation from the I to the B frame BI, and the gyro iases ω : x = BI ω (16) he error of the states, which is tracked y the covariance matrix P xx, is denoted y δx. Since the uaternion represents three degrees of freedom, the corresponding covariance matrix also has to e represented as a three dimensional matrix. he constraint of the unit uaternion influences the positive definiteness of the state covariance. o this end, we employ the uaternion error d corresponding to a small rotation etween the estimated uaternion ˆ BI and the true uaternion BI : BI = d ˆ BI (17) where stands for the uaternion multiplication. In contrast to the additive property of the gyro iases error, the uaternion error is multiplicative, and is derived from (17) as: 1 d = BI ˆ BI (18) Given d, we can extract the rotation error δζ. he error δζ is considered as the vector part of d. 732

3 Having all state errors expressed in the Euclidean space, the derivation of the state covariance P xx, where we make use of the covariance operator Cov(*), is trivial: P xx = Cov(δx) (19) where, δx = δζ δ ω (20) In the aove euation the term δ ω denotes the error related to the ias states. On the other hand given δζ, the corresponding uaternion error d can e computed using the unit norm constraint. As there will e oth positive and negative values of the scalar part of the uaternion that satisfy the uaternion normalization constraint, we choose the positive value. As a conseuence the uaternion error is: d = [ δζ 1δζ δζ ] (21) For the suseuent discretization, we use the function ε( ) which maps an aritrary three dimensional rotation vector v to its corresponding uaternion: v / v sin v /2 ( ) ε :v ε(v) = (22) cos( v /2 ) where stands for the Euclidean norm. B. Prediction Model he propagation of state x is achieved y the following euations: BI = 1 B ω ω η ω 2 0 (23) BI ω = η ω (24) o discretize the aove stochastic differential euations, we integrate the deterministic part separately. Assuming a zero-order hold for the gyroscope measurements, and denoting the time etween two suseuent measurements as Δt, we otain: ˆ k1 = ε Δt ω k ˆ ( ( ω,k )) ˆ k ˆ ω,k1 = ˆ ω,k (25) (26) Furthermore, the discretized process noise covariance matrix is: Q ω Δt Q = (27) Q ω Δt In (27), the diagonal terms represent the discretized uaternion BI and ias ω covariance matrices respectively. C. Measurement Model he measurement euation is: ( ) (28) ˆ BI,m = m θ,ϕ,η m where the term η m expresses the standard measurement noise of the incremental encoders, assumed to e white Gaussian noise, as well as model imperfections and numerical errors. he measurement covariance matrix, which will e used in the UKF, is R and is the main tuning parameter of the filter. D. Unscented Kalman Filter Euations he UKF addresses the linear approximation issues of the EKF. he concept is ased on the oservation that it is easier to approximate a proaility distriution than a nonlinear function. he state distriution is represented y a Gaussian Random Variale (GRV), ut is now specified using a minimal set of carefully selected perturations aout the current state estimate, namely sigma points. hese sigma points completely capture the true mean and covariance of the GRV, and after eing propagated through the nonlinear system, they capture the posterior mean and covariance accurately to the second order (aylor series expansion). he main tool of the selection of the sigma points is the Unscented ransformation (U). For a more in depth study of the U as well as the UKF, please refer to [10]. From a mathematical point of view, to implement a UKF, our system has to e in the form of discrete time euations: ( ) (29) ( ) (30) x k1 = f x k,η x,k y k1 = hx k,η y,k where x k n 1, y k m 1 are the n-dimensional state and m-dimensional measurement vectors respectively, while f and h correspond to nonlinear functions which represent the process model and the measurement model. In addition, the terms η x,k and η y,k are related to the process noise and measurement noise. Here, the process model is given y (25)- (26), whereas the measurement model is descried y (28). Since the uaternion lies on the SO(3) manifold, the uaternion mean cannot e computed as a weighted sum of sample points. As a conseuence, the UKF implementation in uaternion space needs special care. Although the UKF algorithm is known, there is need for its adaptation in the unit uaternion manifold. his is presented next, ased on [11]. Given the n n covariance matrix P xx,k and the process covariance matrix Q k at the timestamp k, a set of (1) sigma points, namely X k n (1), is calculated: X k = X 0,k X 1,k X,k X 1,k where the columns of X k are calculated as: (31) = ˆx k i = 0 (32) = ˆx k u i i = 1,...,n (33) X ni,k = ˆx k u i i = 1,...,n (34) he term u i stands for the i th column of the matrix U; it is computed as the suare root of the sum of the matrices P xx,k and Q k scaled y γ, and derived y employing a lower Cholesky decomposition: U = γ P xx,k Q k he factor γ is given y: ( ) (35) γ = n λ (36) λ = w 2 ( n κ) n (37) In (36) and (37), w determines the sigma points spread around the state ˆx k mean, and is usually set to a small positive value. he constant κ is set etween 3-n and zero, to improve the capture of distriution higher order moments. 733

4 While the selection of sigma points is trivial, when the states are expressed in Euclidean space, using (32)-(34), the uaternion states needs special care since addition is not closed in the SO(3) manifold. Based on this idea, we distinguish etween sigma points related with uaternion states, denoted y X k, and sigma points referring to ias states, namely X k. Furthermore, we denote u i, at the timestamp k, in (33)-(34) as: u i = ξ Τ Τ Τ i,k, ξ i,k (38) where ξ i,k are the first three elements of the ith column of matrix U which represent the vector part of the uaternion and thus they are a three dimensional error. he ξ i,k are the last three elements of the ith column of U related to the error in the gyro ias ω. Oserving (33)-(34), one can see that sigma points are scattered around the current mean ased on the uncertainty of the current estimate, which can e considered as a small perturation to the current mean. Το scatter the uaternion sigma points around the current uaternion estimate ˆ k, the error ξ i,k has to e represented in uaternion space. o this end, we map ξ i,k to the uaternion error dξ i,k y means of the uaternion normalization constraint: dξ i,k = ( ξ i,k ) 1( ξ i,k ) ξ i,k (39) Given the uaternion error dξ i,k the uaternion sigma points are: X k = X 0,k X ni,k i = 1,...,n (40) where: X ni,k = ˆ k i = 0 (41) = dξ i,k ˆ k i = 1,...,n (42) = ( dξ i,k ) 1 ˆ k i = 1,...,n (43) From a mathematical point of view, the definitions of addition and sutraction in the uaternion manifold stand out from the aove euations. In other words, addition is considered as the pre-multiplication etween the two uaternions, namely dξ i,k and ˆ k, while sutraction is considered as the pre-multiplication with the inverse uaternion ( dξ i,k ) 1 and ˆ k. he rest X k sigma points are selected ased in (32)-(34): where: X k = X 0,k X ni,k X ni,k i = 1,...,n (44) = ˆ ω,k i = 0 (45) = ˆ ω,k ξ i,k i = 1,...,n (46) = ˆ ω,k ξ i,k i = 1,...,n (47) hen, the set of sigma points is: X k = X k, X k (48) Additionally, the UKF weights are chosen as: W (m) 0 = λ /(n λ) (49) W 0 (c) = λ /(n λ)(1 w 2 β) (50) W (m) i = W (c) i = 1/ 2(n λ) i = 1,..., (51) where the superscripts m and c stand for mean and covariance respectively. he term β is used to improve the estimates of higher order moments of the distriution; its optimal value is 2 for Gaussian distriutions [10]. he set of sigma points in (48) is then propagated ased on (25)-(26) to form the newly predicted sigma points X k1. Furthermore, the predicted sigma points of the measurements are computed y: Y k1 = HX k1 (52) H = I 4x4 0 4x3 (53) he mean of the state as well as the mean of predicted measurements at timestamp k1, are calculated as: ˆx k1 = W (m) i 1 (54) ŷ k1 = W (m) i Y i,k1 (55) Although, the aove euations hold in the case of the states that are expressed in the Euclidean manifold, the uaternion mean, part of the state mean, cannot e computed in this way. he computed uaternion mean has to lie in the unit sphere and as a conseuence it reuires a different metric. he metric used was the Euclidean norm [11]. As a result, the uaternion mean was computed as: ˆ k1 = W (m) i 1 W (m) i X i,k1 (56) Given the newly predicted mean ˆx k1, the predicted state covariance is: ˆP xx,k1 = W (c) i (1 ˆx k1 )( 1 ˆx k1 ) (57) In (57), when the states are expressed in uaternion terms, to derive the uaternion error at the timestamp k1, the sutraction in the uaternion manifold is employed as: d i,k1 = 1 ( ˆ k1 ) 1 (58) In addition, the aove uaternion error has to e represented in three dimensions; this is achieved using the error δζ we defined earlier. Given δζ, the predicted state covariance matrix, at the timestamp k1, ˆP xx,k1 is computed. he measurement mean ŷ k1 and measurement covariance matrix P yy,k are computed similarly as presented aove: ˆP yy,k1 = (Y i,k1 ŷ k1 )(Y i,k1 ŷ k1 ) R (59) he cross correlation matrix P xy,k1 etween the states and the measurements is computed as: ˆP xy,k1 = (1 ˆx k1 )(Y i,k1 ŷ k1 ) (60) where sutraction as defined in the uaternion space, is used when necessary. hen, the Kalman Gain can e calculated as: ˆK k1 = ˆP 1 ˆPyy,k1 xy,k1 (61) 734

5 he innovation, which is the difference etween the actual measurements and the predicted ones, is expressed as the uaternion error etween ˆ m,k1 and ŷ k1 : d k1 = ˆ m,k1 ( ŷ k1 ) 1 (62) Finally, the correction step of the UKF is: ˆP xx,k1 = ˆP xx,k1 ˆK 1 ˆPyy,k1 k1 ˆK k1 δ x k1 = ˆK k1 d 1, k1 d 2, k1 d 3, k1 ˆx k1 (63) (64) = ˆx k1 δx k1 (65) In (65), addition as defined in uaternion space was used in order to calculate the a posteriori estimate of the uaternion ˆ k1 as well as (21) when the correction vector has to e mapped from the Euclidean Space to the SO(3) manifold. IV. SIMULAION RESULS Experiments were performed in the Weots M simulation environment, see Fig. 2. he uadruped has four legs, each with an actuated rotational hip joint and a compliant passive prismatic joint. he IMU is positioned at the CoM of the uadruped root. he implemented controller can set oth the root speed and its apex height, despite the single per leg actuated hip joint [12]. he root is running in a ounding gait, and achieves an average speed of 1 m/s along the x-axis. see Fig. 3. Some peaks around 0.1 o are related to numerical errors in the Weots M environment. he asolute of the mean of the error in the pitch angle is approximately 0.03 o. In Fig. 4, the error in roll angle is plotted; the corresponding error is similar to that for the pitch angle. Fig. 3. Pitch Error. (a) Fig. 4. Roll Error. o compare our estimation results with results in the literature, we chose the estimation techniue proposed in [4]. While we could choose to fuse the information of the leg kinematics within a UKF, we implemented the EKF due to the computational effort a computer needed to handle a 21- state UKF. Figure 5 depicts the pitch errors for the developed estimation methodology and the referenced one, whereas in Fig. 6 the corresponding roll errors are plotted. () (c) Fig. 2. (a) he uadruped root in the Weots M simulation environment, () ody pitch angle response, and (c) ody roll angle response. o evaluate the performance of the UKF, the noise levels of the gyroscope measurements as well as of the leg encoders must e realistic. In the Weots M environment, this was achieved y including actual sensor measurement noise, otained from static experiments. he root has variale pitch angle; the yaw and roll angles were commanded y the controller to remain zero. During the simulation, the root was executing a ounding gait for approximately one minute. he terrain was set to e even. he sensor sampling freuency was set at 1ms, and the w, κ and β filter parameters were chosen as 0.03, -2.0 and 2.0 respectively. he time response of the root pitch, roll, and yaw angles is shown in Fig. 2 for reference. he pitch angle error which is of high significance for control, oscillates etween 0.08 o and o, Fig. 5. Pitch error for different estimation techniues. Comparing the two responses, we can oserve that the mean of the pitch error in the developed estimator is close to zero, (0.029 o ), whereas in the referenced one, the mean is approximately 0.33 o, see Fig. 5. Regarding the roll angle error, despite the fact that the means of the errors are close, namely 0.02 o and 0.09 o, the difference in the magnitude of the oscillations, as illustrated in Fig. 6, is significant. 735

6 he pitch and roll errors corresponding to the developed UKF estimator and the simplest sensor fusion filter, i.e. the complementary filter are given in Figs. 7 and 8. he resulting pitch and roll angle estimation error means, considering the complementary filter, are 0.16 o and o respectively; i.e. three times larger than the error otained from the UKF. Fig. 6. Roll error for different estimation techniues. V. CONCLUSION AND FUURE WORK In this paper, a novel approach for the estimation of roll and pitch angles of a ounding uadruped was proposed. Assuming even terrain and an ideal ounding gait, the roll and pitch angles were computed, to e fused with on-oard IMU measurements in an UKF. he simulation results were very encouraging as the error, even for durations up to a minute, was not drifting ut instead was staying elow 0.07 o. he main ottleneck of the UKF is the computation of the suare root of the covariance matrix, which has complexity O(n 3 ). his was overcome via a minimal state selection. In the near future, full experimental results will e otained after our uadruped root under development is operational. Although, the simulation environment can model many realistic situations, we did not simulate slippage. We anticipate that our estimator will handle slippage, since no assumption of zero velocity at the foot points is made. hus we aim at developing algorithms for slippage detection. Finally, the use of sensors such as the Κinect M will e considered in order to improve position relative estimates. VI. REFERENCES Fig. 7. Pitch errors for the developed UKF and the complementary filter. Fig. 8. Roll errors for the developed UKF and the complementary filter. o conclude, the developed estimator yields angle errors which are ten times smaller compared to the referenced techniue. he main reason for these superior results is that in our measurement model, the actual states are eing oserved, whereas in the referenced estimator the measurement model consists of a comination of all states; as a conseuence, an error even in one state affects all its states. From an implementational point of view, since the referenced estimator consists of more states, it is prone to more numerical errors than the proposed one. [1] G. P. Roston and E. P. Krotkov, Dead Reckoning Navigation For Walking Roots, IEEE/RSJ Int. Conference on Intelligent Roots and Systems, July 1992, pp [2] P.-C. Lin, H. Komsuoglu and D. E. Koditschek, A Leg Configuration Measurement System for Full-Body Pose Estimates in a Hexapod Root, IEEE rans. Rootics, vol. 21, no. 3, June 2005, pp [3] P.-C. Lin, H. Komsuoglu and D. E. Koditschek, Sensor data fusion for ody state estimation in a hexapod root with dynamical gaits, IEEE rans. Rootics, vol. 22, no. 5, Octoer 2006, pp [4] M. Bloesch, M. Hutter, M. A. Hoepflinger, S. Leutenegger, C. Gehring, D. C. Remy and R. Siegwart, State Estimation for Legged Roots - Consistent Fusion of Leg Kinematics and IMU, Proceedings of Rootics: Science and Systems, July [5] M. Bloesch, M. Hutter, M. A. Hoepflinger, S. Leutenegger, C. Gehring, D. C. Remy and R. Siegwart, State Estimation for Legged Roots on Unstale and Slippery errain, IEEE/RSJ Int. Conf. on Intelligent Roots & Systems (IROS), 3-7 Nov. 2013, pp [6] M. Reinstein and M. Hoffmann, Dead reckoning in a dynamic uadruped root: inertial navigation system aided y a legged odometer, IEEE Int. Conference on Rootics and Automation (ICRA), 9-13 May 2011, pp [7] A. Chilian and H. Hirschmuller, Multisensor Data Fusion for Roust Pose Estimation of a Six-Legged Walking Root, IEEE/RSJ Int. Conference on Intelligent Roots and Systems (IROS), Septemer 2011, pp [8] S. P. Singh and K. J. Waldron, A Hyrid Motion Model for Aiding State Estimation in Dynamic Quadrupedal Locomotion, IEEE Int. Conf. on Rootics and Automation, April 2007, pp [9] G. Kurz, I. Gilitschenski, S. Julier and U. D. Haneeck, Recursive Estimation of Orientation Based on the Bingham Distriution, 16th Int. Conference on Information Fusion (FUSION), 2013, pp [10] N. El-Sheimy, H. Hou and X. Niu, Analysis and Modeling of Inertial Sensors Using Allan Variance, IEEE rans. Instrumentation and Measurement, vol. 57, no. 1, January 2008., pp [11] S. J. Julier and J. K. Uhlmann, Unscented Filtering and Nonlinear Estimation, Proc. IEEE, vol. 92, no. 3, pp , March [12] Y.-J. Cheon and J.-H. Kim, Unscented Filtering in a Unit Quaternion Space for Spacecraft Attitude Estimation, IEEE Int. Symposium on Industrial Electronics, 4-7 June 2007, pp [13] N. Cherouvim and E. Papadopoulos, Speed and Height Control for a Special Class of Running Quadruped Roots, IEEE Int. Conference on Rootics and Automation (ICRA), May 2008, pp