Quaternionkinematicsfortheerror-stateKalmanfilter

error-state

卡尔曼滤波

5星 · 超过95%的资源需积分: 50 179 浏览量更新于2023-05-28 评论 1 收藏 997KB PDF 举报

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

领优惠券(最高得80元）

资源详情

资源评论

资源推荐

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

评论1

会员权益专享

最新资源

Quaternion kinematics for the error-state Kalman filter

评论1

Quaternion kinematics for the error-state Kalman filter.pdf

quaternion kinematics for the error-state kalman filter

Quaternion kinematics for the error-state KF.pdf

quaternion kinematics for the error-state kalman ?lter

Kalman1960.pdf

ros卡尔曼滤波组合导航

使用matlab编写将一个三维方向余弦矩阵转换为四元数，并对四元数进行单位化

Quaternion 引用呢

ERROR: Could not find a version that satisfies the requirement numpy-quaternion (from versions: none)

ERROR: Could not build wheels for numpy-quaternion which use PEP 517 and cannot be installed directly

AttributeError: module 'numpy' has no attribute 'quaternion'

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

ModuleNotFoundError: No module named 'quaternion'

quaternion for computer graphics pdf

orientation2rpy_msgs

from geometry_msgs.msg import Quaternion中的Quaternion类的格式

matlab axang2quat

fatal error: quaternion_Solution.h: No such file or directory

会员权益专享

最新资源