三子样原理
载体的姿态描述指的是载体坐标系绕定轴转动到导航坐标系所转过的方位角信息。

载体坐标系（b 系）可以通过绕定轴转动与导航坐标系（n 系）重合，由b 系到n 系的姿态
变换矩阵n
b C 如下所示：
其中，三个姿态角θ，ϕ，γ分别为俯仰角，偏航角，横滚角。

姿态更新是指根据惯性器件
的输出实时计算出n b C 矩阵，两坐标系间空间角位置关系可以理解成刚体的定点转动，从这一思想出发可获得姿态更新四元数及旋转矢量算法。

由四元数确定的姿态变换矩阵n b C 如下
所示：
利用四元数求解刚体转动微分方程时，采用角增量法会造成不可交换误差，为了减小这种误差，我们使用等效旋转矢量法。

一个更新周期内，包含的角增量子样数越多，姿态解算越精确。

本文采用三子样等效旋转矢量法，具体步骤如下：
1） 分别计算,,3k k h t t ⎡⎤+
⎢⎥⎣⎦
2,,33k k h h t t ⎡⎤++⎢⎥⎣⎦2,3k k h t t h ⎡⎤
++⎢⎥⎣⎦
三个时间段内的角增量123,,θθθ∆∆∆； 2） 计算旋转矢量()h Φ：
3）由旋转矢量构造姿态更新四元数()q h ：
cos cos cos sin cos sin sin sin sin cos cos sin sin cos cos sin cos cos sin cos sin sin sin cos sin sin sin cos cos n b C θϕγθϕγϕγθϕγϕθγθγθθϕγθϕγϕγθϕγϕ-++⎡⎤⎢⎥=-⎢
⎥⎢⎥-+-+⎣
⎦0
22220123120313022222
1203123230122221302230101232()2()2()2()2()2()n b q q q q q q q q q q q q C q q q q q q q q q q q q q q q q q q q q q q q q ⎡⎤+---+⎢⎥=+-+--⎢⎥⎢⎥-+--+⎣
⎦12312231927
()()2040
h θθθθθθθθΦ=∆+∆+∆+
∆⨯∆+∆⨯∆-∆θθθθθθθθθθ⎡⎤∆⎢⎥⎢⎥∆∆⎢⎥⎢⎥
∆⎢
⎥=∆⎢⎥
∆⎢⎥∆⎢⎥
∆∆⎢⎥⎢⎥∆⎣⎦
z
cos 2sin 2()sin 2sin 2x
y q h
4）利用 计算更新后的四元数1()k Q t +；
5）利用更新后的四元数得到姿态变换矩阵n
b C ，提取相应姿态角：
a r c s i n (2,1)
(3,1)a r c t a n (1,1)(2,3)a r c t a n (2,2)
n b
n b n b n b n b C C C C C θϕγ==-=-
1()()()k k Q t Q t q h +=⊗。