Разработка и исследование алгоритмического обеспечения для основных режимов функционирования бесплатформенной инерциальной системы управления движением и навигации малогабаритного космического аппарата - page 5

Разработка и исследование алгоритмического обеспечения …
5
Обоснованно предполагая, что угловая скорость коррекции ωк и
угловое рассогласование (векторная часть / 2 ∇θ кватерниона δB)
являются малыми величинами, можно получить линеаризованное
описание, руководствуясь следующим логическим доказательством.
Поскольку кинематические уравнения для точного и вычисляе-
мого базиса углового положения имеют вид
2 ,
ˆ
2 ,
=
=
γ γ
γ γ
ω
ω
B B
B B
то кватернион текущего углового положения имеет вид
ˆ
δ =
γ γ
B B B .
Дифференцируя последнее выражение, имеем
ˆ ˆ ˆ 2δ = − δ = δ − δ
γ γ γ γ
ω ω ω ω B B B B B B B B ,
а с учетом того, что
ˆ
nw κ
= + + δ
ω ω ω ω ,
окончательно получим искомую форму линеаризованного описания
( )
( ) ( )
2
.
nw
nw
κ
κ
δ = δ + δ + δ − δ =
= δ + δ + δ − δ δ
ω ω ω ω
ω ω ω B ω
B B B B
B B B
Принимая во внимание наличие величин второго порядка мало-
сти (переменные ω κ и / 2 ∇θ являются малыми), полученное выраже-
ние можно существенно упростить и привести к виду
nw κ
∇ ≅ + δθ ω ωB .
Расчет коэффициента фильтра Калмана [5, 10] выполняется для
линеаризованных уравнений, записанных в стандартной форме и
включенных в состав структуры так, как показано на рис. 5. Здесь
,
,
δ ≅ δ +
δ = δ
x u w
y x
где δx — вектор состояния системы, размерностью 3×1; δu — вектор
управления, размерностью 3×1; А — матрица состояния системы
(нулевая), размерностью 3×3; С = I — матрица выхода системы (еди-
ничная), размерностью 3×3; w — вектор шума размерностью 3×1.
Алгоритм БИНС соответствует кинематическим уравнениям уг-
лового движения (КИН) на рис. 3.