Стр. 4 - Алгоритм оценки параметров ориентации космического аппарата с использованием фильтра Калмана

ISSN 2305-5626. Вестник МГТУ им. Н.Э. Баумана: электронное издание. 2013 4
Для работы ФК требуется линеаризованная модель. Поэтому
сначала необходимо найти матрицу частных производных по фор-
муле
, 1,...,6
i
ij
j i j
f
F
x
=
∂
=
∂
(2)
и определить переходную матрицу состояния
Φ ( , )Δ ,k k k I F x t T +
≈ + (3)
где I — единичная матрица размерности 6 6× ; ΔT — шаг модели-
рования.
На следующем шаге определяют априори оценки вектора состояния
1 1
ˆ ˆ Φ
k k k x x
− +
− −
= (4)
и ковариационную матрицу ошибки вектора состояния (матрица P )
т
1 1 1 1 Φ Φ ,
k k k k k P P Q − +
− − − −
= + (5)
где 1 k Q − — ковариационная матрица модели движения на предыду-
щем шаге.
После того как найдены априори значения данных величин,
определяют апостериори их оценки, которые рассчитывают по сле-
дующим выражениям:
ˆ ˆ ˆ ;
k k k k k x x K z Hx+ − − ⎡ ⎤ = + − ⎣ ⎦
( ) , k k k k P I K H P+ −
= −
где k K — матрица коэффициентов обратной связи на шаге k, опреде-
ляемая по формуле
1
т т
;
k k k k k k k K P H H P H R
−
− − ⎡ ⎤ = +
⎣ ⎦ k z — вектор изме-
рений на k-м шаге; k H — матрица чувствительности; I — единичная
матрица размерности 6 6× ; k
R — ковариационная матрица шумов
измерений на k-м шаге.
При этом модель измерений будет представлять собой вектор-
ную часть кватерниона, которую находят как сумму истинного зна-
чения векторной части кватерниона и белого шума определенной
амплитуды:
k k k z q= + ν .