没有合适的资源?快使用搜索试试~ 我知道了~
首页Quaternion kinematics for the error-state Kalman filter.pdf
Quaternion kinematics for the error-state Kalman filter.pdf
需积分: 50 28 下载量 132 浏览量
更新于2023-03-03
评论 1
收藏 1.92MB PDF 举报
Quaternion kinematics for the error-state Kalman filter.pdf Quaternion kinematics for the error-state Kalman filter.pdf Quaternion kinematics for the error-state Kalman filter.pdf Quaternion kinematics for the error-state Kalman filter.pdf
资源详情
资源评论
资源推荐
Quaternion kinematics for the error-state Kalman filter
Joan Sol`a
October 12, 2017
Contents
1 Quaternion definition and properties 4
1.1 Definition 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
t
. . . . . . . . . . . . . . . . . . . 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
1
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 flavors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.1.1 Order of the quaternion components . . . . . . . . . . . . . . . . . 35
3.1.2 Specification 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 definitions . . . . . . . . . . . . . . . . . . . . 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
2
5 Error-state kinematics for IMU-driven systems 50
5.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
5.2 The error-state Kalman filter 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 filter correction . . . . . . . . . . . . . . 60
6.1.1 Jacobian computation for the filter 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 Simplified IMU example . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
B.3 Full IMU example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
3
C Approximate methods using truncated series 80
C.1 System-wise truncation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
C.1.1 First order truncation: the finite differences method . . . . . . . . . 81
C.1.2 N-th order truncation . . . . . . . . . . . . . . . . . . . . . . . . . . 82
C.2 Block-wise truncation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
D The transition matrix via Runge-Kutta integration 84
D.1 Error-state example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
E Integration of random noise and perturbations 86
E.1 Noise and perturbation impulses . . . . . . . . . . . . . . . . . . . . . . . . 89
E.2 Full IMU example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
E.2.1 Noise and perturbation impulses . . . . . . . . . . . . . . . . . . . . 90
1 Quaternion definition and properties
1.1 Definition of quaternion
One introduction to the quaternion that I find 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 defining k , ij yields a number in the space of quaternions
H,
Q = a + bi + cj + dk ∈ H , (1)
where {a, b, c, d} ∈ R, and {i, j, k} are three imaginary unit numbers defined so that
i
2
= j
2
= k
2
= 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 definition, 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 define numbers in the tri-dimensional
imaginary subspace of H. We refer to them as pure quaternions, and may note H
p
= Im(H)
the space of pure quaternions,
Q = bi + cj + dk ∈ H
p
⊂ H . (4)
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
0
= z ·x), “extended complex
numbers” or quaternions of unit length q = e
(u
x
i+u
y
j+u
z
k)θ/2
encode rotations in the 3D
space (with a double quaternion product, x
0
= q ⊗ x ⊗ q
∗
, as we explain later in this
document).
CAUTION: Not all quaternion definitions 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 different in the details. Please refer to Section 3 for further
explanations and disambiguation.
CAUTION: There are additional conventions that also make the formulation different
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 different conventions exposed above, this document concentrates on
the Hamilton convention, whose most remarkable property is the definition (2). A proper
and grounded disambiguation requires to first develop a significant 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
w
+ q
x
i + q
y
j + q
z
k ⇔ Q = q
w
+ q
v
, (5)
where q
w
is referred to as the real or scalar part, and q
v
= q
x
i + q
y
j + q
z
k = (q
x
, q
y
, q
z
) as
the imaginary or vector part.
1
It can be also defined as an ordered pair scalar-vector
Q = hq
w
, q
v
i . (6)
We mostly represent a quaternion Q as a 4-vector q ,
q ,
q
w
q
v
=
q
w
q
x
q
y
q
z
, (7)
1
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.
5
双四元数乘积
剩余92页未读,继续阅读
白菜888
- 粉丝: 7
- 资源: 75
上传资源 快速赚钱
- 我的内容管理 收起
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
会员权益专享
最新资源
- ExcelVBA中的Range和Cells用法说明.pdf
- 基于单片机的电梯控制模型设计.doc
- 主成分分析和因子分析.pptx
- 共享笔记服务系统论文.doc
- 基于数据治理体系的数据中台实践分享.pptx
- 变压器的铭牌和额定值.pptx
- 计算机网络课程设计报告--用winsock设计Ping应用程序.doc
- 高电压技术课件:第03章 液体和固体介质的电气特性.pdf
- Oracle商务智能精华介绍.pptx
- 基于单片机的输液滴速控制系统设计文档.doc
- dw考试题 5套.pdf
- 学生档案管理系统详细设计说明书.doc
- 操作系统PPT课件.pptx
- 智慧路边停车管理系统方案.pptx
- 【企业内控系列】企业内部控制之人力资源管理控制(17页).doc
- 温度传感器分类与特点.pptx
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功
评论0