Quaternionkinematicsfortheerror-stateKalmanfilter.pdf资源-CSDN文库

需积分: 50 199 浏览量 2020-08-25 12:05:00 上传评论 1 收藏 1.92MB PDF 举报

资源详情

资源评论

Quaternion kinematics for the error-state Kalman ﬁlter

Joan Sol`a

October 12, 2017

Contents

1 Quaternion deﬁnition and properties 4

1.1 Deﬁnition of quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.1.1 Alternative representations of the quaternion . . . . . . . . . . . . . 5

1.2 Main quaternion properties . . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.2.1 Sum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.2.2 Product . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.2.3 Identity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.2.4 Conjugate . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.2.5 Norm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.2.6 Inverse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.2.7 Unit or normalized quaternion . . . . . . . . . . . . . . . . . . . . . 9

1.3 Additional quaternion properties . . . . . . . . . . . . . . . . . . . . . . . 9

1.3.1 Quaternion commutator . . . . . . . . . . . . . . . . . . . . . . . . 9

1.3.2 Product of pure quaternions . . . . . . . . . . . . . . . . . . . . . . 9

1.3.3 Natural powers of pure quaternions . . . . . . . . . . . . . . . . . . 10

1.3.4 Exponential of pure quaternions . . . . . . . . . . . . . . . . . . . . 10

1.3.5 Exponential of general quaternions . . . . . . . . . . . . . . . . . . 11

1.3.6 Logarithm of unit quaternions . . . . . . . . . . . . . . . . . . . . . 11

1.3.7 Logarithm of general quaternions . . . . . . . . . . . . . . . . . . . 11

1.3.8 Exponential forms of the type q

. . . . . . . . . . . . . . . . . . . 11

2 Rotations and cross-relations 12

2.1 The 3D vector rotation formula . . . . . . . . . . . . . . . . . . . . . . . . 12

2.2 The rotation group SO(3) . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.3 The rotation group and the rotation matrix . . . . . . . . . . . . . . . . . 15

2.3.1 The exponential map . . . . . . . . . . . . . . . . . . . . . . . . . . 16

2.3.2 The capitalized exponential map . . . . . . . . . . . . . . . . . . . 17

2.3.3 Rotation matrix and rotation vector: the Rodrigues rotation formula 18

2.3.4 The logarithmic maps . . . . . . . . . . . . . . . . . . . . . . . . . 18

2.3.5 The rotation action . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

2.4 The rotation group and the quaternion . . . . . . . . . . . . . . . . . . . . 19

2.4.1 The exponential map . . . . . . . . . . . . . . . . . . . . . . . . . . 20

2.4.2 The capitalized exponential map . . . . . . . . . . . . . . . . . . . 21

2.4.3 Quaternion and rotation vector . . . . . . . . . . . . . . . . . . . . 22

2.4.4 The logarithmic maps . . . . . . . . . . . . . . . . . . . . . . . . . 22

2.4.5 The rotation action . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

2.4.6 The double cover of the manifold of SO(3). . . . . . . . . . . . . . 23

2.5 Rotation matrix and quaternion . . . . . . . . . . . . . . . . . . . . . . . . 24

2.6 Rotation composition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

2.7 Spherical linear interpolation (SLERP) . . . . . . . . . . . . . . . . . . . . 27

2.8 Quaternion and isoclinic rotations: explaining the magic . . . . . . . . . . 29

3 Quaternion conventions. My choice. 33

3.1 Quaternion ﬂavors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

3.1.1 Order of the quaternion components . . . . . . . . . . . . . . . . . 35

3.1.2 Speciﬁcation of the quaternion algebra . . . . . . . . . . . . . . . . 35

3.1.3 Function of the rotation operator . . . . . . . . . . . . . . . . . . . 36

3.1.4 Direction of the rotation operator . . . . . . . . . . . . . . . . . . . 37

4 Perturbations, derivatives and integrals 38

4.1 The additive and subtractive operators in SO(3) . . . . . . . . . . . . . . . 38

4.2 The four possible derivative deﬁnitions . . . . . . . . . . . . . . . . . . . . 38

4.2.1 Functions from vector space to vector space . . . . . . . . . . . . . 38

4.2.2 Functions from SO(3) to SO(3) . . . . . . . . . . . . . . . . . . . . 39

4.2.3 Functions from vector space to SO(3) . . . . . . . . . . . . . . . . . 39

4.2.4 Functions from SO(3) to vector space . . . . . . . . . . . . . . . . . 39

4.3 Useful, and very useful, Jacobians of the rotation . . . . . . . . . . . . . . 40

4.3.1 Jacobian with respect to the vector . . . . . . . . . . . . . . . . . . 40

4.3.2 Jacobian with respect to the quaternion . . . . . . . . . . . . . . . 40

4.3.3 Right Jacobian of SO(3) . . . . . . . . . . . . . . . . . . . . . . . 41

4.3.4 Jacobian with respect to the rotation vector . . . . . . . . . . . . . 42

4.4 Perturbations, uncertainties, noise . . . . . . . . . . . . . . . . . . . . . . . 43

4.4.1 Local perturbations . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

4.4.2 Global perturbations . . . . . . . . . . . . . . . . . . . . . . . . . . 43

4.5 Time derivatives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

4.5.1 Global-to-local relations . . . . . . . . . . . . . . . . . . . . . . . . 45

4.5.2 Time-derivative of the quaternion product . . . . . . . . . . . . . . 46

4.5.3 Other useful expressions with the derivative . . . . . . . . . . . . . 46

4.6 Time-integration of rotation rates . . . . . . . . . . . . . . . . . . . . . . . 46

4.6.1 Zeroth order integration . . . . . . . . . . . . . . . . . . . . . . . . 47

4.6.2 First order integration . . . . . . . . . . . . . . . . . . . . . . . . . 48

5 Error-state kinematics for IMU-driven systems 50

5.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

5.2 The error-state Kalman ﬁlter explained . . . . . . . . . . . . . . . . . . . . 51

5.3 System kinematics in continuous time . . . . . . . . . . . . . . . . . . . . . 52

5.3.1 The true-state kinematics . . . . . . . . . . . . . . . . . . . . . . . 53

5.3.2 The nominal-state kinematics . . . . . . . . . . . . . . . . . . . . . 54

5.3.3 The error-state kinematics . . . . . . . . . . . . . . . . . . . . . . . 54

5.4 System kinematics in discrete time . . . . . . . . . . . . . . . . . . . . . . 57

5.4.1 The nominal state kinematics . . . . . . . . . . . . . . . . . . . . . 58

5.4.2 The error-state kinematics . . . . . . . . . . . . . . . . . . . . . . . 58

5.4.3 The error-state Jacobian and perturbation matrices . . . . . . . . . 59

6 Fusing IMU with complementary sensory data 60

6.1 Observation of the error state via ﬁlter correction . . . . . . . . . . . . . . 60

6.1.1 Jacobian computation for the ﬁlter correction . . . . . . . . . . . . 61

6.2 Injection of the observed error into the nominal state . . . . . . . . . . . . 62

6.3 ESKF reset . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

6.3.1 Jacobian of the reset operation with respect to the orientation error 63

7 The ESKF using global angular errors 64

7.1 System kinematics in continuous time . . . . . . . . . . . . . . . . . . . . . 65

7.1.1 The true- and nominal-state kinematics . . . . . . . . . . . . . . . . 65

7.1.2 The error-state kinematics . . . . . . . . . . . . . . . . . . . . . . . 65

7.2 System kinematics in discrete time . . . . . . . . . . . . . . . . . . . . . . 67

7.2.1 The nominal state . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

7.2.2 The error state . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

7.2.3 The error state Jacobian and perturbation matrices . . . . . . . . . 67

7.3 Fusing with complementary sensory data . . . . . . . . . . . . . . . . . . . 68

7.3.1 Error state observation . . . . . . . . . . . . . . . . . . . . . . . . . 68

7.3.2 Injection of the observed error into the nominal state . . . . . . . . 69

7.3.3 ESKF reset . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

A Runge-Kutta numerical integration methods 70

A.1 The Euler method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

A.2 The midpoint method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

A.3 The RK4 method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

A.4 General Runge-Kutta method . . . . . . . . . . . . . . . . . . . . . . . . . 72

B Closed-form integration methods 73

B.1 Integration of the angular error . . . . . . . . . . . . . . . . . . . . . . . . 74

B.2 Simpliﬁed IMU example . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74

B.3 Full IMU example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78

It is noticeable that, while regular complex numbers of unit length z = e

iθ

can encode

rotations in the 2D plane (with one complex product, x

= z ·x), “extended complex

numbers” or quaternions of unit length q = e

i+u

j+u

k)θ/2

encode rotations in the 3D

space (with a double quaternion product, x

= q ⊗ x ⊗ q

∗

, as we explain later in this

document).

CAUTION: Not all quaternion deﬁnitions are the same. Some authors write the

products as ib instead of bi, and therefore they get the property k = ji = −ij, which results

in ijk = 1 and a left-handed quaternion. Also, many authors place the real part at the end

position, yielding Q = ia+jb+kc+d. These choices have no fundamental implications but

make the whole formulation diﬀerent in the details. Please refer to Section 3 for further

explanations and disambiguation.

CAUTION: There are additional conventions that also make the formulation diﬀerent

in details. They concern the “meaning” or “interpretation” we give to the rotation op-

erators, either rotating vectors or rotating reference frames –which, essentially, constitute

opposite operations. Refer also to Section 3 for further explanations and disambiguation.

NOTE: Among the diﬀerent conventions exposed above, this document concentrates on

the Hamilton convention, whose most remarkable property is the deﬁnition (2). A proper

and grounded disambiguation requires to ﬁrst develop a signiﬁcant amount of material;

therefore, this disambiguation is relegated to the aforementioned Section 3.

1.1.1 Alternative representations of the quaternion

The real + imaginary notation {1, i, j, k} is not always convenient for our purposes. Pro-

vided that the algebra (2) is used, a quaternion can be posed as a sum scalar + vector,

Q = q

+ q

i + q

j + q

k ⇔ Q = q

+ q

, (5)

where q

is referred to as the real or scalar part, and q

= q

i + q

j + q

k = (q

, q

) as

the imaginary or vector part.

It can be also deﬁned as an ordered pair scalar-vector

Q = hq

, q

i . (6)

We mostly represent a quaternion Q as a 4-vector q ,

q ,

















, (7)

Our choice for the (w, x, y, z) subscripts notation comes from the fact that we are interested in the

geometric properties of the quaternion in the 3D Cartesian space. Other texts often use alternative

subscripts such as (0, 1, 2, 3) or (1, i, j, k), perhaps better suited for mathematical interpretations.

双四元数乘积

剩余92页未读，继续阅读

评论收藏

内容反馈

Quaternion kinematics for the error-state Kalman filter.pdf

评论0

最新资源

Quaternion kinematics for the error-state Kalman filter.pdf

评论0

最新资源

相关推荐

Quaternion kinematics for the error-state Kalman filter

quaternion kinematics for the error-state kalman filter

Quaternion_kinematics_for_the_error-state_KF.pdf

Kalman filter.pdf

Quaternion kinematics for the error-state KF.pdf

基于视觉的Constraint Kalman Filter

无人驾驶算法学习：多传感器融合MSF算法

epdf.pub_quaternions-for-computer-graphics.pdf.zip

Arduino-PyTeapot-Quaternion-Euler-cube-rotation.zip

Python库 | numba_quaternion-0.2.0-py3-none-any.whl

numpy_quaternion-2019.12.12-cp27-cp27m-win_amd64

numpy_quaternion-2021.7.28.16.11.54-cp38-cp38-win_amd64

numpy_quaternion-2021.7.28.16.11.54-cp39-cp39-win32

PyPI 官网下载 | numba_quaternion-0.2.0-py3-none-any.whl

numpy_quaternion-2021.7.28.16.11.54-cp37-cp37m-win_amd64

numpy_quaternion-2019.12.12-cp27-cp27m-win32

Python库 | numpy_quaternion-2022.3.1-cp38-cp38-win_amd64.whl

国家开放大学计算机应用基础终结性考试（大作业）

离散数学知识点整理（超级全面详细！）

《科研伦理与学术规范》期末考试文档2（40题）

Word2Recite 桌面单词

2021全国及分省市县行政区划矢量图层shp文件.rar

Revit 各版本官方族库及项目样板下载和安装方法，2016-2021族库离线包下载.rar

38000词汇思维导图（1-50词根）β版.rar

博士“申请-考核制”面试——英文提问问题/答案模板

Zotero及常用插件