Quaternion based Extended Kalman Filter

Transcription

1 Quaternion based Extended Kalman Filter, Sergio Montenegro

2 About this lecture General introduction to rotations and quaternions. Introduction to Kalman Filter for Attitude Estimation How to implement and use it Derivation of required system model Not a derivation or proof of Kalman Filter itself 2 nd year in Würzburg Robotics II 2 nd year in Helsinki Estimation and Sensor Fusion Methods

3 Content Motivation Background Normal distribution Jacobian Matrix State-space representation Kalman Filter Introduction Filter concept Kalman Filter (KF) Simple Example Attitude Estimation Linear vs. Non-linear Systems Extended Kalman Filter (EKF) Rotations Quaternion Based EKF

4 Motivation Measurements are noisy, have an offset or drift In order to eliminate the measurement noise we predict the next measurement based on our current observations and the knowledge we have about our system Combine different sensors in such a way, that their offsets are eliminated and their drift removed

5 Normal distribution Let the measurement of a sensor be a random variable x with a normal distribution based on its standard deviation σ and mean μ: f (x)= 1 (x μ) 2 σ 2π e 2σ 2 E.g.: Noisy accelerometer measurement Motors on

6 Jacobian Matrix The Jacobian matrix is the matrix of all first-order partial derivatives of a vector-valued function. =[ f 1 f 1 J= d f d x = [ f f ] x 1 x n ] x 1 x n f m f m x 1 x n Example: x= [ x 1 2] f (x)=[f ]=[2 1 x1 2 x 2 ] f x 2 x 2 2 f 3 x 1 +x 2 F= d f d x =[ f 1 f 1 x 1 x 2 ]=[4 x1 1 f 2 f ] x 1 x x f 3 f 3 x 1 x 2

7 State-space representation The state-space model describes the behaviour of a system. The most general form of a linear discrete time-invariant sytem is described by the two following equations: x(k +1)=F x(k )+G u(k)+v (k ) y(k)=h x(k)+ D u(k)+w (k) x(k): state vector at discrete time k (e.g. Attitude) u(k): system input at discrete time k (e.g. Motor Command) y(k): system output at discrete time k (e.g. Measurements) F: state or system matrix G: input matrix H: output matrix D: feedthrough matrix v(k): process noise w(k): measurement noise

8 Kalman Filter concept Not a traditional frequency filter, but a recursive state estimator Based on system state and state covariance State covariance describes how reliable a state estimate is and how much state variables change together Prediction Estimate the next state and its measurement based on the state-space model Update Compare the estimation with the latest measurement and update the state accordingly

9 Kalman Filter State-space model without feedthrough: x(k +1)=F x(k)+g u(k)+v (k) y(k)=h x(k)+w (k) Measurement prediction z(k +1)=H x est (k +1) Measurement residual v= y(k+1) z(k+1) State prediction x est (k+1)=f x(k)+gu (k) Updated state x(k +1)=x est (k +1)+W v Current state x(k) W: Kalman Gain

10 Kalman Filter State-space model without feedthrough: x(k +1)=F x(k)+g u(k)+v (k) y(k)=h x(k)+w (k) Measurement prediction Measurement residual Residual covariance z(k +1)=H x est (k +1) v= y(k+1) z(k+1) S=H P est (k+1) H T +R State prediction State prediction covariance x est (k+1)=f x(k)+gu (k) P est (k +1)=F P (k)f T +Q Updated state x(k +1)=x est (k +1)+W v Filter gain W =P(k+1)H T S 1 Current state x(k) Current state covariance P(k) Updated state covariance P(k+1)=P est (k+1) W SW T Q: process noise covariance matrix ( how reliable is my state-space model ) R: measurement noise covariance matrix ( how reliable are my measurements )

11 Simple Example 1D KF 1D distance measurement with an infrared sensor Sensor gives distance measurement y to an object Measurements are noisy with a variance σir² Process noise is described by variance σp² System state is the distance d State-Space model (1D use case only scalars): x(k)=[d ] old = new x(k +1)=Fx (k)+gu(k)=1 x(k)+0 y (k)=hx (k)=1 x(k) no input distance can be directly measured

12 Simple Example 1D distance measurement state-space model: x(k +1)=Fx (k)+gu(k)=1 x (k)+0 y(k)=hx (k)=1 x(k) Measurement prediction Measurement residual Residual covariance z(k+1)=x est (k+1) v= y(k+1) z(k+1) S=P est (k+1)+r State prediction State prediction covariance Updated state Filter gain x est (k+1)=x(k) P est (k +1)=P(k)+Q x(k +1)=x est (k +1)+W v W =P est (k+1) S 1 Current state x(k) Current state covariance P(k) Updated state covariance P(k+1)=P est (k +1) W S W T x(k +1)=x(k)+W ( y (k+1) x(k)) W =(P (k)+q) ((P (k )+Q)+R ) 1

13 Simple Example P? Q? R? x(k +1)=x(k)+W ( y (k+1) x(k)) W =(P (k)+q) ((P (k )+Q)+R ) 1 Q: process noise, how reliable is the state-space model R: measurement noise, how good are the measurements P: state covariance, how reliable is my current state estimate Case 1 Q = 0 system model represents real system behavior exactly P0 = 0 the starting state is exactly known But from Q = 0 and P0 = 0 W = 0 x(k+1) = x(k) If the starting state and the system are perfectly known, measurements are not needed, since our best estimate is always our starting state. Since we want to use our measurements it should be Q > 0 and P0 > 0

14 Simple Example P? Q? R? x(k +1)=x(k)+W ( y (k+1) x(k)) W =(P (k)+q) ((P (k )+Q)+R ) 1 Q: process noise, how reliable is the state-space model R: measurement noise, how good are the measurements P: state covariance, how reliable is my current state estimate Case 2 Q > 0 non-perfect system model P0 = 0 the starting state is exactly known But from Q > 0 and P0 = 0 W = Q (Q+R) ¹ The Kalman Filter Gain is a relation between process noise and the sum of measurement and process noise

15 Simple Example P? Q? R? x(k +1)=x(k)+W ( y (k+1) x(k)) W =(P (k)+q) ((P (k )+Q)+R ) 1 Q: process noise, how reliable is the state-space model R: measurement noise, how good are the measurements P: state covariance, how reliable is my current state estimate Case 3 R = 0 perfect sensor without noise But from R = 0 W = 1 x(k+1) = y(k+1) The best estimate of our system would be always our latest measurement, therefore should be R > 0

16 Simple Example P? Q? R? x(k +1)=x(k)+W ( y (k+1) x(k)) W =(P (k)+q) ((P (k )+Q)+R ) 1 Q: process noise, how reliable is the state-space model R: measurement noise, how good are the measurements P: state covariance, how reliable is my current state estimate Conclusion The Kalman Filter is permanently weighing the residual based on the relation of process and measurement noise. The weighted residual is then used to update the system state. Measurement and process noise can vary with time. Different information sources can be weighted dynamically depending on a given situation. In the given 1D example one would set R = σir² and Q = σp², P can be initially set to a high value and will converge to a small value over time

17 Linear vs. Non-Linear System The Kalman Filter assumes a system of linear functions. The linear transformation of a normal distributed variable is normal distributed again. This is not valid for non-linear systems. In real world, almost every system is non-linear. In order to handle non-linear systems, the Kalman Filter is extended by approximating non-linearities with a Taylor Series Expansion. A 1 st order expansion is often sufficient.

18 Extended Kalman Filter Consider a non-linear system: System transition: x(k +1)=f [k, x(k), u(k)]+v(k) Measurement equation: z(k)=h [ k, x(k)]+w(k) Then the equations to estimate and update the state are: State prediction Measurement prediction Measurement residual x est (k+1)=f [ k, x(k),u(k) ] z(k+1)=h [ k+1, x est (k+1)] v (k+1)= y(k+1) z(k +1) Current State x(k) Updated state x(k +1)=x est (k+1)+w v(k+1)

19 x(k +1)=f [k, x(k), u(k)]+v(k) z(k)=h [ k, x(k)]+w(k) Extended Kalman Filter The 1 st order Taylor Series Expansion is equivalent to the evaluation of the Jacobians of the functions f and h at a certain point x: F(k)= f [k, x(k), u (k)] x x=x (k) h[ k+1, x(k+1)] H(k+1)= x x=x est (k+1 ) To estimate and update the state covariance is similar to KF: State prediction covariance Residual covariance Filter gain P est (k +1)=F (k) P(k)F (k)'+q S =H (k+1)p est (k+1)h (k+1) T +R W =P est (k+1) H (k+1) T S 1 v (k+1)= y(k+1) z(k+1) Current state covariance P(k) Updated state covariance P(k+1)=P est (k+1) W S W T x(k +1)=x est (k+1)+w v(k+1)

20 Quaternion based EKF QEFK Goal: Combine 3DoF Gyro, 3DoF Mag and 3DoF Accel measurements y to get a full attitude estimation Measurement models Gyroscope y g =ω b +x g +v g Body frame Magnetometer Accelerometer y m =m b + x m +v m y a =a b g b +x a +v a ω is the angular velocity m is the magnetic field a is the acceleration g is the gravitational constant xi is the bias on each sensor vi is the noise of each sensor

21 QEKF What do we need? x est (k+1)=f [ k, x(k), y g (k)] z(k+1)=h [ k+1, x est (k+1)] v (k+1)= y(k+1) z(k+1) x(k +1)=x est (k+1)+w v(k+1) System state X System propagation f(x,u) System Jacobian FX System state covariance P Process noise Q P est (k +1)=F (k) P(k)F (k ) T +Q S=H (k+1)p est (k+1)h (k+1) T +R W =P est (k+1) H (k+1) T S 1 P(k+1)=P est (k+1) W SW T Measurement prediction h(x) Observation vector y Measurement noise R Measurement Jacobian H

22 Alias vs. Alibi rotation

23 Euler Angles Tait-Bryan IEEE DIN 9300 Body and Navigation frame overlap for Ψ = Θ = Φ = 0 Rotation sequence: Yaw Ψ Pitch Θ Roll Φ

24 Quaternion A quaternion is a hyper complex number of rank 4. We usually write: q=q 0 +q 1 i+q 2 j+q 3 k q=[q 0 q 1 q 2 q 3 ] T =[q 0 q] T Conjugate of the Quaternion q: q =[q 0 q ] T And its norm: q = q 0 2 +q 1 2 +q 2 2 +q 3 2 A unit quaternion (norm equals to 1) can be efficiently used to describe rotations in 3D space

25 Quaternion Multiplication of two quaternions q and p is defined by the Kronecker product: q p = ( p 0 q 0 p q, q 0 p+ p 0 q+ q p) = q p0 p 1 3] p 2 p [ 0 q 1 q 2 q 3 q 1 q 0 q 3 q 2 q 2 q 3 q 0 q 1 q 3 q 2 q 1 q 0 ][ = Q (q ) p The quaternion derivative for an angular velocity w measured in the body frame is given by: q= 1 2 q [ 0 ] ω = 1 b 2 Q (q) [ 0 ] ω b

26 Quaternion A vector v can be rotated from the navigation frame to the body frame by pre-multiplying it with (alibi): [ v'=q 0 q ] v q =R v=[ 2 0 +q 2 1 q q 3 2 (q 1 q 2 q 0 q 3 ) 2 (q 0 q 2 +q 1 q 3 ) ] nb 2 (q 1 q 2 +q 0 q 3 ) q 2 0 q 2 1 +q q 3 2 (q 2 q 3 q 0 q 1 ) v 2 (q 1 q 3 q 0 q 2 ) 2 (q 0 q 1 +q 2 q 3 ) q 2 0 q 2 1 q q 3 Similar, a vector v' in the body frame can be rotated to the navigation frame by pre-multiplying it with (alibi): v=q [ 0 q ] 0 v' q=rbn v '=[ 2 +q 2 1 q q 3 2 (q 1 q 2 +q 0 q 3 ) 2 (q 1 q 3 q 0 q 2 ) ] 2 (q 1 q 2 q 0 q 3 ) q 2 0 q 2 1 +q q 3 2 (q 0 q 1 +q 2 q 3 ) 2 (q 0 q 2 +q 1 q 3 ) 2 (q 2 q 3 q 0 q 1 ) q 2 0 q 2 1 q q 3 v '

27 Quaternion For representing quaternions in a more intuitive matter they can be converted to Euler angles with: [ Φ ] =[atan 2(2(q 0q 1+q 2q 3),1 2 (q 2 1 +q 22 ))] Θ asin (2(q 0 q 2 q 1 q 3 ) ) Ψ atan 2(2(q 0 q 3 +q 1 q 2 ),1 2 (q 2 2 +q 32 Euler angles can be converted to a quaternion with: 0 q nb q ini =[q 1 3]=[ q 2 q cos(ψ /2)cos(Θ /2)cos(Φ/2) + sin(ψ /2)sin (Θ/2)sin(Φ/2) cos(ψ /2)cos(Θ/2)sin(Φ/2) sin(ψ / 2)sin (Θ/ 2)cos(Φ/2) cos(ψ /2)sin (Θ/2)cos(Φ/2) + sin (Ψ /2)cos(Θ/2)sin(Φ/2) sin (Ψ/ 2)cos(Θ/2) cos(φ/2) cos(ψ / 2)sin(Θ/ 2) sin(φ/2)]

28 QEKF system model Goal: the attitude should be represented by a quaternion q. Furthermore the angular speed ω, as well as the gyro bias x should be estimated from navigation to body frame System state X (k)=[q nb (k) ω b (k) x g (k)] T System model NOTE: The computational costs are linked to the size of the residual covariance matrix S, since this matrix has to be inverted. We can decrease the size of this matrix by using the gyroscope measurement as a system input directly, rather than adding it to the EKF update equations q nb (k+1) = q nb (k)+δ t (k) [ (k)] qnb ωb Quaternion derivative : ω b (k+1) = y g (k) x g (k)[rad/ s] x g (k+1) = (1 λ xg Δ t ) x g (k) Quaternion product q nb (k) λ is a correlation time factor that models how fast the bias can vary

29 QEKF state propagation f(x,u) The function f to propagate the system is therefore given by: f (X, u)=f (X, y g )=[q nb +Δ t 1 2 qnb [ 0 ω b] y g x g (1 λ xg Δ t ) x g ] Gyroscope is system input

30 QEKF Jacobian Fx of f(x,u) The 1 st order Taylor Series Expansion of f(x,u) is given by its Jacobian for the system covariance propagation: F X = d f d X =[ nb fq fq nb 0 q nb ω b 4 x3 ] f ω b 0 3 x x 3 x g fx 0 3 x 4 0 g 3 x 3 x g

31 QEKF Accelerometer update Assuming that our object is not accelerating, the only force measured with the accelerometer is the gravitational force with some additive noise. In order to predict the accelerometer measurement, the gravity vector from the navigation frame needs to be expressed in our body frame using our current attitude estimation: 2 (q1q3 q0 q2) 2 (q 2 q 3 +q 0 q 1 ) alias The observation is given by: The measurement covariance of the accelerometer is: The measurement Jacobian of the accelerometer is: T[ h a (X )=(R nb ) 0 0 1] =[ (q 0 2 q 1 2 q 2 2 +q 3 2 )] y a [g=9.81 m/ s 2 ] R a =I 3 x3 σ a 2 H a = h a X = [ h a q nb h a ω b h a ] x g

32 QEKF Magnetometer update Since the accelerometer provides a more robust solution for the roll and pitch axis drift estimate, the influence of the magnetometer should be limited to the yaw axis only. Therefore, we do not predict the Earth's magnetic field, but rather the yaw angle we would obtain from the magnetometer. The heading can be predicted from our current state estimate as follows: In order to limit the vertical influence of the magnetometer, we have to set the measurements z-component in the navigation frame to zero. We do this by first rotating ym from the body into the navigation frame: Next, we rotate this vector back to the body frame: The observation is then simply calculated as: h Ψ ( X)=atan 2 (2(q 0 q 3 +q 1 q 2 ),1 2 (q 2 2 +q 32 ) ) m n =( R bn ) T y m m h n =[m x n m y n m b =(R nb ) T m h n 0 ] T y Ψ =atan2( m y b,m x b )[rad ]

33 QEKF Magnetometer update The measurement covariance of the magnetometer is: R Ψ =σ Ψ 2 The measurement Jacobian of the magnetometer is: H Ψ = h Ψ X = [ h Ψ q nb h Ψ ω b h Ψ ] x g

34 QEKF Measurement update The full measurement prediction is given by: The full observation vector is: h( X )=[ h a ( X ) h Ψ ( X)] y=[ y a y Ψ] The full measurement covariance is: R=[ R a 0 3 x 1 ] 0 1 x 3 R Ψ The full measurement Jacobian for the residual covariance is: H=[ H a H Ψ]

35 QEKF What did we forget? We have: System propagation f(x,u) System Jacobian FX Measurement prediction h(x) Observation vector y Measurement covariance R Measurement Jacobian H x est (k+1)=f [ k, x(k), y g (k)] z(k+1)=h [ k+1, x est (k+1)] v (k+1)= y(k+1) z(k+1) x(k +1)=x est (k+1)+w v(k+1) P est (k +1)=F (k) P(k)F (k ) T +Q S=H (k+1)p est (k+1)h (k+1) T +R W =P est (k+1) H (k+1) T S 1 P(k+1)=P est (k+1) W SW T Initial states and system covariance are missing

36 QEKF System Covariance How good is our system model??? it is as good as our gyro and the bias estimate The full model used to propagate the bias estimate is: bias(k+t )=(1 λ Δ t ) bias(k)+v bias Bias noise with σxg² The system noise covariance matrix Q can be described as: F U =[ f y g Q= F U U F U T, where FU and U are given by: f v xg ] =[ 04 x 3 04x 3 f ω b y g 0 3x x 3 f x g v xg U ]=[04 x 3 04 x 3 I 3 x 3 0 3] 3x 3 0 3x 3 I 3 x =diag [σ 2 g I 3 x 3 σ 2 xg I 3 x 3 ]

37 QEKF Initial states Calculate the mean over T values for each sensor: T T g b = 1 y T a (i) m b = 1 i=1 T i=1 y m (i) x g(ini) = 1 T i=1 T y g (i) With this we can calculate the initial roll & pitch directly: Φ ini =atan2 (g 2, g 3 ) Θ ini =atan2 ( g 1, g 2 2 +g 32 ) In order to calculate the initial yaw, we have to rotate the magnetic field measurement into the world frame first: =[ cos(θ) sin (Θ)sin (Φ) sin (Θ)cos(Φ) m w 0 cos(φ) sin(φ) sin (Θ) cos(θ)sin(φ) cos(θ)cos(φ)]m b w w w =[m x m y m z ]T From this the initial yaw angle can be obtained as: Ψ ini =atan2 ( m y w, m xw )

38 QEKF Initial states The initial quaternion can be calculated as: The initial state is given as: q nb ini =euler 2 quat (Ψ,Θ,Φ) nb X ini =[ (q ini ) T T 0 1x 3 x g(ini) ] T For the sake of simplicity we set the initial state covariance to a fixed value. It will converge against true state covariance with time: P ini =100 I 10 x 10 Note: State covariance P can be also initially estimated, see reference [2]

39 QEKF σs and λ Finally, the all variances σ and the parameter λ from the gyro bias model have to be determined. The variance for the bias model as well as λ can not be measured. We can assume very low bias noise: σ 2 xg = [(rad /s 2 ) 2 ] λ is a correlation time factor and describes how fast the bias of the gyro varies. A good value for λ is: λ= [1/s] All other parameters have to be measured. σ g =? [rad /s ] σ a =? [g ] σ Ψ =? [rad]

40 Practical hints Variances σ g =? [rad /s ] σ a =?[g ] Record accelerometer and gyroscope data with a resting device, calculate variance from a few hundred samples. σ Ψ =? [rad] Record magnetometer data with a resting device and convert it into a yaw angle, calculate variance from a few hundred samples.

41 Practical hints Jacobian You can verify your Jacobians using mathematical tools, e.g. for the yaw measurement update: h yaw (X )=atan 2(2(q 0 q 3 +q 1 q 2 ),1 2 (q 2 2 +q 32 ) )

42 Practical hints Matlib 2.0 RODOS can be used with a upgraded version of support_libs/matlib.h This version can handle matrices of generic sizes:

43 References [1] Y. Bar-Shalom, X. Rong Li and T. Kirubarajan. Estimation with Applications to Tracking and Navigation [2] A. L. Schwab. Quaternions, Finite Rotation and Euler Parameters [3] R. Munguia and A. Grau. A Practical Method for Implementing an Attitude and Heading Reference System