046demo_cone_error

工具箱版本：psins140410
圆锥误差补偿仿真程序
相关公式可以参考《惯性导航（第二版）》（秦永元）9.3节以及9.4节。

---

首先该搞清楚的是这部分在讲什么。
捷联惯导中姿态四元数的更新，引入了等效旋转矢量的概念，建立等效旋转矢量微分方程，即Bortz方程。
该方程的求解比较麻烦，首先通过多项式拟合、然后泰勒展开的方式求解等效旋转矢量，根据多项式的不同得到各个子样数条件下的求解方法。
前面的求解方法，由于算法存在漂移，所以对其进行优化。优化的标准就是算法的漂移最小。工具箱中等效旋转矢量的求解，就是根据这种优化的方法。
这个示例程序就是比较一下在圆锥运动下，上述两种方法计算得到的四元数与理论四元数的误差，以及两种算法的算法漂移。

---

文中afa为半锥角，相当于秦书中的a，程序中设计的子样数为3，采样间隔为0.01s，仿真的时间为6s。下面说一下conesimu函数。

    wm = [ -2*omega*ts*sin(afa/2)^2*ones(size(t)), ...  
           -2*sin(afa)*sin(omega*ts/2)*sin(omega*(t+ts/2)), ...
            2*sin(afa)*sin(omega*ts/2)*cos(omega*(t+ts/2))  ];

角度增量，相当于秦书中公式9.4.5，只不过公式中的h/3变为ts。接下来的四元数以及角速度也是如此。

cnscl函数在此为此进行旋转矢量的补偿。程序中比较了两种补偿，分别是常用的旋转矢量优化算法和多项式拟合算法conepolyn，后者的公式可以在秦书268页看到。另外cnscl函数还有严讲义中31页提到的“单子阳+前一周期”算法，程序中没有进行该算法的比较。

另外还有一个重要的函数conedrift，圆锥误差漂移，可参考严书35、37页。

epsilon = afa^2*(omega*ts)^(2*nn+1) * nn*factorial(nn)/(2^(nn+1)*II);
epsilon = epsilon/(nn*ts);

求解漂移角速率，见37页或参考论文经典圆锥误差补偿算法中剩余误差估计的局限性研究(资源汇总页中下载)；

dphimc = cross(cm,wm(nn,:));
dphim = 2*sin(afa/2)^2*(omega*nts-sin(omega*nts)); 
epsilon(2,1) = (dphim-abs(dphimc(1)))/nts;
dphim = dphim/nts;

第一行计算的是补偿的不可交换误差，第二行计算角增量代替旋转矢量产生的理论误差，第三、四行计算未补偿的误差。

---

下面该说我想了一天的问题了，手动尴尬。
在程序中的绘图部分:

res(ki,:) = [q2att(q0); qq2phi(q1,q0); qq2phi(q2,q0)]';
subplot(322), plot(t,res(:,[5,8])/glv.sec), xygo('\it\phi_x\rm / \prime\prime');
subplot(324), plot(t,res(:,[6,9])/glv.sec), xygo('\it\phi_y\rm / \prime\prime');
subplot(326), plot(t,res(:,[4,7])/glv.sec), xygo('\it\phi_z\rm / \prime\prime');

q0为理论四元数，q1为优化算法计算得到的四元数，q2是多项式算法计算得到的四元数；
第一行代码q0转化为姿态角分别为俯仰角、横滚角和航向角，占据1-3列；
4-6列的输出，由于$Q_b^n=Q_{n^{\prime }}^n\otimes Q_b^{n^{\prime }}$,式中分别为理论四元数、误差四元数和计算四元数，推得$Q_{n^{\prime }}^n=Q_b^n\otimes Q_{n^{\prime }}^b$，然后将误差四元数转换为等效旋转矢量，我的问题是，按照程序中的表示方法，4-6列分别为${\varphi }_x,{\varphi }_y,{\varphi }_z$，这是相对于三个坐标轴zxy的失准角吗？
然而，如果把误差四元数转换为姿态角，求得的结果与转换为等效旋转矢量完全相同，但是此时却是$\theta ,\gamma ,\psi$，这不是相对于xyz轴吗？
测试用的

qerr = qmul(q0, qconj(q1));
a = q2att(qerr);
b = qq2phi(q1,q0);

想了一天没想明白，可能我掉进坑了。把这个问题留在这里，方便回顾。