四元数运动学在误差状态卡尔曼滤波中的应用

需积分: 13 198 浏览量更新于2024-07-15 收藏 944KB PDF 举报

"Quaternion kinematics for the error-state KF.pdf" 这篇文档主要探讨了四元数在误差状态卡尔曼滤波（Error-State Kalman Filter, ESKF）中的应用，特别是针对惯性测量单元（IMU）的数据处理。卡尔曼滤波是一种广泛应用在传感器融合和估计理论中的算法，用于从噪声数据中提取最精确的状态估计。在IMU数据融合中，四元数由于其数学上的优势，如避免万向锁问题和保持旋转连续性，常被用来表示姿态信息。四元数是一种扩展的复数形式，特别适合处理三维空间中的旋转。在运动学中，四元数描述了物体随时间的旋转变化。在误差状态卡尔曼滤波框架下，四元数的动态模型被用来描述系统误差如何随着时间演变。文章可能涵盖了以下几点： 1. 四元数基础：首先，文档可能介绍了四元数的基本概念，包括实部和虚部，以及它们如何组合成一个四元数来表示三维旋转。 2. 四元数与旋转：四元数的乘法定义了一个非线性的旋转操作，这使得它们在表示和计算连续旋转时非常有效，且避免了欧拉角表示中可能出现的万向锁问题。 3. 四元数的误差状态表示：在ESKF中，状态通常包含系统的实际值和估计误差。文档可能详细解释了如何将四元数误差纳入到这种表示中，以便在滤波过程中处理姿态误差。 4. IMU动力学模型：文档可能会涵盖IMU测量的物理背景，如加速度和角速度，以及如何将这些测量值转化为四元数更新的公式。 5. 递推方程：描述了误差如何随着时间推移而变化的微分方程，这是卡尔曼滤波器设计的关键部分。 6. 数据融合：文档可能讨论了如何将IMU数据与其他传感器（如GPS）的数据融合，以提高系统估计的精度和鲁棒性。 7. 卡尔曼滤波实现：最后，文档可能会涉及ESKF的具体实现细节，包括状态转移矩阵、测量矩阵和协方差矩阵的构建。作者Joan Solà在机器人和工业信息学研究所工作，参与了CargoANTS和PICAS$O等项目，他在传感器融合和四元数动力学方面的研究具有较高的引用量，表明该文档内容的专业性和影响力。这篇文档为理解如何在误差状态卡尔曼滤波中利用四元数处理IMU数据提供了深入的理论和实践指导，对于从事视觉惯性导航系统（Visual-Inertial Odometry, VIO）或其他需要姿态估计和传感器融合的应用开发者来说，是非常有价值的参考资料。

1.4.4 Rotation matrix and quaternion

Unit quaternions act as rotation operators in a way somewhat similar to rotation matrices,

= q ⊗ x ⊗ q

∗

, x

= R x .

Here, we used the bar notation x to indicate that the vector x in the left expression is

expressed in quaternion form (47), thus diﬀerentiating it from that on the right. However,

this circumstance is mostly unambiguous and can be derived from the context, and espe-

cially by the presence of the quaternion product ⊗. In what is to follow, we are omitting

this bar and writing simply, x

= q ⊗x ⊗q

∗

, which allows us to abuse of the notation and

write

q ⊗ x ⊗q

∗

= R x . (58)

As both sides of this identity are linear in x, an expression of the rotation matrix equivalent

to the quaternion is found by developing the left hand side and identifying terms on the

right, yielding

R =





+ q

− q

2(q

− q

) 2(q

+ q

)

2(q

+ q

) q

− q

+ q

− q

2(q

− q

)

2(q

− q

) 2(q

+ q

) q

− q

+ q





, (59)

denoted throughout this document by R = R{q}. The rotation matrix R has the following

properties with respect to the quaternion,

R{[1, 0, 0, 0]

} = I (60)

R{−q} = R{q} (61)

R{q

∗

} = R{q}

(62)

R{q

⊗ q

} = R{q

}R{q

} , (63)

where we observe that: (60) the identity quaternion encodes the null rotation; (61) a

quaternion and its negative encode the same rotation; (62) the conjugate quaternion en-

codes the inverse rotation; and (63) the quaternion product composes consecutive rotations

in the same order as rotation matrices do.

The matrix form of the quaternion product (14–15) provides us with an alternative

formula for the rotation matrix, since

q ⊗ x ⊗q

∗

= Q

−

∗

) Q

(q)







R x



, (64)

which leads after some easy developments to

R = (q

− q

) I + 2 q

+ 2 [q

]

. (65)

The proper expressions would be, q ⊗ x ⊗ q

∗

= Rx , or q ⊗





⊗ q

∗





. See Section 1.2.

1.5 Quaternion conventions. My choice.

There are several ways to determine the quaternion. They are basically related to four

binary choices:

• The order of its elements — real part ﬁrst or last:

q =





vs. q =





. (66)

• The multiplication formula — deﬁnition of the quaternion algebra:

ij = −ji = k vs. ji = −ij = k , (67a)

which correspond to diﬀerent handedness, respectively:

right-handed vs. left-handed. (67b)

• The function of the rotation operator — rotating frames or rotating vectors:

Passive vs. Active. (68)

• In the passive case, the direction of the operation — local-to-global or global-to-local:

global

= q ⊗ x

local

⊗ q

∗

vs. x

local

= q ⊗ x

global

⊗ q

∗

(69)

This variety of choices leads to 12 diﬀerent combinations. Historical developments have

favored some conventions over others (Chou, 1992; Yazell, 2009). Today, in the available

literature, we ﬁnd many quaternion ﬂavors such as the Hamilton, the STS

, the JPL

, the

ISS

, the ESA

, the Engineering, the Robotics, and possibly a lot more denominations.

Many of these forms might be identical, others not, but this fact is rarely explicitly stated,

and many works simply lack a suﬃcient description of their quaternion with regard to the

four choices above.

The two most commonly used conventions, which are also the best documented, are

Hamilton (the options on the left in (66–69)) and JPL (the options on the right, with the

exception of (68)). Table 1 shows a summary of their characteristics. JPL is mostly used

in the aerospace domain, while Hamilton is more common to other engineering areas such

as robotics —though this should not be taken as a rule.

My choice is to take the Hamilton convention, which is right-handed and coincides with

many software libraries of widespread use in robotics, such as Eigen, ROS, Google Ceres,

and with a vast amount of literature on Kalman ﬁltering for attitude estimation using

Space Transportation System, commonly known as NASA’s Space Shuttle.

Jet Propulsion Laboratory.

International Space Station.

European Space Agency.

Table 1: Hamilton vs. JPL quaternion conventions with respect to the 4 binary choices

Quaternion type Hamilton JPL

1 Components order (q

, q

) (q

, q

)

Algebra ij = k ij = −k

Handedness Right-handed Left-handed

3 Function Passive Passive

Right-to-left products mean

Local-to-Global Global-to-Local

Default notation, q q , q

q , q

Default operation x

= q ⊗ x

⊗ q

∗

= q ⊗ x

⊗ q

∗

IMUs (Chou, 1992; Kuipers, 1999; Pini´es et al., 2007; Roussillon et al., 2011; Martinelli,

2012, and many others).

The JPL convention is possibly less commonly used, at least in the robotics ﬁeld. It

is extensively described in (Trawny and Roumeliotis, 2005), a reference work that has an

aim and scope very close to the present one, but that concentrates exclusively in the JPL

convention. The JPL quaternion is used in the JPL literature (obviously) and in key papers

by Li, Mourikis, Roumeliotis, and colleagues (see e.g. (Li and Mourikis, 2012; Li et al.,

2014)), which draw from Trawny and Roumeliotis’ document. These works are a primary

source of inspiration when dealing with visual-inertial odometry and SLAM —which is

what we do.

In the rest of this section we analyze these two quaternion conventions with a little

more depth.

1.5.1 Order of the quaternion components

Though not the most fundamental, the most salient diﬀerence between Hamilton and JPL

quaternions is in the order of the components, with the scalar part being either in ﬁrst

(Hamilton) or last (JPL) position. The implications of such change are quite obvious and

should not represent a great challenge of interpretation. In fact, some works with the

quaternion’s real component at the end (e.g., the C++ library Eigen) are still considered

as using the Hamilton notation, as long as the other three aspects are maintained.

We have used the subscripts (w, x, y, z) for the quaternion components for increased

clarity, instead of the other commonly used (0, 1, 2, 3). When changing the order, q

will

always denote the real part, while it is not clear whether q

would also do —in some

occasions, one might ﬁnd things such as q = (q

, q

), with q

real and last, but in

the general case of q = (q

, q

), the real part at the end would be q

When passing

from one convention to the other, we must be careful of formulas involving full 4 × 4 or

四元数运动学在误差状态卡尔曼滤波中的应用

机器人运动姿态分析基本知识

Quaternion kinematics for the error-state Kalman filter

Quaternion kinematics for the error-state KF.pdf

利用c语言实现四元数乘法Quaternion Quaternion_Mult( Quaternion quat_a , Quaternion quat_b );

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

quaternion_from_euler函数的用法

quaternion kinematics for the error-state kalman ?lter

利用c语言实现Quaternion Quaternion_Integral_Runge1( Quaternion quat , vector3_float delta_angle0 );四元数积分

最新资源