本文详细解析了惯性导航系统中姿态更新与速度更新的算法原理，包括姿态矩阵的更新公式、圆锥误差补偿算法，以及比力速度增量、有害加速度增量的计算方法。特别介绍了旋转误差和划桨误差的补偿算法，为提高惯导系统的精度提供了理论依据。
一、姿态更新算法
对于地固系下的方向余弦阵有：
Cb(m)e(m)=Ce(m−1)e(m)Cb(m−1)e(m−1)Cb(m)b(m−1) C_{b(m)}^{e(m)} = C_{e(m-1)}^{e(m)}C_{b(m-1)}^{e(m-1)} C_{b(m)}^{b(m-1)} Cb(m)e(m)=Ce(m−1)e(m)Cb(m−1)e(m−1)Cb(m)b(m−1)
Cb(m−1)e(m)，Cb(m)e(m)C_{b(m-1)}^{e(m)}， C_{b(m)}^{e(m)}Cb(m−1)e(m)，Cb(m)e(m)分别是tm−1,tmt_{m-1},t_mtm−1,tm时刻的姿态矩阵。那么姿态更新问题就是求解Ce(m−1)e(m)C_{e(m-1)}^{e(m)}Ce(m−1)e(m)和Cb(m)b(m−1)C_{b(m)}^{b(m-1)}Cb(m)b(m−1)的问题。设在时间段T=tm−tm−1T=t_m-t_{m-1}T=tm−tm−1内b系产生的等效旋转矢量为ϕib(m)b\phi_{ib(m)}^bϕib(m)b，其模为ϕ\phiϕ，反对称矩阵为Φ\PhiΦ，地球自转的等效旋转矢量的反对称阵为Φe\Phi_eΦe，角速度为ωe\omega_eωe，那么：
Ce(m−1)e(m)≈I−Φe=I−[0−ωe0ωe00000]T C_{e(m-1)}^{e(m)} \approx I-\Phi_e= I - \begin{bmatrix} 0 & -\omega_e & 0 \\ \omega_e & 0 & 0\\ 0 & 0 & 0 \end{bmatrix}T Ce(m−1)e(m)≈I−Φe=I−⎣⎡0ωe0−ωe00000⎦⎤T
Cb(m)b(m−1)=I+sinϕϕΦ+1−cosϕϕ2Φ2 C_{b(m)}^{b(m-1)} = I + \frac{sin\phi}{\phi}\Phi + \frac{1-cos\phi}{\phi^2} \Phi^2 Cb(m)b(m−1)=I+ϕsinϕΦ+ϕ21−cosϕΦ2
可能有人会说：Ce(m−1)e(m)=IC_{e(m-1)}^{e(m)}=ICe(m−1)e(m)=I，其实这是不对的。无论是机体所在的b系转动所产生的等效旋转矢量，还是地球自转的等效旋转矢量，都是以地球惯性系为参考基准的，所以不能认为Ce(m−1)e(m)=IC_{e(m-1)}^{e(m)}=ICe(m−1)e(m)=I，同样在速度更新中也会遇见同样的问题。
下面利用等效旋转矢量法求解ϕib(m)b\phi_{ib(m)}^bϕib(m)b。
1、圆锥误差补偿算法
由于该部分的算法不因选取的惯导更新坐标系的不同而不同，在此不进行逐步推导，具体算法如下所示：
假设陀螺在[tm−1,tm][t_{m-1},t_m][tm−1,tm]内进行了两次等间隔采样(二子样算法)，角增量分别为Δθm1,Δθm2\Delta \theta_{m1},\Delta \theta_{m2}Δθm1,Δθm2，采用二子样圆锥误差补偿算法，有：
ϕib(m)b=(Δθm1+Δθm2)+23Δθm1×Δθm2 \phi_{ib(m)}^b=(\Delta \theta_{m1}+\Delta \theta_{m2})+\frac{2}{3}\Delta \theta_{m1}×\Delta \theta_{m2} ϕib(m)b=(Δθm1+Δθm2)+32Δθm1×Δθm2
二、速度更新算法
比力方程：
v˙e(t)=Cbe(t)fsfb(t)−2Ωiee(t)ve(t)+ge(t) \dot v^e(t) = C_b^e(t) f_{sf}^b(t) - 2\Omega_{ie}^e(t) v^e(t) + g^e(t) v˙e(t)=Cbe(t)fsfb(t)−2Ωiee(t)ve(t)+ge(t)
时间段[tm−1,tm][t_{m-1},t_m][tm−1,tm]内积分：
∫tm−1tmv˙e(t)dt=∫tm−1tmCbe(t)fsfb(t)−2Ωiee(t)ve(t)+ge(t)dt \int_{t_{m-1}}^{t_m} \dot v^e(t) dt = \int_{t_{m-1}}^{t_m} C_b^e(t) f_{sf}^b(t) - 2\Omega_{ie}^e(t) v^e(t) + g^e(t) dt ∫tm−1tmv˙e(t)dt=∫tm−1tmCbe(t)fsfb(t)−2Ωiee(t)ve(t)+ge(t)dt
即：
vme−vm−1e=∫tm−1tmCbe(t)fsfb(t)dt+∫tm−1tm−2Ωiee(t)ve(t)+ge(t)dt=Δvsf(m)e+Δvcor/g(m)e \begin{aligned} v_m^e - v_{m-1}^e &= \int_{t_{m-1}}^{t_m} C_b^e(t) f_{sf}^b(t)dt + \int_{t_{m-1}}^{t_m} -2\Omega_{ie}^e(t) v^e(t) + g^e(t) dt \\ &= \Delta v_{sf(m)}^e + \Delta v_{cor/g(m)}^e \end{aligned} vme−vm−1e=∫tm−1tmCbe(t)fsfb(t)dt+∫tm−1tm−2Ωiee(t)ve(t)+ge(t)dt=Δvsf(m)e+Δvcor/g(m)e
其中：
(2-1)Δvsf(m)e=∫tm−1tmCbe(t)fsfb(t)dt \tag{2-1} \Delta v_{sf(m)}^e = \int_{t_{m-1}}^{t_m} C_b^e(t) f_{sf}^b(t)dt Δvsf(m)e=∫tm−1tmCbe(t)fsfb(t)dt(2-1)
(2-2)Δvcor/g(m)e=∫tm−1tm−2Ωiee(t)ve(t)+ge(t)dt \tag{2-2} \Delta v_{cor/g(m)}^e = \int_{t_{m-1}}^{t_m} -2\Omega_{ie}^e(t) v^e(t) + g^e(t) dt Δvcor/g(m)e=∫tm−1tm−2Ωiee(t)ve(t)+ge(t)dt(2-2)
上述两项分别为相应时间段T=tm−tm−1T=t_m-t_{m-1}T=tm−tm−1内地固系内比力速度增量、有害加速度增量。
则有：
vme=vm−1e+Δvsf(m)e+Δvcor/g(m)e v_m^e = v_{m-1}^e + \Delta v_{sf(m)}^e + \Delta v_{cor/g(m)}^e vme=vm−1e+Δvsf(m)e+Δvcor/g(m)e
1、有害速度增量
一般认为有害加速度Δvcor/g(m)e\Delta v_{cor/g(m)}^eΔvcor/g(m)e的被积函数是时间的缓慢量，可采用tm−1/2=(tm−1+tm)/2t_{m-1/2}=(t_{m-1}+t_m)/2tm−1/2=(tm−1+tm)/2时刻的值进行近似代替，则公式(2-2)近似为：
Δvcor/g(m)e≈[−2Ωie(m−1/2)evm−1/2e+gm−1/2e]T \Delta v_{cor/g(m)}^e \approx [-2\Omega_{ie(m-1/2)}^e v^e_{m-1/2} + g^e_{m-1/2}] T Δvcor/g(m)e≈[−2Ωie(m−1/2)evm−1/2e+gm−1/2e]T
上式中tm−1/2t_{m-1/2}tm−1/2时刻各量需使用外推法：
xm−1/2=xm−1+xm−1−xm−22=3xm−1−xm−22 x_{m-1/2} = x_{m-1} + \frac{x_{m-1}-x_{m-2}}{2} = \frac{3x_{m-1}-x_{m-2}}{2} xm−1/2=xm−1+2xm−1−xm−2=23xm−1−xm−2
其中，x=Ωiee,ve,gex = \Omega_{ie}^e, v^e, g^ex=Ωiee,ve,ge
2、比力速度增量
将公式(2-1)进行如下分解：
Δvsf(m)e=∫tm−1tmCe(m−1)e(t)Cb(m−1)e(m−1)Cb(t)b(m−1)fsfb(t)dt \Delta v_{sf(m)}^e = \int_{t_{m-1}}^{t_m} C_{e(m-1)}^{e(t)} C_{b(m-1)}^{e(m-1)} C_{b(t)}^{b(m-1)} f_{sf}^b(t)dt Δvsf(m)e=∫tm−1tmCe(m−1)e(t)Cb(m−1)e(m−1)Cb(t)b(m−1)fsfb(t)dt
注意，这里Ce(m−1)e(t)=IC_{e(m-1)}^{e(t)}=ICe(m−1)e(t)=I也是不成立的！！
假设与Ce(m−1)e(t)C_{e(m-1)}^{e(t)}Ce(m−1)e(t)对应的等效旋转矢量为ϕiee(t,tm−1)\phi_{ie}^e(t,t_{m-1})ϕiee(t,tm−1)，角增量为θiee(t,tm−1)\theta_{ie}^e(t,t_{m-1})θiee(t,tm−1)(近似为地球自转角速度*时间)，与Cb(t)b(m−1)C_{b(t)}^{b(m-1)}Cb(t)b(m−1)对应的等效旋转矢量为ϕibb(t,tm−1)\phi_{ib}^b(t,t_{m-1})ϕibb(t,tm−1)，角增量为θibb(t,tm−1)\theta_{ib}^b(t,t_{m-1})θibb(t,tm−1)，根据变换矩阵与等效旋转矢量之间的关系，当等效旋转矢量为小量时，可取如下一阶近似：
Ce(m−1)e(t)≈I−ϕiee(t,tm−1)×≈I−θiee(t,tm−1)×Cb(t)b(m−1)≈I+ϕibb(t,tm−1)×≈I+θibb(t,tm−1)× \begin{aligned} C_{e(m-1)}^{e(t)} &\approx I - \phi_{ie}^e(t,t_{m-1})× \approx I - \theta_{ie}^e(t,t_{m-1})× \\ C_{b(t)}^{b(m-1)} &\approx I + \phi_{ib}^b(t,t_{m-1})× \approx I + \theta_{ib}^b(t,t_{m-1})× \\ \end{aligned} Ce(m−1)e(t)Cb(t)b(m−1)≈I−ϕiee(t,tm−1)×≈I−θiee(t,tm−1)×≈I+ϕibb(t,tm−1)×≈I+θibb(t,tm−1)×
那么便有：
Δvsf(m)e=∫tm−1tmCe(m−1)e(t)Cb(m−1)e(m−1)Cb(t)b(m−1)fsfb(t)dt≈∫tm−1tm[I−θiee(t,tm−1)×]Cb(m−1)e(m−1)[I+θibb(t,tm−1)×]fsfb(t)dt≈∫tm−1tmCb(m−1)e(m−1)fsfb(t)+Cb(m−1)e(m−1)[θibb(t,tm−1)×]fsfb(t)−[θiee(t,tm−1)×]Cb(m−1)e(m−1)fsfb(t)dt=Cb(m−1)e(m−1)∫tm−1tmfsfb(t)dt+Cb(m−1)e(m−1)∫tm−1tm[θibb(t,tm−1)×]fsfb(t)dt−[θiee(t,tm−1)×]Cb(m−1)e(m−1)∫tm−1tmfsfb(t)dt=(I−[θiee(t,tm−1)×])Cb(m−1)e(m−1)∫tm−1tmfsfb(t)dt+Cb(m−1)e(m−1)∫tm−1tm[θibb(t,tm−1)×]fsfb(t)dt \begin{aligned} \Delta v_{sf(m)}^e &= \int_{t_{m-1}}^{t_m} C_{e(m-1)}^{e(t)} C_{b(m-1)}^{e(m-1)} C_{b(t)}^{b(m-1)} f_{sf}^b(t)dt \\ & \approx \int_{t_{m-1}}^{t_m} [I - \theta_{ie}^e(t,t_{m-1})×] C_{b(m-1)}^{e(m-1)} [I + \theta_{ib}^b(t,t_{m-1})×] f_{sf}^b(t)dt \\ & \approx \int_{t_{m-1}}^{t_m} C_{b(m-1)}^{e(m-1)} f_{sf}^b(t) + C_{b(m-1)}^{e(m-1)} [\theta_{ib}^b(t,t_{m-1})×] f_{sf}^b(t) -[\theta_{ie}^e(t,t_{m-1})×]C_{b(m-1)}^{e(m-1)} f_{sf}^b(t)dt \\ &= C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} f_{sf}^b(t)dt + C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} [\theta_{ib}^b(t,t_{m-1})×] f_{sf}^b(t)dt - [\theta_{ie}^e(t,t_{m-1})×]C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} f_{sf}^b(t)dt \\ &= (I-[\theta_{ie}^e(t,t_{m-1})×])C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} f_{sf}^b(t)dt + C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} [\theta_{ib}^b(t,t_{m-1})×] f_{sf}^b(t)dt \\ \end{aligned} Δvsf(m)e=∫tm−1tmCe(m−1)e(t)Cb(m−1)e(m−1)Cb(t)b(m−1)fsfb(t)dt≈∫tm−1tm[I−θiee(t,tm−1)×]Cb(m−1)e(m−1)[I+θibb(t,tm−1)×]fsfb(t)dt≈∫tm−1tmCb(m−1)e(m−1)fsfb(t)+Cb(m−1)e(m−1)[θibb(t,tm−1)×]fsfb(t)−[θiee(t,tm−1)×]Cb(m−1)e(m−1)fsfb(t)dt=Cb(m−1)e(m−1)∫tm−1tmfsfb(t)dt+Cb(m−1)e(m−1)∫tm−1tm[θibb(t,tm−1)×]fsfb(t)dt−[θiee(t,tm−1)×]Cb(m−1)e(m−1)∫tm−1tmfsfb(t)dt=(I−[θiee(t,tm−1)×])Cb(m−1)e(m−1)∫tm−1tmfsfb(t)dt+Cb(m−1)e(m−1)∫tm−1tm[θibb(t,tm−1)×]fsfb(t)dt
(1) 旋转和划桨误差
对于上式的第二项中∫tm−1tm[θibb(t,tm−1)×]fsfb(t)dt\int_{t_{m-1}}^{t_m} [\theta_{ib}^b(t,t_{m-1})×] f_{sf}^b(t)dt∫tm−1tm[θibb(t,tm−1)×]fsfb(t)dt，我们称为旋转与划船误差。下面对其进行分析，由于
d[θibb(t,tm−1)×vsfb(t,tm−1)]dt=ωibb(t)×vsfb(t,tm−1)+θibb(t,tm−1)×fsfb(t)=−vsfb(t,tm−1)×ωibb(t)−θibb(t,tm−1)×fsfb(t)+2θibb(t,tm−1)×fsfb(t) \begin{aligned} \frac{d[\theta_{ib}^b(t,t_{m-1}) × v_{sf}^b(t,t_{m-1})]}{dt} &= \omega_{ib}^b(t) × v_{sf}^b(t,t_{m-1}) + \theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) \\ &= -v_{sf}^b(t,t_{m-1}) × \omega_{ib}^b(t) - \theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) + 2\theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) \end{aligned} dtd[θibb(t,tm−1)×vsfb(t,tm−1)]=ωibb(t)×vsfb(t,tm−1)+θibb(t,tm−1)×fsfb(t)=−vsfb(t,tm−1)×ωibb(t)−θibb(t,tm−1)×fsfb(t)+2θibb(t,tm−1)×fsfb(t)
移项整理可得：
θibb(t,tm−1)×fsfb(t)=12d[θibb(t,tm−1)×vsfb(t,tm−1)]dt+12[θibb(t,tm−1)×fsfb(t)+vsfb(t,tm−1)×ωibb(t)] \theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) \\= \frac{1}{2} \frac{d[\theta_{ib}^b(t,t_{m-1}) × v_{sf}^b(t,t_{m-1})]}{dt} + \frac{1}{2} [\theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) + v_{sf}^b(t,t_{m-1}) × \omega_{ib}^b(t)] θibb(t,tm−1)×fsfb(t)=21dtd[θibb(t,tm−1)×vsfb(t,tm−1)]+21[θibb(t,tm−1)×fsfb(t)+vsfb(t,tm−1)×ωibb(t)]
时间段[tm−1,tm][t_{m-1},t_m][tm−1,tm]内积分：
∫tm−1tmθibb(t,tm−1)×fsfb(t)dt=12θibb(tm,tm−1)×vsfb(tm,tm−1)+12∫tm−1tmθibb(t,tm−1)×fsfb(t)+vsfb(t,tm−1)×ωibb(t)dt=Δvrot(m)b(m−1)+Δvscul(m)b(m−1) \int_{t_{m-1}}^{t_m} \theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) dt \\= \frac{1}{2} \theta_{ib}^b(t_m,t_{m-1}) × v_{sf}^b(t_m,t_{m-1}) + \frac{1}{2} \int_{t_{m-1}}^{t_m} \theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) + v_{sf}^b(t,t_{m-1}) × \omega_{ib}^b(t) dt \\ = \Delta v_{rot(m)}^{b(m-1)} + \Delta v_{scul(m)}^{b(m-1)} ∫tm−1tmθibb(t,tm−1)×fsfb(t)dt=21θibb(tm,tm−1)×vsfb(tm,tm−1)+21∫tm−1tmθibb(t,tm−1)×fsfb(t)+vsfb(t,tm−1)×ωibb(t)dt=Δvrot(m)b(m−1)+Δvscul(m)b(m−1)
其中：
Δvrot(m)b(m−1)=12Δθm×Δvm \Delta v_{rot(m)}^{b(m-1)} = \frac{1}{2}\Delta \theta_m × \Delta v_m Δvrot(m)b(m−1)=21Δθm×Δvm
Δvscul(m)b(m−1)=12∫tm−1tmθibb(t,tm−1)×fsfb(t)+vsfb(t,tm−1)×ωibb(t)dt \Delta v_{scul(m)}^{b(m-1)} = \frac{1}{2} \int_{t_{m-1}}^{t_m} \theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) + v_{sf}^b(t,t_{m-1}) × \omega_{ib}^b(t) dt Δvscul(m)b(m−1)=21∫tm−1tmθibb(t,tm−1)×fsfb(t)+vsfb(t,tm−1)×ωibb(t)dt
{Δθm=θibb(tm,tm−1)=∫tm−1tmωibb(t)dtΔvm=vsfb(tm,tm−1)=∫tm−1tmfsfb(t)dt \begin{cases} \Delta \theta_m = \theta_{ib}^b(t_m,t_{m-1}) = \int_{t_{m-1}}^{t_m} \omega_{ib}^b(t) dt \\ \Delta v_m = v_{sf}^b(t_m,t_{m-1}) = \int_{t_{m-1}}^{t_m} f_{sf}^b(t) dt \\ \end{cases} {Δθm=θibb(tm,tm−1)=∫tm−1tmωibb(t)dtΔvm=vsfb(tm,tm−1)=∫tm−1tmfsfb(t)dt
Δvrot(m)b(m−1)\Delta v_{rot(m)}^{b(m-1)}Δvrot(m)b(m−1)称为速度的旋转误差补偿量；
Δvscul(m)b(m−1)\Delta v_{scul(m)}^{b(m-1)}Δvscul(m)b(m−1)称为划桨误差补偿量；
(2) 划桨误差补偿算法
假设动坐标系(b)绕其x轴做角振动，同时绕y轴做线振动，两者的频率相同但相位正好差90°，即角速度和比力分别为：
ωibb(t)=[αΩsinΩt00],fsfb(t)=[0βΩcosΩt0] \omega_{ib}^b(t) = \begin{bmatrix} \alpha \Omega sin \Omega t \\ 0 \\ 0 \\ \end{bmatrix}, f_{sf}^b(t) = \begin{bmatrix} 0 \\ \beta \Omega cos \Omega t \\ 0 \\ \end{bmatrix} ωibb(t)=⎣⎡αΩsinΩt00⎦⎤,fsfb(t)=⎣⎡0βΩcosΩt0⎦⎤
其中，α\alphaα为角振动的角度幅值，β\betaβ为线振动的比力增量幅值，Ω\OmegaΩ为震动频率。
积分得其角增量和比力增量：
θibb(t,tm−1)=∫tm−1tmωibb(τ)dτ=[−α(cosΩt−cosΩtm−1)00] \theta_{ib}^b(t,t_{m-1}) = \int_{t_{m-1}}^{t_m} \omega_{ib}^b(\tau) d\tau = \begin{bmatrix} -\alpha(cos\Omega t - cos\Omega t_{m-1})\\ 0 \\ 0 \\ \end{bmatrix} θibb(t,tm−1)=∫tm−1tmωibb(τ)dτ=⎣⎡−α(cosΩt−cosΩtm−1)00⎦⎤
vsfb(t,tm−1)=∫tm−1tmfsfb(τ)dτ=[0β(sinΩt−sinΩtm−1)0] v_{sf}^b(t,t_{m-1}) = \int_{t_{m-1}}^{t_m} f_{sf}^b(\tau) d\tau = \begin{bmatrix} 0 \\ \beta(sin\Omega t - sin\Omega t_{m-1})\\ 0 \\ \end{bmatrix} vsfb(t,tm−1)=∫tm−1tmfsfb(τ)dτ=⎣⎡0β(sinΩt−sinΩtm−1)0⎦⎤
那么：
Δvscul(m)b(m−1)=12∫tm−1tmθibb(t,tm−1)×fsfb(t)+vsfb(t,tm−1)×ωibb(t)dt=12∫tm−1tm[−α(cosΩt−cosΩtm−1)00]×[0βΩcosΩt0]+[0β(sinΩt−sinΩtm−1)0]×[αΩsinΩt00]dt=12∫tm−1tm[−α(cosΩt−cosΩtm−1)β(sinΩt−sinΩtm−1)0]×[αΩsinΩtβΩcosΩt0]dt=12∫tm−1tm[θibb(t,tm−1)+vsfb(t,tm−1)]×[fsfb(t)+ωibb(t)]dt \begin{aligned} \Delta v_{scul(m)}^{b(m-1)} &= \frac{1}{2} \int_{t_{m-1}}^{t_m} \theta_{ib}^b(t,t_{m-1}) × f_{sf}^b(t) + v_{sf}^b(t,t_{m-1}) × \omega_{ib}^b(t) dt\\ &= \frac{1}{2} \int_{t_{m-1}}^{t_m} \begin{bmatrix} -\alpha(cos\Omega t - cos\Omega t_{m-1}) \\ 0 \\ 0 \\ \end{bmatrix} × \begin{bmatrix} 0 \\ \beta \Omega cos \Omega t \\ 0 \\ \end{bmatrix} \\ &+ \begin{bmatrix} 0 \\ \beta(sin\Omega t - sin\Omega t_{m-1})\\ 0 \\ \end{bmatrix} × \begin{bmatrix} \alpha \Omega sin \Omega t \\ 0 \\ 0 \\ \end{bmatrix} dt\\ &= \frac{1}{2} \int_{t_{m-1}}^{t_m} \begin{bmatrix} -\alpha(cos\Omega t - cos\Omega t_{m-1}) \\ \beta(sin\Omega t - sin\Omega t_{m-1}) \\ 0 \\ \end{bmatrix} × \begin{bmatrix} \alpha \Omega sin \Omega t \\ \beta \Omega cos \Omega t \\ 0 \\ \end{bmatrix} dt \\ &= \frac{1}{2} \int_{t_{m-1}}^{t_m} [\theta_{ib}^b(t,t_{m-1}) + v_{sf}^b(t,t_{m-1})] × [f_{sf}^b(t) + \omega_{ib}^b(t)] dt \end{aligned} Δvscul(m)b(m−1)=21∫tm−1tmθibb(t,tm−1)×fsfb(t)+vsfb(t,tm−1)×ωibb(t)dt=21∫tm−1tm⎣⎡−α(cosΩt−cosΩtm−1)00⎦⎤×⎣⎡0βΩcosΩt0⎦⎤+⎣⎡0β(sinΩt−sinΩtm−1)0⎦⎤×⎣⎡αΩsinΩt00⎦⎤dt=21∫tm−1tm⎣⎡−α(cosΩt−cosΩtm−1)β(sinΩt−sinΩtm−1)0⎦⎤×⎣⎡αΩsinΩtβΩcosΩt0⎦⎤dt=21∫tm−1tm[θibb(t,tm−1)+vsfb(t,tm−1)]×[fsfb(t)+ωibb(t)]dt
若设Umi=Δθmi+ΔvmiU_{mi}=\Delta \theta_{mi}+\Delta v_{mi}Umi=Δθmi+Δvmi，其中Δθmi=∫tm−1+(i−1)htm−1+ihωibb(t)dt,Δvmi=∫tm−1+(i−1)htm−1+ihfsfb(t)dt\Delta \theta_{mi}=\int_{t_{m-1}+(i-1)h}^{t_{m-1}+ih} \omega_{ib}^b(t)dt, \Delta v_{mi}=\int_{t_{m-1}+(i-1)h}^{t_{m-1}+ih} f_{sf}^b(t)dtΔθmi=∫tm−1+(i−1)htm−1+ihωibb(t)dt,Δvmi=∫tm−1+(i−1)htm−1+ihfsfb(t)dt， 类似于圆锥误差补偿公式，有划桨误差补偿算法(估计公式)：
Δv^scul(m)b(m−1)=∑i=1N−1kN−iUmi×UmN=∑i=1N−1kN−i(Δθmi+Δvmi)×(ΔθmN+ΔvmN) \begin{aligned} \Delta \hat v_{scul(m)}^{b(m-1)} &= \sum_{i=1}^{N-1} k_{N-i} U_{mi} × U_{mN} \\ &= \sum_{i=1}^{N-1} k_{N-i} (\Delta \theta_{mi}+\Delta v_{mi})× (\Delta \theta_{mN}+\Delta v_{mN}) \end{aligned} Δv^scul(m)b(m−1)=i=1∑N−1kN−iUmi×UmN=i=1∑N−1kN−i(Δθmi+Δvmi)×(ΔθmN+ΔvmN)
在划桨运动下，注意到Δθmi×ΔθmN=Δvmi×ΔvmN=0\Delta \theta_{mi} × \Delta \theta_{mN} = \Delta v_{mi} × \Delta v_{mN} = 0Δθmi×ΔθmN=Δvmi×ΔvmN=0，故：
Δv^scul(m)b(m−1)=∑i=1N−1kN−iΔθmi×ΔvmN+∑i=1N−1kN−iΔvmi×ΔθmN \Delta \hat v_{scul(m)}^{b(m-1)} = \sum_{i=1}^{N-1} k_{N-i} \Delta \theta_{mi} × \Delta v_{mN} + \sum_{i=1}^{N-1} k_{N-i} \Delta v_{mi} × \Delta \theta_{mN} Δv^scul(m)b(m−1)=i=1∑N−1kN−iΔθmi×ΔvmN+i=1∑N−1kN−iΔvmi×ΔθmN
其中系数kN−ik_{N-i}kN−i同圆锥误差补偿系数。
整理比力速度增量：
Δvsf(m)e≈(I−[θiee(t,tm−1)×])Cb(m−1)e(m−1)∫tm−1tmfsfb(t)dt+Cb(m−1)e(m−1)∫tm−1tm[θibb(t,tm−1)×]fsfb(t)dt≈(I−[θiee(t,tm−1)×])Cb(m−1)e(m−1)Δvm+Cb(m−1)e(m−1)(Δvrot(m)b(m−1)+Δv^scul(m)b(m−1)) \begin{aligned} \Delta v_{sf(m)}^e & \approx (I-[\theta_{ie}^e(t,t_{m-1})×]) C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} f_{sf}^b(t)dt + C_{b(m-1)}^{e(m-1)} \int_{t_{m-1}}^{t_m} [\theta_{ib}^b(t,t_{m-1})×] f_{sf}^b(t)dt \\ & \approx (I-[\theta_{ie}^e(t,t_{m-1})×]) C_{b(m-1)}^{e(m-1)} \Delta v_m + C_{b(m-1)}^{e(m-1)} (\Delta v_{rot(m)}^{b(m-1)} + \Delta \hat v_{scul(m)}^{b(m-1)}) \\ \end{aligned} Δvsf(m)e≈(I−[θiee(t,tm−1)×])Cb(m−1)e(m−1)∫tm−1tmfsfb(t)dt+Cb(m−1)e(m−1)∫tm−1tm[θibb(t,tm−1)×]fsfb(t)dt≈(I−[θiee(t,tm−1)×])Cb(m−1)e(m−1)Δvm+Cb(m−1)e(m−1)(Δvrot(m)b(m−1)+Δv^scul(m)b(m−1))
其中，∫tm−1tmfsfb(t)dt≈fsfb(t)∗T≈Δvm\int_{t_{m-1}}^{t_m} f_{sf}^b(t)dt \approx f_{sf}^b(t) * T \approx \Delta v_m∫tm−1tmfsfb(t)dt≈fsfb(t)∗T≈Δvm
3、公式汇总
重写速度更新如下：
vme=vm−1e+Δvsf(m)e+Δvcor/g(m)eΔvsf(m)e≈(I−[θiee(t,tm−1)×])Cb(m−1)e(m−1)Δvm+Cb(m−1)e(m−1)(Δvrot(m)b(m−1)+Δv^scul(m)b(m−1))Δvrot(m)b(m−1)=12Δθm×ΔvmΔv^scul(m)b(m−1)=∑i=1N−1kN−iΔθmi×ΔvmN+∑i=1N−1kN−iΔvmi×ΔθmNΔvcor/g(m)e≈[−2Ωie(m−1/2)evm−1/2e+gm−1/2e]T \begin{aligned} v_m^e &= v_{m-1}^e + \Delta v_{sf(m)}^e + \Delta v_{cor/g(m)}^e \\ \Delta v_{sf(m)}^e & \approx (I-[\theta_{ie}^e(t,t_{m-1})×]) C_{b(m-1)}^{e(m-1)} \Delta v_m + C_{b(m-1)}^{e(m-1)} (\Delta v_{rot(m)}^{b(m-1)} + \Delta \hat v_{scul(m)}^{b(m-1)}) \\ \Delta v_{rot(m)}^{b(m-1)} &= \frac{1}{2}\Delta \theta_m × \Delta v_m \\ \Delta \hat v_{scul(m)}^{b(m-1)} &= \sum_{i=1}^{N-1} k_{N-i} \Delta \theta_{mi} × \Delta v_{mN} + \sum_{i=1}^{N-1} k_{N-i} \Delta v_{mi} × \Delta \theta_{mN} \\ \Delta v_{cor/g(m)}^e & \approx [-2\Omega_{ie(m-1/2)}^e v^e_{m-1/2} + g^e_{m-1/2}] T \end{aligned} vmeΔvsf(m)eΔvrot(m)b(m−1)Δv^scul(m)b(m−1)Δvcor/g(m)e=vm−1e+Δvsf(m)e+Δvcor/g(m)e≈(I−[θiee(t,tm−1)×])Cb(m−1)e(m−1)Δvm+Cb(m−1)e(m−1)(Δvrot(m)b(m−1)+Δv^scul(m)b(m−1))=21Δθm×Δvm=i=1∑N−1kN−iΔθmi×ΔvmN+i=1∑N−1kN−iΔvmi×ΔθmN≈[−2Ωie(m−1/2)evm−1/2e+gm−1/2e]T
参考：
严恭敏.捷联惯导算法与组合导航原理讲义
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
