1 of 15

Aerial Robotics

State Estimation: Quaternion EKF & SE(3) Optimization

C. Papachristos

Robotic Workers (RoboWork) Lab

University of Nevada, Reno

CS-491/691

2 of 15

Quaternion-based Attitude & Heading Estimation

 

CS491/691 C. Papachristos

 

 

 

Hamilton Product

Skew-symmetric

 

3 of 15

Quaternion-based Attitude & Heading Estimation

 

CS491/691 C. Papachristos

 

 

 

 

 

Full Process Model:

 

4 of 15

Quaternion-based Attitude & Heading Estimation

 

CS491/691 C. Papachristos

 

 

 

5 of 15

Quaternion-based Attitude & Heading Estimation

 

CS491/691 C. Papachristos

 

 

 

 

 

6 of 15

Quaternion-based Attitude & Heading Estimation

 

CS491/691 C. Papachristos

 

 

 

 

 

 

 

 

 

 

7 of 15

Quaternion-based Attitude & Heading Estimation

 

CS491/691 C. Papachristos

 

 

 

8 of 15

Remember: 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:

9 of 15

Remember: Quaternion-based EKF Example

 

CS491/691 C. Papachristos

10 of 15

Remember: Graph SLAM

 

 

 

 

 

 

 

 

C. Papachristos

 

  • Transition Constraints:

 

 

Solve:

 

By solving (linear) system:

 

11 of 15

SE(3) Optimization

 

 

 

 

 

 

 

 

 

 

 

 

Mapping function-&-its-inverse of a Differentiable Manifold onto another both smooth

Group Isomorphism: Function b/w 2 groups that sets up a one-to-one correspondence of elements in a way that respects the group operations

CS491/691 C. Papachristos

12 of 15

SE(3) Optimization

 

 

 

 

 

 

 

 

 

 

Lie Bracket operator: The derivative of vector field B� along flow of vector field A

CS491/691 C. Papachristos

13 of 15

SE(3) Optimization

 

 

 

 

 

 

 

 

Note:

Also a Quaternion-form mapping exists

 

 

 

 

 

 

CS491/691 C. Papachristos

14 of 15

SE(3) Optimization

 

 

 

 

 

 

 

 

 

 

 

  • through small increments:

 

  • obtained by solving:

 

 

 

 

 

Exponential map of Lie Group

Group Product operation

 

C. Papachristos

15 of 15

CPE491/691 C. Papachristos

Time for Questions !

CS-491/691