Применение квазилинейной вычислительной схемы фильтра Кал
-
мана
[3, 4]
ˆ¯
x
k
+1
/k
= ¯
λ
( ˆ¯
x
k/k
, k
);
ˆ¯
x
k
+1
/k
+1
= ˆ¯
x
k
+1
/k
+
K
k
+1
[¯
z
k
+1
−
¯
ζ
( ˆ¯
x
k
+1
/k
, k
+ 1)];
K
k
+1
=
P
k
+1
/k
H
T
k
+1
(
H
k
+1
P
k
+1
/k
H
T
k
+1
+
R
k
+1
)
−
1
;
P
k
+1
/k
+1
= (
I
−
K
k
+1
H
k
+1
)
P
k
+1
/k
;
P
k
+1
/k
=
F
k
P
k/k
F
T
k
+
Q,
(8)
где
Q
(
k
)
и
R
(
k
+ 1)
—
ковариационные матрицы случайных векторов
¯
w
(
t
k
)
,
¯
v
(
t
k
+1
)
соответственно
,
требует линеаризации исходной модели
на основе степенного разложения Тейлора в окрестности номинального
движения
(
модифицированный фильтр Калмана
[5]):
F
k
=
∂
¯
λ
(¯
x
k,k
)
∂x
k
m m m m
¯
x
k
=ˆ¯
x
k
|
k
;
H
k
+1
=
∂
¯
ζ
(¯
x
k
+1
,k
+1
)
∂x
k
+1
m m m m
¯
x
k
+1
=ˆ¯
x
k
+1
|
k
.
(9)
Для решения этих уравнений необходимы оценки
ˆ¯
x
(0
/
0)
и ковариаци
-
онной матрицы
P
(0
/
0)
.
Вектор состояния исходной модели движения представим в форме
x
= [
V
x
, V
y
, V
z
, x
u
, y
u
, z
u
, ω
x
, ω
y
, ω
z
, λ
∗
0
, λ
∗
1
, λ
∗
2
, . . . , λ
∗
n
]
т
,
(10)
где
λ
∗
i
(
i
= 0
,
1
,
2
, . . . , n
)
—
компоненты расширенного вектора состоя
-
ния
,
подлежащие идентификации
.
Для принятой модели движения матрица
F
записывается в виде
:
F
(1
,
1) =
−
SC
x
ρV
x
/m
;
F
(1
,
2) =
ω
z
−
SC
x
ρV
y
/m
;
F
(1
,
3) =
−
ω
y
−
SC
x
ρV
z
/m
;
F
(1
,
4) =
−
g
r
A
11
;
F
(1
,
5) =
−
g
r
A
12
;
F
(1
,
6) =
−
g
r
A
13
;
F
(1
,
7) =
−
ω
y
ρ
y
−
ω
z
ρ
z
;
F
(1
,
8) =
−
V
z
−
ω
x
ρ
y
;
F
(1
,
9) =
V
y
−
ω
x
ρ
z
;
F
(1
,
10) =
−
g
r
[2
λ
0
x
u
+ 2
λ
3
y
u
−
2
λ
2
z
u
] ;
F
(1
,
11) =
−
g
r
[2
λ
1
x
u
+ 2
λ
2
y
u
+ 2
λ
3
z
u
] ;
F
(1
,
12) =
−
g
r
[
−
2
λ
2
x
u
+ 2
λ
1
y
u
−
2
λ
0
z
u
] ;
F
(1
,
13) =
−
g
r
[
−
2
λ
3
x
u
+ 2
λ
0
y
u
+ 2
λ
1
z
u
] ;
30 ISSN 0236-3941.
Вестник МГТУ им
.
Н
.
Э
.
Баумана
.
Сер
. “
Машиностроение
”. 2004.
№
4
