第1页 / 共73页
第2页 / 共73页
第3页 / 共73页
第4页 / 共73页
第5页 / 共73页
第6页 / 共73页
第7页 / 共73页
第8页 / 共73页
Quaternion kinematics for the error-state KF.pdf
Quaternions and rotation operations
Definition of quaternion
Alternative representations of the quaternion
Some quaternion properties
Rotations and cross-relations
Quaternion conventions. My choice.
Frame composition
Perturbations and time-derivatives
Time-integration of rotation rates
Useful, and very useful, Jacobians of the rotation
Error-state kinematics for IMU-driven systems
Motivation
The error-state Kalman filter explained
System kinematics in continuous time
System kinematics in discrete time
Fusing IMU with complementary sensory data
Observation of the error state via filter correction
Injection of the observed error into the nominal state
ESKF reset
The ESKF using global angular errors
System kinematics in continuous time
System kinematics in discrete time
Fusing with complementary sensory data
Runge-Kutta numerical integration methods
The Euler method
The midpoint method
The RK4 method
General Runge-Kutta method
Closed-form integration methods
Integration of the angular error
Simplified IMU example
Full IMU example
Approximate methods using truncated series
System-wise truncation
Block-wise truncation
The transition matrix via Runge-Kutta integration
Error-state example
Integration of random noise and perturbations
Noise and perturbation impulses
Full IMU example
Quaternion kinematics for the error-state KF
Joan Sol`a
July 24, 2016
Contents
1 Quaternions and rotation operations
1.1 Definition of quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.2 Alternative representations of the quaternion . . . . . . . . . . . . . . . . .
1.3 Some quaternion properties
. . . . . . . . . . . . . . . . . . . . . . . . . .
1.4 Rotations and cross-relations . . . . . . . . . . . . . . . . . . . . . . . . . .
1.5 Quaternion conventions. My choice. . . . . . . . . . . . . . . . . . . . . . .
1.6 Frame composition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.7 Perturbations and time-derivatives
. . . . . . . . . . . . . . . . . . . . . .
1.8 Time-integration of rotation rates . . . . . . . . . . . . . . . . . . . . . . .
1.9 Useful, and very useful, Jacobians of the rotation . . . . . . . . . . . . . .
2 Error-state kinematics for IMU-driven systems
2.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2 The error-state Kalman filter explained . . . . . . . . . . . . . . . . . . . .
2.3 System kinematics in continuous time . . . . . . . . . . . . . . . . . . . . .
2.4 System kinematics in discrete time
. . . . . . . . . . . . . . . . . . . . . .
3 Fusing IMU with complementary sensory data
3.1 Observation of the error state via filter correction . . . . . . . . . . . . . .
3.2
Injection of the observed error into the nominal state . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.3 ESKF reset
4 The ESKF using global angular errors
4.1 System kinematics in continuous time . . . . . . . . . . . . . . . . . . . . .
4.2 System kinematics in discrete time
. . . . . . . . . . . . . . . . . . . . . .
4.3 Fusing with complementary sensory data . . . . . . . . . . . . . . . . . . .
2
2
3
4
8
17
23
24
26
29
31
31
31
32
38
40
41
43
43
45
45
47
48
1
A Runge-Kutta numerical integration methods
A.1 The Euler method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
A.2 The midpoint method . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
A.3 The RK4 method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
A.4 General Runge-Kutta method . . . . . . . . . . . . . . . . . . . . . . . . .
B Closed-form integration methods
B.1 Integration of the angular error
. . . . . . . . . . . . . . . . . . . . . . . .
B.2 Simplified IMU example . . . . . . . . . . . . . . . . . . . . . . . . . . . .
B.3 Full IMU example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
C Approximate methods using truncated series
C.1 System-wise truncation . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
C.2 Block-wise truncation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
D The transition matrix via Runge-Kutta integration
D.1 Error-state example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
E Integration of random noise and perturbations
E.1 Noise and perturbation impulses . . . . . . . . . . . . . . . . . . . . . . . .
E.2 Full IMU example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
50
52
52
52
53
54
54
55
58
61
61
63
64
65
67
69
70
1 Quaternions and rotation operations
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,
(1)
(2a)
(2b)
where {a, b, c, d} ∈ R, and {i, j, k} are three imaginary unit numbers defined so that
Q = a + bi + cj + dk ∈ H ,
from which we can derive
i2 = j2 = k2 = ijk = −1 ,
ij = −ji = k ,
jk = −kj = i ,
ki = −ik = j .
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)
2
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),
Q = bi + cj + dk ∈ I3 ⊂ H .
(4)
It is noticeable that, while regular complex numbers of unit length z = eiθ can encode
numbers” or quaternions of unit length q = e(uxi+uyj+uzk)θ/2 encode rotations in the 3D
rotations in the 2D plane (with one complex product, x = z· x), “extended complex
space (with a double quaternion product, x = 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 1.5 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 1.5 for further explanations and disambiguation.
1.2 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 = qw + qxi + qyj + qzk
⇔
Q = qw + qv ,
(5)
where qw is referred to as the real or scalar part, and qv = qxi + qyj + qzk = (qx, qy, qz) as
the imaginary or vector part.1 It can be also defined as an ordered pair scalar-vector
Q = qw, qv .
We mostly represent a quaternion Q as a 4-vector q ,
q
=
qw
qv
,
qw
qx
qy
qz
(6)
(7)
1Our 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) (more suited for algebraic interpretations) or (1, i, j, k) (for mathematical
interpretations).
3
which allows us to use matrix algebra for operations involving quaternions. At certain
occasions, we may allow ourselves to mix notations by abusing of the sign “=”. Typical
examples are real quaternions and pure quaternions,
qw
qv
qw
0v
0
qv
q = qw + qv =
,
real: qw =
,
pure: qv =
1.3 Some quaternion properties
1.3.1 Sum
The sum is straightforward,
p q =
pw
pv
qw
qv
=
pw
qw
pv qv
.
By construction, the sum is commutative and associative,
p + q = q + p
p + (q + r) = (p + q) + r .
.
(8)
(9)
(10)
(11)
Thus, the set of quaternions endowed with the sum operation form a commutative group,
where the identity is the zero quaternion, q0 = 0, and the inverse is the negative −q.
1.3.2 Product
Denoted by ⊗, the quaternion product requires using the original form (1) and the quater-
nion algebra (2). Writing the result in vector form gives
pwqx + pxqw + pyqz − pzqy
pwqy − pxqz + pyqw + pzqx
pwqz + pxqy − pyqx + pzqw
This can be posed also in terms of the scalar and vector parts,
p ⊗ q =
.
pwqw − pxqx − pyqy − pzqz
pwqw − p
v qv
pwqv + qwpv + pv×qv
,
p ⊗ q =
(12)
(13)
where the presence of the cross-product reveals that the quaternion product is not com-
mutative in the general case,
(14)
Exceptions to this general non-commutativity are limited to the cases where pv×qv = 0,
which happens whenever one quaternion is real, p = pw or q = qw, or when both vector
parts are parallel, pvqv. Only in these cases the quaternion product is commutative.
p ⊗ q = q ⊗ p .
4
[q]L = qw I +
[q]R = qw I +
0
−q
qv − [qv]×
v
0 −q
v
[qv]×
qv
[a]×
,
0 −az
az
−ay
,
ay
0 −ax
0
ax
(17)
(18)
(19)
The quaternion product is however associative,
and distributive over the sum,
(p ⊗ q) ⊗ r = p ⊗ (q ⊗ r) ,
(15)
p ⊗ (q + r) = p ⊗ q + p ⊗ r
and
(p + q) ⊗ r = p ⊗ r + q ⊗ r .
(16)
Thus, quaternions endowed with the product operation ⊗ form a (non-commutative) group.
The group’s elements identity, q1 = 1, and inverse, q−1, are explored below.
The product of two quaternions is bi-linear and can be expressed as two equivalent
matrix products, namely
q1 ⊗ q2 = [q1]L q2
and
q1 ⊗ q2 = [q2]R q1 ,
where [q1]L and [q2]R are respectively the left- and right- quaternion-product matrices,
Here, the skew operator 2 [•]× produces the cross-product matrix,
which is a skew-symmetric matrix, [a]
i.e.,
× = − [a]×, left-hand-equivalent to the cross product,
(20)
∀ a, b ∈ R3 .
[a]× b = a×b ,
Finally, since
q ⊗ r ⊗ p = [p]R [q]L r
and
q ⊗ r ⊗ p = [q]L [p]R r
we have the relation
[p]R [q]L = [q]L [p]R .
Further properties of these matrices are provided in Section 1.4.7.
(21)
(22)
2The skew-operator can be found in the literature in a number of different names and notations, either
related to the cross operator ×, or to the ‘hat’ operator ∧, so that all these forms are equivalent,
× ≡ [a×] ≡ a× ≡ a× ≡ [a] ≡a ≡ a∧ ,
[a]
5
Identity
1.3.3
The identity quaternion q 1 with respect to the product is such that q 1 ⊗ q = q⊗ q 1 = q.
It corresponds to the real product identity ‘1’ expressed as a quaternion,
1
0v
.
qw
−qv
.
q2
q 1 = 1 =
1.3.4 Conjugate
The conjugate of a quaternion is defined by
q∗ qw − qv =
This has the properties
q ⊗ q∗ = q∗
and
1.3.5 Norm
,
(23)
(24)
(25)
(26)
(27)
(28)
(29)
⊗ q = q2
w + q2
x + q2
y + q2
z =
w + q2
y + q2
z
x + q2
0v
(p ⊗ q)∗ = q∗
⊗ p∗ .
q2
w + q2
x + q2
y + q2
z .
The norm of a quaternion is defined by
q √q ⊗ q∗ = √q∗ ⊗ q =
It has the property
p ⊗ q = pq .
Inverse
1.3.6
The inverse quaternion q−1 is such that
It can be computed with
q ⊗ q−1 = q−1 ⊗ q = q 1 .
q−1 = q∗/q2 .
6
1.3.7 Unit or normalized quaternion
For unit quaternions, q = 1, and therefore
q−1 = q∗ .
(30)
commutative) group.
From (27), unit quaternions endowed with the product operation ⊗ form a (non-
When interpreting the unit quaternion as an orientation specification, or as a rotation
operator, this property implies that the inverse rotation can be accomplished with the
conjugate quaternion. Unit quaternions can always be written in the form,
cos θ
u sin θ
q =
,
(31)
where u = uxi + uyj + uzk is a unit vector and θ is a scalar.
1.3.8 Product of pure quaternions
Pure quaternions are those with null real or scalar part, Q = qv or q = [0, qv]. From (13)
we have
This implies
pv ⊗ qv = −p
v qv + pv×qv =
pv ⊗ qv − qv ⊗ pv = 2 pv×qv .
.
(32)
(33)
−p
v qv
pv×qv
1.3.9 Powers of pure quaternions
Let us define qn as the n-th power of q using the quaternion product ⊗. Then, if v is a
pure quaternion and we let v = u θ, with θ = v ∈ R and u unitary, we get from (32)
the cyclic pattern
v2 = −θ2
, v3 = −u θ3
, v4 = θ4
, v5 = u θ5
, v6 = −θ6
,
···
(34)
1.3.10 Exponential of pure quaternions
The exponential of a general quaternion is defined as the absolutely convergent series
eq
qn
k!
∈ H .
(35)
Then, the exponential of a pure quaternion v = vxi + vyj + vzk is a new quaternion
defined by the absolutely convergent series,
∞
k=0
∞
k=0
ev =
∈ H .
vn
k!
7
(36)
Letting v = u θ, with θ = v ∈ R and u unitary, and considering (34), we group the
scalar and vector terms in the series, and recognize in them, respectively, the series of cos θ
and sin θ.3 This results in
ev = eu θ = cos θ + u sin θ =
,
(37)
cos θ
u sin θ
which constitutes a beautiful extension of the Euler formula, eiθ = cos θ + i sin θ. Notice
that since ev2 = cos2 θ + sin2 θ = 1, the exponential of a pure quaternion is a unit
quaternion.
1.3.11 Exponential of general quaternions
From the non-commutativity property of the quaternion product, we cannot write for
general quaternions p and q that ep+q = epeq. However, commutativity holds when any
of the product members is a scalar, therefore,
Then, using (37) with v = u θ = qv we get
eq = eqw+qv = eqw eqv .
cosqv
qvqv sinqv
.
eq = eqw
1.3.12 Logarithm of unit quaternions
It is immediate to see that, if q = 1,
log q = log(cos θ + u sin θ) = log(eu θ) = u θ =
that is, the logarithm of a unit quaternion is a pure quaternion.
0
u θ
,
(38)
(39)
(40)
1.3.13 Logarithm of general quaternions
By extension, if q is a general quaternion,
log q = log(q
q
q
) = log q + log
q
q
= log q + u θ =
1.4 Rotations and cross-relations
1.4.1 The 3D vector rotation formula
log qu θ
.
(41)
We illustrate in Fig. 1 the rotation, following the right-hand rule, of a general 3D vector
x, by an angle φ, around the axis defined by the unit vector u. This is accomplished by
3We remind that cos θ = 1 − θ2
2! + θ4
4! − ··· and sin θ = θ − θ3
3! + θ5
5! − ··· .
8
相关推荐
2023年江西萍乡中考道德与法治真题及答案.doc
2012年重庆南川中考生物真题及答案.doc
2013年江西师范大学地理学综合及文艺理论基础考研真题.doc
2020年四川甘孜小升初语文真题及答案I卷.doc
2020年注册岩土工程师专业基础考试真题及答案.doc
2023-2024学年福建省厦门市九年级上学期数学月考试题及答案.doc
2021-2022学年辽宁省沈阳市大东区九年级上学期语文期末试题及答案.doc
2022-2023学年北京东城区初三第一学期物理期末试卷及答案.doc
2018上半年江西教师资格初中地理学科知识与教学能力真题及答案.doc
2012年河北国家公务员申论考试真题及答案-省级.doc
2020-2021学年江苏省扬州市江都区邵樊片九年级上学期数学第一次质量检测试题及答案.doc
2022下半年黑龙江教师资格证中学综合素质真题及答案.doc
