12.25号之前飞起来，11月初结束嘛？
刚体运动及姿态描述（欧拉角、轴角法）
坐标的旋转，左乘右乘，向量旋转，二维实部虚部怎么变成三维
轴角法，几何的角度
xyz轴旋转表示，轴角法p到p1含角度，几何
（目前猜测是p（x，y）到p1（x1，y1）和那个u和旋转通过几何关系得到转换公式）
机体坐标系，大地坐标系
一步一步推，跟着思路走，思考时间
【要讲的：
欧拉角，左手法则，左手系和右手系，内旋和外旋
旋转的顺序（基准是什么）（要有一个统一的约定）
以向量为切入，最后转到坐标的旋转
矩阵的相乘，旋转矩阵
罗德里德斯公式，将欧拉角和四元数统一起来】
参考资料
三维空间刚体运动
重点视频
内外旋回退证明
有实物且讲得很清楚（后面有很多还可以用到的视频）
纯数学证明：四元数，轴角法
飞控模型及仿真演示
1 刚体运动
1.1 旋转矩阵与变换矩阵
- 三维空间由3个轴组成，则一个空间点的位置可以由3个坐标指定
- 刚体：在运动和受力作用后，形状和大小不变，而且内部各点的相对位置不变的物体
- 对于一个刚体，不光有位置，还有自身的姿态
- eg：相机可以看成三维空间中的刚体，则位置就是说相机在空间中的哪个地方，姿态是指相机的朝向(数学语言描述：相机处于空间(0，0，0)处，朝向正前方)
4.1 刚体的定轴转动
- 刚体：在外力作用下，形状和大小都不发生变化的物体。(任意两质点间距离保持不变的特殊质点组。)
- 刚体时理想模型，为简化问题而引入。
- 刚体运动
- 平动：刚体中所有点的运动轨迹都保持完全相同。（刚体上任意两点连线轨迹后仍然保持平行）
- 特点:各点运动状态一样如:v、a 等都相同。
- 所以随便选择一个质点就可以研究，运动规律可以直接引用前面讲的质点的运动的规律。
- 转动：分为定轴转动和非定轴转动
- 刚体的一般运动：（定轴转动）随质心的平动+绕质心的转动的合成
- 刚体转动的角速度和角加速度
- 右手螺旋方向：右手伸出，四指指向转动方向，拇指方向就是角速度的方向
- 可以用角速度的正负表示角速度的方向，这里也和选取的正方向有关
- 角量和线量的关系
- X乘的关系：右手先指向矢量w，然后再转向矢量r的方向。拇指方向就是v线速度的方向
- 点乘和叉乘
- 向量的数量积（点积）
- 向量的向量积（叉积）
- 正负符号由行号列号之和决定（-1）**（m+n）
角加速度为常量的时候，刚体做匀变速运动。
- 力矩
- 用来描述力对刚体的转动作用的物理量。
注意：需要移动到共起点
平行的力不产生力矩
无人机DIY入门系列教程：(三）姿态解算上：姿态角、欧拉角与旋转矩阵
- 欧拉角
- 姿态角
- 四元数
- 旋转矩阵
- 方向余弦矩阵
- 轴角
世界坐标系（东北天）
机体坐标系（右前上）
- 姿态角
- 描述飞机姿态的三个角度：偏航角、俯仰角、横滚角
- 偏航角：飞机机体坐标系y轴在水平面上的投影与世界坐标系y轴之间的角度
- 俯仰角：飞机机体坐标系y轴与水平面之间的角度
- 横滚角:飞机机体坐标系z轴和y轴所在的铅锤面之间的角度
- 欧拉角描述旋转：把原始姿态到目标姿态转化过程分成绕x轴、绕y轴、绕z轴的三次旋转（绕原来的xyz轴）。旋转的顺序很重要
（这里可以用unity讲）
欧拉角旋转所绕的轴都是飞机机体坐标系的轴，内旋，动态欧拉角。
相对的：绕世界坐标系的静态欧拉角。最终可以等效
那么排列组合一共就有六种欧拉角
其中只有一种绕xyz3轴旋转的角度刚好与姿态角的定义相吻合
根据东北天，右前上的坐标定义方式按照ZXY顺序旋转的欧拉角刚好和姿态角相吻合
从二维坐标系旋转来说，将坐标系逆时针旋转一定角度
转化成三维坐标系下绕z轴所做的旋转，绕z轴的旋转矩阵
绕y轴的旋转矩阵
绕x轴的旋转矩阵
通过这个旋转矩阵可以求出坐标系1中的某个向量在坐标系2中的坐标
例如：加速度计可以测量三个轴的加速度，当加速度计水平静置在
当飞机在空中有一定的姿态角之后， 三种加速度
通过旋转矩阵可以知道重力加速度在机体坐标系下的分量
由于重力加速度总是与水平面垂直，凭借重力加速度，我们是无法感知偏航角变化的，在仅有加速度计的情况下，偏航角转了和没转相同，干脆令偏航角为0，实际上就是把绕z轴旋转的那个矩阵去掉
只需构造一下正反切就可以求出俯仰角和横滚角了
实际飞行的话，仅凭加速度计是不行的，加速度计对噪声敏感，一般会加上陀螺仪来与之融合。旋转矩阵的缺点：万向锁问题、较多三角函数运算。如今飞控中大多数使用四元素来作为姿态更新的方式，仅仅在输出的时候再转化为欧拉角
将第一个视频的知识点弄懂
线性代数知识（17：00~17：55）
晚上将第二个视频和结构弄懂
1 姿态解算预备知识
1.1 姿态解算背景
在飞行器中，飞行姿态是非常重要的参数，以飞机自身的中心建立坐标系，当飞机绕坐标轴旋转的时候，会分别影响偏航角（ψ）、横滚角（φ）及俯仰角（θ）。如果我们知道飞机初始状态是图1中左上角的状态，只要想办法测量出基于原始状态的三个姿态角的变化量，再进行叠加，就可以获知它的实时姿态了。
地面上飞行器是一个姿态——初始姿态，飞行器升空是一个姿态，每个时刻对应一个姿态——实时姿态。
运动是绝对的，静置是相对的。我们确定了初始姿态，再寻找到实际的姿态和初始姿态的差异。将差异加给初始坐标，就得到了实时坐标。
图1 yaw(偏航ψ), pitch(俯仰θ), roll(横滚φ)，对应z,y,x三个坐标轴
- 偏航角（Yaw）：绕Z轴旋转，改变物体的朝向。
- 俯仰角（Pitch）：绕Y轴旋转，改变物体的上下倾斜。
- 横滚角（Roll）：绕X轴旋转，改变物体的左右倾斜。
右手定则：如图2所示，右手食指指向OX轴正方向，中指指向OY轴正方向，大拇指所指方向即OZ轴正方向。进一步，如图2所示，要确定旋转正方向，用右手大拇指指向旋转轴正方向，弯曲四指，则四指所指方向即旋转正方向。
我们所有的推导基于右手坐标系，因为姿态不止是一个坐标系，还要围绕坐标轴进行旋转。需要规范旋转的方向。
1.2 坐标系
地理坐标系（地球固连坐标系）：该坐标系用于研究飞行器相对于地面的运动状态，通常以飞行器起飞位置作为坐标原点，Z轴沿当地地理垂线的方向(重力加速度方向)，XY轴可沿当地经纬线的切线方向，也可先让X轴在水平面内指向某一方向，Z轴沿重力加速度方向垂直于地面向下，然后按照右手定则来确定Y轴。
载体坐标系（机体坐标系）：载体坐标系以运载体的质心为原点，一般根据运载体自身结构方向构成坐标系，如Z轴上由原点指向载体顶部，Y轴指向载体头部，X轴沿载体两侧方向。上面说基于飞机建立的坐标系就是一种载体坐标系，可类比到汽车、舰船、人体、动物或手机等各种物体。（可以以机体质心为原点，前进方向为x轴，拇指指向，Z轴垂直向下，然后食指指向y轴，合理的更能理解好三个角了）
抽象来说，姿态是“载体坐标系”与“地理坐标系”之间的转换关系。
两种姿态，初始姿态是已知的，实时姿态是未知的，通过角度变化来推断实时姿态。
两个坐标系→对应两种姿态
1.3 欧拉角
欧拉角用来描述刚体/移动坐标系在一个固定坐标系中的姿态。简单的说是使用XYZ三个轴的旋转分量,来描述一个6自由度的旋转。根据欧拉定理，刚体绕固定点的旋转可以看成绕该点的若干次有限旋转的合成，地球固连坐标系绕固定点经过三次基本旋转，可以得到机体坐标系。
在这三次旋转中，旋转轴是待转动坐标系的某一对应坐标轴，旋转角度即为欧拉角，因此，姿态矩阵与三次基本旋转的顺序密切相关可以用三次基本旋转矩阵的乘积表示。
虽然目前还不太懂数学，但根据概念可以知道我们可以通过三个欧拉角来描述通过某个顺序旋转所得到的某一个坐标系。
旋转次序：欧拉角的旋转顺序不能随意改变，要注意描述时所参考的坐标系。【本质上是因为：线性变换不满足乘法交换律。由于不同旋转次序所表述的姿态不同，为了得到一个确定的姿态，我们就需要规定一个确定的旋转次序。】
我们接触的比较多的是yaw(偏航ψ), pitch(俯仰θ), roll(横滚φ)。三个变量一般对应(车体,飞行器的)z,y,x三个坐标轴.
姿态矩阵：一个3x3的矩阵，用于描述一个坐标系相对于另一个坐标系的旋转。这个矩阵可以通过将三个基本旋转矩阵（每个旋转对应一个矩阵）相乘来得到。旋转矩阵的乘积顺序必须与欧拉角的旋转顺序相匹配。
1.3 方向余弦矩阵与旋转矩阵
旋转矩阵可以用欧拉角来表示也可以用四元数来表示
1.3.1 方向余弦矩阵
在坐标系R中设v为一个空间向量，在R坐标系下的投影为（vx,vy,vz），与其x y z 轴分别成α β γ角度，则cosα,cosβ,cosγ分别为v在三轴的方向余弦，大小分别于|vx|,|vy|,|vz|。
在一个平面内对向量进行旋转相当于对坐标进行旋转，初始状态下令载体坐标系和参考坐标系完全重合，载体转动时，载体坐标系会相对于参考坐标系转动，把载体坐标系的三个轴当做三个单位向量（vx,vy,vz）,每个载体坐标轴都可以在参考坐标系上找到三个对应的方向余弦。最终得到九个方向余弦，把九个方向余弦写成矩阵形式就是方向余弦矩阵。
方向余弦矩阵令机体坐标系与地球坐标系（参考全局坐标系）初始重合，原点始终重合，通过机体到参考坐标系各个轴的投影也就是9个余弦值来表述。
1.3.2 旋转矩阵
任意物体绕自己的各个轴旋转，其姿态的变化情况可以用旋转矩阵R来表示。
所谓姿态的变化情况，可以这样理解:
前提:大地坐标系G系与大地固连(G系为地球固连坐标系)，物体坐标系B系与飞机固连(B系为机体坐标系)。初始状态下，令两个坐标系完全重合。
问题:现在我在飞机上任取一点A，该点在大地系的坐标为 A0=(x0,y0,z0)，(显然此时该点在机体系的坐标是一模一样的)，那么当飞机绕自己的X轴旋转θ弧度后，问该点在大地系的新坐标A1?
答:A1=Rx*A0，这就是旋转矩阵所代表的姿态变化情况。而且，如果飞机先绕自己的X轴转(横滚 Roll)，再绕自己的Y轴转(俯仰Pitch)，再绕自己的Z轴转(航向Yaw)，求点A在大地系的新坐标，只需把坐标A0依次左乘3个旋转矩阵即可，也即A1=Rz*Ry*Rx*A0。如果旋转顺序不同，这3个R矩阵的连续左乘的结果，肯定是不同的(因为矩阵乘法不满足交换律，除了一类特殊矩阵以外，显然旋转矩阵几乎不存在满足交换律的情形)，这就是所谓的欧拉角的旋转是有顺序的。Roll、pitch、yaw三次旋转的角度就叫欧拉角。
1.3.4 用刚体旋转矩阵推导方向余弦矩阵
以XYZ次序旋转的泰特布莱恩角为例(下述推导基于此进行)，根据欧拉角的定义，从地球固连坐标系到机体坐标系的旋转可以通过三次旋转完成，即A1=Rz*Ry*Rx*A0。那么我们可以由欧拉角求得三个旋转矩阵，进而求得方向余弦矩阵Rz*Ry*Rx，反之，我们也可以由方向余弦矩阵求得欧拉角。
我们来思考一下：物体旋转是不是可以理解为坐标系反向旋转。所以物体的旋转可以理解为双向奔赴。如果只看物体，物体自身的旋转，也可以看作物体没有旋转，坐标系反向旋转。所以就叫两种矩阵，物体自身的旋转矩阵叫刚体旋转矩阵。另外一种旋转矩阵叫做坐标系旋转矩阵。
XYZ方向旋转，我们从Z轴正方向看过去（Z轴和原点重合）
最后一个实际上还漏掉了一个矩阵[X,Y,Z,1](竖式)。实际上就是[X,Y,Z,1](竖式)左乘上这样一个矩阵来表示变化。其实就代表旋转，初始坐标左乘旋转矩阵得到旋转后的坐标。这个矩阵就是Z轴刚体旋转矩阵。
伏笔：由于物体的逆时针旋转等效为坐标系的顺时针旋转
由于R是一个正交矩阵，所以可以求得这个矩阵的转置。再通过kesai=-kesai。代入计算得和坐标系旋转矩阵是一样的。所以我们可以得出结论，坐标系矩阵就是刚体矩阵的转置。
我们的旋转次序是X,Y,Z，所以依次左乘X旋转矩阵，Y旋转矩阵，Z旋转矩阵得到在机体坐标系中的坐标。e代表earth，地球坐标系。b表示body，机体坐标系。
方向余弦矩阵的第一种表现形式，用欧拉角来表示
1.3.5 用坐标系旋转矩阵推导方向余弦矩阵
当前的机体坐标可以通过对大地坐标进行三次绕轴的基本旋转得到,用坐标变换矩阵的形式可以表示为R从e到b（从地球到机身）
这表示了从大地坐标到机体坐标的变化,而姿态解算是通过机体坐标的变化来计算机体坐标相对于大地坐标的状态。
即讲一个不确定的机体的坐标转换到确定的大地坐标
按照ZYX的旋转矩阵
因为这个矩阵是围绕Z轴旋转的，所以我们成为RZ
线性代数
了解同一个点在不同坐标系的关系，可以使用线性代数
结果是两个向量所围成平行四边形的主对角线和副对角线
Q：为什么一些二维向量可以构成整个平面内的向量，一些不行只能构成一条直线上的向量——线性无关和线性相关
标准正交基
坐标系的变化
画图法在一些复杂的坐标系里或者高维坐标系无法实现
实现坐标系的变化，通过矩阵，可以看作有N个M维的向量
如果矩阵列是线性相关，某些基向量就是共线的，所占用的空间就只会在一条直线上，即这个矩阵就是一个高维到低维的线性变化，会被压缩到低维坐标系上。
矩阵向量乘法:向量的变换，把矩阵所描述的线性变换作用于这个向量上。将矩阵理解成一个法则F,向量是X，其实矩阵乘法就是一个函数F（X）。矩阵相乘的行数，需要和列向量所包含的元素数量相等。
这个矩阵称为“基变换矩阵”或“坐标变换矩阵”，它的作用是将向量从基坐标系下的坐标转换到标准坐标系下的坐标。
反之，如果我们想从标准坐标转换到基B下的坐标，需要求矩阵A逆（如果基是线性无关的）。
矩阵→变换 逆矩阵→还原
从右往左读：在几何上先进行右边那个矩阵所代表的线性变化，然后再进行左边那个矩阵所代表的线性变化
实线加法，虚线减法
通过A*（A的伴随矩阵来求逆矩阵）
- 基底与坐标变换
- 在我们整个的研究中，我们一直采用标准正交基进行研究
- 坐标系变化：通过矩阵向量乘法
- 向量的变换的本质，把矩阵所描述的线性变换作用于这个向量上。将矩阵理解成一个法则F,向量是X，其实矩阵乘法就是一个函数F（X）。
- 每个线性变换都可以由一个矩阵来表示。将变换后坐标系的基底构成矩阵。这个矩阵叫做这个矩阵称为“基变换矩阵”或“坐标变换矩阵”。
- 几何意义是：把一个向量在该基坐标系下的坐标（a，b）转换成标准坐标系下的坐标。
- 反之，我们想要从标准坐标系转换到新基坐标系的坐标。根据数学可知，我们需要两边乘以A逆（因为基底是线性无关的向量，所以构成的矩阵满秩，行列式不为0，所以A可逆。也由此可推出基地坐标系一定可逆）
总结：
- 坐标变换矩阵*该坐标系下的向量坐标=标准坐标系下的向量坐标
- 坐标变换矩阵的逆矩阵*标准坐标系下的向量坐标=变换后的向量坐标
旋转矩阵
考虑两个框架，一个空间框架s和一个身体框架b。编写单位坐标来表示框架b相对于s的方向。b坐标系的轴（方向向量）用s坐标系的坐标来表示。
R_sb
第二个下标b表示其方向代表的坐标系。第一个下标s代表参考系。有时我们隐式显示下标
（？）矩阵的9个条目必须受到6个约束
- 旋转矩阵的三个用途
- 表示方向
- 更改向量参考系
- 旋转矢量/坐标系
演示参考系的变化，参考旋转矩阵R_bc,表示帧c在帧b坐标中的方向。
我们可以编写坐标矩阵来表示相对于s的c，这个矩阵是c的轴放到s的坐标系中，产生旋转矩阵R_sc。
如果我们将s的坐标轴写入c的坐标系，那么生成旋转矩阵就是R_cs。R_cs是R_sc的转置或者逆。
演示参考系的变化，可以考虑旋转矩阵R_bc,表示帧c在帧b坐标中的方向。
如果我们想用s坐标系而不是b坐标系来表示c坐标系
下标消除原则，如果第一个矩阵的第二个下标和第二个矩阵的第一个下标匹配，他们相互抵消，使得剩余的两个下标按照正确的顺序排列
更改向量的参考系
p_b是点p以b坐标系表示时的位置。如果需要用s坐标表示p，我们可以将p_b
最终用途：旋转向量/坐标系
举例，如果b坐标系是通过旋转从s坐标系得到，即s坐标系绕Z_s轴旋转90°。所以我们可以将R_sb视为绕z轴旋转的运算。如果我们将向量p_b*这个旋转算子，我们会得到p_s。
但如果向量p_s在s坐标中，则没有下标取消。且我们得到一个新的向量p_prime_s，是通过将p_s绕z_s旋转90度得到。
矢量已经旋转，但是仍然在原始坐标系s中表示。
这里的预乘和后乘，就代表着右乘和左乘
如果预乘R，则旋转轴可以解释为坐标系的Z轴的第一个下标s。最终得到一个新的坐标系c_prime,仍然以s表示。
如果后乘R，则旋转轴将解释为坐标系的Z的第二个下标c，得到一个不同的新坐标系，仍然用s表示
再理解一下这里的预乘R和后乘R
核心差别就是 “矩阵作用在向量上（active / 预乘）” 和 “矩阵作用在基向量／坐标系上（passive / 后乘）”。
- 预乘R（写成 v′=R v ）通常表示对向量做主动旋转（active rotation）。这时旋转轴是用向量当前所用的坐标系来解释的——也就是“轴在v的坐标系里”（如果v用s表示，那就是s的z轴）。
- 后乘R（把R右乘在一组基向量／坐标变换矩阵上，例如S′=S R）通常表示对坐标系/基做被动旋转（passive，换坐标系）。这时旋转轴是用基矩阵那一侧的坐标系来解释。
这个矩阵告诉我们坐标系1相对于坐标系0如何旋转（在坐标系0内旋转）
假设方向向量（基底表示坐标轴）长度为1
（y1在x0的坐标投影是负数）
针对正交矩阵推出的旋转矩阵如下
然后再绕着新的x1轴旋转
