Quaternionkinematicsfortheerror-stateKalmanfilter资源-CSDN文库

error-state

卡尔曼滤波

5星 · 超过95%的资源需积分: 50 151 浏览量 2017-12-06 15:15:04 上传评论 1 收藏 997KB 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页未读，继续阅读

评论收藏

内容反馈

lishanggui

2018-12-29

很好的资源

知识收藏

粉丝: 3
资源: 1

Quaternion kinematics for the error-state Kalman filter

最新资源

Quaternion kinematics for the error-state Kalman filter

quaternion kinematics for the error-state kalman filter

Quaternion-kinematics

Quaternion kinematics for the error-state Kalman filter.pdf

Quaternion_kinematics_for_the_error-state_KF.pdf

Quaternion kinematics for the error-state KF.pdf

四元数(quaternion)的Matlab工具箱

matlab开发-quaternion

基于视觉的Constraint Kalman Filter

Combined hierarchical based recursive least squares algorithm and Kalman filter for Hammerstein state space systems

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

Kalman filter.pdf

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

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

论文研究-Quad-quaternion MUSIC for DOA Estimation Using Electromagnetic Vector-Sensors.pdf

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

numpy_quaternion-2019.12.12-cp27-cp27m-win_amd64

numpy_quaternion-2019.12.12-cp27-cp27m-win32

numpy_quaternion-2018.12.24-cp34-cp34m-win_amd64

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

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

YOLOv8-deepsort 实现智能车辆目标检测+车辆跟踪+车辆计数

YOLOv8网络结构图，自制visio文件，yolov8.vsds，需要的自取，在原有的基础上直接改就行了

yolov8(2023年8月版本),已经下好yolov8s.pt和yolov8n.pt

Transformer模型实现长期预测并可视化结果（附代码+数据集+原理介绍）

社交平台上经济类话题的文章热度信息，数据是真实的，但不是真实日期

Unet眼底血管图像分割数据集+代码+模型+系统界面+教学视频.zip

最新资源