⎧⎪⎨
⎪⎪⎩
˙
p
11
= 2
ωp
12
;
˙
p
12
=
ω
(
p
22
−
p
11
);
˙
p
22
=
−
2
ωp
12
.
(32)
Начальные условия для систем (31) и (32) ищутся в соответствии
с уравнениями (19). Заметим, что в этом случае сумма дисперсий
ошибок оценок координат маяка постоянна.
2. В момент измерений
⎧⎪⎪⎪⎪⎪⎪⎪⎨
⎪⎪⎪⎪⎪⎩
ˆ
x
+
k
ˆ
y
+
k
=
ˆ
x
k
ˆ
y
k
+
k
1
k
2
z
k
−
H
k
ˆ
x
k
ˆ
y
k
;
p
+
11
k
p
+
12
k
p
+
21
k
p
+
22
k
=
I
−
k
1
k
2
H
k
p
11
k
p
12
k
p
21
k
p
22
k
;
k
1
k
2
=
p
11
k
p
12
k
p
21
k
p
22
k
H
т
k
H
k
p
11
k
p
12
k
p
21
k
p
22
k
H
т
k
+
σ
2
−
1
.
(33)
В уравнениях (31)–(33)
ˆ
x ,
ˆ
y
— оценки координат маяка в связанной
системе координат;
p
ij
— элементыматрицыковариации ошибки оцен-
ки;
z
k
— измерения, формируемые в соответствии с соотношением (29)
(напомним, что в действительности мысчитаем, что измеряется угол
на маяк);
ω
,
v
— программные угловая и линейная скорости робота
соответственно;
σ
— среднеквадратическая ошибка в измерениях.
Введем следующие допущения.
1. Матрица измерений
Н
, представленная отношением (30), явля-
ется случайной, что, вообще говоря, не соответствует классической
модели фильтра Калмана.
2. Качество оценки, определяемое соотношениями (32), (33) (эле-
ментами
p
11
и
p
22
)
, зависит от собственного движения робота. Это ка-
жется естественным: например, если робот неподвижен, то очевидно,
что измерения не улучшают начальную оценку. Однако вопрос выбо-
ра движения
v
=
v
(
t
)
,
ω
=
ω
(
t
)
, обеспечивающего наиболее быструю
сходимость оценок, выходит за рамки настоящей работы.
На рис. 6 показанырезультатымоделирования приведенного алго-
ритма оценивания координат маяков
1
. Робот совершал вращательное
движение с угловой скоростью
ω
= cos(3
,
5
t
)
, при этом линейная ско-
рость
v
= 0
. Измерения угла на маяк проводились с частотой 5 Гц. Как
видно, в этих условиях время сходимости оценок составляет 3,5. . . 4 с.
1
Моделирование проводил В.В. Лепилкин
ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. “Приборостроение”. 2006. № 3 45
