捷联惯导系统学习4.1(惯导数值更新算法)

1 常用坐标系的定义

（1）地心惯性坐标系(i 系,inertial frame)

用oixiyizio_ix_iy_iz_ioi​xi​yi​zi​表示，原点以地球为中心，

原点oio_ioi​在地球中心

oixio_ix_ioi​xi​在地球平面内，指向春分点

oizio_iz_ioi​zi​为地球自转轴，指向北极

oiyio_iy_ioi​yi​轴在地球平面内，垂直于oixio_ix_ioi​xi​和oizio_iz_ioi​zi​

（2）地球坐标系(e 系,earth frame)（地心固坐标系）

用oexeyezeo_ex_ey_ez_eoe​xe​ye​ze​表示，原点为地球中心，与地球固连。

ogxgo_gx_gog​xg​：在赤道平面内指向本初子午线

ogygo_gy_gog​yg​：在地球赤道平面内

ogzgo_gz_gog​zg​：为地球自转轴，指向北极

地球坐标系相对于惯性坐标系的角运动就是地球自转速度，常用：

wie=7.2921151467×10−5rad/sw_{ie}=7.2921151467×10^{-5} rad/swie​=7.2921151467×10−5rad/s

（3）地理坐标系(g系,geographic frame)

ogxgygzgo_gx_gy_gz_gog​xg​yg​zg​表示“东北天”坐标系作为导航参考坐标系

ogo_gog​定义为运载体的中心或重心。

ogxgo_gx_gog​xg​指向地理东向

ogygo_gy_gog​yg​指向地理北向

ogzgo_gz_gog​zg​垂直于当地旋转椭圆面，指向天向

（4）导航坐标系(n系,mavigation frame)

onxnynzno_nx_ny_nz_non​xn​yn​zn​表示，惯导系统求解导航参数所用坐标系

常用当地坐标系作为参考坐标系，常常为地理坐标系

（5）载体坐标系(b系,体系，body frame)

obxbybzbo_bx_by_bz_bob​xb​yb​zb​

obo_bob​：原点定义为运载体的重心或中心

obxbo_bx_bob​xb​沿载体面向右

obybo_by_bob​yb​沿载体纵轴向前

obzbo_bz_bob​zb​沿载体立轴向上

2 姿态更新算法

选择东北天坐标系地理坐标系，作为导航参考坐标系，记为n系

则n系的微分方程为：

wbn×:坐标系n下，载体（b系）的角速度，（实际中不能直接获得）w_b^n×:坐标系n下，载体（b系）的角速度，（实际中不能直接获得）wbn​×:坐标系n下，载体（b系）的角速度，（实际中不能直接获得）

Cbn:n−>b系的转换矩阵C_b^n:n->b系的转换矩阵Cbn​:n−>b系的转换矩阵

C˙bn=Cbn(wbn×)\dot C_b^n=C_b^n(w_b^n×)C˙bn​=Cbn​(wbn​×)

由于：(wbn×)(w_b^n×)(wbn​×)不能直接测得：

wbnn=wibb+wnib=wibb−winbw_{bn}^{n}=w_{ib}^b+w^b_{ni}=w_{ib}^b-w^b_{in}wbnn​=wibb​+wnib​=wibb​−winb​

wbnn:n系相对于b的角速度，在n系下的转速w_{bn}^{n}:n系相对于b的角速度，在n系下的转速wbnn​:n系相对于b的角速度，在n系下的转速

wibb:陀螺仪的输出:载体在b系相对于i系的转速，在b系下的表示w_{ib}^b:陀螺仪的输出:载体在b系相对于i系的转速，在b系下的表示wibb​:陀螺仪的输出:载体在b系相对于i系的转速，在b系下的表示

C˙bn=Cbn(wbn×)=Cbn[(wibb−winb)×]=Cbn(wibb×)−(winn×)Cbn\dot C_b^n=C_b^n(w_b^n×)=C_b^n[(w_{ib}^b-w^b_{in})×]=C_b^n(w_{ib}^b×)-(w_{in}^n×)C_b^nC˙bn​=Cbn​(wbn​×)=Cbn​[(wibb​−winb​)×]=Cbn​(wibb​×)−(winn​×)Cbn​

winn=wien+wennw_{in}^n=w_{ie}^n+w_{en}^nwinn​=wien​+wenn​

wie:地球自转速度;L:地理纬度;h：高度w_{ie}:地球自转速度;L:地理纬度;h：高度wie​:地球自转速度;L:地理纬度;h：高度

wien=[0wiecosLwiesinL]Tw_{ie}^n=\left[\begin{matrix} 0&w_{ie}cosL&w_{ie}sinL\\ \end{matrix}\right]^Twien​=[0​wie​cosL​wie​sinL​]T

wenn=[−vNRM+h−vERN+hvERN+htanL]Tw_{en}^n=\left[\begin{matrix} -\frac{v_N}{R_M+h}&-\frac{v_E}{R_N+h}&\frac{v_E}{R_N+h}tanL\\ \end{matrix}\right]^Twenn​=[−RM​+hvN​​​−RN​+hvE​​​RN​+hvE​​tanL​]T

RM=Re(1−e2)(1−e2sin2L)3/2(Re:地球长半径，e:椭圆偏心率)R_M=\frac{R_e(1-e^2)}{(1-e^2sin^2L)^{3/2}}(R_e:地球长半径，e:椭圆偏心率)RM​=(1−e2sin2L)3/2Re​(1−e2)​(Re​:地球长半径，e:椭圆偏心率)

RN=Re1−e2sin2LR_N=\frac{R_e}{\sqrt{1-e^2sin^2L}}RN​=1−e2sin2L​Re​​

捷联惯导导数值更新算法

Cb(m)n(m)=Cn(m−1)n(m)Cb(m−1)n(m−1)Cb(m)b(m−1)C_{b(m)}^{n(m)}=C_{n(m-1)}^{n(m)}C_{b(m-1)}^{n(m-1)}C_{b(m)}^{b(m-1)}Cb(m)n(m)​=Cn(m−1)n(m)​Cb(m−1)n(m−1)​Cb(m)b(m−1)​

若采用二子样法测得Δθ1,Δθ2\Delta\theta_1,\Delta\theta_2Δθ1​,Δθ2​

得到:ϕib(m)b=(Δθ1+Δθ2)+23Δθ1×Δθ2\phi_{ib(m)}^b=(\Delta\theta_1+\Delta\theta_2)+\frac{2}{3}\Delta\theta_1×\Delta\theta_2ϕib(m)b​=(Δθ1​+Δθ2​)+32​Δθ1​×Δθ2​

Cb(m)b(m−1)=MRV(ϕib(m)b)=I+sin∣ϕib(m)b∣∣ϕib(m)b∣(ϕib(m)b×)+1−cos∣ϕib(m)b∣∣ϕib(m)b∣2(ϕib(m)b×)2C_{b(m)}^{b(m-1)}=M_{RV}(\phi_{ib(m)}^b)=I+\frac{sin|\phi_{ib(m)}^b|}{|\phi_{ib(m)}^b|}(\phi_{ib(m)}^b\times)+\frac{1-cos|\phi_{ib(m)}^b|}{|\phi_{ib(m)}^b|^2}(\phi_{ib(m)}^b\times)^2Cb(m)b(m−1)​=MRV​(ϕib(m)b​)=I+∣ϕib(m)b​∣sin∣ϕib(m)b​∣​(ϕib(m)b​×)+∣ϕib(m)b​∣21−cos∣ϕib(m)b​∣​(ϕib(m)b​×)2

Cn(m−1)n(m)=MRVT(ϕin(m)n)≈MRVT(Twin(m)n)（win(m)n)由速度和位置引起这里我们认为是常量）C_{n(m-1)}^{n(m)}=M^T_{RV}(\phi_{in(m)}^n)\approx M_{RV}^T(Tw_{in(m)}^n)（w_{in(m)}^n)由速度和位置引起这里我们认为是常量）Cn(m−1)n(m)​=MRVT​(ϕin(m)n​)≈MRVT​(Twin(m)n​)（win(m)n​)由速度和位置引起这里我们认为是常量）

3 比力方程

比力：加速度计测量的载体相对惯性空间的绝对加速度和引力加速度之差

运载体在导航系下的惯导结算方程：

运载体在地球表面运动时比力方程，可达10−7g10^{-7}g10−7g对于惯性级导航可忽略。

gn:重力加速度g^n:重力加速度gn:重力加速度

fsfb:加速度计测量的比力f_{sf}^b:加速度计测量的比力fsfb​:加速度计测量的比力

wenn×venn：由载体引起的对地向心加速度w_{en}^n×v_{en}^n：由载体引起的对地向心加速度wenn​×venn​：由载体引起的对地向心加速度

2wien×venn：由载体和地球自转引起的哥氏加速度2w_{ie}^n×v_{en}^n：由载体和地球自转引起的哥氏加速度2wien​×venn​：由载体和地球自转引起的哥氏加速度

v˙enn:运载体在导航系下的运动加速度\dot v_{en}^n:运载体在导航系下的运动加速度v˙enn​:运载体在导航系下的运动加速度

Cbn:b系到n系的转换矩阵C_b^n:b系到n系的转换矩阵Cbn​:b系到n系的转换矩阵

v˙enn=Cbnfsfb−(2wien+wenn)×venn+gn\dot v_{en}^n=C_b^nf_{sf}^b-(2w_{ie}^n+w_{en}^n)×v_{en}^n+g^nv˙enn​=Cbn​fsfb​−(2wien​+wenn​)×venn​+gn

4 速度更新算法

利用比力方程在[tm−1,tm][t_{m-1},t_m][tm−1​,tm​]内积分，求得速度的递推更新公式为：

vmn(m):tm时刻惯导速度v_m^{n(m)}:t_m时刻惯导速度vmn(m)​:tm​时刻惯导速度

vm−1n(m−1):tm−1时刻惯导速度v_{m-1}^{n(m-1)}:t_{m-1}时刻惯导速度vm−1n(m−1)​:tm−1​时刻惯导速度

Δvsf(m)n:导航比力速度增量\Delta v_{sf(m)}^n:导航比力速度增量Δvsf(m)n​:导航比力速度增量

Δvcor/g(m)n:有害加速度增量\Delta v_{cor/g(m)}^n:有害加速度增量Δvcor/g(m)n​:有害加速度增量

vmn(m)=vm−1n(m−1)+Δvsf(m)n+Δvcor/g(m)nv_m^{n(m)}=v_{m-1}^{n(m-1)}+\Delta v_{sf(m)}^n+\Delta v_{cor/g(m)}^nvmn(m)​=vm−1n(m−1)​+Δvsf(m)n​+Δvcor/g(m)n​

Δvsf(m)n=∫tm−1tmCbn(t)fsfb(t)dt\Delta v_{sf(m)}^n=\int_{t_{m-1}}^{t_m}C_b^n(t)f_{sf}^b(t)dtΔvsf(m)n​=∫tm−1​tm​​Cbn​(t)fsfb​(t)dt

Δvcor/g(m)n=∫tm−1tm{−[2wien(t)+wenn(t)]×vn(t)+gn(t)}dt\Delta v_{cor/g(m)}^n=\int_{t_{m-1}}^{t_m}\{-[2w_{ie}^n(t)+w_{en}^n(t)]×v^n(t)+g^n(t)\}dtΔvcor/g(m)n​=∫tm−1​tm​​{−[2wien​(t)+wenn​(t)]×vn(t)+gn(t)}dt

Δvcor/g(m)n\Delta v_{cor/g(m)}^nΔvcor/g(m)n​的计算()：

Δvcor/g(m)n≈{−[2wie(m−1/2)n+wen(m−1/2)n]×vm−1/2n+gm−1/2n}T\Delta v_{cor/g(m)}^n\approx \{ -[2w_{ie(m-1/2)}^n+w_{en(m-1/2)}^n]×v_{m-1/2}^{n}+g_{m-1/2}^{n}\}TΔvcor/g(m)n​≈{−[2wie(m−1/2)n​+wen(m−1/2)n​]×vm−1/2n​+gm−1/2n​}T

使用外推法计算：

xm−1/2=xm−1+xm−1−xm−22(x=wien,wenn,vn,gn)x_{m-1/2}=x_{m-1}+\frac{x_{m-1}-x_{m-2}}{2}(x=w_{ie}^n,w_{en}^n,v^n,g^n)xm−1/2​=xm−1​+2xm−1​−xm−2​​(x=wien​,wenn​,vn,gn)

Δvsf(m)n\Delta v_{sf(m)}^nΔvsf(m)n​的计算（）：

θibb(t,tm−1):角增量\theta^{b}_{ib}(t,t_{m-1}):角增量θibb​(t,tm−1​):角增量

Δvsf(m)n≈Cb(m−1)n(m−1)∫tm−1tmfsfb(t)dt−∫tm−1tmθinn(t,tm−1)×[Cb(m−1)n(m−1)fsfb(t)]dt+Cb(m−1)n(m−1)∫tm−1tmθibb(t,tm−1)fsfb(t)dt\Delta v_{sf(m)}^n \approx C_{b(m-1)}^{n{(m-1)}}\int_{t_{m-1}}^{t_m}f_{sf}^b(t)dt-\int_{t_{m-1}}^{t_m}\theta_{in}^n(t,t_{m-1})×[C_{b(m-1)}^{n(m-1)}f_{sf}^b(t)]dt+C_{b(m-1)}^{n{(m-1)}}\int_{t_{m-1}}^{t_m}\theta^{b}_{ib}(t,t_{m-1})f_{sf}^b(t)dtΔvsf(m)n​≈Cb(m−1)n(m−1)​∫tm−1​tm​​fsfb​(t)dt−∫tm−1​tm​​θinn​(t,tm−1​)×[Cb(m−1)n(m−1)​fsfb​(t)]dt+Cb(m−1)n(m−1)​∫tm−1​tm​​θibb​(t,tm−1​)fsfb​(t)dt

∫tm−1tmθibb(t,tm−1)fsfb(t)dt=Δvrot(m)b(m−1)+Δvscul(m)b(m−1)\int_{t_{m-1}}^{t_m}\theta^{b}_{ib}(t,t_{m-1})f_{sf}^b(t)dt=\Delta v^{b(m-1)}_{rot(m)}+\Delta v^{b(m-1)}_{scul(m)}∫tm−1​tm​​θibb​(t,tm−1​)fsfb​(t)dt=Δvrot(m)b(m−1)​+Δvscul(m)b(m−1)​:

{Δvrot(m)b(m−1)=12Δθm×Δvm速度的旋转误差补偿Δvscul(m)b(m−1)=12∫tm−1tmθibb(t,tm−1)×fsfb(t)+vsfb(t,tm−1)×wibb(t)dt划桨误差补偿Δθm=∫tm−1mwibb(t)dt陀螺仪采样角增量Δvm=∫tm−1tmfsfb(t)dt加速计采样比力速度增量\begin{cases} \Delta v^{b(m-1)}_{rot(m)}=\frac{1}{2} \Delta \theta_m×\Delta v_m&速度的旋转误差补偿 \\ \Delta v^{b(m-1)}_{scul(m)}=\frac{1}{2}\int_{t_{m-1}}^{t_m}\theta^{b}_{ib}(t,t_{m-1})×f_{sf}^b(t)+v_{sf}^b(t,t_{m-1})×w_{ib}^b(t)dt&划桨误差补偿\\ \Delta \theta_m=\int_{t_{m-1}}^mw_{ib}^b(t)dt&陀螺仪采样角增量 \\ \Delta v_m=\int_{t_{m-1}}^{t_m}f_{sf}^b(t)dt&加速计采样比力速度增量\\ \end{cases} ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧​Δvrot(m)b(m−1)​=21​Δθm​×Δvm​Δvscul(m)b(m−1)​=21​∫tm−1​tm​​θibb​(t,tm−1​)×fsfb​(t)+vsfb​(t,tm−1​)×wibb​(t)dtΔθm​=∫tm−1​m​wibb​(t)dtΔvm​=∫tm−1​tm​​fsfb​(t)dt​速度的旋转误差补偿划桨误差补偿陀螺仪采样角增量加速计采样比力速度增量​

假设陀螺仪角速度和加速度比力测量输出均为线性模型

即：

{wibb(t)=a+2b(t−tm−1)陀螺仪采样角增量fsfb(t)=A+2B(t−tm−1)加速计采样比力速度增量\begin{cases} w_{ib}^b(t)=a+2b(t-t_{m-1})&陀螺仪采样角增量 \\ f_{sf}^b(t)=A+2B(t-t_{m-1})&加速计采样比力速度增量\\ \end{cases} {wibb​(t)=a+2b(t−tm−1​)fsfb​(t)=A+2B(t−tm−1​)​陀螺仪采样角增量加速计采样比力速度增量​

得到

{Δθm=∫tm−1tmwibb(t)dt=a(t−tm−1)+b(t−tm−1)2陀螺仪采样角增量Δvm=∫tm−1tmfsfb(t)dt=A(t−tm−1)+B(t−tm−1)2加速计采样比力速度增量\begin{cases} \Delta \theta_m=\int_{t_{m-1}}^{t_m}w_{ib}^b(t)dt=a(t-t_{m-1})+b(t-t_{m-1})^2&陀螺仪采样角增量 \\ \Delta v_m=\int_{t_{m-1}}^{t_m}f_{sf}^b(t)dt=A(t-t_{m-1})+B(t-t_{m-1})^2&加速计采样比力速度增量\\ \end{cases} {Δθm​=∫tm−1​tm​​wibb​(t)dt=a(t−tm−1​)+b(t−tm−1​)2Δvm​=∫tm−1​tm​​fsfb​(t)dt=A(t−tm−1​)+B(t−tm−1​)2​陀螺仪采样角增量加速计采样比力速度增量​

可以得到：

{Δvrot(m)b(m−1)=12Δθm×Δvm速度的旋转误差补偿Δvscul(m)b(m−1)=(a×B+A×b)(tm−tm−1)36划桨误差补偿\begin{cases} \Delta v^{b(m-1)}_{rot(m)}=\frac{1}{2} \Delta \theta_m×\Delta v_m&速度的旋转误差补偿 \\ \Delta v^{b(m-1)}_{scul(m)}=(a×B+A×b)\frac{(t_m-t_{m-1})^3}{6}&划桨误差补偿\\ \end{cases} {Δvrot(m)b(m−1)​=21​Δθm​×Δvm​Δvscul(m)b(m−1)​=(a×B+A×b)6(tm​−tm−1​)3​​速度的旋转误差补偿划桨误差补偿​

若采用2子样法，在[tm−1,tm][t_{m-1},t_m][tm−1​,tm​]时间两次等间隔采样数据为:

Δθm1,Δθm2,Δvm1,Δvm2,h=T2=tm−tm−12\Delta\theta_{m1},\Delta\theta_{m2},\Delta v_{m1},\Delta v_{m2},h=\frac{T}{2}=\frac{t_m-t_{m-1}}{2}Δθm1​,Δθm2​,Δvm1​,Δvm2​,h=2T​=2tm​−tm−1​​

可以得到：

a=3Δθm1−Δθm22h,b=θm2−θm12h2a=\frac{3\Delta\theta_{m1}-\Delta\theta_{m2}}{2h},b=\frac{\theta_{m2}-\theta_{m1}}{2h^2}a=2h3Δθm1​−Δθm2​​,b=2h2θm2​−θm1​​

A=3Δvm1−Δvm22h,B=vm2−vm12h2A=\frac{3\Delta v_{m1}-\Delta v_{m2}}{2h},B=\frac{v_{m2}-v_{m1}}{2h^2}A=2h3Δvm1​−Δvm2​​,B=2h2vm2​−vm1​​

{Δvrot(m)b(m−1)=12Δθm×Δvm速度的旋转误差补偿Δvscul(m)b(m−1)=23(Δθm1×Δvm2+Δvm1×Δθm2)划桨误差补偿\begin{cases} \Delta v^{b(m-1)}_{rot(m)}=\frac{1}{2} \Delta \theta_m×\Delta v_m&速度的旋转误差补偿 \\ \Delta v^{b(m-1)}_{scul(m)}=\frac{2}{3}( \Delta \theta_{m1}×\Delta v_{m2}+\Delta v_{m1}×\Delta \theta_{m2})&划桨误差补偿\\ \end{cases} {Δvrot(m)b(m−1)​=21​Δθm​×Δvm​Δvscul(m)b(m−1)​=32​(Δθm1​×Δvm2​+Δvm1​×Δθm2​)​速度的旋转误差补偿划桨误差补偿​

∫tm−1tmθinn(t,tm−1)×[Cb(m−1)n(m−1)fsfb(t)]dt≈T6win(m−1/2)n×[Cb(m−1)n(m−1)(Δvm1+Δvm2)]\int_{t_{m-1}}^{t_m}\theta_{in}^n(t,t_{m-1})×[C_{b(m-1)}^{n(m-1)}f_{sf}^b(t)]dt\approx \frac{T}{6}w_{in(m-1/2)}^n×[C_{b(m-1)}^{n(m-1)}(\Delta v_{m1}+\Delta v_{m2})]∫tm−1​tm​​θinn​(t,tm−1​)×[Cb(m−1)n(m−1)​fsfb​(t)]dt≈6T​win(m−1/2)n​×[Cb(m−1)n(m−1)​(Δvm1​+Δvm2​)]

最后得到：

Δvsf(m)n=Cb(m−1)n(m−1)Δvm−T6win(m−1/2)n×[Cb(m−1)n(m−1)(Δvm1+5Δvm2)]+Cb(m−1)n(m−1)(Δvrot(m)b(m−1)+Δvscul(m)b(m−1))\Delta v_{sf(m)}^n=C_{b(m-1)}^{n(m-1)}\Delta v_m-\frac{T}{6}w_{in(m-1/2)}^n×[C_{b(m-1)}^{n(m-1)}(\Delta v_{m1}+5\Delta v_{m2})]+C_{b(m-1)}^{n(m-1)}(\Delta v_{rot(m)}^{b(m-1)}+\Delta v_{scul(m)}^{b(m-1)})Δvsf(m)n​=Cb(m−1)n(m−1)​Δvm​−6T​win(m−1/2)n​×[Cb(m−1)n(m−1)​(Δvm1​+5Δvm2​)]+Cb(m−1)n(m−1)​(Δvrot(m)b(m−1)​+Δvscul(m)b(m−1)​)

若在[tm−1,tm][t_{m-1},t_m][tm−1​,tm​]内的比力变化不大，可以近似看作Δvm1≈Δvm2≈12Δvm\Delta v_{m1} \approx \Delta v_{m2} \approx \frac{1}{2}\Delta v_mΔvm1​≈Δvm2​≈21​Δvm​

Δvsf(m)n≈[I−T2(win(m−1/2)n×)]Cb(m−1)n(m−1)(Δvm+Δvrot(m)b(m−1)+Δvscul(m)b(m−1))\Delta v_{sf(m)}^n \approx [I-\frac{T}{2}(w_{in(m-1/2)}^n×)]C_{b(m-1)}^{n(m-1)}(\Delta v_m+\Delta v_{rot(m)}^{b(m-1)}+\Delta v_{scul(m)}^{b(m-1)})Δvsf(m)n​≈[I−2T​(win(m−1/2)n​×)]Cb(m−1)n(m−1)​(Δvm​+Δvrot(m)b(m−1)​+Δvscul(m)b(m−1)​)

5 划桨误差补偿

（1）使用二子样法，测到划桨误差在多项式条件下的误差补偿如下（已在上方求解）：

Δvscul(m)b(m−1)=23(Δθm1×Δvm2+Δvm1×Δθm2)\Delta v^{b(m-1)}_{scul(m)}=\frac{2}{3}( \Delta \theta_{m1}×\Delta v_{m2}+\Delta v_{m1}×\Delta \theta_{m2})Δvscul(m)b(m−1)​=32​(Δθm1​×Δvm2​+Δvm1​×Δθm2​)

（2）理想条件下的划桨误差补偿

划桨运动：假设坐标系（b系：物体坐标系）：绕其x轴做角振动，绕其y轴做线振动，两者振动频率相同为Ω\OmegaΩ，单相差90。90^。90。

则其角度速度和比例分别为：

wibb(t)=[aΩtsinΩt00]w_{ib}^b(t)=\left[\begin{matrix} a\Omega tsin\Omega t&0&0\\ \end{matrix}\right]wibb​(t)=[aΩtsinΩt​0​0​]

fsfb(t)=[0βΩcosΩt0]f_{sf}^b(t)=\left[\begin{matrix} 0&\beta\Omega cos\Omega t&0\\ \end{matrix}\right]fsfb​(t)=[0​βΩcosΩt​0​]

在时间[tm−1,t][t_{m-1},t][tm−1​,t]内的角增量和比力增量为：

θibb(tm−1,t)=∫tm−1twibb(τ)dτ=[−a(sin⁡Ωt−cos⁡Ωtm−1)00]T\theta_{ib}^b(t_{m-1},t)=\int_{t_{m-1}}^{t}w_{ib}^b(\tau)d\tau=\left[\begin{matrix} -a(\sin\Omega t-\cos\Omega t_{m-1})&0&0\\ \end{matrix}\right]^Tθibb​(tm−1​,t)=∫tm−1​t​wibb​(τ)dτ=[−a(sinΩt−cosΩtm−1​)​0​0​]T

vsfb(tm−1,tm)=∫tm−1tfsfb(τ)dτ=[0β(sin⁡Ωt−sinΩttm−1)0]Tv_{sf}^b(t_{m-1},t_m)=\int_{t_{m-1}}^{t}f_{sf}^b(\tau)d\tau=\left[\begin{matrix} 0&\beta(\sin\Omega t-sin\Omega t_{t_{m-1}})&0\\ \end{matrix}\right]^Tvsfb​(tm−1​,tm​)=∫tm−1​t​fsfb​(τ)dτ=[0​β(sinΩt−sinΩttm−1​​)​0​]T

带入:

Δvscul(m)b(m−1)=12∫tm−1tmθibb(t,tm−1)×fsfb(t)+vsfb(t,tm−1)×wibb(t)dt\Delta v^{b(m-1)}_{scul(m)}=\frac{1}{2}\int_{t_{m-1}}^{t_m}\theta^{b}_{ib}(t,t_{m-1})×f_{sf}^b(t)+v_{sf}^b(t,t_{m-1})×w_{ib}^b(t)dtΔvscul(m)b(m−1)​=21​∫tm−1​tm​​θibb​(t,tm−1​)×fsfb​(t)+vsfb​(t,tm−1​)×wibb​(t)dt

=12∫tm−1tm[θibb(t,tm)+vsfb(t,tm−1)]×[fsfb(t)+wibb(t)]dt=\frac{1}{2}\int_{t_{m-1}}^{t_m}[ \theta_{ib}^b(t,t_m)+v_{sf}^b(t,t_{m-1})]×[f_{sf}^b(t)+w_{ib}^b(t)]dt=21​∫tm−1​tm​​[θibb​(t,tm​)+vsfb​(t,tm−1​)]×[fsfb​(t)+wibb​(t)]dt

划桨误差与圆锥误差的不可交换式形式上一致，因此将圆锥误差的补偿算法应用于划桨误差算法，圆锥误差求解，

求得划桨误差的补偿公式如下：

Δvscul(m)b(m−1)=∑i=1N−1kN−iΔθmi×ΔvmN+∑i=1N−1kN−iΔvmi×ΔθmN\Delta v^{b(m-1)}_{scul(m)}=\sum_{i=1}^{N-1}k_{N-i}\Delta\theta_{mi}\times\Delta v_{mN}+\sum_{i=1}^{N-1}k_{N-i}\Delta v_{mi}\times\Delta \theta_{mN}Δvscul(m)b(m−1)​=i=1∑N−1​kN−i​Δθmi​×ΔvmN​+i=1∑N−1​kN−i​Δvmi​×ΔθmN​

剩余误差为（未补偿的高阶误差项）：ρn与圆锥误差补偿ρn相同\rho_n与圆锥误差补偿\rho_n相同ρn​与圆锥误差补偿ρn​相同

▽n=1T[Δv‾scul(m)b(m−1)−Δvscul(m)b(m−1)]z=ρnαβΩ(ΩT)2N+1T(N≥1)\bigtriangledown n=\frac{1}{T}[\Delta \overline v^{b(m-1)}_{scul(m)}-\Delta v^{b(m-1)}_{scul(m)}]_z=\rho_n\frac{\alpha\beta\Omega(\Omega T)^{2N+1}}{T}(N\geq1)▽n=T1​[Δvscul(m)b(m−1)​−Δvscul(m)b(m−1)​]z​=ρn​TαβΩ(ΩT)2N+1​(N≥1)

6 位置更新算法

公式（1）公式（1）公式（1）：计算等效旋转矢量

ϕib(m)b=(Δθ1+Δθ2)+23Δθ1×Δθ2\phi_{ib(m)}^b=(\Delta\theta_1+\Delta\theta_2)+\frac{2}{3}\Delta\theta_1×\Delta\theta_2ϕib(m)b​=(Δθ1​+Δθ2​)+32​Δθ1​×Δθ2​

公式（2）公式（2）公式（2）：计算误差补偿值

Δvrot(m)b(m−1)=12Δθm×Δvm\Delta v^{b(m-1)}_{rot(m)}=\frac{1}{2} \Delta \theta_m×\Delta v_mΔvrot(m)b(m−1)​=21​Δθm​×Δvm​

Δvscul(m)b(m−1)=23(Δθm1×Δvm2+Δvm1×Δθm2)\Delta v^{b(m-1)}_{scul(m)}=\frac{2}{3}( \Delta \theta_{m1}×\Delta v_{m2}+\Delta v_{m1}×\Delta \theta_{m2})Δvscul(m)b(m−1)​=32​(Δθm1​×Δvm2​+Δvm1​×Δθm2​)

公式（3）公式（3）公式（3）：捷联惯导导数值更新算法

Cb(m)n(m)=Cn(m−1)n(m)Cb(m−1)n(m−1)Cb(m)b(m−1)C_{b(m)}^{n(m)}=C_{n(m-1)}^{n(m)}C_{b(m-1)}^{n(m-1)}C_{b(m)}^{b(m-1)}Cb(m)n(m)​=Cn(m−1)n(m)​Cb(m−1)n(m−1)​Cb(m)b(m−1)​

公式（4）公式（4）公式（4）速度更新算法

Δvsf(m)n≈[I−T2(win(m−1/2)n×)]Cb(m−1)n(m−1)(Δvm+Δvrot(m)b(m−1)+Δvscul(m)b(m−1))\Delta v_{sf(m)}^n \approx [I-\frac{T}{2}(w_{in(m-1/2)}^n×)]C_{b(m-1)}^{n(m-1)}(\Delta v_m+\Delta v_{rot(m)}^{b(m-1)}+\Delta v_{scul(m)}^{b(m-1)})Δvsf(m)n​≈[I−2T​(win(m−1/2)n​×)]Cb(m−1)n(m−1)​(Δvm​+Δvrot(m)b(m−1)​+Δvscul(m)b(m−1)​)

vmn(m)=vm−1n(m−1)+Δvsf(m)n+Δvcor/g(m)nv_m^{n(m)}=v_{m-1}^{n(m-1)}+\Delta v_{sf(m)}^n+\Delta v_{cor/g(m)}^nvmn(m)​=vm−1n(m−1)​+Δvsf(m)n​+Δvcor/g(m)n​

公式（5）公式（5）公式（5）位置更新算法

pm=pm−1+Mpv(m−1/2)(vm−1n(m−1)+vmn(m))T2p_m=p_{m-1}+M_{pv(m-1/2)}(v_{m-1}^{n(m-1)}+v_m^{n(m)})\frac{T}{2}pm​=pm−1​+Mpv(m−1/2)​(vm−1n(m−1)​+vmn(m)​)2T​

RNh=Rn+h;RMh=RM+hR_{Nh}=R_n+h;R_{Mh}=R_M+hRNh​=Rn​+h;RMh​=RM​+h

Mpv=[01/RMh0secL/RNh00001]TM_{pv}=\left[\begin{matrix} 0&1/R_{Mh}&0\\ secL/R_{Nh}&0&0\\ 0&0&1\\ \end{matrix}\right]^TMpv​=⎣⎡​0secL/RNh​0​1/RMh​00​001​⎦⎤​T

公式（6）公式（6）公式（6）外推公式

xm−1/2=xm−1+xm−1−xm−22(x=wien,wenn,vn,gn)x_{m-1/2}=x_{m-1}+\frac{x_{m-1}-x_{m-2}}{2}(x=w_{ie}^n,w_{en}^n,v^n,g^n)xm−1/2​=xm−1​+2xm−1​−xm−2​​(x=wien​,wenn​,vn,gn)