若圆锥运动的四元数更新方程为：
Q(tm)=Q(Tm−1)。Q(T)Q(t_m)=Q(T_{m-1})。Q(T)Q(tm)=Q(Tm−1)。Q(T)
（ 。。。四元数乘法）
（ Q(T)Q(T)Q(T)四元数周期内变化量）
得到：（Ω\OmegaΩ震动频率;θ=∣ϕ∣\theta=|\phi|θ=∣ϕ∣）
Q(T)=Q∗(tm−1)Q(Tm)=MQ∗(tm−1)Q(tm)Q(T)=Q^*(t_{m-1})Q(T_{m})=M_{Q^*(t_{m-1})}Q(t_m)Q(T)=Q∗(tm−1)Q(Tm)=MQ∗(tm−1)Q(tm)
=[1−2(sin(θ2)sinΩT2)2−sinθsinΩT2sinΩ(tm−T2)sinθsinΩT2cosΩ(tm−T2)−sin2θ2sinΩT](推导略)=[cosθ2u∗sin(θ2)]=\left[\begin{matrix} 1-2(sin(\frac{\theta}{2})sin\frac{\Omega T}{2})^2 \\ -sin\theta sin\frac{\Omega T}{2}sin\Omega(t_m-\frac{T}{2})\\ sin\theta sin\frac{\Omega T}{2}cos\Omega(t_m-\frac{T}{2})\\ -sin^2\frac{\theta}{2}sin\Omega T \end{matrix}\right](推导略)=\left[\begin{matrix} cos\frac{\theta}{2}\\ u*sin(\frac{\theta}{2})\\ \end{matrix}\right]=⎣⎢⎢⎡1−2(sin(2θ)sin2ΩT)2−sinθsin2ΩTsinΩ(tm−2T)sinθsin2ΩTcosΩ(tm−2T)−sin22θsinΩT⎦⎥⎥⎤(推导略)=[cos2θu∗sin(2θ)]
若：在[tm−1,tm][t_{m-1},tm][tm−1,tm]时间内的等效旋转矢量为ϕ(T)\phi(T)ϕ(T);u=ϕ(T)θu=\frac{\phi(T)}{\theta}u=θϕ(T)
得到ϕ(T)θ(t)∗sin(θ(t)2)=[−sinθsinΩT2sinΩ(tm−T2)sinθsinΩT2cosΩ(tm−T2)−sin2θ2sinΩT]\frac{\phi(T)}{\theta(t)}*sin(\frac{\theta(t)}{2})=\left[\begin{matrix} -sin\theta sin\frac{\Omega T}{2}sin\Omega(t_m-\frac{T}{2})\\ sin\theta sin\frac{\Omega T}{2}cos\Omega(t_m-\frac{T}{2})\\ -sin^2\frac{\theta}{2}sin\Omega T \end{matrix}\right]θ(t)ϕ(T)∗sin(2θ(t))=⎣⎡−sinθsin2ΩTsinΩ(tm−2T)sinθsin2ΩTcosΩ(tm−2T)−sin22θsinΩT⎦⎤
左右两边取模值，得到
sinΩT2=sin2θsin2ΩT2+sin4θ2sin2ΩTsin\frac{\Omega T}{2}=\sqrt{sin^2\theta sin^2 \frac{\Omega T}{2}+sin^4\frac{\theta}{2}sin^2 {\Omega T}}sin2ΩT=sin2θsin22ΩT+sin42θsin2ΩT
当θ和ΩT\theta和\Omega Tθ和ΩT均为小量时：
sinΩT2≈sin2θsin2ΩT2≈θΩT2sin\frac{\Omega T}{2}\approx\sqrt{sin^2\theta sin^2 \frac{\Omega T}{2}}\approx\frac{\theta \Omega T}{2}sin2ΩT≈sin2θsin22ΩT≈2θΩT
从而进一步的到： θ(t)=θΩT\theta(t)=\theta\Omega Tθ(t)=θΩT
多子样补偿算法
当使用角增量代替旋转矢量进行姿态更新时会在z轴产生误差，这个误差会随时间不断积累，常使用多子样补偿算法，弥补这一算法(证略)
在[tm−1,tm][t_{m-1},t_{m}][tm−1,tm]内采样N次，h=TNh=\frac{T}{N}h=NT为采样间隔
Δθmi=∫tm−1+(i−1)htm−1+ihw(t)dt\Delta \theta_{mi}=\int_{t_{m-1}+(i-1)h}^{t_{m-1}+ih}w(t)dtΔθmi=∫tm−1+(i−1)htm−1+ihw(t)dt
=[−2sinθsinΩT2sinΩ(tm−1+ih−h2)sinθsinΩT2cosΩ(tm−1+ih−h2)−sin2θ2sinΩT]=\left[\begin{matrix} -2sin\theta sin\frac{\Omega T}{2}sin\Omega(t_{m-1}+ih-\frac{h}{2})\\ sin\theta sin\frac{\Omega T}{2}cos\Omega(t_{m-1}+ih-\frac{h}{2})\\ -sin^2\frac{\theta}{2}sin\Omega T \end{matrix}\right]=⎣⎡−2sinθsin2ΩTsinΩ(tm−1+ih−2h)sinθsin2ΩTcosΩ(tm−1+ih−2h)−sin22θsinΩT⎦⎤
记λ=ΩT\lambda=\Omega Tλ=ΩT得到在[tm−1,tm][t_{m-1},t_{m}][tm−1,tm]内的总角增量为：
Δθm=∫tm−1tmw(t)dt=∑i=1NΔθmi\Delta\theta_m=\int_{t_{m-1}}^{t_m}w(t)dt=\sum_{i=1}^N\Delta\theta_{mi}Δθm=∫tm−1tmw(t)dt=i=1∑NΔθmi
求解不同子样的角增量之间的叉积（θ和λ\theta和\lambdaθ和λ均为小量）
Δθmi×Δθmj=[8λsin2θ2sinθsinλ2sin(i−j)λ2sinΩ(tm−1−i+j−12)h−8λsin2θ2sinθsinλ2sin(i−j)λ2cosΩ(tm−1−i+j−12)h−4sin2θ2sin2λ2sin(i−j)λ]\Delta \theta_{mi}\times\Delta \theta_{mj}=\left[\begin{matrix} 8\lambda sin^2\frac{\theta}{2}sin\theta sin\frac{\lambda}{2}sin\frac{(i-j)\lambda}{2}sin\Omega(t_{m-1}-\frac{i+j-1}{2})h\\ -8\lambda sin^2\frac{\theta}{2}sin\theta sin\frac{\lambda}{2}sin\frac{(i-j)\lambda}{2}cos\Omega(t_{m-1}-\frac{i+j-1}{2})h\\ -4sin^2\frac{\theta}{2}sin^2\frac{\lambda}{2}sin(i-j)\lambda\\ \end{matrix}\right]Δθmi×Δθmj=⎣⎡8λsin22θsinθsin2λsin2(i−j)λsinΩ(tm−1−2i+j−1)h−8λsin22θsinθsin2λsin2(i−j)λcosΩ(tm−1−2i+j−1)h−4sin22θsin22λsin(i−j)λ⎦⎤
θ和λ\theta和\lambdaθ和λ均为小量
≈[(i−j)(θλ)32sinΩ(tm−1−i+j−12)h−(i−j)(θλ)32cosΩ(tm−1−i+j−12)h−4sin2θ2sin2λ2sin(i−j)λ]\approx \left[\begin{matrix} \frac{(i-j)(\theta\lambda)^3}{2}sin\Omega(t_{m-1}-\frac{i+j-1}{2})h\\ -\frac{(i-j)(\theta\lambda)^3}{2}cos\Omega(t_{m-1}-\frac{i+j-1}{2})h\\ -4sin^2\frac{\theta}{2}sin^2\frac{\lambda}{2}sin(i-j)\lambda\\ \end{matrix}\right]≈⎣⎢⎡2(i−j)(θλ)3sinΩ(tm−1−2i+j−1)h−2(i−j)(θλ)3cosΩ(tm−1−2i+j−1)h−4sin22θsin22λsin(i−j)λ⎦⎥⎤
定义圆锥误差补偿系数
δθ∧(T)=∑j=2N∑i=1j−1kijΔθmi×Δθmj=∑i=1N−1kN−1∗Δθmi×Δθmj\delta\theta^\wedge(T)=\sum_{j=2}^N\sum_{i=1}^{j-1}k_{ij}\Delta\theta_{mi}\times\Delta\theta_{mj}=\sum_{i=1}^{N-1}k^*_{N-1}\Delta\theta_{mi}\times\Delta\theta_{mj}δθ∧(T)=j=2∑Ni=1∑j−1kijΔθmi×Δθmj=i=1∑N−1kN−1∗Δθmi×Δθmj
kN−1=k1(N−1)∗k_{N-1}=k^*_{1(N-1)}kN−1=k1(N−1)∗
kN−2=k1(N−1)∗+k2(N)∗k_{N-2}=k^*_{1(N-1)}+k^*_{2(N)}kN−2=k1(N−1)∗+k2(N)∗
…
k2=k13∗+k24∗+k35∗+...+k(N−2)(N)∗k_{2}=k^*_{13}+k^*_{24}+k^*_{35}+...+k^*_{(N-2)(N)}k2=k13∗+k24∗+k35∗+...+k(N−2)(N)∗
k1=k12∗+k23∗+k34∗+...+k(N−1)(N)∗k_{1}=k^*_{12}+k^*_{23}+k^*_{34}+...+k^*_{(N-1)(N)}k1=k12∗+k23∗+k34∗+...+k(N−1)(N)∗
因为x,y轴上的误差为(θλ)3(\theta\lambda)^3(θλ)3量级，不会起时姿态累计漂移，所以只用考虑z轴上的分量,将z轴分量，用泰勒级数展开为：
δθ∧(T)z=(ϕ(T)−Δθm)z=2sin2θ2(ΩT−sinΩT)\delta\theta^\wedge(T)_z=(\phi(T)-\Delta\theta_m)_z=2sin^2\frac{\theta}{2}(\Omega T-sin\Omega T)δθ∧(T)z=(ϕ(T)−Δθm)z=2sin22θ(ΩT−sinΩT)
=4sin2θ2∑i=1∞(−1)i+1ciλ2i+1(ci=N2i+12(2i+1)!)=4sin^2\frac{\theta}{2}\sum_{i=1}^{\infty}(-1)^{i+1}c_i\lambda^{2i+1}(c_i=\frac{N^{2i+1}}{2(2i+1)!})=4sin22θi=1∑∞(−1)i+1ciλ2i+1(ci=2(2i+1)!N2i+1)
δθ∧(T)z=∑j=2N∑i=1j−1kijΔθmi×Δθmj=sin2θ∑i=1∞(−1)i+1∑j=1N−1Aijkjλ2i+1\delta\theta^\wedge(T)_z=\sum_{j=2}^N\sum_{i=1}^{j-1}k_{ij}\Delta\theta_{mi}\times\Delta\theta_{mj}=sin^2\theta\sum_{i=1}^{\infty}(-1)^{i+1}\sum_{j=1}^{N-1}A_{ij}k_j\lambda^{2i+1}δθ∧(T)z=j=2∑Ni=1∑j−1kijΔθmi×Δθmj=sin2θi=1∑∞(−1)i+1j=1∑N−1Aijkjλ2i+1
使λ2i+1\lambda^{2i+1}λ2i+1对应系数相等，建立线性方程求解KKK可以得到：
AK=CAK=CAK=C
其中:(i,j=1,2,3,…,N-1)
A=(Aij)(N−1)(N−1)=(j+1)2i+1+(j+1)2i−1−(2j)2i+1(2i+1)!A=(A_{ij})_{(N-1)(N-1)}=\frac{(j+1)^{2i+1}+(j+1)^{2i-1}-(2j)^{2i+1}}{(2i+1)!}A=(Aij)(N−1)(N−1)=(2i+1)!(j+1)2i+1+(j+1)2i−1−(2j)2i+1
K=(kj)(N−1)×1K=(k_j)_{(N-1)\times1}K=(kj)(N−1)×1
C=(ci)(N−1)×1=N2i+12(2i+1)!C=(c_i)_{(N-1)\times1}=\frac{N^{2i+1}}{2(2i+1)!}C=(ci)(N−1)×1=2(2i+1)!N2i+1
误差漂移系数为:(只要θ2(ΩT)2N+1T<1。h\frac{\theta^2(\Omega T)^{2N+1}}{T}<\frac{1^。}{h}Tθ2(ΩT)2N+1<h1。就能满足多数惯性导航系统要求)
ξn=ρnθ2(ΩT)2N+1T(N≥1)\xi_n=\rho_n\frac{\theta^2(\Omega T)^{2N+1}}{T}(N\geq1)ξn=ρnTθ2(ΩT)2N+1(N≥1)
圆锥误差补偿系数和漂移误差分数解
EX:四子样子求解等效旋转矢量
ϕ(T)=Δθm+δθ∧(T)=∑i=14Δθmi+∑i=14−1k4−1∗Δθmi×Δθmj\phi(T)=\Delta\theta_m+\delta\theta^\wedge(T)=\sum_{i=1}^4\Delta\theta_{mi}+\sum_{i=1}^{4-1}k^*_{4-1}\Delta\theta_{mi}\times\Delta\theta_{mj}ϕ(T)=Δθm+δθ∧(T)=i=1∑4Δθmi+i=1∑4−1k4−1∗Δθmi×Δθmj
(Δθm1+Δθm2+Δθm3+Δθm4)+(k1Δθm1+k2Δθm2+k3Δθm3)×Δθm4(\Delta\theta_{m1}+\Delta\theta_{m2}+\Delta\theta_{m3}+\Delta\theta_{m4})+(k_1\Delta\theta_{m1}+k_2\Delta\theta_{m2}+k_3\Delta\theta_{m3})\times\Delta\theta_{m4}(Δθm1+Δθm2+Δθm3+Δθm4)+(k1Δθm1+k2Δθm2+k3Δθm3)×Δθm4
