高精度矢量汉字的一种填充方法_惯导解算数学基础4(等效旋转矢量解，圆锥补偿解)...

等效旋转矢量及Bortz方程

由于姿态阵和四元数的方法都有不可交换误差的问题，不可回避，所以现代高精度算法都使用等效旋转矢量来表达。

还记上次那个 角速率和等效旋转矢量的关系式吧？ 为了书写整洁，改写如下：

等效旋转矢量微分方程-推导Bortz方程的基础

这个方程，经过很复杂的推导变换可以得出著名的Bortz方程：

重点 Bortz方程

Bortz方程，也叫做等效旋转矢量微分方程

Bortz方程的近似数值解

Bortz在理论上严格成立，但是实际应用比较纠结，当等效转动角度

为小量时，将方程右边的cot用泰勒级数展开，可以做如下近似：

Bortz方程二阶近似

还可以更变态一点：一阶近似：

Bortz方程一阶近似

等效旋转矢量微分方程的泰勒级数解

等效旋转矢量与四元数配合的姿态更新公式：

与之前的四元数角增量递推公式相比。两者虽然在形式上完全一样，但本质含义上存在重要区别：前者仅简单地使用角增量进行变化四元数计算，理论上只能适用于定轴转动情形；而后者在求解等效旋转矢量过程中考虑了转动不可交换误差的补偿，非定轴转动情况下算法精度更高。下面我们就求这个等效旋转矢量：

我们的目的是用角增量(陀螺能采集到的值)来代表等效旋转矢量：就可以得到大名鼎鼎的二子样算法：

二子样算法，光纤陀螺用这个

三子样算法,根本没必要

如果直接把等效旋转矢量当成角增量，就是单子样

单子样，MEMS用单子样就可以了

圆锥运动

19世纪50年代是机械陀螺仪飞速发展的一个重要时期，也正是在那时发现了著名的圆锥运动现象，即当陀螺仪在其旋转轴和输出轴出现同频不同相的角振动时，尽管其测量输入轴净指向不变（从整体上看没有随时间改变的趋势项），但陀螺仪在输入轴上还是会敏感到并输出常值角速率信号。在这种环境下，陀螺仪承受的运动角速度可表示如下：

圆锥运动数学描述

圆锥运动直观表示

圆锥补偿算法

这里直接给出圆锥补偿算法的补偿系数，其中一阶和二阶 是和之前的等效旋转矢量微分方程的泰勒级数解式一样的：

圆锥运动等效旋转矢量补偿系数

以四子样算法为例：

四子样算法的等效旋转矢量圆锥补偿算法公式

圆锥补偿算法和等效旋转矢量微分方程的泰勒级数解的优缺点对比

对比本节圆锥误差补偿多子样算法与2.5节基于泰勒级数展开的多子样算法，理论上，前者比后者更适合应用于圆锥运动环境，而后者比前者更适合应用于多项式角运动环境。对于实际系统，在角运动过程中，通常认为剧烈的多项式角运动只会短暂出现，而更容易激发的是较长时间的周期性振动，它可近似为圆锥运动，因此实际中一般优先考虑采用基于圆锥误差补偿的多子样算法。因此，相对于2.5节而言，本节在圆锥运动假设条件下获得的圆锥误差补偿算法也常常称为多子样优化算法。