互补滤波算法姿态解算
互补滤波算法姿态解算
互补滤波算法姿态解算互补滤波算法是一种常用的姿态解算方法，广泛应用于飞行控制系统、惯性导航系统和姿态测量装置等领域。
该算法通过融合加速度计和陀螺仪的测量数据，实现对目标物体的姿态估计。
下面将详细介绍互补滤波算法的原理和应用。
一、互补滤波算法原理互补滤波算法主要基于两个观察结果：加速度计测量的重力矢量与陀螺仪测量的角速度。
加速度计测量的重力矢量可以提供一个参考方向，而陀螺仪测量的角速度可以提供连续更新的姿态角度信息。
互补滤波算法通过将这两个信息进行互补，实现对目标物体的姿态解算。
互补滤波算法的核心思想是将陀螺仪测量的角速度逐步累加，计算出目标物体的旋转角度，并将该角度作为姿态角。
由于陀螺仪的测量存在漂移等误差，随着时间的增加，角度积分的误差会越来越大。
为了避免这个问题，互补滤波算法通过加速度计测量的重力矢量进行补偿。
具体地，互补滤波算法使用一个权衡参数α（取值范围为0到1）来控制加速度计和陀螺仪的权重。
在每次更新姿态角时，新的姿态角为上一次的角度加上陀螺仪积分得到的角度乘以α，再加上加速度计测量的角度乘以1-α。
姿态角更新公式如下：θ(n) = α * (θ(n-1) + Δθ(n)) + (1-α) * θ_acc(n)其中，θ(n)为当前的姿态角，θ(n-1)为上一次的姿态角，Δθ(n)为陀螺仪积分得到的角度变化，θ_acc(n)为加速度计测量的角度。
二、互补滤波算法应用飞行器的姿态控制需要实时获取飞行器的姿态信息，并根据预设的姿态稳定控制侧滑、上升/下降等动作。
互补滤波算法可以通过融合加速度计和陀螺仪的测量数据，快速准确地获取飞行器的姿态信息。
在飞行器的姿态控制中，将陀螺仪测量的角速度作为连续更新的角度信息，用于控制飞行器的姿态稳定。
同时，通过加速度计测量的重力矢量可以提供一个参考方向，以补偿陀螺仪测量的角速度的误差。
1.初始化姿态角为加速度计测量的角度。
2.按照一定的时间间隔，读取加速度计和陀螺仪的测量数据。
姿态解算方法（1）_一种互补滤波方法
姿态解算⽅法（1）_⼀种互补滤波⽅法姿态解算⽅法（1）_ ⼀种互补滤波⽅法本⽂内容主要是参考《An efficient orientation filter for inertial and inertial/magnetic sensor arrays 》此篇论⽂，由于其在普通飞控中的⼴泛运⽤，所以这⾥简单记录学习感受以期后续详细思索。
当然也是因为这篇⽐较易懂，正好可以锻炼⼀下我捉襟见肘的表述能⼒。
写到这⾥可能已经开始被骂了。
⼏个较好的博⽂链接如下：（1）（2）这是在博客园的第⼀篇博⽂，想来⾃⼰⼀直攒在电脑⾥的东西，不仅容易被弄没了，也没办法被⼤家纠错批评，故也就不在乎好坏把想到的都写下来。
⾔归正传，⾸先，需要了解下四元数的知识，这在维基（3）和（1）中都有很好的解释了，这⾥为了争取篇幅讲述算法核⼼部分就不再赘述。
（3）⾓加速度的事那么进⼊关于姿态确定的部分。
三轴陀螺仪模块可以测量传感器所在平⾯的⾓加速度，⼀般来说MPU6050模块返回的是三个⾓速度值。
如果初始姿态是已知的，时间的计量⼜是准确的，那么就可以积分出姿态了。
可惜的是⾓速度的测量准确度实在是不能积分出⼀个准确的位置值。
不过这⾥还是说⼀下这个积分过程。
sω=0ωxωyωzS E ˙q=12SEˆq⊗Sωsω为传感器获得的⾓速度值，再次提醒这⾥的值可能是不准确的。
SEˆq是表⽰传感器相对于地⾯坐标系的SO（3）变换四元数的估计，这⾥⽤于表⽰传感器姿态的估计。
⊗是四元数的⼀种运算⽅式，在上⽂的参考⽂献中可以找得到的。
差分的形式可以表⽰为：δS E qω,t=12SEˆqt−1⊗SωtSE qω,t=S E ˆqt−1+δSE qω,tΔtΔt是两次预测之间的时间差，准确的说是传感器上⼀次数据和这⼀次数据获取时间差。
其他的部分和连续的形式对应就不多说了。
基于场⽅向观测的姿态求解⾸先把姿态观测问题看做是⼀个求尽可能满⾜观测结果的姿态的优化问题。
基于互补滤波的飞行器姿态解算
姿态解算一、主线姿态表示方式：矩阵表示，轴角表示，欧拉角表示，四元数表示。
惯性测量单元IMU（Inertial Measurement Unit）：MPU6050芯片，包含陀螺仪和加速度计，分别测量三轴加速度和三轴角速度。
注意，传感器所测数据是原始数据，包含了噪声，无法直接用于飞行器的姿态解算，因此需要对数据进行滤波。
滤波算法：非线性互补滤波算法，卡尔曼滤波算法，Mahony互补滤波算法。
二、知识点补充加速度计和陀螺仪加速度计：加速度计,可以测量加速度,包括外力加速度和重力加速度,因此，当被测物体在静止或匀速运动（匀速直线运动）的时候,加速度计仅仅测量的是重力加速度,而重力加速度与R坐标系（绝对坐标系）是固连的,通过这种关系,可以得到加速度计所在平面与地面的角度关系也就是横滚角和俯仰角。
把加速度传感器水平静止放在桌子上，它的Z轴输出的是1g的加速度。
因为它Z轴方向被重力向下拉出了一个形变。
可惜的是，加速度传感器不会区分重力加速度与外力加速度。
所以，当系统在三维空间做变速运动时，它的输出就不正确了，或者说它的输出不能表明物体的姿态和运动状态。
陀螺仪：陀螺仪测量角速度。
陀螺仪模型如图1所示，陀螺仪的每个通道检测一个轴的旋转。
图1[引自网络]上图中，Rxz是R在XZ面上的投影，与Z轴的夹角为Axz。
Ryz是R在ZY面上的投影，与Z轴的夹角为Ayz。
陀螺仪就是测量上面定义角度的变化率，换句话说，它会输出一个与上面这些角度变化率线性相关的值。
加速度计工作原理介绍（摘自网络）大多数加速度计可归为两类：数字和模拟。
数字加速度计可通过I2C，SPI或USART方式获取信息，而模拟加速度计的输出是一个在预定范围内的电压值，你需要用ADC（模拟量转数字量）模块将其转换为数字值。
不管使用什么类型的ADC模块，都会得到一个在一定范围内的数值。
例如一个10位ADC模块的输出值范围在0-1023间。
假设我们从10位ADC模块得到了以下的三个轴的数据：===586,630,561AdcRx AdcRy AdcRz每个ADC 模块都有一个参考电压，假设在我们的例子中，它是3.3V 。
欧拉角的求取与互补滤波器在姿态解算中的应用
欧拉角的求取与互补滤波器在姿态解算中的应用李天助（1）根据欧拉定理：刚体绕固定点的位移也町以是绕该点若干次有限转动的合成。
（2）在欧拉转动中，将参考坐标系（绝对坐标系，实验室参考系，是静止不动的，地理坐标系，也是就是正北正东和垂直地面，由于相对很小的四轴飞行器而言，地球可以看成是一个无限大的平面。
其原点是飞行器的旋转中心，也就是说E彳J : &次进行欧拉转动时：个旋转轴的交点就是其原点？如果是的话，那么这两个原点就是重合的了。
）转动三次得到載体坐标系（也称为机坐标系，其原点是飞行器載体的中心）。
图参考坐标系与我体坐标系的关系图如何通过三次绕载体坐标系三个轴来进行参考坐标系到载体坐标系的转换（此图中的旋转过程好似与绕分立的三个柚旋转的过程有点不一样）欧拉角确定的姿态矩阵是三次坐标转换矩阵的乘积。
（将英分解为三次绕以载体坐标轴的二个轴为旋转轴的旋转，总旋转的介成就是三次分立旋转矩阵的乘积。
通过阅读以卜的分析也就可以知道这是合理的，因为这是一个分步的过程，所以使用相乘。
）这些坐标转换矩阵都有如下标准形式：'10 ocos 8 0 一 C0S0 Sill# 0" &(©) = 0COS0 sin0 &(0)= W 0 1 0 & (以_S1I10 cos?/ 0 0 ■ 一sin© cos 。
SLI10 ■ 0 cos 8 ■ 0 ■ 0 1 ■公式⑴上而的三个旋转矩阵分别是绕x 、y 、z 轴旋转得到的，其代体的推算过程是通过投影和 来实现的。
（具体推导过程可以参见周建龙等人编写的《几何与代数》中P118页例3.19）。
显然,我们知道矩阵的乘法是不满足交换律的，所以轴旋转的H 宇不凤比如x->y->z, 与x->z->y 是完全不一样的。
通过以上的分析，就可以知道姿态矩阵还有三次转动的顺序有关。
为了避免万向节锁（Gimbal Lock ）,选择介适的转动顾序是十分关键的。
无人机导航与控制系统中的姿态解算算法优化
无人机导航与控制系统中的姿态解算算法优化随着无人机技术的不断发展，无人机导航与控制系统的研究成为了人们关注的焦点。
在无人机的飞行过程中，如何准确地获取和解算无人机的姿态信息成为了一个重要的问题。
本文将讨论无人机导航与控制系统中的姿态解算算法优化。
姿态解算算法是无人机导航与控制系统中的一个关键环节。
它主要负责实时获取无人机的姿态信息，包括滚转角、俯仰角和偏航角等，以实现无人机的稳定飞行和精确控制。
目前，常用的姿态解算算法包括互补滤波算法、卡尔曼滤波算法和扩展卡尔曼滤波算法等。
互补滤波算法是一种简单且实用的姿态解算算法。
它基于加速度计和陀螺仪的测量数据，通过加权和叠加的方式计算得出无人机的姿态信息。
互补滤波算法的优点是实时性好，计算简单，但准确性相对较差。
在复杂环境中，由于传感器的误差和噪声等因素的影响，互补滤波算法容易出现累积误差，导致姿态解算结果不准确。
卡尔曼滤波算法是一种基于状态空间模型的姿态解算算法。
它通过对系统的状态和观测方程进行建模，利用先验信息和观测数据进行动态的状态估计和参数拟合。
卡尔曼滤波算法的优点是具有较高的准确性和鲁棒性，能够有效地抑制传感器误差和噪声的影响。
但由于需要对系统进行建模和参数估计，卡尔曼滤波算法的计算复杂度较高。
扩展卡尔曼滤波算法是卡尔曼滤波算法的一种改进算法。
它通过对非线性系统进行线性化，采用线性卡尔曼滤波的方式进行状态估计。
扩展卡尔曼滤波算法的优点是兼具卡尔曼滤波算法和互补滤波算法的优点，既具有高准确性和鲁棒性，又具有较低的计算复杂度。
但扩展卡尔曼滤波算法也存在一些问题，如对传感器误差和观测噪声的敏感性较高，对系统非线性程度的依赖性较强。
在姿态解算算法的优化方面，可以采取多种策略来提高其准确性和鲁棒性。
首先，可以通过引入更多的传感器来获取更多的姿态信息，如磁力计和气压计等。
这样可以增加姿态解算的可观测性，提高解算结果的准确性。
其次，可以通过优化传感器的精度和性能来减小误差和噪声的影响。
互补滤波算法姿态解算matlab仿真程序
互补滤波算法姿态解算matlab仿真程序互补滤波算法姿态解算是一种常用的姿态解算方法，下面是一个基于MATLAB的仿真程序，本文不包含任何网址、超链接和电话。
1. 系统模型建立假设一个惯性测量单元（IMU）包含3轴陀螺仪和3轴加速度计，其输出分别为角速度 $\omega$ 和加速度 $a$。
设重力矢量在IMU本体坐标系中的表示为 $g$。
根据牛顿第二定律，在本体坐标系中，加速度 $a$ 和重力矢量 $g$ 的叠加产生了一个力 $F$，即：$$F = m(a - g)$$其中 $m$ 是IMU上的物体质量。
2. 姿态解算通过对IMU输出的角速度 $\omega$ 和加速度 $a$ 进行互补滤波可以得到 IMU 的姿态信息，具体姿态解算过程如下：（1）根据加速度计的读数 $a$，计算加速度矢量在 IMU本体坐标系中的表示 $a_b$，即：$$a_b = R_b^i a$$其中 $R_b^i$ 是从惯性坐标系到本体坐标系的旋转矩阵。
（2）计算得到滤波后的角速度 $\omega_f$，即：$$\omega_f = \alpha \omega_f + (1-\alpha)\omega$$其中 $\alpha$ 是互补滤波因子。
（3）根据滤波后的角速度 $\omega_f$，计算姿态的改变速率$\dot{q}$，即：$$\dot{q} = \dfrac{1}{2}q\begin{pmatrix}0 & \omega_f^T\\-\omega_f & 0\end{pmatrix}$$其中 $q$ 是四元数。
（4）根据姿态的改变速率 $\dot{q}$ 计算姿态 $q$ 的改变量$dq$，即：$$dq = \dot{q} dt$$其中 $dt$ 是采样时间。
（5）根据姿态 $q$ 和姿态改变量 $dq$ 计算新的姿态$q_{new}$，即：$$q_{new} = q \times dq$$其中 $\times$ 表示四元数的乘法。
互补滤波和卡尔曼滤波的融合姿态解算方法
传感器与微系统 （ Ｔ ｒ ａ ｎ ｓ ｄ ｕ ｃ ｅ ｒ ａ ｎ ｄ Ｍｉ ｃ ｒ ｏ ｓ ｙ ｓ ｔ ｅ ｍ Ｔ ｅ ｃ ｈ ｎ ｏ ｌ ｏ ｇ ｉ ｅ ｓ ）
２ ０ １ ７年 第 ３ ６卷 第 ３期
ＤＯＩ ： １ ０ ． １ ３ ８ ７ ３ ／ Ｊ ． １ ０ ０ ０ － ９ ７ ８ ７ （ ２ ０ １ ７ ） ０ ３ － － ０ ０ ６ ２ － － ０ ４
Ｂ ａ ｏ ｄ ｉ ｎ ｇ ０ ７ １ ０ ０ ３ ， Ｃ ｈ ｉ ｎ ａ ）
Ａｂ ｓ ｔ ｒ ａ ｃ ｔ ：Ａ ｉ ｍｉ ｎ ｇ ａ ｔ ｐ ｒ ｏ ｂ ｌ ｅ ｍ ｏ ｆ ｈ ｉ ｇ ｈ ｎ ｏ ｉ ｓ ｅ ， ｌ ｏ ｗ ｐ ｒ ｅ ｃ ｉ ｓ ｉ ｏ ｎ ｏ ｆ ｉ ｎ ｅ ｒ ｔ ｉ ａ ｌ ｍ ｅ ａ ｓ ｕ ｒ ｅ ｍｅ ｎ ｔ ｕ ｎ ｉ ｔ （ Ｉ ＭＵ） ａ ｎ ｄ ｌ ｏ ｗ ｐ ｒ ｅ ｃ ｉ ｓ ｉ ｏ ｎ ｏ ｆ
出了一种互补滤波和卡尔曼滤波相结合的融合算法 。该算法基于姿态角微分方程建 立系统的状态 方程 模 型， 利用互 补滤 波后 的姿态角作为 系统 的观测量 ， 再应用扩展 卡尔曼滤波 （ Ｅ Ｋ Ｆ ） 算法融合 了陀螺 仪 、 加 速
度计 和电子罗盘 的测量数据 。为验证该算法有效性 ， 用带有传感器 的开发板依次进行静态和动态测试 ， 实 验结果表 明： 结合 了互补 滤波 和卡尔曼滤波 的融合算 法 ， 在静 态时能够抑 制姿态角漂移 和滤 出噪声 ， 在 动 态时能够快速跟踪姿态 的变化 ， 提高了姿态角的解算精度 。 关键词 ：卡尔曼滤波 ； 互补 滤波 ； 姿态估计 ； 数据融合 ；惯性测量单元
ｐ ｒ ｏ ｐ ｏ ｓ ｅ ｄ ． Ｔ ｈ ｅ ａ ｌ ｇ ｏ ｉ ｒ ｔ ｈ ｍ ｅ ｓ ｔ ａ ｂ ｌ ｉ ｓ ｈ ｔ ｈ ｅ ｓ ｔ ａ ｔ ｅ ｅ ｑ ｕ ａ ｔ ｉ ｏ ｎ ｍｏ ｄ ｅ ｌ ｂ ａ ｓ ｅ ｄ ｏ ｎ ｄ ｉ ｆ ｆ ｅ ｒ ｅ ｎ ｔ ｉ ａ ｌ ｅ ｑ ｕ ａ ｔ ｉ ｏ ｎ ｏ ｆ ａ ｔ ｔ ｉ ｔ ｕ ｄ ｅ ａ ｎ ｇ ｌ ｅ ａ ｎ ｄ
互补滤波算法姿态解算
地磁场向量 可不可以采用和加速度计一样的修正方法来修正？ 加速度计在静止时测量的是重力加速度(0,0,1)T ，是有大小和方向的； 同理，地磁计同样测量的是地球磁场的大小和方向，只不过这个方向不再是竖直向下 而是与x轴（或者y轴）呈一个角度，与z轴呈一个角度。记作(bx,by,bz)T ，这里我 们让x轴对准北边，所以by=0，即 (bx,0,bz)T 倘若我们知道bx和bz的精确值，那么我们就可以采用和加速度计一样的修正方法来)
23
01
q q q q 2
2
2
2
0
1
2
3
这样，四元数可以用方向余弦、欧拉角表示，同样，欧拉角也可以用 方向余弦和四元数表示。
用方向余弦表示四元数
对于小角度位移，四元数参数可以用表示为
q 0
1 2
1
(1c11c22c33) 2
q 1
1
4q
c c (
)
32
23
0
三、姿态表示方法
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示，是一个3*3阶的矩阵，矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示，是一个3*3阶的矩阵，矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
q 2
1
4q
c c (
)
13
31
0
q 3
1
4q
c c (
)
21
12
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
而q0 称为四元数q 的实部，
称 qi ? q j ? q k 为q的虚部。
1
2
3
四元数的共轭为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义
q q ,q q q q q q 设 q ? ? i ? j ? k , ,
, ?R
0
1
2
3
01 23
其中，i，j，k满足
i^2=j^2=k^2=-1
基于互补滤波AHRS 姿态解算算法介绍
Mini INS/GPS 姿态仪
介绍内容： 1、四元数 2、姿态表示的方法 3、姿态解算原理
一、四元数
1.1 四元数定义
其中，i，j，k满足
i^2=j^2=k^2=-1
ij=-ji=k, jk=-kj=i, ki=-ik=j
则称数q为四元数，而q0称为四元数q的实部，称
次转动来实现。 四元数用符号q 表示，它是一个具有4个元素的矢量，这些元素是该矢量
方向和转动大小的函数。
定义 ? 的大小和方向是使参考系绕 ? 转动一个角度 ? ，就能与载体坐
标系重合。
利用四元数进行矢量变换
首先定义一个四元数 rb' rb=ix+jy+kz rb'=0+ix+jy+kz
有：
rn'=q*rb'*q' rn'=(q0+iq1+jq2+kq3)(0+ix+jy+kz)(q0 -iq1-jq2-kq3)
的单
次转动来实现。
四元数用符号q 表示，它是一个具有4个元素的矢量，这些元素是该矢量
方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
转动一个角度 ，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标
系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 ? 的单
连续转动来实现。从参考系到一个新的坐标系的变换可以表示：
绕参考坐标系的z轴转动 ? 角
绕新坐标系的y轴转动 ? 角
绕新坐标系的x轴转动 ? 角
? ? ? 称为欧拉转动角
3.3 四元数
四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标
系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量
称
为q的虚部。
四元数的共轭为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义
q q ,q q q q q q 设 q ? ? i ? j ? k , ,
, ?R
0
1
2
3
01 23
其中，i，j，k满足
i^2=j^2=k^2=-1
ij=-ji=k, jk=-kj=i, ki=-ik=j
则称数q 为四元数，
次转动来实现。 四元数用符号q 表示，它是一个具有4个元素的矢量，这些元素是该矢量
方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
转动一个角度 ，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标
系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 ? 的单
ij=-ji=k, jk=-kj=i, ki=-ik=j
则称数q 为四元数，
而q0 称为四元数q 的实部，
称 qi ? q j ? q k 为q的虚部。
1
2
3
?
四元数的共轭为
q ? q ?qi?q j?q k
0
1
2
3
1.2 四元数的示方式
1.3 四元数运算
二、东北天坐标系
东北天坐标系（表示为n系）是一种当地地理坐标系，原点位于导航系统 所处的位置P点，坐标轴指向北、东和当地垂线方向（向下），也有称为 北东地坐标系
次转动来实现。 四元数用符号q 表示，它是一个具有4个元素的矢量，这些元素是该矢量
方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
转动一个角度 ，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标
系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 ? 的单
为
q 的虚部。
四元数的共轭为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义
q q ,q q q q q q 设 q ?
?
0
i?
1
j?
2
k
3
,
,
01
, ?R
23
其中，i ，j ，k 满足
i^2=j^2=k^2=-1
ij=-ji=k, jk=-kj=i, ki=-ik=j
则称数q 为四元数，
而q0 称为四元数q 的实部，
为
q 的虚部。
四元数的共轭为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义
q q ,q q q q q q 设 q ?
?
0
i?
1
j?
2
k
3
,
,
01
, ?R
23
其中，i ，j ，k 满足
i^2=j^2=k^2=-1
ij=-ji=k, jk=-kj=i, ki=-ik=j
则称数q为四元数，而q0称为四元数q的实部，称
?
? ?
11 21
12 22
13 ? 23 ?
c c c ?? 31
32
33 ??
第i 行、j 列的元素表示参考坐标系 i 轴和载体坐标系 j 轴夹角的余弦。
在载体坐标系中定义的矢量 r b ，可以通过该矢量左乘方向余弦矩阵
r C r C n ，即 b
n?
nb b
3.2 欧拉角 一个坐标系到另一个坐标系的变换，可以通过绕不同坐标轴的 3次
?
?
c c c n C c c c b
?
? ?
11 21
12 22
13 ? 23 ?
c c c ?? 31
32
33 ??
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示，是一个3*3阶的矩阵，矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
?
?
c c c n C c c c b
=0+{(q0^2+q1^2-q2^2-q3^2)x+2(q1q2-q0q3)y+2(q1q3+q0q2)z}i +{2(q1q2+q0q3)x+(q0^2-q1^2+q2^2-q3^2)y+2(q2q3-q0q1)z}j +{2(q1q3-q0q2)x+2(q2q3+q0q1)y+(q0^2-q1^2-q2^2+q3^2)z}k
三、姿态表示方法
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示，是一个3*3阶的矩阵，矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示，是一个3*3阶的矩阵，矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
为
q的虚部。
四元数的共轭为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义
q q ,q q q q q q 设 q ?
?
0
i?
1
j?
2பைடு நூலகம்
k
3
,
,
01
, ?R
23
其中，i，j，k满足
i^2=j^2=k^2=-1
ij=-ji=k, jk=-kj=i, ki=-ik=j
则称数q为四元数，而q0称为四元数q的实部，称