9-axis sensor计算角位移 调研资料

#基础知识

了解角速度及角加速度的相关知识和公式，搜索：刚体运动的描述

卡尔曼滤波参考资料，搜索：
基于陀螺仪及加速度计信号融合的姿态角度测量.pdf
卡尔曼滤波学习及应用.pdf

http://www.docin.com/p-270671133.html
基于MEMS加速度传感器的空间运动轨迹系统设计与实现

如何用MCU设计可穿戴电子产品
http://www.eet-china.com/ART_8800704371_865371_TA_3fd1ad3a_3.HTM

 

 

#手势识别 调研

模式识别
pattern recognization

----------------------------------------------------------------------------------
指环指势指令的输入方式
1. 识别动态平面轨迹，类似鼠标手势识别
2. 识别静态手指姿态

平面轨迹的手势识别，要么利用Android GestureDetector(触摸屏手势)，要么完全自己实现识别算法

----------------------------------------------------------------------------------
识别率

隐马尔可夫模型，人工智能中一种决策模型
    http://zh.wikipedia.org/wiki/%E9%9A%B1%E9%A6%AC%E7%88%BE%E5%8F%AF%E5%A4%AB%E6%A8%A1%E5%9E%8B
模式识别
    http://zh.wikipedia.org/wiki/%E6%A8%A1%E5%BC%8F%E8%AF%86%E5%88%AB
    
---------------------------------------------------------------------------
手势识别技术综述
    http://wenku.baidu.com/link?url=lmSmzDfksPSbMlJG2Z5FX1Qtyk4AH3TlEgBn4A5FejoP566kQU-9jnVwf2xYg2yQKrXOgiYbCQPz7OMDq6jcOOOuoH4EoMqTVfmlyCMtx9a

手势识别的不同手段：
1. 基于鼠标/笔 （2D），基于sensors（3D）
2. 基于数据手套
3. 基于计算机视觉

手势识别方法（模式识别技术的一种）：
1. 模板匹配技术，通过对采样数据和预先存储的模板进行相似度的对比
2. 神经网络技术，具有自组织和自学习能力，具有分布性特点
3. 传统分机技术，通过统计样本特征向量来确定分类器的一种基于概率的分类方法。
    （如采用贝叶斯极大似然理论确定分类函数）

手势识别的几个部分：
图像预处理（计算机图形学的问题）
特征提取，识别算法（模式识别的问题）
    

--------------------------------------------------------------------
基于计算机视觉的手势识别的两种方式
1. 基于形状特征的特征提取和识别
    在基于形状特征的特征提取和识别中,该文首先根据手势图像中手指的方向及数目进行粗分类,然后在边界图像及二值图像中提取手势的周长、面积、重心距等形状特征向量,再进行基于类似度的模板匹配,实现对字母手势的细分类

2. 基于傅立叶描述子的特征提取和识别

 

 

 

    在基于傅立叶描述子的特征提取和识别中,该文提出并分析了具有旋转、平移和尺度变换不变性,平且与边界的起点位置无关的傅立叶描述子,并把傅立叶描述子及欧式距离应用于字母手势的识别中,通过计算输入手势的归一化傅立叶描述子与样本库中各类图像的映射向量的欧式距离,判定输入图像与样本图像间的匹配程度

 

 

 

Conception

 

极坐标：一种二位坐标系的表示方式

方向：direction，在3D中，只要用两个数字就可以表示一个方向。我的理解，例如用一支圆珠笔指向某一空间中的方向，首先在水平面上（极坐标系中）确定“水平面角度”a（0 - 360度），然后以圆珠笔的尾端为原点，在垂直于水平面的垂直平面上确定“垂直面角度”b（0-360度），那么a和b两个参数即可确定空间中任意一个方向。

方位：orientation，需要三个数字来确定。基于对方向的理解，当只有两个参数a和b时，虽然方向确定了，但是空间中同一方向上的存在无数个互相平行的直线，所以，当再添加第三个参数量可以唯一确定空间任意一个方位，也就是物体的具体姿态。

物体坐标轴：物体自身的坐标轴，随物体旋转而变化方位

惯性坐标轴：转动的起始坐标轴，方位固定不变

欧拉角：将角位移分解为绕三个互相垂直轴的三个旋转组成的序列，即章动角θ、旋进角（即进动角）ψ和自转角j组成；JC：有时我们用这三个称呼：roll（滚转角），Pitch（俯仰角）和yaw(偏航角)

方向余弦：在解析几何里，一个向量的三个方向余弦分别是这向量与三个坐标轴之间的角度的余弦；参见：http://zh.wikipedia.org/wiki/%E6%96%B9%E5%90%91%E9%A4%98%E5%BC%A6

正交矩阵：AA'=E（E为单位矩阵，A'表示“矩阵A的转置矩阵”。）或A′A=E，则n阶实矩阵A称为正交矩阵

牛顿—柯特斯(Newton-Cotes)求积公式:包含复化simpson求积公式

http://wenku.baidu.com/link?url=EfRyCeVL7AVKVKdNOkL4TvW75RHwem11VCWTHNGC5UMPiIiPbYXLmXYYcRylO0Xjop8T-AHK4q6uoQJAxOooFjppQlQLqCFNKx-7_JqdRI7

由欧拉角计算矢量坐标

From

http://wenku.baidu.com/link?url=FInh-biTGegI7gUcVOx4N5mcsanIVOSNuejDiDlZ5i878Av8nlCTqcSeND_VmpFTF22dFzmYRIJrBd1cFccxx6n2btSOuYqEi6xL3zD3Lk3

 

 

SR: 当坐标轴尺度/比例不变，并且原点也不变更的情况下，△x,△y,△z都为0，λ为1，公式（1）简化为，矢量在惯性坐标系的坐标值，等于旋转矩阵乘以物理坐标系的坐标值

几何变换详解

2012-10-23 10:59:44|  分类： 图形学|举报|字号 订阅

在三维图形学中，几何变换大致分为三种，平移变换（Translation），缩放变换（Scaling），旋转变换（Rotation）。以下讨论皆针对DirectX，所以使用左手坐标系。

平移变换

将三维空间中的一个点[x, y, z, 1]移动到另外一个点[x', y', z', 1]，三个坐标轴的移动分量分别为dx=Tx, dy=Ty, dz=Tz, 即

x' = x + Tx

y' = y + Ty

z' = z + Tz

平移变换的矩阵如下。

 

缩放变换

将模型放大或者缩小，本质也是对模型上每个顶点进行放大和缩小（顶点坐标值变大或变小），假设变换前的点是[x, y, z, 1]，变换后的点是[x', y', z', 1]，那么

x' = x * Sx

y' = y * Sy

z' = z * Sz

缩放变换的矩阵如下。

 

旋转变换

这是三种变换中最复杂的变换，这里只讨论最简单的情况，绕坐标轴旋转，关于绕任意轴旋转，在后续的随笔中介绍。

绕X轴旋转

 

绕X轴旋转时，顶点的x坐标不发生变化，y坐标和z坐标绕X轴旋转θ度，旋转的正方向为顺时针方向（沿着旋转轴负方向向原点看）。[x, y, z, 1]表示变换前的点，[x', y', z', 1]表示变换后的点。变换矩阵如下。

关于旋转的正方向，OpenGL与多数图形学书籍规定旋转正方向为逆时针方向（沿着坐标轴负方向向原点看），比如Computer Graphics C Version，p409。

 

绕Y轴旋转

 

绕Y轴旋转时，顶点的y坐标不发生变化，x坐标和z坐标绕Y轴旋转θ度。[x, y, z, 1]表示变换前的点，[x', y', z', 1]表示变换后的点。变换矩阵如下。

 

绕Z轴旋转

 

绕Z轴旋转时，顶点的z坐标不发生变化，x坐标和y坐标绕Z轴旋转θ度。[x, y, z, 1]表示变换前的点，[x', y', z', 1]表示变换后的点。变换矩阵如下。

 

 

绕坐标轴旋转的矩阵推导

上面三个旋转矩阵是如何得来的呢？我们推导一下，首先看一下二维的情况，再扩展到三维即可。实际上上面三种绕坐标轴旋转的情况属于特殊的二维旋转，比如绕Z轴旋转，相当于在与XOY平面上绕原点做二维旋转。

假设点P(x, y)是平面直角坐标系内一点，其到原点的距离为r，其与X轴的夹角为A，现将点P绕原点旋转θ度，得到点P'(x', y')，P'与X轴的夹角为B，则A = B - θ。（注意，在二维坐标中，逆时针旋转时角度为正，顺时针旋转时角度为负，下图中由P旋转到P'，角度为θ，若是由P'转到P，则角度为-θ）。

 

 

 

 

 

 

于是可得下面的转换方程

（式一）

写成矩阵的形式就是

 

求得旋转矩阵为

 

由于这里使用齐次坐标，所以还需加上一维，最终变成

 

和前面给出的绕Z轴旋转矩阵完全吻合。

对于绕X轴旋转的情况，我们只需将式一中的x用y替换，y用z替换，z用x替换即可。替换后得到

(式二)

对应的旋转矩阵为

 

对于绕Y轴旋转的情况，只需对式二做一次同样的替换即可，的到的变换方程为

 

对应的变换矩阵为

 

== Happy Coding ==

 

 

3D中的方位和角位移(1) - (8)

From http://www.cppblog.com/lovedday/archive/2008/02/16/42801.html

Recommand book:　《3D.Math.Primer.for.Graphics.and.Game.Development》writtenby Fletcher.Dunn  Chapter 10 Orientationand Angular Displacement in 3D

Original articles:

3D中的方位和角位移（1）

什么是方位

直观地说，我们知道物体的“方位”主要描述的是物体的朝向。然而“方向”和“方位”并不完全一样。向量有“方向”但没有“方位”，区别在于，当一个 向量指向特定方向时，可以让向量自转（如图10.1所示），但向量（或者说它的方向）却不会有任何变化，因为向量的属性只有“大小”，而没有“厚度”和 “宽度”。

 

然而，当一个物体朝向特定的方向时，让它和上面的向量一样自转，你会发现物体的方位改变了，如图10.2所示：

 

从技术角度来讲，这就说明在3D中，只要用两个数字（例如：极坐标），就能用参数表示一个方向（direction）。但是，要确定一个方位（orientation），却至少需要需要三个数字。

我们知道不能用绝对坐标来描述物体的位置，要描述物体的位置，必须把物体放置于特定的参考系中。描述位置实际上就是描述相对于给定参考点（通常是坐标系的原点）的位移。

同样，描述物体方位时，也不能使用绝对量。与位置只是相对已知点的位移一样，方位是通过与相对已知方位（通常称为"单位"方位或"源"方位）的旋转来描述的。旋转的量称作角位移。换句话说，在数学上描述方位就等价于描述角位移。

"方位"和"角位移"的区别就像"点"和"向量"的区别 ---- 两个术语都只是在数学上等价而在概念上是不同的。方位和点主要用来描述一个单一的状态，而角位移和向量描述的是两个状态间的差别。具体来说，我们用矩阵和四元数来表示"角位移"，用欧拉角来表示" 方位"。

 

矩阵形式

3D中，描述坐标系中方位的一种方法就是列出这个坐标系的基向量，这些基向量是用其他的坐标系来描述的。用这些基向量构成一个3x3矩阵，然后就能用矩阵 形式来描述方位。换句话说，能用一个旋转矩阵来描述这两个坐标系之间的相对方位，这个旋转矩阵用于把一个坐标系中的向量转换到另外一个坐标系中，如图10.3所示：

 

我们通过描述一个坐标系到另一个坐标系的旋转（无论采用哪种变换）来确定一个方位。矩阵变换的具体方向是一个实现细节，因为旋转矩阵是正交的，如果必要的话，只需简单的转置就可求得逆变换。

 

矩阵形式的优点

矩阵是一种非常直接的描述方位的形式，这种直接性带来了如下优点：

（1）可以立即进行向量的旋转。矩阵形式最重要的性质就是利用矩阵能在物体和惯性坐标系间旋转向量，这是其他描述方位所做不到的。为了旋转向量，必须将方位转换成矩阵形式。

（2）矩阵形式被图形API所采用。图形API使用矩阵来描述方位。（API就是应用程序接口，基本上它们就是实现你和显卡交流的代码。）当你和图 形API交流时，最终必须用矩阵来描述所需的变换。程序中怎样保存方位由你决定，但如果选择了其他形式，则必须在渲染管道的某处将其转换为矩阵。

（3）多个角位移连接。矩阵形式的第二个优点就是可以 “打破”嵌套坐标系间的关系。例如，如果知道A关于B的方位，又知道B关于C的方位，使用矩阵可以求得A关于C的方位。

（4）矩阵的逆。用矩阵形式表达角位移时，逆矩阵就是"反" 角位移。因为旋转矩阵是正交的，所以这个计算只是简单的矩阵转置运算。

 

矩阵形式的缺点

（1）矩阵占用了更多的内存。如果需要保存大量方位，如动画序列中的关键帧，9个数会导致数目可观的额外空间损失。举一个或许不太合适的例子，假设现在做 的是一个人的模型动画，该模型被分解为15块。动画的完成实际是严格地控制子块和父块之间的相对方位。假设每一帧为每一块保存一个方位，动画频率是15HZ ，这意味这每秒需要保存225个方位。使用矩阵和32位浮点数，每一帧有8100字节，而使用欧拉角，同样的数据只需2700字节。对于30s的动画数 据，矩阵就比欧拉角多占用162k字节。

（2）难以使用。矩阵对人类来说并不直观，有太多的数，并且它们都在-1到1之间。人类考虑方位的直观方法是角度，而矩阵使用的是向量。通过实践， 我们能从一个给定的矩阵中得到它所表示的方位。但这仍比欧拉角困难得多，其他方面也不尽如人意。用手算来构造描述任意方位的矩阵几乎是不可能的。总之，矩 阵不是人类思考方位的直观方法。

（3）矩阵可能是病态的。矩阵使用9个数，其实只有3个数是必须的。也就是说，矩阵带有6阶冗余。描述方位的矩阵必须满足6个限制条件。行必须是单位向量，而且它们互相垂直。

病态矩阵是怎样出现的呢？有多种原因：

1、矩阵可能包含缩放、切变或镜像操作，这些操作会对物体的"方位"产生什么影响呢？确实，对此没有一个清晰的定义。任何非正交的矩阵都不是一个定义良好的旋转矩阵。虽然镜像矩阵也是正交的，但它不是有效的旋转矩阵。

2、可能从外部数据源获得"坏"数据。例如，使用物体数据获取设备（如动作捕捉器）时，捕获过程中可能产生错误。许多建模包就是因为会产生病态矩阵而变得声名狼藉。

3、可能因为浮点数的舍入错误产生"坏"数据。例如，对一个方位作大量的加运算，这在允许人们手动控制物体方位的游戏中是很常见的。由于浮点精度的限制，大量的矩阵乘法最终可能导致病态矩阵，这种现象称作"矩阵蠕变"。

3D中的方位和角位移（2）

 

另一种描述方位的常用方法是欧拉角，这项技术以著名的数学家LeonhardEuler(1707 - 1783)的名字命名，他证明了角位移序列等价于单个角位移。

 

什么是欧拉角

欧拉角的基本思想是将角位移分解为绕三个互相垂直轴的三个旋转组成的序列。这听起来很复杂，其实它是非常直观的（事实上，易于使用正是它的主要优点之一）。之所以有"角位移"的说法正是因为欧拉角能用来描述任意旋转。

欧拉角将方位分解为绕三个互相垂直轴的旋转，那么是哪三个轴？按什么顺序？其实，任意三个轴和任意顺序都可以，但最有意义的是使用笛卡尔坐标系并按一定顺序所组成的旋转序列。最常用的约定是所谓的"heading - pitch - bank"约定。在这个系统中，一个方位被定义为一个heading角，一个pitch角，一个bank角。它的基本思想是让物体开始于"标准"方位 --- 就是物体坐标轴和惯性坐标轴对齐。在标准方位上，让物体作heading、pitch、bank旋转，最后物体到达我们想要描述的方位。

如图10.4所示，此时物体坐标系和惯性坐标系重合，heading为绕y轴的旋转量，向右旋转为正（如果从上面看，旋转正方向就是顺时针方向）。

 

经过heading旋转后，pitch为绕x轴的旋转量，注意是物体坐标系的x轴，不是原惯性坐标系的x轴。依然遵守左手法则，向下旋转为正，如图10.5所示：

最后，经过了heading和pitch，bank为绕z轴的旋转量。再次提醒，是物体坐标系的z轴，不是原惯性坐标系的z轴。依据左手法则，从原点向+z看，逆时针方向为正。如图10.6所示：

 

记住，当我们说旋转的顺序是heading-pitch-bank时，是指从惯性坐标系到物体坐标系。如果从物体坐标系变换到惯性坐标系，旋转的顺 序就是相反的。"heading-pitch-bank"也叫作"roll-pitch-yaw"，roll类似于bank，yaw类似于 heading（事实上，yaw并不严格等于heading）。注意，在roll-pitch-yaw系统中，角度的命名顺序与从物体坐标系到惯性坐标系 的旋转顺序一致的。

 

关于欧拉角的其他约定

heading-pitch-bank系统不是唯一的欧拉角系统。绕任意三个互相垂直轴的任意旋转都能定义一个方位，所以多种选择导致了欧拉角约定的多样性：

（1）heading-pitch-bank系统有多个名称。当然，不同的名字并不代表不同的约定，这其实并不重要。一组常用的术语是roll-pitch-yaw，其中的roll等价于bank，yaw基本上等价于heading。注意，它的顺序和heading-pitch-bank的顺序相 反，这只是语义上的。它定义了向量从物体坐标系变换到惯性坐标系的旋转顺序。（事实上，yaw和heading还是有技术上的差别，yaw是绕物体坐标系 y轴的旋转，heading是绕惯性坐标系y轴的旋转。因为这里的旋转是在物体坐标系y轴和惯性坐标系y轴重合时进行的，所以这个区别并不重要。）JC:旋转坐标系实质上是反方向旋转物体

（2）任意三个轴都能作为旋转轴，不一定必须是笛卡尔轴，但使用笛卡尔轴最有意义。

（3）决定每个旋转的正方向时不一定必须遵守左手或右手法则。例如，完全可以定义pitch的正方向是向上的，并且这种定义方法非常常见。

（4）也是最重要的，旋转可以以不同的顺序进行。顺序并不重要，任何系统都能用来定义一个方位，但heading-pitch-bank顺序最为实 用。heading度量绕竖直轴的旋转，它之所以有意义主要是因为我们所在的环境经常有某种形式的"地面"。一般来讲绕惯性坐标系的x或z轴的旋转没有什 么意义。heading-pitch-bank顺序下的另外两个角的意义是：pitch度量水平方向的倾角，bank度量的是绕z轴的旋转量。

 

欧拉角的优点

欧拉角仅使用三个数来表达方位，并且这三个数都是角度。这两个特点使欧拉角具有其他形式所没有的优点：

（1）欧拉角对我们来说很容易使用。它比矩阵和四元数简单得多，这可能是因为欧拉角中的数都是角度，符合人们思考方位的方式。如果我们选择了与所要处理的 情况最符合的约定，那么就能直接描述出最重要的角度，例如，用heading-pitch-bank系统就能直接地描述出偏差角度。便于使用是其最大的优 点，当需要显示方位或用键盘输入方位时，欧拉角是唯一的选择。

（2）最简洁的表达方式。欧拉角用三个数来表达方位。在3D中，表达方位不能少于三个数，如果要考虑内存的因素，欧拉角是最合适的描述方位的方法。

（3）任意三个数都是合法的。取任意三个数，它们都能构成合法的欧拉角，而且可以把它看成一个对方位的描述。从另一方面说，没有"不合法"的欧拉角。当然数值可能不对，但至少它们是合法的。可矩阵和四元数就不一定是这样了。

 

欧拉角的缺点

用欧拉角表达方位时的缺点主要有：

（1）给定方位的表达方式不唯一。

（2）两个角度间求插值非常困难。

让我们仔细讨论这些问题。第一个问题是对于一个给定方位，存在多个欧拉角可以描述它。这称作别名问题，有时候会引起麻烦。因为这个原因，连一些基本的问题（如"两组欧拉角代表的角位移相同吗？"）都很难回答。

第一种，在将一个角度加上360度的倍数时，我们就会遇到形式最简单的别名问题。显然，加上360度并不会改变方位，尽管它的数值改变了。

第二种，更加麻烦的别名问题是由三个角度不互相独立而导致的。例如，pitch135度等价于heading180度，pitch45度，然后 bank180度。为了保证任意方位都只有独一无二的表示，必须限制角度的范围。一种常用的技术是将heading和bank限制在+180度到-180度之间，pitch限制在+90度到-90度之间。这就建立了欧拉角的一个"限制范围"。对于任意方位，仅存在一个限制欧拉角能代表这个方位（事实上，还有一个违反唯一性的现象需要处理。）

欧拉角最著名的别名问题是这样的：先heading45度再pitch90度，这与先pitch90度再bank45度是等价的。事实上，一旦选 择+(-)90度为pitch角，就被限制在只能绕竖直轴旋转。这种现象，角度为+(-)90度的第二次旋转使得第一次和第三次旋转的旋转轴相同，称作万向锁。为了消除限制欧拉角的这种别名现象，规定万向锁情况下由heading完成绕竖直轴的全部旋转。换句话说，在限制欧拉角中，如果pitch为+(-)90度，则bank为0。

如果是为了描述方位，特别是在使用了限制欧拉角的情况下，别名是不会造成太大的问题的。现在来看两个方位A和B间求插值的问题，也就是说，给定参数t，0 ≤ t ≤ 1，计算临时方位C，当t从0变化到1时，C也平滑地从A变化到B。

这个问题的简单解法是分别对三个角度作标准线性插值，公式如下：

 

但这里面有很多问题。

第一，如果没有使用限制欧拉角，将得到很大的角度差。例如，方位A的heading为720度，方位B的heading为45度，720 = 360 x2，也就是0度。所以heading值只相差45度，但简单的插值会在错误的方向上绕将近两周。如图10.7所示：

 

解决问题的方法是使用限制欧拉角，然而，即使是限制欧拉角也不能完全解决问题。插值的第二个问题是由旋转角度的周期性引起的。设A的heading 为-170度，B的heading为170度。这些值在heading的限制范围内，都在-180度到180度之间。这两个值只相差20度，但插值操作又 一次发生了错误，旋转是沿 "长弧"绕了340度而不是更短的20度，如图10.8所示：

 

解决这类问题的方法是将插值的"差"角度折到-180度到180度之间，以找到最短弧。

 

即使使用了这两个角度限制，欧拉角插值仍然可能碰到万向锁的问题，它在大多数情况下会产生抖动、路径错误等现象，物体会突然飘起来像是"挂"在某个地方。根本问题是插值过程中角速度不是恒定的。

欧拉角插值的前两个问题虽然烦人，但并不是不可克服的。限制欧拉角和将角度差限制在一定范围内提供了简单的解决方法。而对于万向锁，非常不幸，它非常令人 讨厌，是一个底层的问题。你可能会考虑重新规划旋转，发明一种不会遭遇这些问题的系统。不幸的是，这不可能。这是一个用3个数表达3D方位的方法与生俱来 的问题。我们可以改变问题，但不能消灭它们。任何使用三个数来表达3D方位的系统，若能保证空间的唯一性，就都会遇到这些问题，如万向锁。

3D中的方位和角位移（3）

 

四元数记法

一个四元数包含一个标量和一个3D向量分量，经常记标量分量为w，记向量分量为单一的 v 或分开的x、y、z。两种记法分别如下:

[w v ]

[w, (x, y, z)]

在某些情况下，用 v 这样的短记法更方便，但在另一些情况下，"扩展"的记法会更清楚。

也可以将四元数竖着写，有时这会使等式的格式一目了然，"行"或"列"四元数没有明显的区别。

 

四元数和复数

复数对(a, b)定义了数a+bi，i是所谓的虚数，满足i2= -1：a称作实部，b称作虚部。任意实数k都能表示为复数(k, 0)=k + 0i。

复数能够相加、相减、相乘，如公式10.1所示:

 

通过使虚部变负，还能够计算复数的共轭，记法如公式10.2：

 

还能够计算复数的模。这个运算的记法和解释与实数的绝对值类似，实际上，如果将实数表示成复数，它们将产生相同的结果。公式10.3是计算复数大小的公式：

 

复数集存在于一个2D平面上，可以认为这个平面有两个轴：实轴和虚轴。这样，就能将复数（x,y）解释为2D向量。用这种方法解释复数时，它们能用来表达平面中的旋转。看看复数p绕原点旋转角度θ的情况，如图10.9所示：

 

为进行这个旋转，引入第二个复数 q = (cosθ, sinθ)。现在，旋转后的复数p'能用复数乘法计算出来：

p = x + yi

q = cosθ + i sinθ

p' = pq = (x + yi)(cosθ + i sinθ) =(xcosθ - ysinθ) + (xsinθ + ycosθ)i

JC：以上公式正是复数乘法的几何意义，暂时记住就好

引入复数q和用2x2旋转矩阵达到的效果是一样的，但复数提供了另一种有趣的记法。

四元数扩展了复数系统，它使用三个虚部i, j, k。它们的关系如下：

 

一个四元数[w, (x, y, z)]定义了复数 w+xi+yj+zk，很多标准复数的性质都能应用到四元数上。更重要的是，和复数能用来旋转2D中的向量类似，四元数也能用来旋转3D中的向量。

 

四元数和轴-角对

欧拉证明了一个旋转序列等价于单个旋转。因此，3D中的任意角位移都能表示为绕单一轴的单一旋转（这里的轴是一般意义上的旋转轴，不要和笛卡尔坐标轴混 淆。显然，旋转轴的方向是任意的）。当一个方位用这种形式来描述时称作轴-角描述法（实际上，能将轴-角形式作为描述方位的第四种表达方式。但是，轴-角 对很少用到，经常被欧拉角或四元数替代）。

设 n为旋转轴，对于旋转轴来说长度并不重要，将 n 定义为单位长度会比较方便。根据左手或右手法则， n 的方向定义了哪边将被认为是旋转"正"方向。设θ为绕轴旋转的量，因此，轴-角对（ n , θ）定义了一个角位移：绕 n 指定的轴旋转θ角。

四元数能被解释为角位移的轴-角对方式。然而， n 和θ不是直接存储在四元数的四个数中，它们的确在四元数里，但不是那么直接。公式10.4列出了四元数中的数和 n ，θ的关系，两种四元数加法都被使用了。

 

记住， q的w分量和θ有关系，但它们不是一回事。同样， v 和 n 也有关系但不完全相同。

 

负四元数

四元数能求负，做法很直接，将每个分量对变负，见公式10.5：

- q= -[w  (x  y  z)] = [-w  (-x  -y  -z)] = -[w v ] = [-w  - v]      

公式10.5  四元数求负

q 和- q代表的实际角位移是相同的，很奇怪吧！如果我们将θ加上360度的倍数，不会改变 q 代表的角位移，但它使 q 的四个分量都变负了。因此，3D中的任意角位移都有两种不同的四元数表示方法，它们互相为负。

 

单位四元数

几何上，存在两个"单位"四元数，它们代表没有角位移，[1, 0 ]和[-1, 0]（注意粗体 0，它们代表零向量）。当θ是360度的偶数倍时，有第一种形式，cos(θ/2)=1；θ是360度的奇数倍时，cos( θ /2)=-1。在两种情况下，都有sin(θ/2)=0，所以 n的值无关紧要。它的意义在于：

当旋转角θ是360度的整数倍时，方位并没有改变，并且旋转轴也是无关紧要的。

数学上，实际只有一个单位四元数：[1, 0 ]。用任意四元数 q乘以单位四元数[1, 0 ]，结果仍是 q 。任意四元数 q乘以另一个"几何单位"[-1,0 ]时得到- q。几何上，因为 q 和- q 代表的角位移相同，可认为结果是相同的。但在数学上， q 和- q不相等，所以[-1,0 ]并不是"真正"的单位四元数。

 

四元数的模

和复数一样，四元数也有模。记法和公式都和向量类似，如公式10.6所示：

 

让我们看看它的几何意义，代入 θ 和 n ，可得到：

 

n 为单位向量，所以：

 

应用三角公式sin2x + cos2x =1，得到：

 

如果为了用四元数来表示方位，我们仅使用符合这个规则的单位四元数。

 

四元数共轭和逆

四元数的共轭记作 q *，可通过让四元数的向量部分变负来获得，见公式10.7：

 

四元数的逆记作 q ^-1，定义为四元数的共轭除以它的模，见公式10.8：

 

四元数的逆和实数的倒数有着有趣的对应关系。对于实数a，它的逆a-1为1/a，从另一方面说，aa-1= a-1a = 1。四元数的逆也有着同样的性质，一个四元数 q 乘以它的逆 q-1，即可得到单位四元数[1,0 ]。

公式10.8是四元数逆的正式定义，但我们只使用单位四元数，所以四元数的逆和共轭是相等的。

共轭非常有趣，因为 q 和 q *代表相反的角位移。很容易验证这种说法，使 v 变负，也就是使旋转轴反向，它颠倒了我们所认为的旋转正方向。因此， q 绕轴旋转θ角，而 q *沿相反的方向旋转相同的角度。

3D中的方位和角位移（4）

 

四元数乘法（叉乘）

四元数能根据复数乘法解释来相乘，如下：

 

这导出了四元数乘法的标准定义，下面以两种四元数记法给出，见公式10.9：

 

不用为四元数叉乘使用乘号，"行"或 "列"四元数也没有什么区别。

四元数叉乘满足结合律，但不满足交换律，如公式10.10所示：

 

现在看看两个四元数叉乘的模:

 

展开合并同类项:

 

最后应用四元数模的定义得到公式 10.11:

 

因此，四元数乘积的模等于模的乘积。这个结论非常重要，因为它保证了两个单位四元数相乘的结果还是单位四元数。

四元数乘积的逆等于各个四元数的逆以相反的顺序相乘，如公式10.12所示：

 

现在到了四元数非常有用的性质。让我们"扩展"一个标准3D点(x, y, z)到四元数空间，通过定义四元数p=[0, (x, y, z)]即可（当然，在一般情况下，p不会是单位四元数）。设q为我们讨论的旋转四元数形式[cos(θ/2),nsin(θ/2)]，n为旋转轴，单位向量；θ为旋转角。你会惊奇地发现，执行下面的乘法可以使3D点p绕n旋转：

 

已经证明，四元数乘法和3D向量旋转的对应关系，更多的是理论上的意义，不是实践上的。实际上，它几乎和把四元数转换到矩阵形式然后再用矩阵乘以向量所用的时间一样。

让我们看多次旋转的情况，将点p用一个四元数a旋转然后再用另一个四元数b旋转：

 

注意，先进行a旋转再进行b旋转等价于执行乘积ba代 表的单一旋转。因此，四元数乘法能用来连接多次旋转，这和矩阵乘法的效果一样。根据四元数乘法的标准定义，这个旋转是以从右向左的顺序发生的。这非常不 幸，因为它迫使我们以 "由里向外"的顺序连接多次旋转，这和以矩阵形式作同样的运算是不同的（至少在使用行向量时是不同的）。

针对公式10.9所导致的"顺序颠倒"问题，我们将违背标准定义，以相反的运算顺序来定义四元数乘法。注意，仅仅向量叉乘部分受到了影响，见公式10.13：

 

这并没有改变四元数的基本性质和用v、θ的几何解释，仍然能用四元数乘法来直接旋转向量，唯一不同的是，根据我们的定义，将四元数放在向量右边，而把它的逆放在向量的左边：

 

能看到下面这个表达了多个旋转连接的等式，它是自左向右的，与旋转发生的顺序一致：

 

对于我们来说，让四元数代表角位移的"高级"能力，使其易于使用，这比坚持正式标准更加重要。我们的目的在于理解四元数的本质和它提供给我们的操作，设计一个类将直接引出这些操作，在需要的地方使用这个类，永远不需要再去摆弄里面的数。

 

四元数的"差"

利用四元数的乘法和逆，就能够计算两个四元数的"差"。"差"被定义为一个方位到另一个方位的角位移。换句话说，给定方位a和b，能够计算从a旋转到b的角位移d。用四元数等式更加紧凑地表示为：ad=b。

两边同时左乘a-1:

 

现在，我们就有了求得代表一个方位到另一个方位角位移的四元数的方法。

数学上，两个四元数之间的角度"差"更类似于"除"，而不是真正的"差"（减法）。

 

四元数点乘

四元数也有点乘运算，它的记法、定义和向量点乘非常类似，如公式10.14所示：

 

注意，和向量点乘一样，其结果是标量。对于单位四元数a和b，有-1 ≤ a.b ≤ 1。通常我们只关心 a . b 的绝对值，因为a.b = -(a . -b)，所以b和-b代表相同的角位移。

四元数点乘的几何解释类似于向量点乘的几何解释，四元数点乘 a. b 的绝对值越大，a和b代表的角位移越"相似"。

3D中的方位和角位移（5）

 

四元数的对数、指数和标量乘运算

首先，让我们重写四元数的定义，引入一个新的变量α，等于半角θ/2：

α = θ/2

|| n || = 1

q = [cosα   n sinα] =[cosα   xsinα   ysinα   zsinα]

q 的对数定义为公式10.15：

log q = log([cosα   n sinα]) ≡ [0  α n ]

公式10.15   四元数的对数

≡表示"恒等于"，注意log q 的结果，它一般不是单位四元数。

指数以严格相反的方式定义，首先，设四元数 p 的形式为[0  α n ]， n 为单位向量：

p = [0  α n ] = [0 (αx   αy   αz)]

|| n || = 1

接着，指数定义为公式10.16:

exp p = exp([0 α n ]) = [cosα  n sinα]

公式10.16  四元数的指数

根据定义，exp p 总是返回单位四元数。

四元数的对数和指数类似于它们的标量形式，回忆一下，对于标量α，有下列关系成立:

 

同样，四元数指数运算为四元数对数运算的逆运算：

exp(log q ) = q

最后，四元数能与一个标量相乘。其计算方法非常直接：每个分量都乘以这个标量，给定标量k和四元数 q，有公式 10.17：

k q= k[w  v] = [kw   k v] = k[w  (x  y  z)] = [kw  kx  ky  kz]

公式10.17   四元数和标量相乘

一般不会得到单位四元数，这也是为什么在表达角位移的场合中标量乘不是那么有用的原因。

 

四元数求幂

四元数能作为底数，记作 qt （不要和指数运算混淆，指数运算只接受一个四元数作为参数，而四元数求幂有两个参数---- 四元数和指数）。四元数求幂的意义类似于实数求幂。回忆一下，a0= 1， a1 = a，a为非零标量。当t从0变到1时，at从1到a。四元数求幂有类似的结论：当t从0变到1， qt从[1,0]到 q。

这对四元数求幂非常有用，因为它可以从角位移中抽取"一部分"。例如，四元数 q 代表一个角位移，现在想要得到代表1/3这个角位移的四元数，可以这样计算： q1/3。

指数超出[0, 1]范围外的几何行为和预期的一样（但有一个重要的注意事项）。例如， q2代表的角位移是 q的两倍。假设 q代表绕x轴顺时针旋转30度，那么 q2代表绕x轴顺时针旋转60度， q-1/3代表绕x轴逆时针旋转10度。

上面提到的注意事项是，四元数表达角位移时使用最短圆弧，不能"绕圈"。继续上面的例子， q4不是预期的绕x轴顺时针旋转240度，而是逆时针80度。显然，向一个方向旋转240度等价于向相反的方向旋转80度，都能得到正确的"最终结果"。但是，在此基础上的进一步运算，产生的就可能不是预期的结果了。例如，(q4)1/2不是 q2，尽管我们感觉应该是这样。一般来说，凡是涉及到指数运算的代数公式，如(as)t= a(st)，对四元数都不适用。

现在，我们已经理解四元数求幂可以为我们做什么了。让我们看看它的数学定义，四元数求幂定义在前一节讨论的"有用"运算上，定义如公式10.18：

 

注意，对于标量求幂，也有类似结论：

 

不难理解为什么当t从0变到1时 q'从单位四元数变到 q。注意到对数运算只是提取了轴 n 和角度θ；接着，和指数t进行标量乘时，结果是θ乘以t；最后，指数运算"撤销"了对数运算，从tθ和 n重新计算w和 v 。上面给出的定义就是标准数学定义，在理论上非常完美，但直接转换到代码却是很复杂的。程序清单10.1所示的代码展示了怎样计算 q'的值。

Listing 10.1: Code to raise a quaternion to a power

// Quaternion (input and output)
float w,x,y,z;

// Input exponent
float exponent;

// Check for the case of an identity quaternion.
// This will protect against divide by zero
if (fabs(w) < .9999f) 
{
  // Extract the half angle alpha (alpha = theta/2)
  float alpha = acos(w);

  // Compute new alpha value
  float newAlpha = alpha * exponent;

  // Compute new w value
  w = cos(newAlpha);

  // Compute new xyz values
  float mult = sin(newAlpha) / sin(alpha);

  x *= mult;
  y *= mult;
  z *= mult;
}

关于这些代码，需要注意的地方有:

（1）有必要做单位四元数的检查。因为w=+(-)1会导致mult的计算中出现除零现象。单位四元数的任意次方还是单位四元数。因此，如果检测到输入是单位四元数，忽略指数直接返回原四元数即可。

（2）计算alpha时，使用了acos函数，它的返回值是正的角度。这并不会违背一般性，任何四元数都能解释成有正方向的旋转角度，因为绕某轴的负旋转等价于绕指向相反方向的轴的正旋转。

 

四元数插值 ---- "slerp"

当今3D数学中四元数存在的理由是由于一种称作slerp的运算，它是球面线性插值的缩写（Spherical Linear Interpolation）。slerp运算非常有用，因为它可以在两个四元数间平滑插值。slerp运算避免了欧拉角插值的所有问题。

slerp是一种三元运算，这意味着它有三个操作数。前两个操作数是两个四元数，将在它们中间插值。设这两个"开始"和"结束"四元数分别为 q0和 q1。插值参数设为变量t，t在0到1之间变化，slerp函数：slerp( q0,q1, t)，将返回 q 0到 q1之间的插值方位。

能否利用现有的数学工具推导出slerp公式呢？如果是在两个标量a0和a1间插值，我们会使用下面的标准线性插值公式：

Δa = a1 - a0

lerp(a0, a1, t) =a0 + tΔa

标准线性插值公式从a0开始，并加上a0和a1差的t倍，有三个基本步骤：

（1）计算两个值的差。

（2）取得差的一部分。

（3）在初始值上加上差的一部分。

可以使用同样的步骤在四元数间插值：

（1）计算两个值的差， q 0到 q1的角位移由Δ q =q 0-1 q 1给出。

（2）计算差的一部分，四元数求幂可以做到，差的一部分由(Δ q )t给出。

（3）在开始值上加上差的一部分，方法是用四元数乘法来组合角位移： q 0(Δ q )t

这样，得到slerp的公式如公式10.19所示：

 

这是理论上的slerp计算过程，实践中，将使用一种更加有效的方法。

我们在4D空间中解释四元数，因为所有我们感兴趣的四元数都是单位四元数，所以它们都"存在"于一个 4D"球面"上。

slerp的基本思想是沿着4D球面上连接两个四元数的弧插值(这就是球面线性插值这个名称的由来)。

可以把这种思想表现在平面上，设两个2D向量 v0和 v 1，都是单位向量。我们要计算 v 1，它是沿 v 0到 v1弧的平滑插值。设w是 v0到 v1弧所截的角，那么 vt就是绕 v 1沿弧旋转tw的结果，如图10.10所示：

 

将 vt表达成 v 0和 v1的线性组合，从另一方面说，存在两个非零常数k0和k1，使得：

v t = k0 v 0 + k1 v1

可以用基本几何学求出k0和k1，图10.11展示了计算的方法：

 

对以k1 v1为斜边的直角三角形应用三角公式得：

 

这里有两点需要考虑。第一，四元数 q 和- q代表相同的方位，但它们作为slerp的参数时可能导致不一样的结果，这是因为4D球面不是欧式空间的直接扩展。而这种现象在2D和3D中不会发生。解决方法是选择 q 0和 q1的符号使得点乘 q 0 .q1的结果是非负。第二个要考虑的是如果 q 0和 q1非常接近，sinθ会非常小，这时除法可能会出现问题。为了避免这样的问题，当sinθ非常小时使用简单的线性插值。程序清单10.2把所有的建议都应用到了计算四元数的slerp中：

Listing 10.2: How slerp is computed in practice

// The two input quaternions
float w0,x0,y0,z0;
float w1,x1,y1,z1;

// The interpolation parameter
float t;

// The output quaternion will be computed here
float w,x,y,z;

// Compute the "cosine of the angle" between the
// quaternions, using the dot product
float cosOmega = w0*w1 + x0*x1 + y0*y1 + z0*z1;

// If negative dot, negate one of the input
// quaternions to take the shorter 4D "arc"
if (cosOmega < 0.0f) {
  w1 = –w1;
  x1 = –x1;
  y1 = –y1;
  z1 = –z1;
  cosOmega = –cosOmega;
}

// Check if they are very close together to protect
// against divide-by-zero
float k0, k1;

if (cosOmega > 0.9999f) {
  // Very close - just use linear interpolation
  k0 = 1.0f–t;
  k1 = t;
} else {
  // Compute the sin of the angle using the
  // trig identity sin^2(omega) + cos^2(omega) = 1
  float sinOmega = sqrt(1.0f – cosOmega*cosOmega);

  // Compute the angle from its sin and cosine
  float omega = atan2(sinOmega, cosOmega);

  // Compute inverse of denominator, so we only have
  // to divide once
  float oneOverSinOmega = 1.0f / sinOmega;

  // Compute interpolation parameters

  k0 = sin((1.0f – t) * omega) * oneOverSinOmega;
  k1 = sin(t * omega) * oneOverSinOmega;
}

// Interpolate
w = w0*k0 + w1*k1;
x = x0*k0 + x1*k1;
y = y0*k0 + y1*k1;
z = z0*k0 + z1*k1;

3D中的方位和角位移（6）

　

四元数样条 ---- “squad”

Slerp提供了两个方位间的插值，当有多于两个的方位序列（它描述了我们想要经过的插值“路径”）时怎么办？我们可以在”控制点“之间使用slerp。类似于基本几何学中的线性插值，控制点之间是以直线连接的。显然，控制点上会有不连续性 ---- 这是我们想要避免的，我们给出squad(Spherical and Quadrangle)的公式，用来描绘控制点间的路径。

设控制点由四元数序列所定义：

q1，q2，q3，…qn-2，qn-1，qn

另外，引进一个辅助四元数si，将它作为临时控制点：

 

注意，qi-1至qi+1计算出si，所以s1和sn是未定义的。换句话说，曲线从q2延伸到qn-1，第一个和最后一个控制点仅用于控制中间的曲线。如果曲线一定要经过这两点，必须在头部和尾部增加虚控制点，一个显而易见的方法就是复制这两个控制点。

给定四个相邻的控制点，squad用于计算中间两点间的插值，这点非常像三次样条。

设四个控制点为：qi-1，qi，qi+1，qi+2

还要引入一个插值变量h，h从0变化到1时，squad描绘qi到qi+1之间的曲线。

整条插值曲线能够分段应用squad方法来获得，如公式10.12所示：

 

　

四元数的优点和缺点

四元数有一些其他角位移表示方法所没有的优点：

（1）平滑插值。slerp和squad提供了方位间的平滑插值，没有其他方法能提供平滑插值。

（2）快速连接和角位移求逆。四元数叉乘能将角位移序列转换为单个角位移，用矩阵作同样的操作明显会慢一些。四元数共轭提供了一种有效计算反角位移的方法，通过转置旋转矩阵也能达到同样的目的，但不如四元数来得容易。

（3）能和矩阵形式快速转换。四元数和矩阵间的转换比欧拉角与矩阵之间的转换稍微快一点。

（4）仅用四个数。四元数仅包含4个数，而矩阵用了9个数，它比矩阵"经济"得多（当然仍然比欧拉角多33%）。

 

要获得这些优点是要付出代价的，四元数也有和矩阵相似的问题，只不过问题程度较轻：

（1）比欧拉角稍微大一些。这个额外的数似乎没有太大关系，但在需要保存大量角位移时，如存储动画数据，这额外的33%也是数量可观的。

（2）四元数可能不合法。坏的输入数据或浮点数舍入误差积累都可能使四元数不合法（能通过四元数标准化解决这个问题，确保四元数为单位大小）。

（3）难于使用。在所有三种形式中，四元数是最难于直接使用的。

　

各方法比较

 

　

不同的方位表示方法适用于不同的情况，下面是我们对合理选择格式的一些建议：

（1）欧拉角最容易使用。当需要为世界中的物体指定方位时，欧拉角能大大简化人机交互，包括直接的键盘输入方位、在代码中指定方位（如为渲染设定摄像机）、在调试中测试。这个优点不应被忽视，不要以优化为名义而牺牲易用性，除非你确定这种优化的确有效果。

（2）如果需要在坐标系间转换向量，那么就选择矩阵形式。当然，这并不意味着你就不能用其他格式来保存方位，并在需要的时候转换到矩阵形式。另一种方法是用欧拉角作为方位的主拷贝，但同时维护一个旋转矩阵，当欧拉角发生改变时矩阵也要同时进行更新。

（3）当需要大量保存方位数据（如动画）时，就使用欧拉角或四元数。欧拉角将少占用25%的内存，但它在转换到矩阵时要稍微慢一些。如果动画数据需要嵌套坐标系之间的连接，四元数可能是最好的选择。

（4）平滑的插值只能用四元数完成。如果你用其他格式，也可以先转换到四元数然后再插值，插值完毕后再转换回原来的形式。

　

　

从欧拉角转换到矩阵

欧拉角描述了一个旋转序列。分别计算出每个旋转的矩阵再将它们连接成一个矩阵，这个矩阵就代表了整个角位移。当然，它和我们是想要物体到惯性坐标的变换矩阵还是惯性到物体坐标的变换矩阵是相关的。

我们对欧拉角的定义是一个旋转序列，该旋转序列将物体（和它的坐标空间）从惯性坐标空间转换到物体坐标空间。因此，可以用欧拉角定义的直接转换来直接产生惯性---- 物体旋转矩阵的一般形式：

M惯性--> 物体 = HPB

H、P、B分别为heading、pitch、bank的旋转矩阵，它们分别绕y、x、z轴旋转，仅仅旋转"坐标空间"就是旋转"点"的严格相反操作。可以想象这些旋转发生时点是固定在空间中不变的，例如，pitch使坐标空间向下，点实际上关于坐标空间向上。欧拉角公式明确指明是物体和它的坐标空间旋转，但我们需要的是变换"点"的矩阵，所以计算矩阵H、P、B时，用相反的旋转量来旋转。设heading、pitch、bank的旋转角分别为变量h、p、b:

 

　

以适当的顺序连接这些矩阵得到公式10.21：

 

如果要从物体坐标空间变换到惯性坐标空间，应该使用惯性----物体旋转矩阵的逆。因为旋转矩阵是正交的，所以求它的逆就是求它的转置，下面验证这一点。

为了从物体坐标空间变换到惯性坐标空间，顺序应该为"un-bank"、"un-pitch"、"un-heading"，公式表示为：

M物体->惯性 = （M惯性->物体）-1= （HPB）-1=B-1P-1H-1

注意，可以认为旋转矩阵B-1、P-1、H-1为它们对应矩阵的逆，或者是使用相反旋转角b、p、h的一般旋转矩阵。（惯性--- 物体矩阵中，使用负的旋转角，因此这里的角不用变负。）

 

以适当的顺序连接这些矩阵得到公式10.22:

 

比较公式10.21和公式10.22，可以看到物体---惯性矩阵确实惯性---物体矩阵的转置。

　

从矩阵转换到欧拉角

将角位移从矩阵形式转换到欧拉角需要考虑以下几点：

（1）必须清楚矩阵代表什么旋转：物体 -- 惯性还是 惯性 -- 物体，这里讨论使用惯性 -- 物体矩阵的技术，物体 -- 惯性矩阵转换成欧拉角的过程与之类似。

（2）对任意给定角位移，存在无穷多个欧拉角可以用来表示它。因为"别名"问题，这里讨论的技术总是返回"限制欧拉角"，heading和bank的范围±180°，pitch的范围为±90°。

（3）矩阵可能是病态的，我们必须忍受浮点数精度的误差。有些矩阵还包括除旋转外的变换，如缩放、镜像等。这里只讨论工作在旋转矩阵上的变换。

考虑这些因素后，我们尝试从公式10.21直接解得欧拉角：

 

 

If cos p = 0, then we cannot use the above trick sinceall the matrix elements involved are zero.
However, notice that when cos p = 0, then p = 90°, which means we are eitherlooking straight
up or straight down. This is the Gimbal lock situation, where heading and bankeffectively rotate
about the same physical axis (the vertical axis). In this case, we willarbitrarily assign all rotation
about the vertical axis to heading and set bank equal to zero. Now we know thevalue of pitch and
bank, and all we have left is to solve for heading. Armed with the followingsimplifying
assumptions:

cosp = 0

b = 0

sinb = 0

cosb = 1

将它们代入公式10.21:

 

　

现在，能够通过-m31和m11计算h，它们分别包含了h的sin和cos值。

让我们看看使用上面的技术从惯性---- 物体旋转矩阵中抽取欧拉角的代码，为了使示例简单，假设输入输出为全局变量。

     Listing 10.3: Extracting Euler angles from an inertial-to-object rotation matrix
   
     // Assume the matrix is stored in these variables:
    float m11,m12,m13;
    float m21,m22,m23;
    float m31,m32,m33;
   
       // We will compute the Euler angle values in radians and store them here:
    float h,p,b;
   
       // Extract pitch from m23, being careful for domain errors with asin(). We could have
    // values slightly out of range due to floating point arithmetic.
    float sp = –m23;
   
    if (sp <= –1.0f) {
      p = –1.570796f; // –pi/2
    } else if (sp >= 1.0) {
      p = 1.570796; // pi/2
    } else {
      p = asin(sp);
    }
   
    // Check for the Gimbal lock case, giving a slight tolerance
    // for numerical imprecision
    if (sp > 0.9999f) {
      // We are looking straight up or down.
     // Slam bank to zero and just set heading
      b = 0.0f;
      h = atan2(–m31, m11);
    } else {
      // Compute heading from m13 and m33
      h = atan2(m13, m33);
   
      // Compute bank from m21 and m22
      b = atan2(m21, m22);
    }

3D中的方位和角位移（7）

　

从四元数转换到矩阵

为了将角位移从四元数转换到矩阵形式，可以利用旋转矩阵，它能计算绕任意轴的旋转：

 

这个矩阵是用n和θ表示的，但四元数的分量是：

w = cos(θ/2)

x = nx sin(θ/2)

y = ny sin(θ/2)

z = nz sin(θ/2)

让我们看看能否将矩阵变形以代入w、x、y、z，矩阵的9个元素都必须这样做。幸运的是，这个矩阵的结构非常好，一旦解出对角线上的一个元素，其他元素就能用同样的方法求出。同样，非对角线元素之间也是彼此类似的。

考虑矩阵对角线上的元素，我们将完整地解出m11，m22和m33解法与之类似：

m11 = nx2(1- cosθ) + cosθ

我们将从上式的变形开始，变形方法看起来像是在绕圈子，但你马上就能理解这样做的目的：

 

现在需要消去cosθ项，而代之以包含cosθ/2或sinθ/2的项，因为四元数的元素都是用它们表示的，像以前那样，设α=θ/2，先用α写出cos的倍角公式，再代入θ:

 

上式是正确的，但它和其他的标准形式不同，即:

m11 = 1 - 2y2 -2z2

实际上，还有其他的形式存在。最著名的一个形式是m11 = w2+ x2 - y2- z2，因为w2+ x2 + y2 +z2 = 1，所以这三种形式是等价的。现在回过头来看看能不能直接导出其他标准形式，第一步，n是单位向量，nx2+ny2 +nz2= 1，则1 - nx2=ny2 + nz2。

m11 = 1 - (1 - nx2)(2sin2(θ/2))

       =1 - (ny2+nz2)(2sin2(θ/2))

       =1 - 2ny2sin2(θ/2)- 2nz2sin2(θ/2)

       =1 - 2y2 - 2z2

元素m22和m33可以用同样的方法求得。

让我们来看看非对角线元素，它们比对角线元素简单一些，以m12为例子：

m12 = nxny(1 - cosθ)+nzsinθ

 

其他非对角线元素可用同样的方法导出。

最后，给出从四元素构造的完整旋转矩阵，如公式10.23所示：

 

　

从矩阵转换到四元数

为了从旋转矩阵中抽出相应的四元数，可以直接利用公式 10.23，检查对角线元素的和（也称作矩阵的轨迹）得到：

 

通过使轨迹中三个元素中的两个为负，可以用类似的方法求得其他三个元素:

 

不幸的是，这种方法并不总是能正确工作，因为平方根的结果总是正值。（更加准确地说，没有选择正根还是负根的依据。）但是，q和-q代表相同的方位，我们能任意选择用非负根作为4个分量中的一个并仍能得到正确的四元数，只是不能对四元数的所有4个数都用这种方法。

另一个技巧是检查相对于对角线的对称位置上元素的和与差：

 

那么应选用四种方法中的哪个呢？似乎最简单的策略是总是先计算同一个分量，如w，然后再计算x、y、z。这伴随着问题，如果w=0，除法就没有定义；如果w非常小，将会出现数值不稳定。Shoemake建议先判断w、x、y、z中哪个最大（不用做平方根运算），根据上面的表，用矩阵对角线计算该元素，再用它计算其他三个。

下面的代码用一种非常直接的方式实现了这个方法。

          Listing 10.4: Converting a rotation matrix to a quaternion
   
    // Input matrix:
    float m11,m12,m13;
    float m21,m22,m23;
    float m31,m32,m33;
   
    // Output quaternion
    float w,x,y,z;
   
       // Determine which of w, x, y, or z has the largest absolute value
    float fourWSquaredMinus1 = m11 + m22 + m33;
    float fourXSquaredMinus1 = m11 – m22 – m33;
    float fourYSquaredMinus1 = m22 – m11 – m33;
    float fourZSquaredMinus1 = m33 – m11 – m22;
   
    int biggestIndex = 0;
   
    float fourBiggestSquaredMinus1 = fourWSquaredMinus1;
   
    if (fourXSquaredMinus1 > fourBiggestSquaredMinus1) {
      fourBiggestSquaredMinus1 = fourXSquaredMinus1;
      biggestIndex = 1;
    }
   
    if (fourYSquaredMinus1 > fourBiggestSquaredMinus1) {
     fourBiggestSquaredMinus1 = fourYSquaredMinus1;
      biggestIndex = 2;
    }
   
    if (fourZSquaredMinus1 > fourBiggestSquaredMinus1) {
     fourBiggestSquaredMinus1 = fourZSquaredMinus1;
      biggestIndex = 3;
    }
   
       // Perform square root and division
    float biggestVal = sqrt(fourBiggestSquaredMinus1 + 1.0f) * 0.5f;
    float mult = 0.25f / biggestVal;
   
       // Apply table to compute quaternion values
    switch (biggestIndex) {
    case 0:
      w = biggestVal;
     x = (m23 – m32) * mult;
     y = (m31 – m13) * mult;
     z = (m12 – m21) * mult;
      break;
   
    case 1:
      x = biggestVal;
     w = (m23 – m32) * mult;
     y = (m12 + m21) * mult;
     z = (m31 + m13) * mult;
      break;
   
    case 2:
      y = biggestVal;
     w = (m31 – m13) * mult;
     x = (m12 + m21) * mult;
     z = (m23 + m32) * mult;
      break;
   
    case 3:
      z = biggestVal;
     w = (m12 – m21) * mult;
     x = (m31 + m13) * mult;
     y = (m23 + m32) * mult;
      break;
    }

3D中的方位和角位移（8）

　

从欧拉角转换到四元数

为了将角位移从欧拉角转换到四元数，可以使用从欧拉角构造矩阵类似的方法。先将这三个旋转分别转换为四元 数，这是一个简单的运算。再将这三个四元数连接成一个四元数。和矩阵一样，有两种情况需要考虑，第一种是惯性-- 物体四元数，第二种是物体-- 惯性四元数。因为它们互为共轭关系，所以我们只推导惯性--物体四元数。

设欧拉角为变量h、p、b，设h、p、b分别绕轴y、x、z旋转的四元数。记住，使用负旋转量，因为它们指定坐标系中的旋转角度。

 

用正确的顺序连接它们得到公式10.24：

 

（记住，四元数乘法定义是按旋转的顺序从左向右乘。）

物体--惯性四元数是惯性--物体四元数的共轭，见公式10.25：

 

　

从四元数转换到欧拉角

根据前面的公式发现：

 

 

现在可以将它直接转换到代码中，如程序清单10.5所示，它能把惯性--物体四元数转换成欧拉角。

       Listing 10.5: Converting an inertial-to-object quaternion to Euler angles
    
        // Use global variables for input and output
    float w,x,y,z;
    float h,p,b;
    
    // Extract sin(pitch)
    float sp = –2.0f * (y*z + w*x);
    
    // Check for Gimbal lock, giving slight tolerance for numerical imprecision
    if (fabs(sp) > 0.9999f) {
      // Looking straight up or down
      p = 1.570796f * sp; // pi/2
    
     // Compute heading, slam bank to zero
         h = atan2(–x*z – w*y, 0.5f – y*y – z*z);
      b = 0.0f;
    } else {
      // Compute angles
      p = asin(sp);
     h = atan2(x*z – w*y, 0.5f – x*x – y*y);
     b = atan2(x*y – w*z, 0.5f – x*x – z*z);
    }

将物体--惯性四元数转换到欧拉角，所用的代码和上面非常类似。只是将x、y、z值变负，因为物体--惯性四元数是惯性--物体四元数的共轭。

        Listing 10.6: Converting an object-to-inertial quaternion to Euler angles
    
    // Extract sin(pitch)
    float sp = –2.0f * (y*z – w*x);
    
    // Check for Gimbal lock, giving slight tolerance for numerical imprecision
    if (fabs(sp) > 0.9999f) {
      // Looking straight up or down
      p = 1.570796f * sp; // pi/2
    
      // Compute heading, slam bank to zero
         h = atan2(–x*z + w*y, 0.5f – y*y – z*z);
      b = 0.0f;
    } else {
      // Compute angles
      p = asin(sp);
     h = atan2(x*z + w*y, 0.5f – x*x – y*y);
     b = atan2(x*y + w*z, 0.5f – x*x – z*z);
    }

 

 

Use low-pass filter to isolate gravityfrom output data measured by G-sensor

针对下面一组公式：
android/frameworks/base/core/java/android/hardware/SensorEvent.java

public void onSensorChanged(SensorEvent event)

     {

          // alpha is calculated as t / (t + dT)

          // with t, the low-pass filter's time-constant

          // and dT, the event delivery rate

 

          final float alpha = 0.8;

 

          gravity[0] = alpha * gravity[0] + (1 - alpha) * event.values[0];

          gravity[1] = alpha * gravity[1] + (1 - alpha) * event.values[1];

          gravity[2] = alpha * gravity[2] + (1 - alpha) * event.values[2];

 

          linear_acceleration[0] = event.values[0] - gravity[0];

          linear_acceleration[1] = event.values[1] - gravity[1];

          linear_acceleration[2] = event.values[2] - gravity[2];

     }

 

 

搜索到网上有人评论如下：

http://bbs.csdn.net/topics/390503938

这里的滤波器不是一个设备 整个算法是低通滤波的实现 
所谓低通滤波 是指只有低频信号可以通过的装置或者处理过程
因为重力只有在设备旋转的时候会发生变化 因此一定是低频信号
通过对传感器三轴的测量值进行低通滤波 可以获得重力在各个轴上的分量
然后再在三轴测量值上减去重力的分量 即可获得设备实际的加速度值

alpha是滤波参数 可以根据需要进行更改
event.values 是传感器收集到数据
gravity 是重力加速度在三轴上的分量
linear_acceleration 是三轴线性加速度

 

另一个低通滤波算法的帖子：http://blog.csdn.net/maray/article/details/7730388

 

G-sensor计算空间位移的调研  理论结论总结

1.     GoogleAOSP source code中有段被注释掉的legacy代码片段，（见下面上封邮件的红色字体部分）

·      广泛了解低通滤波器的计算方法后，我的结论是：此公式并不适用于指环移动控制的使用场景。

·      原因是：指环移动动作过程中，重力在三个轴的输出分量是持续变化的，并非较低频率的事件。

·       

·      下面是详细的解释过程，我的粗浅理解（请大家也发表下自己的理解）

·       

·      第一步：分离出重力

·       gravity[0] = alpha * gravity[0] + (1 - alpha) *event.values[0];  （e.g. alpha is 0.8）

              

 

·       第二步：去除重力得到物体自身的加速度

·       linear_acceleration[0] = event.values[0] -gravity[0];

 

·       其中gravity[0] 是分离/过滤出来的重力，alphais filter factor, event.values[0] is output data measured by G-sensor.

·       linear_acceleration[0] 是物体真实的线性加速度。

·        

·       引用Google代码中注释的原话（Alow-pass filter can be used to isolate the force of gravity），

·       再结合网上多方面资料的理解，这里是把重力的变化认为是低频活动事件，比如手机多半是平稳的把持，不会总是晃动，

·       于是，套用低频滤波的概念，将alpha设定为0.8后，即可过滤掉突然的瞬时加速度从而得到真正的重力，

·       换句话说，重力分量的变化因为不会非常频繁，

·       所以使之变化的很平缓，即平滑的缓慢的变化，这点从实际的测试数据中也可以看出来。

·       举个例子，如果套用这个公式，原本物体静止时x=0，y=0，z=9.8，当突然将物体状态改变为x=9.8，y=0，z=0，

·       得到重力三个轴分量的输出gravity[0],gravity[1],gravity[2]会慢慢的趋近于9.8，0，0，而不是立马变成9.8，0，0。

               所以，单单从实验数据上看，这种简单的一阶低通滤波并不适合我们计算实时空间轨迹位移的场景

 

2.    GoogleAOSP source code中有段被注释掉的legacy代码片段，（见下面上封邮件的红色字体部分）

·       广泛了解低通滤波器的计算方法后，我的结论是：此公式并不适用于指环移动控制的使用场景。

·       原因是：指环移动动作过程中，重力在三个轴的输出分量是持续变化的，并非较低频率的事件。

·        

·       下面是详细的解释过程，我的粗浅理解

·        

·       第一步：分离出重力

·       gravity[0] = alpha * gravity[0] + (1 - alpha) *event.values[0];  （e.g. alpha is 0.8）              

 

·       第二步：去除重力得到物体自身的加速度

·       linear_acceleration[0] = event.values[0] -gravity[0];

 

·       其中gravity[0]是分离/过滤出来的重力，alpha is filter factor, event.values[0]is output data measured by G-sensor.

·       linear_acceleration[0] 是物体真实的线性加速度。

·        

·       引用Google代码中注释的原话（Alow-pass filter can be used to isolate the force of gravity），

·       再结合网上多方面资料的理解，这里是把重力的变化认为是低频活动事件，比如手机多半是平稳的把持，不会总是晃动，

·       于是，套用低频滤波的概念，将alpha设定为0.8后，即可过滤掉突然的瞬时加速度从而得到真正的重力，

·       换句话说，重力分量的变化因为不会非常频繁，

·       所以使之变化的很平缓，即平滑的缓慢的变化，这点从实际的测试数据中也可以看出来。

·       举个例子，如果套用这个公式，原本物体静止时x=0，y=0，z=9.8，当突然将物体状态改变为x=9.8，y=0，z=0，

·       得到重力三个轴分量的输出gravity[0],gravity[1],gravity[2]会慢慢的趋近于9.8，0，0，而不是立马变成9.8，0，0。

              所以，单单从实验数据上看，这种简单的一阶低通滤波并不适合我们计算实时空间轨迹位移的场景

·        

3.    复化Simpson积分公式的运用

·       或许对结果有一定影响，具体数据明日做详细的统计。

·        

·       这里简单介绍下Simpson公式的大致思想。

·       先从积分计算说起，(都是我的理解，难免存在错误和不当)，

·       求一个函数f（x）的a -> b的积分，往往通过把a到b分成n份，

·       依次求出n个长方体的面积，再求和来近似等于积分数值。

·       份数分的越多，数值越接近真实值，直至极限。

·        

·       插值型求积思想就是在现有的等分周期内再插入多个数值，

·       并进行积分，使之最终的积分值更趋近于真实值。

·        

·       Newton-Cotes公式是一种插值型求积公式，

·       当求积系数中的n=2时，公式为

·      

·       称作：Simpson公式

·        

·       但不幸的是，在我们利用加速度计算位移的场景中，

·       我们并不知道加速度对时间的方程式，

·       可以利用的只是一些基于固定时间周期的加速度序列。

·       所以，要套用Simson公式，就只能每三个点作为计算速度积分的一个周期。

·       这也是下面这篇论文中提出的方法，链接如下：

·       http://wenku.baidu.com/link?url=ljWZZ_2Xr9CoQ9KwfsdwY7D9adM8gedYk3Xuxn_OUtbghR029uTGH_YjfxtN86ChDESzZrcyUf7lxwNUHpRrWmun6-qusIi782_fcnHlNke

·        

·       总结一下，之前是每两个数据周期计算一次速度和位移，

·       现在套用simpson公式，每三个数据周期计算速度和位移，

·       效果如何，只能看后面的测试数据。

·        

4.    用MCU计算出来的角度来去除掉G-sensor数据中的重力分量

·       输入：MCU计算出的欧拉角和重力加速计的数据A

·       方法：利用旋转矩阵乘法算得当前重力在三个轴的分量a，然后A-a得到物体实际的加速度

·        

·       但这种方法存在两个问题会产生误差：               

·                  1. MCU计算的欧拉角其中yaw值（航向角）没有进行滤波修正，误差会逐渐累计，后续会采集数据来形象的说明

·                              解决方法一：更新MCU固件，添加在任意时刻进行航向角yaw的修正接口，缺点是：因为需要人工手动校准，便会降低用户使用感受

·                              解决方法二：更新MCU固件，并加入磁力计进行滤波修正，缺点是：磁力计的噪声问题会影响数据的准确性，但是依然可以将yaw值修正到容差范围内

·                  2. MCU计算的欧拉角其中pitch和poll即使有滤波修正，也并非瞬时修正，而是逐渐收敛的，并且当指环静止时才会被修正为正确的数值   

·                              换句话说，在指环不停的运动过程中，即使pitch和poll数值误差已经很大，但此时用重力加速计计算的角度已经存在误差，所以相当于拿着错误的数据来滤波修正，自然也就不够准确了。当指环移动速度越快，误差越大。后续也会采集数据来形象的说明