Laboratrio de guiagem, navegao e controle A

Universidade Federal do ABC

Prof. Leandro Baroni
Atividade 9: Filtro de Kalman
Objetivo: Implementar um ltro de Kalman estendido (FKE) usado para estimar a posio de
um VANT usando medidas de distncia a trs estaes de referncia.
A atividade baseada no artigo de Mao et al. (2007), que descreve uma aplicao do FKE no
qual o ltro usado para estimar a posio de um VANT usando medidas de distcia.
Problema:
O modelo dinmico discreto do VANT dado por:
x
k+1
= x
k
+ v
k
T cos
k
y
k+1
= y
k
+ v
k
T sen
k

k+1
=
k
+
k
T (1)

k+1
=
k
+
,k

T
v
k+1
= v
k
+
v,k

T
onde (x
k
, y
k
) so as coordenadas do VANT no plano horizontal,
k
a direo do VANT,
k

a velocidade angular (taxa de variao da direo) e v
k
a velocidade com relao ao solo. O
ndice k representa o instante de tempo t
k
e T o perodo de amostragem. As sequncias de
variveis aleatrias
,k
e
v,k
so rudos brancos mutuamente independentes com covarincias
Q

e Q
v
respectivamente.
Usando o vetor
x
k
=
[
x
k
y
k

k

k
v
k
]
T
(2)
pode-se escrever o modelo dinmico (1) na forma matricial:
x
k+1
=

1 0 0 0 T cos
k
0 1 0 0 T sen
k
0 0 1 T 0
0 0 0 1 0
0 0 0 0 1

x
k

f(x
k
)
+

0 0
0 0
0 0

T 0
0

G
[

,k

v,k
]
(3)
As medidas so distncias das estaes de referncia ao VANT. A distncia da i-sima estao
ao VANT
z
k,i
=

(x
k
X
i
)
2
+ (y
k
Y
i
)
2

h(x
k
)
+
k,i
(4)
onde (X
i
, Y
i
) so as coordenadas da i-sima estao de referncia e
k
uma sequncia de
variveis aleatrias de rudo branco. As coordenadas das estaes de referncia, em metros, so:
X
1
= 0 e Y
1
= 0
X
2
= 3000 e Y
2
= 2000
X
3
= 0 e Y
3
= 2000
Dadas a dinmica e a equao de medidas no lineares, uma estimativa da localizao do VANT
ao longo do tempo envolve o uso de um FKE. Este ltro assume que ambas equaes da dinmica
e medida devem ser linearizadas em torno da melhor estimativa disponvel.
Os passos para a implementao do FKE so:
Inicializao: De um estado x
+
0
e uma covarincia P
+
0
iniciais em k = 0. Os valores iniciais
sugeridos so:
x
+
0
=

1
1
0
0
0

e P
+
0
=

1000 0 0 0 0
0 1000 0 0 0
0 0 0.3 0 0
0 0 0 0.01 0
0 0 0 0 1

(5)
Propagao: dados x
+
k1
e P
+
k1
calculados no instante k 1, o valor de x

k
propagado ao
instante k calculado por:
x

k
= f( x
+
k1
) (6)
A matriz de derivadas (matriz de transio) :

k1
=
f
x
( x

k
) =

1 0 v
k1
T sen
k1
0 T cos
k1
0 1 v
k1
T cos
k1
0 T sen
k1
0 0 1 T 0
0 0 0 1 0
0 0 0 0 1

(7)
A matriz de covarincias propagada
P

k
=
k1
P
+
k1

T
k1
+ GQG
T
(8)
onde G denida em (3) e Q a matriz de covarincias dos termos de erros

e
v
. O valor
sugerido
Q =
[
0.0003 0
0 10
]
(9)
Clculo da predio das medidas:
z

k
= h( x

k
) =

( x

k
X
1
)
2
+ ( y

k
Y
1
)
2

( x

k
X
2
)
2
+ ( y

k
Y
2
)
2

( x

k
X
3
)
2
+ ( y

k
Y
3
)
2

(10)
onde h denido em (4). Calcule a matriz de derivadas de h:
H
k
=
h
x
( x

k
) =

k
X
1
z

k,1
y

k
Y
1
z

k,1
0 0 0
x

k
X
2
z

k,2
y

k
Y
2
z

k,2
0 0 0
x

k
X
3
z

k,3
y

k
Y
3
z

k,3
0 0 0

(11)
e a covarincia do erro da predio das medidas
S
k
= H
k

P

k
H
T
k
+ R (12)
R covarincia de
k
, com valor sugerido de
R =

100 0 0
0 100 0
0 0 100

(13)
Atualizao: Calcule o ganho de Kalman:
K
k
= P

k
H
T
k
S
1
k
(14)
Dado o vetor de medidas, atualize a estimativa do estado:
x
+
k
= x

k
+ K
k
(z
k
z

k
) (15)
e sua covarincia:
P
+
k
= (I K
k
H
k
)P

k
(16)
Implementao em MATLAB
Os dados das medidas de distncia das estaes esto em um vetor z, no qual cada linha
representa as medidas de uma estao e cada coluna representa um instante de tempo. H
ainda os valores da trajetria verdadeira, xtrue e ytrue. O perodo de amostragem T 1 s. H
vrios conjunto de dados, escolha um. Para carreg-los no MATLAB, use o comando:
load 'DadosNN.mat'
Atividades:
1. Faa a implementao do ltro de Kalman estendido para a localizao do VANT.
a) Plote os valores estimados dos estados.
b) Calcule e plote o trao da matriz de covarincias P
+
k
. Obs.: O MATLAB possui a funo
trace, para calcular o trao de uma matriz.
2. Compare sua estimativa com os valores reais fornecidos xtrue e ytrue, plotando nos
mesmos grcos que as estimativas.
3. Modique o script e repita os passos anteriores para o caso de estao car inoperante.
4. Discuta o desempenho do ltro nos dois experimentos feitos (com 2 e 3 estaes).
Referncias:
Brown, R. G.; Hwang, P. Y. C. Introduction to random signals and applied Kalman ltering. 3.
ed. New York: John Wiley & Sons, 1996. 480 p.
Mao, G.; Drake, S.; Anderson, B. D. O. Design of a Extended Kalman Filter for UAV Localization.
In: Proceedings of Information, Decision and Control 2007, Adelaide, February 2007. pp. 224-
229.
Welch, G.; Bishop, G. An introduction to the Kalman lter. In: 25 International Conference on
Computer Graphics and Interative Techniques, 12-17 August 2001. Los Angeles. Proceedings...
Los Angeles: SIGGRAPH, 2001.