而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的实部，称