1 of 19

Aerial Robotics

State Estimation: Attitude & Heading – Extended Kalman Filter

C. Papachristos

Robotic Workers (RoboWork) Lab

University of Nevada, Reno

CS-491/691

2 of 19

Attitude & Heading Estimation

 

CS491/691 C. Papachristos

3 of 19

Attitude & Heading Estimation

 

CS491/691 C. Papachristos

 

 

 

 

Note: Heading is unobservable by Inertial Sensing alone

4 of 19

Attitude & Heading Estimation

 

CS491/691 C. Papachristos

5 of 19

Attitude & Heading Estimation

 

CS491/691 C. Papachristos

 

 

 

or:

 

 

  • Remember: From definition:

6 of 19

Attitude & Heading Estimation

 

CS491/691 C. Papachristos

7 of 19

Attitude & Heading Estimation

 

CS491/691 C. Papachristos

8 of 19

Extended Kalman Filtering

Kalman Filtering with Non-Linear Motion / Sensor models

​

  • When model is Linear:

CS491/691 C. Papachristos

  • When model is Non-Linear:

Input: Gaussian

Output: �Gaussian

Output: �Non-Gaussian

Input: Gaussian

9 of 19

Extended Kalman Filtering

Kalman Filtering with Non-Linear Motion / Sensor models

​

  • When model is Non-Linear:

CS491/691 C. Papachristos

  • Linearization outcomes may vary significantly:

Input: Gaussian

Output: �Gaussian

Approximate with a Linear Function

Note: In EKF, noise also assumed to follow zero-mean (multivariate) Gaussian

10 of 19

Extended Kalman Filtering

Kalman Filtering with Non-Linear Motion / Sensor models

​

  • Motion Model Linearization via Taylor Expansion:

​

​

​

​

​

​

​

  • Sensor Model Linearization:

CS491/691 C. Papachristos

 

 

 

Jacobian (Matrix)

 

 

 

Jacobian (Matrix)

Note:

Given a vector-valued�function:

​

Jacobian�Matrix�is:

​

​

​

A generalization of the Gradient of a scalar-valued function

11 of 19

Extended Kalman Filtering

Kalman Filtering with Non-Linear Motion / Sensor models

​

  • EKF Algorithm:

​

​

​

​

​

​

​

​

​

​

​

    • i.e. the KF algorithm, but using the Jacobian Matrices instead, as well as the non-linear Motion and Measurement model equations

CS491/691 C. Papachristos

Prediction

 

 

Correction

 

 

 

Project State Ahead:

Project Error Covariance Ahead:

Update Error Covariance:

Update Estimate with Measurement:

Compute Kalman Gain:

12 of 19

Quaternion-based EKF

 

CS491/691 C. Papachristos

 

 

 

Hamilton Product

Skew-symmetric

 

 

13 of 19

Quaternion-based EKF

 

CS491/691 C. Papachristos

 

 

 

 

 

Full Process Model:�

(closed-form in previous slide)

 

 

14 of 19

Quaternion-based EKF

 

CS491/691 C. Papachristos

 

 

 

15 of 19

Quaternion-based EKF

 

CS491/691 C. Papachristos

 

 

 

 

 

16 of 19

Quaternion-based EKF

 

CS491/691 C. Papachristos

 

 

 

 

 

 

 

 

 

 

17 of 19

Quaternion-based EKF

 

CS491/691 C. Papachristos

 

 

 

18 of 19

Quaternion-based EKF Example

 

CS491/691 C. Papachristos

19 of 19

Time for Questions !

CS-491/691

CS491/691 C. Papachristos