Вычислительные методы алгебры и оценивания. Семушин И.В. - 298 стр.

UptoLike

ВУЗ: 

УлГТУ | Ульяновск

Составители: 

Рубрика: 

13 Устойчивые алгоритмы фильтрации
Пусть на вход фильтра поступают нормализованные псевдоизмерения
(обладающие, как показано выше, единичной ковариационной матрицей
ошибок). Для модели процесса , описываемой уравнениями (13.18), функции
f[·] и h[·] имеют следующий вид:
f[s(t
i
)] =
x + (2/ω) v sin(ω∆t/2) cos(φ + ω∆t/2)
y + (2/ω) v sin(ω∆t/2) sin(φ + ω∆t/2)
v
φ + ω∆t
ω
t
i
,
h[s(t
i
)] =
x
y
t
i
.
Обозначим: ϕ = φ + ω∆t . Вычислим матрицы Якоби:
F (t
i−1
) ,
∂f
∂x
x=ˆx(t
+
i−1
)
, F =
=
1 0
sin(ϕ)−sin(φ)
ω
v(cos(ϕ)−cos(φ))
ω
v(∆t ω cos(ϕ)+sin(φ)−sin(ϕ))
ω
2
0 1
cos(φ)−cos(ϕ)
ω
v(sin(ϕ)−sin(φ))
ω
v(∆t ω sin(ϕ)+cos(ϕ)−cos(φ))
ω
2
0 0 1 0 0
0 0 0 1 ∆t
0 0 0 0 1
t
+
i−1
,
H(t
i
) ,
∂h
∂x
x=ˆx(t
+
i−1
)
≈
1 0 0 0 0
0 1 0 0 0
= H .
Поскольку нормализованные псевдоизмерения на входе фильтра «про-
изводятся» (т. е. формально представлены) в той же систе м е координат, в
которой з а писа ны координаты объекта в векторе состояния, функция h[·]
выполняет линейное преобразование, и матрица H(t
i
) = H не зависит от
времени. Приближенность записи H(t
i
) = H здесь состоит в том , что в усло-
виях малости величин ∆z
sph
косинус угловой погрешности заменен на 1.
При прямолинейном равномерном движении (ω = 0) функции f[·] и F
принимают следующую форму:
298

Страницы