
Estimation of Dynamic Systems: Applications 501
S
u
≡
σ
u
Δt
3/2
(
σ
n
S
v
≡
σ
v
Δt
1/2
(
σ
n
ϑ
=
S
2
u
(4 + S
2
v
)+S
4
u
/12
1/2
ξ
= −
1
2
⎡
⎣
S
2
u
2
+
ϑ
+
/
S
2
u
2
+
ϑ
2
−4S
2
u
⎤
⎦
p
−
θθ
=
σ
2
n
ξ
S
u
2
−1
p
−
ββ
=
σ
n
Δt
2
S
2
u
1
ξ
+
1
2
−
ξ
p
+
θθ
=
σ
2
n
1 −
S
u
ξ
2
p
+
ββ
=
σ
n
Δt
2
S
2
u
1
ξ
−
1
2
−
ξ
• Extended Kalman Filter Application to GPS/INS
˙
ˆ
q =
1
2
Ξ(
ˆ
q) ˆω
B
B/N
(7.155)
ˆω
B
B/N
=(I
3×3
−
ˆ
K
g
)( ˜ω
B
B/I
−
ˆ
β
g
) −A
B
N
(
ˆ
q) ˆω
N
N/I
(7.156)
˙
ˆ
φ
=
ˆv
N
ˆ
R
φ
+
ˆ
h
(7.157)
˙
ˆ
λ
=
ˆv
E
(
ˆ
R
λ
+
ˆ
h)cos
ˆ
φ
(7.158)
˙
ˆ
h = −ˆv
D
(7.159)
˙
ˆv
N
= −
ˆv
E
(
ˆ
R
λ
+
ˆ
h)cos
ˆ
φ
+ 2
ω
e
ˆv
E
sin
ˆ
φ
+
ˆv
N
ˆv
D
ˆ
R
φ
+
ˆ
h
+ ˆa
N
(7.160)
˙
ˆv
E
=
ˆv
E
(
ˆ
R
λ
+
ˆ
h)cos
ˆ
φ
+ 2
ω
e
ˆv
N
sin
ˆ
φ
+
ˆv
E
ˆv
D
ˆ
R
λ
+
ˆ
h
+ 2
ω
e
ˆv
D
cos
ˆ
φ
+ ˆa
E
(7.161)
˙
ˆv
D
= −
ˆv
2
E
ˆ
R
λ
+
ˆ
h
−
ˆv
2
N
ˆ
R
φ
+
ˆ
h
−2
ω
e
ˆv
E
cos
ˆ
φ
+ ˆg + ˆa
D
(7.162)
ˆ
a
N
≡
⎡
⎣
ˆa
N
ˆa
E
ˆa
D
⎤
⎦
= A
N
B
(
ˆ
q)
ˆ
a
B
(7.163)