quaternionkinematicsfortheerror-statekalmanfilter-机器学习文档类

需积分: 10 72 浏览量更新于2023-05-29 评论收藏 1.02MB PDF 举报

身份认证购VIP最低享 7 折!

领优惠券(最高得80元）

资源详情

资源评论

资源推荐

Quaternion kinematics for the error-state Kalman ﬁlter

Joan Sol`a

November 8, 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 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

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 . . . . . . . . . . . . . . . . . . . 12

1.3.8 Exponential forms of the type q

. . . . . . . . . . . . . . . . . . . 12

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 . . . . . . . . . . . . . . . . . 14

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

2.3.2 The capitalized exponential map . . . . . . . . . . . . . . . . . . . 18

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

2.3.4 The logarithmic maps . . . . . . . . . . . . . . . . . . . . . . . . . 19

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

arXiv:1711.02508v1 [cs.RO] 3 Nov 2017

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

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). . . . . . . . . . . . . . 24

2.5 Rotation matrix and quaternion . . . . . . . . . . . . . . . . . . . . . . . . 25

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 . . . . . . . . . . . . . . . . 36

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 . . . . . . . . . . . . . . . . . . . . 39

4.2.1 Functions from vector space to vector space . . . . . . . . . . . . . 39

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

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

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

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 . . . . . . . . . . . . . . . 41

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

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

4.3.5 Jacobians of the rotation composition . . . . . . . . . . . . . . . . . 43

4.4 Perturbations, uncertainties, noise . . . . . . . . . . . . . . . . . . . . . . . 44

4.4.1 Local perturbations . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

4.4.2 Global perturbations . . . . . . . . . . . . . . . . . . . . . . . . . . 45

4.5 Time derivatives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

4.5.1 Global-to-local relations . . . . . . . . . . . . . . . . . . . . . . . . 47

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

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

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

4.6.1 Zeroth order integration . . . . . . . . . . . . . . . . . . . . . . . . 48

4.6.2 First order integration . . . . . . . . . . . . . . . . . . . . . . . . . 50

5 Error-state kinematics for IMU-driven systems 52

5.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

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

5.3 System kinematics in continuous time . . . . . . . . . . . . . . . . . . . . . 53

5.3.1 The true-state kinematics . . . . . . . . . . . . . . . . . . . . . . . 54

5.3.2 The nominal-state kinematics . . . . . . . . . . . . . . . . . . . . . 56

5.3.3 The error-state kinematics . . . . . . . . . . . . . . . . . . . . . . . 56

5.4 System kinematics in discrete time . . . . . . . . . . . . . . . . . . . . . . 59

5.4.1 The nominal state kinematics . . . . . . . . . . . . . . . . . . . . . 60

5.4.2 The error-state kinematics . . . . . . . . . . . . . . . . . . . . . . . 60

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

6 Fusing IMU with complementary sensory data 62

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

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

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

6.3 ESKF reset . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

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

7 The ESKF using global angular errors 66

7.1 System kinematics in continuous time . . . . . . . . . . . . . . . . . . . . . 66

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

7.1.2 The error-state kinematics . . . . . . . . . . . . . . . . . . . . . . . 67

7.2 System kinematics in discrete time . . . . . . . . . . . . . . . . . . . . . . 69

7.2.1 The nominal state . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

7.2.2 The error state . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

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

7.3 Fusing with complementary sensory data . . . . . . . . . . . . . . . . . . . 70

7.3.1 Error state observation . . . . . . . . . . . . . . . . . . . . . . . . . 70

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

7.3.3 ESKF reset . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

A Runge-Kutta numerical integration methods 72

A.1 The Euler method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73

A.2 The midpoint method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73

A.3 The RK4 method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74

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

B Closed-form integration methods 75

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

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

B.3 Full IMU example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80

C Approximate methods using truncated series 82

C.1 System-wise truncation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83

C.1.1 First order truncation: the ﬁnite diﬀerences method . . . . . . . . . 83

C.1.2 N-th order truncation . . . . . . . . . . . . . . . . . . . . . . . . . . 84

C.2 Block-wise truncation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84

D The transition matrix via Runge-Kutta integration 86

D.1 Error-state example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87

E Integration of random noise and perturbations 88

E.1 Noise and perturbation impulses . . . . . . . . . . . . . . . . . . . . . . . . 91

E.2 Full IMU example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91

E.2.1 Noise and perturbation impulses . . . . . . . . . . . . . . . . . . . . 92

Abstract

This article is an exhaustive revision of concepts and formulas related to quater-

nions and rotations in 3D space, and their proper use in estimation engines such as

the error-state Kalman ﬁlter.

The paper includes an in-depth study of the rotation group and its Lie structure,

with formulations using both quaternions and rotation matrices. It makes special

attention in the deﬁnition of rotation perturbations, derivatives and integrals. It

provides numerous intuitions and geometrical interpretations to help the reader grasp

the inner mechanisms of 3D rotation.

The whole material is used to devise precise formulations for error-state Kalman

ﬁlters suited for real applications using integration of signals from an inertial mea-

surement unit (IMU).

1 Quaternion deﬁnition and properties

1.1 Deﬁnition of quaternion

One introduction to the quaternion that I ﬁnd particularly attractive is given by the Cayley-

Dickson construction: If we have two complex numbers A = a + bi and C = c + di, then

constructing Q = A + Cj and deﬁning k , ij yields a number in the space of quaternions

Q = a + bi + cj + dk ∈ H , (1)

where {a, b, c, d} ∈ R, and {i, j, k} are three imaginary unit numbers deﬁned so that

= j

= k

= ijk = −1 , (2a)

from which we can derive

ij = −ji = k , jk = −kj = i , ki = −ik = j . (2b)

From (1) we see that we can embed complex numbers, and thus real and imaginary num-

bers, in the quaternion deﬁnition, in the sense that real, imaginary and complex numbers

are indeed quaternions,

Q = a ∈ R ⊂ H , Q = bi ∈ I ⊂ H , Q = a + bi ∈ Z ⊂ H . (3)

Likewise, and for the sake of completeness, we may deﬁne numbers in the tri-dimensional

imaginary subspace of H. We refer to them as pure quaternions, and may note H

= Im(H)

the space of pure quaternions,

Q = bi + cj + dk ∈ H

⊂ H . (4)

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)

剩余94页未读，继续阅读

qq_39556199

粉丝: 0
资源: 1

会员权益专享

quaternion kinematics for the error-state kalman filter

评论0

会员权益专享

最新资源

quaternion kinematics for the error-state kalman filter

评论0

Quaternion kinematics for the error-state Kalman filter.pdf

Vector3 direction = obj2.transform.position - obj1.transform.position; obj1.transform.rotation = Quaternion.LookRotation(-direction);解释一下

quaternion for computer graphics pdf

unity quaternion.identity

unity Quaternion转欧拉角

Quaternion.LookRotation 怎么使用

unity quaternion.lookrotation

unity quaternion.euler

quaternion.euler用法

解释一下这段代码：“rpy = tf.transformations.euler_from_quaternion([quat.x, quat.y, quat.z, quat.w]) # 四元数转欧拉角 rpy_arc = [0, 0, 0] for i in range(len(rpy)): #弧度转角度 rpy_arc[i] = rpy[i] / math.pi * 180 ”

Mathematics in Computing

unity 如何在代码中让一个物体的-X轴指向另一个物体

quaternion.lookrotation

基于EKF的激光雷达与IMU融合MATLAB仿真程序

写一段根据imu传感器读数来控制gazebo中立方体运动的程序的C++代码

unity 如何在代码中让一个物体的-Z轴指向另一个物体

unity 四元数让物体顺时针旋转以及逆时针旋转

角度如何转为geometry_msgs::Quaternion类型的四元数

会员权益专享

最新资源