梯度下降法

hx H (q 2 q 2 q 2 q 2 ) H 2(q q q q ) h 0 1 2 3 Z 1 3 0 2 x X h R * H h hy H X 2(q1q2 q0 q3 ) H Z 2(q2 q3 q0 q1 ) hy H 2(q q q q ) H (q 2 q 2 q 2 q 2 ) h h 1 3 0 2 Z 0 1 2 3 z z X
有了梯度，剩下的就是确定步长r 确定步长是梯度下降法的核心。步长未必是一成不变
的。
加速度计和磁力计通过梯度下降法得到姿态误差速率
，陀螺仪直接通过四元数微分方程得到姿态四元数速 率，两个加起来积分就可以得到相对精确的姿态四元 数了。以下公式就是四元数微分方程 其中w为陀螺仪的测量值。
H H X 0 HZ
T
注意：此时的测量值都是经过标准化的
（三）四元数矩阵
从机体坐标系到参考坐标系
2 2 2 q0 q12 q2 q3 R 2(q1q2 q0 q3 ) 2(q1q3 q0 q2 )
2(q1q2 q0 q3 )
2 2 2 q0 q12 q2 q3
R * V A V B
说明：A是参考坐标系；B是机体坐标系 向量
V A、 VB
是同一个向量，但分别被定义在坐
标系A中和坐标系B中
二、梯度下降法
梯度下降法是一种求解最优值得方法，就是利用负 梯度方向来决定每次迭代的新的搜索方向，使得每次 迭代能使待优化的目标函数逐步减小。 1、 加速度计和磁力计通过梯度下降法得到姿态误差 速率，陀螺仪直接通过四元数微分方程得到姿态四元 数速率，两个加起来积分就可以得到相对精确的姿态 a 四元数了。 定义： h 为重力加速度与测量的加速度的偏差
Q q0
*
q1
q2
q3
T

（二）参数定义、坐标系定义 如果不存在误差那么 a=Q ⊕G ⊕Q* h=Q ⊕H⊕Q* 实际上是不相等的。 H是参考坐标系下地磁场 G是参考坐标系下重力加速动 参考坐标系：X轴对应北；Y轴对应西；Z轴对应天 机体坐标系：初始与参考坐标同向 T 则： G 0 0 1


为地磁场与测量的磁场的偏差
2、由于相减的向量是单位向量，∆很小时，|∆|就相
当于角度。
ax 2(q1q3 q0 q2 ) ax 2(q q q q ) a a R * G a a 2 3 0 1 y y 2 2 2 2 a q q q q 1 2 3 az z 0
2(q1q2 q0 q3 )
2 2 2 q0 q12 q2 q3
2(q2 q3 q0 q1 )
2(q1q3 q0 q2 ) 2(q2 q3 q0 q1 ) 2 2 2 q0 q12 q2 q3
两个四元数矩阵的用法
R V B V A
四元数微分方程
1 q t qt 1 wt 2

更新四元数
1 q t qt 1 qt 1 wt t 2
融合计算:
1 f ( a、 h) qt qt 1 ( qt 1 wt r ) t 2 f ( a、 h)
规范会处理
qt q qt
一、姿态求解理论
（一）四元数基本理论
假设现在我们已经得到正确的姿态四元数Q，那么我
们可以利用四元数旋转将参考坐标系和机体坐标系下 的向量互相转换。 定义四元数Q是坐标系A（参考坐标）旋转到坐标系B （机体坐标）所得到的姿态四元数。 T Q q0 q1 q2 q3
并且定义从坐标系B到坐标系A的四元数为Q*
4hy H z q3 4hz H x q0 4hz H z q2
f 4ax q1 4a y q2 4az q3 4hx H x q3 4hx H z q1 4hy H x q0 q3
4hy H z q2 4hz H x q1 4hz H z q3
2(q2 q3 q0 q1 )
2(q1q3 q0 q2 ) 2(q2 q3 q0 q1 ) 2 2 2 q0 q12 q2 q3
从参考坐标系到机体坐标系
2 2 2 q0 q12 q2 q3 R* 2(q1q2 q0 q3 ) 2(q1q3 q0 q2 )
Leabharlann Baidu
f 4ax q3 4a y q0 4az q1 4hx H x q1 4hx H z q3 4hy H x q2 q1
4hy H z q0 4hz H x q3 4hz H z q1
f 4ax q0 4a y q3 4az q2 4hx H x q2 4hx H z q0 4hy H x q1 q2
3、创建梯度函数 两个误差要合并成一个函数，这里我们使用标量函数
的梯度下降法。这里用长度的平方和。
f (a、h) a h
2
2
f ( a、 h) f f ( a、 h) q q0
f q1
f q2
f q3
T
求偏导数得： f 4ax q2 4a y q1 4hx H z q2 4hy H x q3 4hy H z q1 4hz H X q2 q0