v
k
n
=
v
k
?
1
n
+
△
v
f
,
k
n
+
△
v
g
/
c
o
r
,
k
n
v_{k}^{n}=v_{k-1}^{n}+\triangle v_{f,k}^{n}+\triangle v_{g/cor,k}^{n}
vkn?=vk?1n?+△vf,kn?+△vg/cor,kn?
n系到e系的四元数
q
n
e
q_{n}^{e}
qne?包含经纬度信息,四元数更新如下：
q
n
(
k
)
e
(
k
)
=
q
e
(
k
?
1
)
e
(
k
)
?
q
n
(
k
?
1
)
e
(
k
?
1
)
?
q
n
(
k
)
n
(
k
?
1
)
q_{n(k)}^{e(k)} = q_{e(k-1)}^{e(k)} \cdot q_{n(k-1)}^{e(k-1)} \cdot q_{n(k)}^{n(k-1)}
qn(k)e(k)?=qe(k?1)e(k)??qn(k?1)e(k?1)??qn(k)n(k?1)?其中，
q
n
(
k
)
n
(
k
?
1
)
=
[
c
o
s
∣
0.5
ζ
k
∣
?
s
i
n
∣
0.5
ζ
k
∣
0.5
ζ
k
0.5
ζ
k
]
q
e
(
k
?
1
)
e
(
k
)
=
[
c
o
s
∣
0.5
?
k
∣
?
s
i
n
∣
0.5
?
k
∣
0.5
?
k
0.5
?
k
]
q_{n(k)}^{n(k-1)}=\begin{bmatrix} cos|0.5\zeta_{k}| \\ -\frac{sin|0.5\zeta_{k}|}{0.5\zeta_{k}}0.5\zeta_{k}\end{bmatrix}\\ q_{e(k-1)}^{e(k)} = \begin{bmatrix} cos|0.5\epsilon_{k}| \\ -\frac{sin|0.5\epsilon_{k}|}{0.5\epsilon_{k}}0.5\epsilon_{k}\end{bmatrix}
qn(k)n(k?1)?=[cos∣0.5ζk?∣?0.5ζk?sin∣0.5ζk?∣?0.5ζk??]qe(k?1)e(k)?=[cos∣0.5?k?∣?0.5?k?sin∣0.5?k?∣?0.5?k??]则
?
k
=
ω
i
e
e
△
t
k
\epsilon_{k} =\omega_{ie}^{e}\triangle t_k
?k?=ωiee?△tk?:表示e(k)相对于e(k-1)的旋转向量。
ζ
k
?
1
/
2
=
[
ω
i
e
n
+
ω
e
n
n
]
k
?
1
△
t
k
2
\zeta_{k-1/2} = [\omega_{ie}^n+\omega_{en}^{n}]_{k-1}\frac{\triangle t_k}{2}
ζk?1/2?=[ωien?+ωenn?]k?1?2△tk??：这个量的计算需要位置和速度信息，首先根据更新后的速度，再次更新中点时刻的速度
v
k
?
1
/
2
n
=
1
2
(
v
k
n
+
v
k
?
1
n
)
v_{k-1/2}^{n}=\frac{1}{2}(v_{k}^{n}+v_{k-1}^{n})
vk?1/2n?=21?(vkn?+vk?1n?)
经纬度外推
单位化四元数
q
n
(
k
)
e
(
k
)
q_{n(k)}^{e(k)}
qn(k)e(k)?并得到对应的方向余弦矩阵，根据方向余弦矩阵求得此刻的经纬度值。
高程外推
h
k
=
h
k
?
1
?
v
D
,
k
?
1
/
2
△
t
k
h_{k} = h_{k-1} -v_{D,k-1/2}\triangle t_k
hk?=hk?1??vD,k?1/2?△tk?
b系到n系的姿态
q
b
n
q_{b}^{n}
qbn?更新算法：
q
b
(
k
)
n
(
k
)
=
q
n
(
k
?
1
)
n
(
k
)
?
q
b
(
k
?
1
)
n
(
k
?
1
)
?
q
b
(
k
)
b
(
k
?
1
)
q_{b(k)}^{n(k)} = q_{n(k-1)}^{n(k)}\cdot q_{b(k-1)}^{n(k-1)}\cdot q_{b(k)}^{b(k-1)}
qb(k)n(k)?=qn(k?1)n(k)??qb(k?1)n(k?1)??qb(k)b(k?1)?
b系四元数
q
b
(
k
)
b
(
k
?
1
)
q_{b(k)}^{b(k-1)}
qb(k)b(k?1)?更新如下：
q
b
(
k
)
b
(
k
?
1
)
=
[
c
o
s
∣
0.5
?
k
∣
?
s
i
n
∣
0.5
?
k
∣
0.5
?
k
0.5
?
k
]
q_{b(k)}^{b(k-1)}=\begin{bmatrix} cos|0.5\phi_{k}| \\ -\frac{sin|0.5\phi_{k}|}{0.5\phi_{k}}0.5\phi_{k}\end{bmatrix}
qb(k)b(k?1)?=[cos∣0.5?k?∣?0.5?k?sin∣0.5?k?∣?0.5?k??]b系的旋转向量计算方法为：
?
k
=
△
θ
k
+
1
12
△
θ
k
?
1
×
△
θ
k
\phi_k=\triangle \theta_k+\frac{1}{12}\triangle \theta_{k-1}\times\triangle\theta_k
?k?=△θk?+121?△θk?1?×△θk?
n系四元数
q
n
(
k
?
1
)
n
(
k
)
q_{n(k-1)}^{n(k)}
qn(k?1)n(k)?更新如下：
q
n
(
k
?
1
)
n
(
k
)
=
[
c
o
s
∣
0.5
ζ
k
∣
?
s
i
n
∣
0.5
ζ
k
∣
0.5
ζ
k
0.5
ζ
k
]
q_{n(k-1)}^{n(k)}=\begin{bmatrix} cos|0.5\zeta_{k}| \\ -\frac{sin|0.5\zeta_{k}|}{0.5\zeta_{k}}0.5\zeta_{k}\end{bmatrix}
qn(k?1)n(k)?=[cos∣0.5ζk?∣?0.5ζk?sin∣0.5ζk?∣?0.5ζk??]基于上边更新的位置信息，更新中点的位置，进一步更新
ζ
k
\zeta_{k}
ζk?。
最后，单位化四元数
q
b
(
k
)
n
(
k
)
q_{b(k)}^{n(k)}
qb(k)n(k)?，并得到对应的方向余弦矩阵，再有方向余弦阵计算欧拉角
X k / k ? 1 = F k / k ? 1 ? X k ? 1 \begin{aligned} \mathbf{X_{k/k-1}} = \mathbf{F_{k/k-1}} * \mathbf{X_{k-1}} \end{aligned} Xk/k?1?=Fk/k?1??Xk?1??
P k / k ? 1 = F k / k ? 1 ? P k ? 1 ? F k / k ? 1 T + 0.5 ? ( F k / k ? 1 ? G k / k ? 1 ? Q k ? 1 G k / k ? 1 T ? F k / k ? 1 T + G k / k ? 1 ? Q k ? 1 ? G k / k ? 1 T ) ? Δ t k \begin{aligned} \mathbf{P_{k/k-1}} &= \mathbf{F_{k/k-1}} * \mathbf{P_{k-1}} * \mathbf{F_{k/k-1}^T}+ 0.5 * (\mathbf{F_{k/k-1}} * \mathbf{G_{k/k-1}} * \mathbf{Q_{k-1}}\mathbf{G_{k/k-1}^T} \\ &* \mathbf{F_{k/k-1}^T} +\mathbf{G_{k/k-1}} * \mathbf{Q_{k-1}} * \mathbf{G_{k/k-1}^T}) * \Delta t_{k} \end{aligned} Pk/k?1??=Fk/k?1??Pk?1??Fk/k?1T?+0.5?(Fk/k?1??Gk/k?1??Qk?1?Gk/k?1T??Fk/k?1T?+Gk/k?1??Qk?1??Gk/k?1T?)?Δtk??