一、姿态更新算法
对于地固系下的方向余弦阵有：
C b ( m ) e ( m ) = C e ( m − 1 ) e ( m ) C b ( m − 1 ) e ( m − 1 ) C b ( 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)
C b ( m − 1 ) e ( m ) ， C b ( m ) e ( m ) C_{b(m-1)}^{e(m)}， C_{b(m)}^{e(m)} Cb(m−1)e(m)，Cb(m)e(m)分别是 t m − 1 , t m t_{m-1},t_m tm−1,tm时刻的姿态矩阵。那么姿态更新问题就是求解 C e ( m − 1 ) e ( m ) C_{e(m-1)}^{e(m)} Ce(m−1)e(m)和 C b ( m ) b ( m − 1 ) C_{b(m)}^{b(m-1)} Cb(m)b(m−1)的问题。设在时间段 T = t m − t m − 1 T=t_m-t_{m-1} T=tm−tm−1内b系产生的等效旋转矢量为 ϕ i b ( m ) b \phi_{ib(m)}^b ϕib(m)b，其模为 ϕ \phi ϕ，反对称矩阵为 Φ \Phi Φ，地球自转的等效旋转矢量的反对称阵为 Φ e \Phi_e Φe，角速度为 ω e \omega_e ωe，那么：
C e ( m − 1 ) e ( m ) ≈ I − Φ e = I − [ 0 − ω e 0 ω e 0 0 0 0 0 ] 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
C b ( m ) b ( m − 1 ) = I + s i n ϕ ϕ Φ + 1 − c o s ϕ ϕ 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
可能有人会说：
C
e
(
m
−
1
)
e
(
m
)
=
I
C_{e(m-1)}^{e(m)}=I
Ce(m−1)e(m)=I，其实这是不对的。无论是机体所在的b系转动所产生的等效旋转矢量，还是地球自转的等效旋转矢量，都是以地球惯性系为参考基准的，所以不能认为
C
e
(
m
−
1
)
e
(
m
)
=
I
C_{e(m-1)}^{e(m)}=I
Ce(m−1)e(m)=I，同样在速度更新中也会遇见同样的问题。
下面利用等效旋转矢量法求解
ϕ
i
b
(
m
)
b
\phi_{ib(m)}^b
ϕib(m)b。
1、圆锥误差补偿算法
由于该部分的算法不因选取的惯导更新坐标系的不同而不同，在此不进行逐步推导，具体算法如下所示：
假设陀螺在 [ t m − 1 , t m ] [t_{m-1},t_m] [tm−1,tm]内进行了两次等间隔采样(二子样算法)，角增量分别为 Δ θ m 1 , Δ θ m 2 \Delta \theta_{m1},\Delta \theta_{m2} Δθm1,Δθm2，采用二子样圆锥误差补偿算法，有：
ϕ i b ( m ) b = ( Δ θ m 1 + Δ θ m 2 ) + 2 3 Δ θ m 1 × Δ θ m 2 \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 ) = C b e ( t ) f s f b ( t ) − 2 Ω i e e ( t ) v e ( t ) + g e ( 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)
时间段 [ t m − 1 , t m ] [t_{m-1},t_m] [tm−1,tm]内积分：
∫ t m − 1 t m v ˙ e ( t ) d t = ∫ t m − 1 t m C b e ( t ) f s f b ( t ) − 2 Ω i e e ( t ) v e ( t ) + g e ( t ) d t \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
即：
v m e − v m − 1 e = ∫ t m − 1 t m C b e ( t ) f s f b ( t ) d t + ∫ t m − 1 t m − 2 Ω i e e ( t ) v e ( t ) + g e ( t ) d t = Δ v s f ( m ) e + Δ v c o r / 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) Δ v s f ( m ) e = ∫ t m − 1 t m C b e ( t ) f s f b ( t ) d t \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) Δ v c o r / g ( m ) e = ∫ t m − 1 t m − 2 Ω i e e ( t ) v e ( t ) + g e ( t ) d t \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 = t m − t m − 1 T=t_m-t_{m-1} T=tm−tm−1内地固系内比力速度增量、有害加速度增量。
则有：
v m e = v m − 1 e + Δ v s f ( m ) e + Δ v c o r / 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、有害速度增量
一般认为有害加速度 Δ v c o r / g ( m ) e \Delta v_{cor/g(m)}^e Δvcor/g(m)e的被积函数是时间的缓慢量，可采用 t m − 1 / 2 = ( t m − 1 + t m ) / 2 t_{m-1/2}=(t_{m-1}+t_m)/2 tm−1/2=(tm−1+tm)/2时刻的值进行近似代替，则公式(2-2)近似为：
Δ v c o r / g ( m ) e ≈ [ − 2 Ω i e ( m − 1 / 2 ) e v m − 1 / 2 e + g m − 1 / 2 e ] 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
上式中 t m − 1 / 2 t_{m-1/2} tm−1/2时刻各量需使用外推法：
x m − 1 / 2 = x m − 1 + x m − 1 − x m − 2 2 = 3 x m − 1 − x m − 2 2 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 = Ω i e e , v e , g e x = \Omega_{ie}^e, v^e, g^e x=Ωiee,ve,ge
2、比力速度增量
将公式(2-1)进行如下分解：
Δ v s f ( m ) e = ∫ 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 s f b ( t ) d t \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
注意，这里 C e ( m − 1 ) e ( t ) = I C_{e(m-1)}^{e(t)}=I Ce(m−1)e(t)=I也是不成立的！！
假设与 C e ( m − 1 ) e ( t ) C_{e(m-1)}^{e(t)} Ce(m−1)e(t)对应的等效旋转矢量为 ϕ i e e ( t , t m − 1 ) \phi_{ie}^e(t,t_{m-1}) ϕiee(t,tm−1)，角增量为 θ i e e ( t , t m − 1 ) \theta_{ie}^e(t,t_{m-1}) θiee(t,tm−1)(近似为地球自转角速度*时间)，与 C b ( t ) b ( m − 1 ) C_{b(t)}^{b(m-1)} Cb(t)b(m−1)对应的等效旋转矢量为 ϕ i b b ( t , t m − 1 ) \phi_{ib}^b(t,t_{m-1}) ϕibb(t,tm−1)，角增量为 θ i b b ( t , t m − 1 ) \theta_{ib}^b(t,t_{m-1}) θibb(t,tm−1)，根据变换矩阵与等效旋转矢量之间的关系，当等效旋转矢量为小量时，可取如下一阶近似：
C e ( m − 1 ) e ( t ) ≈ I − ϕ i e e ( t , t m − 1 ) × ≈ I − θ i e e ( t , t m − 1 ) × C b ( t ) b ( m − 1 ) ≈ I + ϕ i b b ( t , t m − 1 ) × ≈ I + θ i b b ( t , t m − 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)×
那么便有：
Δ v s f ( m ) e = ∫ 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 s f b ( t ) d t ≈ ∫ t m − 1 t m [ I − θ i e e ( t , t m − 1 ) × ] C b ( m − 1 ) e ( m − 1 ) [ I + θ i b b ( t , t m − 1 ) × ] f s f b ( t ) d t ≈ ∫ t m − 1 t m C b ( m − 1 ) e ( m − 1 ) f s f b ( t ) + C b ( m − 1 ) e ( m − 1 ) [ θ i b b ( t , t m − 1 ) × ] f s f b ( t ) − [ θ i e e ( t , t m − 1 ) × ] C b ( m − 1 ) e ( m − 1 ) f s f b ( t ) d t = C b ( m − 1 ) e ( m − 1 ) ∫ t m − 1 t m f s f b ( t ) d t + C b ( m − 1 ) e ( m − 1 ) ∫ t m − 1 t m [ θ i b b ( t , t m − 1 ) × ] f s f b ( t ) d t − [ θ i e e ( t , t m − 1 ) × ] C b ( m − 1 ) e ( m − 1 ) ∫ t m − 1 t m f s f b ( t ) d t = ( I − [ θ i e e ( t , t m − 1 ) × ] ) C b ( m − 1 ) e ( m − 1 ) ∫ t m − 1 t m f s f b ( t ) d t + C b ( m − 1 ) e ( m − 1 ) ∫ t m − 1 t m [ θ i b b ( t , t m − 1 ) × ] f s f b ( t ) d t \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) 旋转和划桨误差
对于上式的第二项中 ∫ t m − 1 t m [ θ i b b ( t , t m − 1 ) × ] f s f b ( t ) d t \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 [ θ i b b ( t , t m − 1 ) × v s f b ( t , t m − 1 ) ] d t = ω i b b ( t ) × v s f b ( t , t m − 1 ) + θ i b b ( t , t m − 1 ) × f s f b ( t ) = − v s f b ( t , t m − 1 ) × ω i b b ( t ) − θ i b b ( t , t m − 1 ) × f s f b ( t ) + 2 θ i b b ( t , t m − 1 ) × f s f b ( 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)
移项整理可得：
θ i b b ( t , t m − 1 ) × f s f b ( t ) = 1 2 d [ θ i b b ( t , t m − 1 ) × v s f b ( t , t m − 1 ) ] d t + 1 2 [ θ i b b ( t , t m − 1 ) × f s f b ( t ) + v s f b ( t , t m − 1 ) × ω i b b ( 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)]
时间段 [ t m − 1 , t m ] [t_{m-1},t_m] [tm−1,tm]内积分：
∫ t m − 1 t m θ i b b ( t , t m − 1 ) × f s f b ( t ) d t = 1 2 θ i b b ( t m , t m − 1 ) × v s f b ( t m , t m − 1 ) + 1 2 ∫ t m − 1 t m θ i b b ( t , t m − 1 ) × f s f b ( t ) + v s f b ( t , t m − 1 ) × ω i b b ( t ) d t = Δ v r o t ( m ) b ( m − 1 ) + Δ v s c u l ( 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)
其中：
Δ v r o t ( m ) b ( m − 1 ) = 1 2 Δ θ m × Δ v m \Delta v_{rot(m)}^{b(m-1)} = \frac{1}{2}\Delta \theta_m × \Delta v_m Δvrot(m)b(m−1)=21Δθm×Δvm
Δ v s c u l ( m ) b ( m − 1 ) = 1 2 ∫ t m − 1 t m θ i b b ( t , t m − 1 ) × f s f b ( t ) + v s f b ( t , t m − 1 ) × ω i b b ( t ) d t \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 = θ i b b ( t m , t m − 1 ) = ∫ t m − 1 t m ω i b b ( t ) d t Δ v m = v s f b ( t m , t m − 1 ) = ∫ t m − 1 t m f s f b ( t ) d t \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
Δ
v
r
o
t
(
m
)
b
(
m
−
1
)
\Delta v_{rot(m)}^{b(m-1)}
Δvrot(m)b(m−1)称为速度的旋转误差补偿量；
Δ
v
s
c
u
l
(
m
)
b
(
m
−
1
)
\Delta v_{scul(m)}^{b(m-1)}
Δvscul(m)b(m−1)称为划桨误差补偿量；
(2) 划桨误差补偿算法
假设动坐标系(b)绕其x轴做角振动，同时绕y轴做线振动，两者的频率相同但相位正好差90°，即角速度和比力分别为：
ω i b b ( t ) = [ α Ω s i n Ω t 0 0 ] , f s f b ( t ) = [ 0 β Ω c o s Ω t 0 ] \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
Ω为震动频率。
积分得其角增量和比力增量：
θ i b b ( t , t m − 1 ) = ∫ t m − 1 t m ω i b b ( τ ) d τ = [ − α ( c o s Ω t − c o s Ω t m − 1 ) 0 0 ] \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⎦⎤
v s f b ( t , t m − 1 ) = ∫ t m − 1 t m f s f b ( τ ) d τ = [ 0 β ( s i n Ω t − s i n Ω t m − 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⎦⎤
那么：
Δ v s c u l ( m ) b ( m − 1 ) = 1 2 ∫ t m − 1 t m θ i b b ( t , t m − 1 ) × f s f b ( t ) + v s f b ( t , t m − 1 ) × ω i b b ( t ) d t = 1 2 ∫ t m − 1 t m [ − α ( c o s Ω t − c o s Ω t m − 1 ) 0 0 ] × [ 0 β Ω c o s Ω t 0 ] + [ 0 β ( s i n Ω t − s i n Ω t m − 1 ) 0 ] × [ α Ω s i n Ω t 0 0 ] d t = 1 2 ∫ t m − 1 t m [ − α ( c o s Ω t − c o s Ω t m − 1 ) β ( s i n Ω t − s i n Ω t m − 1 ) 0 ] × [ α Ω s i n Ω t β Ω c o s Ω t 0 ] d t = 1 2 ∫ t m − 1 t m [ θ i b b ( t , t m − 1 ) + v s f b ( t , t m − 1 ) ] × [ f s f b ( t ) + ω i b b ( t ) ] d t \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
若设 U m i = Δ θ m i + Δ v m i U_{mi}=\Delta \theta_{mi}+\Delta v_{mi} Umi=Δθmi+Δvmi，其中 Δ θ m i = ∫ t m − 1 + ( i − 1 ) h t m − 1 + i h ω i b b ( t ) d t , Δ v m i = ∫ t m − 1 + ( i − 1 ) h t m − 1 + i h f s f b ( t ) d t \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 ^ s c u l ( m ) b ( m − 1 ) = ∑ i = 1 N − 1 k N − i U m i × U m N = ∑ i = 1 N − 1 k N − i ( Δ θ m i + Δ v m i ) × ( Δ θ m N + Δ v m N ) \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)
在划桨运动下，注意到 Δ θ m i × Δ θ m N = Δ v m i × Δ v m N = 0 \Delta \theta_{mi} × \Delta \theta_{mN} = \Delta v_{mi} × \Delta v_{mN} = 0 Δθmi×ΔθmN=Δvmi×ΔvmN=0，故：
Δ v ^ s c u l ( m ) b ( m − 1 ) = ∑ i = 1 N − 1 k N − i Δ θ m i × Δ v m N + ∑ i = 1 N − 1 k N − i Δ v m i × Δ θ m N \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
其中系数
k
N
−
i
k_{N-i}
kN−i同圆锥误差补偿系数。
整理比力速度增量：
Δ v s f ( m ) e ≈ ( I − [ θ i e e ( t , t m − 1 ) × ] ) C b ( m − 1 ) e ( m − 1 ) ∫ t m − 1 t m f s f b ( t ) d t + C b ( m − 1 ) e ( m − 1 ) ∫ t m − 1 t m [ θ i b b ( t , t m − 1 ) × ] f s f b ( t ) d t ≈ ( I − [ θ i e e ( t , t m − 1 ) × ] ) C b ( m − 1 ) e ( m − 1 ) Δ v m + C b ( m − 1 ) e ( m − 1 ) ( Δ v r o t ( m ) b ( m − 1 ) + Δ v ^ s c u l ( 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))
其中， ∫ t m − 1 t m f s f b ( t ) d t ≈ f s f b ( t ) ∗ T ≈ Δ v m \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、公式汇总
重写速度更新如下：
v m e = v m − 1 e + Δ v s f ( m ) e + Δ v c o r / g ( m ) e Δ v s f ( m ) e ≈ ( I − [ θ i e e ( t , t m − 1 ) × ] ) C b ( m − 1 ) e ( m − 1 ) Δ v m + C b ( m − 1 ) e ( m − 1 ) ( Δ v r o t ( m ) b ( m − 1 ) + Δ v ^ s c u l ( m ) b ( m − 1 ) ) Δ v r o t ( m ) b ( m − 1 ) = 1 2 Δ θ m × Δ v m Δ v ^ s c u l ( m ) b ( m − 1 ) = ∑ i = 1 N − 1 k N − i Δ θ m i × Δ v m N + ∑ i = 1 N − 1 k N − i Δ v m i × Δ θ m N Δ v c o r / g ( m ) e ≈ [ − 2 Ω i e ( m − 1 / 2 ) e v m − 1 / 2 e + g m − 1 / 2 e ] 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
参考：
严恭敏.捷联惯导算法与组合导航原理讲义
1210
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
