Quaternionkinematicsfortheerror-stateKalmanfilter.pdf_Kalman滤波中的微小旋转误差

四元数

需积分: 50 637 浏览量更新于2023-05-24 评论 1 收藏 1.92MB 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页未读，继续阅读

白菜888

粉丝: 7
资源: 76

会员权益专享

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

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

提供基于C++的卫星/微惯性组合导航系统示例代码

quaternion for computer graphics pdf

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

unity quaternion.identity

用c语言写个姿态解算的代码

unity第三人称相机跟随鼠标旋转视角，开始隐藏鼠标，按Alt显示鼠标，并跟随玩家

请用在ROS系统中用写出四轮差速机器人的纯跟踪算法

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

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

cesium primitive立方体沿着y轴旋转 90 度代码

unity quaternion.lookrotation

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

unity quaternion.euler

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

会员权益专享

最新资源