1、一、姿态求解理论(一)四元数基本理论假设现在我们已经得到正确的姿态四元数Q,那么我们可以利用四元数旋转将参考坐标系和机体坐标系下的向量互相转换。定义四元数Q是坐标系A(参考坐标)旋转到坐标系B(机体坐标)所得到的姿态四元数。并且定义从坐标系B到坐标系A的四元数为Q*(二)参数定义、坐标系定义如果不存在误差那么a=Q G Q*h=Q HQ*实际上是不相等的。H是参考坐标系下地磁场G是参考坐标系下重力加速动参考坐标系:X轴对应北;Y轴对应西;Z轴对应天机体坐标系:初始与参考坐标同向则:注意:此时的测量值都是经过标准化的(三)四元数矩阵从机体坐标系到参考坐标系从参考坐标系到机体坐标系两个四元数矩阵的
2、用法说明:A是参考坐标系;B是机体坐标系向量 是同一个向量,但分别被定义在坐标系A中和坐标系B中二、梯度下降法 梯度下降法是一种求解最优值得方法,就是利用负梯度方向来决定每次迭代的新的搜索方向,使得每次迭代能使待优化的目标函数逐步减小。1、加速度计和磁力计通过梯度下降法得到姿态误差速率,陀螺仪直接通过四元数微分方程得到姿态四元数速率,两个加起来积分就可以得到相对精确的姿态四元数了。定义:为重力加速度与测量的加速度的偏差 为地磁场与测量的磁场的偏差 2、由于相减的向量是单位向量,很小时,|就相当于角度。3、创建梯度函数两个误差要合并成一个函数,这里我们使用标量函数的梯度下降法。这里用长度的平方和。求偏导数得:有了梯度,剩下的就是确定步长r确定步长是梯度下降法的核心。步长未必是一成不变的。加速度计和磁力计通过梯度下降法得到姿态误差速率,陀螺仪直接通过四元数微分方程得到姿态四元数速率,两个加起来积分就可以得到相对精确的姿态四元数了。以下公式就是四元数微分方程其中w为陀螺仪的测量值。四元数微分方程更新四元数融合计算融合计算:规范会处理