下式是位置、速度、姿态的误差方程式，详细的推导可参考严恭敏的《捷联惯导算法与组合导航原理讲义》
δ
λ
˙
=
v
E
s
e
c
L
t
a
n
L
R
N
h
δ
L
?
v
E
s
e
c
L
R
N
h
2
δ
h
+
s
e
c
L
R
N
h
δ
v
E
δ
L
˙
=
?
v
N
R
M
h
2
δ
h
+
1
R
M
h
δ
v
N
δ
h
˙
=
δ
v
U
v
˙
E
=
[
2
(
v
N
ω
N
+
v
U
ω
U
)
+
v
E
v
N
s
e
c
2
L
R
N
h
]
δ
L
+
v
E
(
v
U
?
v
N
t
a
n
L
)
R
N
h
2
δ
h
+
v
N
t
a
n
L
?
v
U
R
N
h
δ
v
E
+
(
2
ω
U
+
v
E
t
a
n
L
R
N
h
)
δ
v
N
?
(
2
ω
N
+
v
E
R
N
h
)
δ
v
U
?
f
U
?
N
+
f
N
?
U
+
?
E
v
˙
N
=
?
v
E
(
2
ω
N
+
v
E
s
e
c
2
L
R
N
h
)
δ
L
+
(
v
N
v
U
R
M
h
2
+
v
E
2
t
a
n
L
R
N
h
2
)
δ
h
?
2
(
ω
U
+
v
E
t
a
n
L
R
N
h
)
δ
v
E
?
v
U
R
M
h
δ
v
N
?
v
N
R
M
h
δ
v
U
+
f
U
?
E
?
f
E
?
U
+
?
N
v
˙
U
=
?
[
2
ω
U
v
E
+
g
e
s
i
n
2
L
(
β
?
4
β
1
c
o
s
2
L
)
]
δ
L
?
(
v
E
2
R
N
h
2
+
v
N
2
R
M
h
2
?
β
2
)
δ
h
+
?
f
N
?
E
+
f
E
?
N
+
2
(
ω
N
+
v
E
R
N
h
)
δ
v
E
+
2
v
N
R
M
h
δ
v
N
?
U
?
˙
E
=
v
N
R
M
h
2
δ
h
?
1
R
M
h
δ
v
N
+
(
ω
U
+
v
E
t
a
n
L
R
N
h
)
?
N
?
(
ω
N
+
v
E
R
N
h
)
?
U
?
ε
E
?
˙
N
=
?
ω
U
δ
L
?
v
E
R
M
h
2
δ
h
+
1
R
N
h
δ
v
E
?
(
ω
U
+
v
E
t
a
n
L
R
N
h
)
?
E
?
v
N
R
M
h
?
U
?
ε
N
?
˙
U
=
(
ω
N
+
v
E
s
e
c
2
L
R
N
h
)
δ
L
?
v
E
t
a
n
L
R
N
h
2
δ
h
+
t
a
n
L
R
N
h
δ
v
E
+
(
ω
N
+
v
E
R
N
h
)
?
E
+
v
N
R
M
h
?
N
?
ε
U
\begin{aligned} \delta \dot{\lambda} &= \frac{v_EsecLtanL}{R_{Nh}}\delta L - \frac{v_EsecL}{R^2_{Nh}}\delta h + \frac{secL}{R_{Nh}}\delta v_E\\ \delta \dot{L} &= - \frac{v_N}{R^2_{Mh}}\delta h + \frac{1}{R_{Mh}}\delta v_N\\ \delta \dot{h} &= \delta v_U\\ \dot{v}_E &=[2(v_N\omega_N + v_U\omega_U)+\frac{v_Ev_N sec^2L}{R_{Nh}}]\delta L +\frac{v_E(v_U-v_NtanL)}{R^2_{Nh}}\delta h+ \frac{v_NtanL - v_U}{R_{Nh}}\delta_{v_E} +(2\omega_U + \frac{v_E tanL}{R_{Nh}})\delta v_N -(2\omega_N + \frac{v_E}{R_{Nh}})\delta v_U\\ & -f_U\phi_N + f_N\phi_U +\nabla_E\\ \dot{v}_N &= -v_E(2\omega_N + \frac{v_E sec^2L}{R_{Nh}})\delta L +(\frac{v_Nv_U}{R^2_{Mh}}+\frac{v^2_EtanL}{R^2_{Nh}})\delta h - 2(\omega_U + \frac{v_E tanL}{R_{Nh}})\delta v_E -\frac{v_U}{R_{Mh}}\delta v_N - \frac{v_N}{R_{Mh}}\delta v_U + f_U\phi_E - f_E\phi_U\\ &+ \nabla_N\\ \dot{v}_U &= -[2\omega_Uv_E+g_esin2L(\beta-4\beta_1cos2L)]\delta L -(\frac{v^2_E}{R^2_{Nh}} + \frac{v^2_N}{R^2_{Mh}} - \beta_2)\delta_h+-f_N\phi_E + f_E\phi_N + 2(\omega_N + \frac{v_E}{R_{Nh}})\delta v_E +\frac{2v_N}{R_{Mh}}\delta v_N\\ &\nabla_U\\ \dot{\phi}_E &=\frac{v_N}{R_{Mh}^2}\delta h - \frac{1}{R_{Mh}}\delta v_N+ (\omega_U+\frac{v_E tanL}{R_{Nh}})\phi_N - (\omega_N + \frac{v_E}{R_{Nh}})\phi_U - \varepsilon_E\\ \dot{\phi}_N &= - \omega_U\delta L -\frac{v_E}{R^2_{Mh}}\delta h + \frac{1}{R_{Nh}}\delta v_E -(\omega_U+\frac{v_E tanL}{R_{Nh}})\phi_E -\frac{v_N}{R_{Mh}}\phi_U-\varepsilon_N\\ \dot{\phi}_U &= (\omega_N+\frac{v_E sec^2L}{R_{Nh}})\delta L -\frac{v_EtanL}{R^2_{Nh}}\delta h+ \frac{tanL}{R_{Nh}}\delta v_E + (\omega_N+\frac{v_E}{R_{Nh}})\phi_E +\frac{v_N}{R_{Mh}}\phi_N - \varepsilon_U\\ \end{aligned}
δλ˙δL˙δh˙v˙E?v˙N?v˙U??˙?E??˙?N??˙?U??=RNh?vE?secLtanL?δL?RNh2?vE?secL?δh+RNh?secL?δvE?=?RMh2?vN??δh+RMh?1?δvN?=δvU?=[2(vN?ωN?+vU?ωU?)+RNh?vE?vN?sec2L?]δL+RNh2?vE?(vU??vN?tanL)?δh+RNh?vN?tanL?vU??δvE??+(2ωU?+RNh?vE?tanL?)δvN??(2ωN?+RNh?vE??)δvU??fU??N?+fN??U?+?E?=?vE?(2ωN?+RNh?vE?sec2L?)δL+(RMh2?vN?vU??+RNh2?vE2?tanL?)δh?2(ωU?+RNh?vE?tanL?)δvE??RMh?vU??δvN??RMh?vN??δvU?+fU??E??fE??U?+?N?=?[2ωU?vE?+ge?sin2L(β?4β1?cos2L)]δL?(RNh2?vE2??+RMh2?vN2???β2?)δh?+?fN??E?+fE??N?+2(ωN?+RNh?vE??)δvE?+RMh?2vN??δvN??U?=RMh2?vN??δh?RMh?1?δvN?+(ωU?+RNh?vE?tanL?)?N??(ωN?+RNh?vE??)?U??εE?=?ωU?δL?RMh2?vE??δh+RNh?1?δvE??(ωU?+RNh?vE?tanL?)?E??RMh?vN???U??εN?=(ωN?+RNh?vE?sec2L?)δL?RNh2?vE?tanL?δh+RNh?tanL?δvE?+(ωN?+RNh?vE??)?E?+RMh?vN???N??εU??
将上式采用矩阵的形式可以写作：
δ
x
˙
(
t
)
=
F
(
t
)
δ
x
(
t
)
\begin{aligned} \delta\dot{\mathbf{x}}(t) = \mathbf{F}(t)\delta \mathbf{x}(t) \end{aligned}
δx˙(t)=F(t)δx(t)?
其中， F \mathbf{F} F的具体形式可以参见下表。
| \ | δ λ \delta\lambda δλ | δ L \delta L δL | δ h \delta h δh | δ v E \delta v_E δvE? | δ v N \delta v_N δvN? | δ v U \delta v_U δvU? | δ ? p i t c h \delta\phi_{pitch} δ?pitch? | δ ? r o l l \delta\phi_{roll} δ?roll? | δ ? y a w \delta\phi_{yaw} δ?yaw? |
|---|---|---|---|---|---|---|---|---|---|
| δ λ ˙ \delta\dot{\lambda} δλ˙ | v E s e c L t a n L R N h \frac{v_EsecLtanL}{R_{Nh}} RNh?vE?secLtanL? | ? v E s e c L R N h 2 -\frac{v_E secL}{R_{Nh}^{2}} ?RNh2?vE?secL? | s e c L R N h \frac{secL}{R_{Nh}} RNh?secL? | ||||||
| δ L ˙ \delta\dot{L} δL˙ | ? v n R M h 2 -\frac{v_n}{R_{Mh}^{2}} ?RMh2?vn?? | 1 R M h \frac{1}{R_{Mh}} RMh?1? | |||||||
| δ h ˙ \delta\dot{h} δh˙ | 1 | ||||||||
| δ v E ˙ \delta\dot{v_E} δvE?˙? | 2 ( v N ω N + v U ω U ) + v E v N s e c 2 L R N h 2(v_N\omega_N+v_U\omega_U)+\frac{v_Ev_Nsec^2L}{R_{Nh}} 2(vN?ωN?+vU?ωU?)+RNh?vE?vN?sec2L? | v E v U ? v E v n t a n λ R N h 2 \frac{v_Ev_U-v_Ev_ntan\lambda}{R_{Nh}^2} RNh2?vE?vU??vE?vn?tanλ? | v N t a n L ? v U R N h \frac{v_NtanL-v_U}{R_{Nh}} RNh?vN?tanL?vU?? | 2 ω U + v E t a n L R N h 2\omega_U+\frac{v_EtanL}{R_{Nh}} 2ωU?+RNh?vE?tanL? | ? ( 2 ω N + v E R N h ) -(2\omega_N+\frac{v_E}{R_{Nh}}) ?(2ωN?+RNh?vE??) | ? f U -f_U ?fU? | f N f_N fN? | ||
| δ v N ˙ \delta\dot{v_N} δvN?˙? | ? v E ( 2 ω N + v E s e c 2 L R N h ) -v_E(2\omega_N+\frac{v_Esec^2L}{R_{Nh}}) ?vE?(2ωN?+RNh?vE?sec2L?) | v N v U R M h 2 + v E 2 t a n L R N h 2 \frac{v_Nv_U}{R_{Mh}^2}+\frac{v_E^2tanL}{R_{Nh}^2} RMh2?vN?vU??+RNh2?vE2?tanL? | 2 ( ω U + v E t a n L R N h ) 2(\omega_U+\frac{v_EtanL}{R_{Nh}}) 2(ωU?+RNh?vE?tanL?) | ? v U R M h -\frac{v_U}{R_{Mh}} ?RMh?vU?? | ? v N R M h -\frac{v_N}{R_{Mh}} ?RMh?vN?? | ? f U -f_U ?fU? | ? f E -f_E ?fE? | ||
| δ v ˙ U \delta\dot{v}_U δv˙U? | ? 2 ω U v E -2\omega_Uv_E ?2ωU?vE? | ? ( v E 2 R N h 2 + v N 2 R M h 2 ) -(\frac{v_E^2}{R_{Nh}^2}+\frac{v_N^2}{R_{Mh}^2}) ?(RNh2?vE2??+RMh2?vN2??) | 2 ( ω N + v E R N h ) 2(\omega_N+\frac{v_E}{R_{Nh}}) 2(ωN?+RNh?vE??) | 2 v N R M h \frac{2v_N}{R_{Mh}} RMh?2vN?? | ? f N -f_N ?fN? | f E f_E fE? | |||
| δ ? ˙ E \delta\dot{\phi}_E δ?˙?E? | v N R M h 2 \frac{v_N}{R_{Mh}^2} RMh2?vN?? | ? 1 R M h -\frac{1}{R_{Mh}} ?RMh?1? | ω U + v E t a n L R N h \omega_U+\frac{v_EtanL}{R_{Nh}} ωU?+RNh?vE?tanL? | ? ( ω N + v E R N h ) -(\omega_N+\frac{v_E}{R_{Nh}}) ?(ωN?+RNh?vE??) | |||||
| δ ? ˙ N \delta\dot{\phi}_N δ?˙?N? | ω U \omega_U ωU? | v E R M h 2 \frac{v_E}{R_{Mh}^2} RMh2?vE?? | 1 R M h \frac{1}{R_{Mh}} RMh?1? | ? ( ω U + v E t a n L R N h ) -(\omega_U+\frac{v_EtanL}{R_{Nh}}) ?(ωU?+RNh?vE?tanL?) | ? v N R M h -\frac{v_N}{R_{Mh}} ?RMh?vN?? | ||||
| δ ? ˙ U \delta\dot{\phi}_U δ?˙?U? | ω U + v E t a n L R N h \omega_U+\frac{v_EtanL}{R_{Nh}} ωU?+RNh?vE?tanL? | v E t a n L R M h 2 \frac{v_EtanL}{R_{Mh}^2} RMh2?vE?tanL? | t a n L R N h \frac{tanL}{R_{Nh}} RNh?tanL? | ω N + v E R N h \omega_N+\frac{v_E}{R_{Nh}} ωN?+RNh?vE?? | v N R M h \frac{v_N}{R_{Mh}} RMh?vN?? | 1 |
| 续表 | a b i a s x abias_x abiasx? | a b i a s y abias_y abiasy? | a b i a s z abias_z abiasz? | g b i a s x gbias_x gbiasx? | g b i a s y gbias_y gbiasy? | g b i a s z gbias_z gbiasz? |
|---|---|---|---|---|---|---|
| δ λ ˙ \delta\dot{\lambda} δλ˙ | ||||||
| δ L ˙ \delta\dot{L} δL˙ | ||||||
| δ H ˙ \delta\dot{H} δH˙ | ||||||
| δ v ˙ E \delta\dot{v}_E δv˙E? | c 11 c_{11} c11? | c 12 c_{12} c12? | c 13 c_{13} c13? | |||
| δ v ˙ N \delta\dot{v}_N δv˙N? | c 21 c_{21} c21? | c 22 c_{22} c22? | c 23 c_{23} c23? | |||
| δ v ˙ U \delta\dot{v}_U δv˙U? | c 31 c_{31} c31? | c 32 c_{32} c32? | c 33 c_{33} c33? | |||
| δ ? ˙ E \delta\dot{\phi}_E δ?˙?E? | ? c 11 -c_{11} ?c11? | ? c 12 -c_{12} ?c12? | ? c 13 -c_{13} ?c13? | |||
| δ ? ˙ N \delta\dot{\phi}_N δ?˙?N? | ? c 21 -c_{21} ?c21? | ? c 22 -c_{22} ?c22? | ? c 23 -c_{23} ?c23? | |||
| δ ? ˙ U \delta\dot{\phi}_U δ?˙?U? | ? c 31 -c_{31} ?c31? | ? c 32 -c_{32} ?c32? | ? c 33 -c_{33} ?c33? |
进一步考虑加速度计和角速度计的测量噪声和加速度计零偏角速度计零偏噪声的随机模型，INS误差模型及传感器误差模型的联合形式可以进一步表达为：
δ
x
˙
(
t
)
=
F
(
t
)
δ
x
(
t
)
+
G
(
t
)
w
(
t
)
\begin{aligned} \delta\dot{\mathbf{x}}(t) = \mathbf{F}(t)\delta \mathbf{x}(t) + \mathbf{G}(t)\mathbf{w}(t) \end{aligned}
δx˙(t)=F(t)δx(t)+G(t)w(t)?
其中，噪声向量依次为加速度计测量噪声、角速度计测量噪声、加速度计零偏噪声，角速度计零偏噪声为：
w
=
[
w
a
T
,
w
g
T
,
w
a
b
T
,
w
g
b
T
]
T
\begin{aligned} \mathbf{w} = [\mathbf{w}^T_a, \mathbf{w}^T_g, \mathbf{w}^T_{ab}, \mathbf{w}^T_{gb}]^T \end{aligned}
w=[waT?,wgT?,wabT?,wgbT?]T?
噪声向量的设计矩阵为：
G
=
[
0
0
0
0
C
b
n
0
0
0
0
C
b
n
0
0
0
0
I
0
0
0
0
I
]
\begin{aligned} \mathbf{G}=\begin{bmatrix} 0 & 0 & 0 & 0\\ \mathbf{C}_b^n & 0 & 0 & 0\\ 0 & \mathbf{C}_b^n & 0 & 0\\ 0 & 0 & \mathbf{I} & 0\\ 0 & 0 & 0 & \mathbf{I}\\ \end{bmatrix} \end{aligned}
G=
?0Cbn?000?00Cbn?00?000I0?0000I?
??
w
\mathbf{w}
w为噪声向量。
w
(
t
)
\mathbf{w}(t)
w(t)的元素为白噪声，其协方差阵为：
E
[
w
(
t
)
w
(
τ
)
T
]
=
Q
(
t
)
δ
(
t
?
τ
)
\begin{aligned} E[\mathbf{w}(t)\mathbf{w}(\tau)^T] = \mathbf{Q}(t)\delta(t-\tau) \end{aligned}
E[w(t)w(τ)T]=Q(t)δ(t?τ)?由于捷联惯导系统一般使用高频离散采样数据，因此需要将方程转化为离散的形式：
δ
x
(
t
k
+
1
)
=
Φ
(
t
k
+
1
,
t
k
)
δ
x
(
t
k
)
+
G
(
τ
)
w
(
τ
)
d
τ
\begin{aligned} \delta \mathbf{x}(t_{k+1}) = \Phi(t_{k+1},t_k)\delta \mathbf{x}(t_k) + \mathbf{G}(\tau)\mathbf{w}(\tau)d\tau \end{aligned}
δx(tk+1?)=Φ(tk+1?,tk?)δx(tk?)+G(τ)w(τ)dτ?
Φ
k
\Phi_k
Φk?称为状态转移矩阵。
如果
△
t
k
+
1
=
t
k
+
1
?
t
k
\triangle t_{k+1} = t_{k+1} -t_k
△tk+1?=tk+1??tk?很小，因此在这段时间内可以认为
F
(
t
)
F(t)
F(t)为常量。转移矩阵的近似计算方法如下：
Φ
k
=
e
x
p
(
F
(
t
)
△
t
k
+
1
)
≈
I
+
F
(
t
)
△
t
k
+
1
\Phi_k = exp(F(t)\triangle t_{k+1})\approx I+F(t)\triangle t_{k+1}
Φk?=exp(F(t)△tk+1?)≈I+F(t)△tk+1?状态时间更新对应的协方差更新为：
Q
δ
x
(
t
k
+
1
)
=
Φ
(
t
k
+
1
,
t
k
)
Q
δ
x
(
t
k
)
Φ
T
(
t
k
+
1
+
G
(
τ
)
Q
w
(
τ
)
d
τ
G
T
(
τ
)
\begin{aligned} Q_{\delta \mathbf{x}(t_{k+1})} = \Phi(t_{k+1},t_k)Q_{\delta \mathbf{x}(t_k)}\Phi^T(t_{k+1} + \mathbf{G}(\tau)Q_{\mathbf{w}(\tau)d\tau}\mathbf{G}^T(\tau) \end{aligned}
Qδx(tk+1?)?=Φ(tk+1?,tk?)Qδx(tk?)?ΦT(tk+1?+G(τ)Qw(τ)dτ?GT(τ)?使用式子(1.6)和(1.8)即可完成时间更新。