Разработка и исследование алгоритмического обеспечения …
15
где
1
(τ) τ
i
t
t
d
−
∇ =
∫
θ ω
— угол кажущегося поворота,
Δ
t
— шаг интегри-
рования.
1
1
1 0
ˆ
,
0,
0,1, 2,...,
1
k
k
t
k
k
k
k
t
dt
k
K
+
+
+
= +
= + ∇
= =
−
∫
Θ Θ ω Θ Θ Θ
;
[
]
1
1
1
0
1
1
1
ˆ
δ
δ
( ) ( )
δ
,
2
2
6
δ 0,
k
k
t
k
k
k
k
k
k
t
t
t dt
+
+
+
⎛
⎞
= +
×
= +
+ ∇ × ∇
⎜
⎟
⎝
⎠
=
∫
Θ Θ Θ ω
Θ Θ Θ Θ
Θ
∇Θ
0
берется из предыдущего интервала!
Окончательное выражение для оценки вектора истинного пово-
рота на конец интервала
t
m
имеет вид
[
]
1
1
1
2
3
ˆ
ˆ
ˆ
δ ,
Φ , Φ , Φ
m K
K m
m m m
m
−
−
= +
=
Φ
m
Φ Θ Θ Φ
', Φ º
.
(6)
Знание вектора истинного поворота дает возможность найти со-
ответствующее ему приращение кватерниона
ˆ
m
N
1
2
3
ˆ
cos ,
sin ,
sin ,
sin
2
2
2
2
m m
m m
m m
m
m
m
m
Φ Φ Φ Φ Φ Φ Φ
=
Φ
Φ
Φ
m
N
.
Далее происходит приближенное вычисление вспомогательной
функции sin (
x
/2)/
x
2
4
1
sin
0, 5 0, 02083333333333 0, 000260416666666
2
x
x
x
x
⎛ ⎞
≅ −
+
⎜ ⎟ ⎝ ⎠
.
Приближение для cos (
x
/2)
2
4
cos
1 0,125 0, 00260416666667
2
x
x
x
⎛ ⎞
≅ −
+
⎜ ⎟ ⎝ ⎠
.
Вычисление компонент кватерниона приращения
ˆ
k
N
при
k
x
Φ
0
1
2
3
1
cos ,
(1) sin ,
2
2
1
1
(2) sin ,
(3) sin ,
2
2
k
k
k
x
x
N
N
x
x
x
N
N
x
x
⎛ ⎞
⎛ ⎞
=
= Φ
⎜ ⎟
⎜ ⎟
⎝ ⎠
⎝ ⎠
⎛ ⎞
⎛ ⎞
= Φ
= Φ
⎜ ⎟
⎜ ⎟
⎝ ⎠
⎝ ⎠
