Topicos em Engenharia (Robotica Probabilstica)

Trabalho 3
Lucas Silva Lopes, 11/0129482
16 de Fevereiro de 2016

1 Tarefas
O algortmo do filtro de Kalman e:

Inicializac
ao:
+
x 0 = E{x0 }
P0+ = E{(x0 x
+ +
0 )(x0 x
T
0) }

Loop:

x +
k = Fk x k1 + Gk uk
Pk = Fk Pk1
+
FkT + Qk
Kk = Pk HkT (Hk Pk HkT + Rk )1
+
x
k =x
k + Kk (yk Hk x k)
Pk+ = (I Kk Hk )Pk (I Kk Hk )T + Kk Rk KkT

Nesse problema:

Fk = I
Gk = ts Rr2m
Hk = Rm2r
yk = (ydp2r [k] Rm2r Pporta )

1
 1 1
Figura 1: Resultado da simulacao com R[k] = R = 9 I, Q[k] = Q = 100000 I,
1
P [0] = 10000 I e x[0] = [10; 0.5].

2
Figura 2: data.xy est , data.P est (estimativa do filtro de Kalman e as
vari
ancias de x e y ao longo do tempo).

Figura 3: kf (struct com toda a informacao do filtro de Kalman).

3
1.1 Primeira Figura

Figura 4: Comparac ao da estimativa vs a posicao real (data.xy est e

data.xy real) ao longo do tempo (data.t).

4
1.2 Segunda Figura

ao (data.xy real data.xy est) ao longo do tempo

Figura 5: Erro de estimac
(data.t).

Figura 6: Mesmos gr
aficos da Figura 5, porem com a regiao esta-
cion
ariaampliada.

5
1.3 Terceira Figura

Figura 7: Vari
ancia de x e y (data.P est) ao longo do tempo (data.t).

Figura 8: Mesmos gr
aficos da Figura 7, porem com parte da regiao estacionaria
ampliada.

6
1.4 Quarta Figura

Figura 9: Gr afico unindo kf.sum var priori e kf.sum var posteriori ao longo
do tempo (data.t) e Gr afico com a norma do ganho de Kalman (kf.gain norm)
ao longo
PP 2 do tempo (data.t). A norma utilizada foi a norma de Frobenius kAk2F =
aij conforme consta na referencia [1].

7
Figura 10: Mesmos graficos da Figura 9, porem com o primeiro grafico ampliado
no joelhoda curva.

Figura 11: Mesmos gr aficos da Figura 9, porem com o primeiro grafico ampliado
na regi
ao estacion
aria.

8
1.5 Relacione o gr
afico das vari
ancias (terceira figura) com
o erro de estimacao (segunda figura). Lembre-se do
intervalo de confianca e sua relac
ao com a vari ancia.
Voce acredita que sua estimativa e consistente com a
vari
ancia obtida?
Conforme se observa na Terceira Figura ampliada (Figura 8), a variancia de x
e a vari
ancia de y convergiram para aproximadamente 0.00105 nesta simulacao.
Isto significa que:

2 = 0.00105
= 0.032403703492039

Sabe-se que, dada uma distribuicao normal: corresponde a um intervalo

de confianca de 68%; 2 corresponde a um intervalo de confianca de 95%; e
3 corresponde a um intervalo de confianca de 99.7%. Portanto um intervalo
de confianca de 99.7% para o erro de estimacao de x ou de y seria:

IC = 3
IC = (0) 3(0.032403703492039)
IC 0.1

Conforme se observa na Segunda Figura ampliada (Figura 6), realmente

parece que 99.7% dos pontos se encontram dentro de um intervalo entre 0.1 e
+0.1. Portanto, acredito que a estimativa obtida e consistente com a variancia
obtida.

1.6 O que aconteceria com a estimativa e a covari

ancia se a
c
amera nao encontrasse a porta por alguns instantes?
O que voce faria?
Se a c
amera n ao encontrasse a porta por alguns instantes, as medicoes dei-
xariam de ser confi aveis durante esse tempo. Eu aumentaria a covariancia do
rudo de medicao (R[k]) para sinalizar ao filtro de Kalman que desse um peso
maior `a predic
ao (sistema dinamico) enquanto a camera nao encontrasse a porta.
Depois eu retornaria R[k] para seu valor habitual.

9
1.7 Espera-se que voc es realizem v arias simulac
oes com
diferentes valores de Q[k] e P0 . A partir do que vimos
em aula, explique as diferencas observadas na sada
do filtro de Kalman. O que acontece se aumentarmos
Q[k] e se diminuirmos? O que acontece se alteramos
P0 ?
Partindo dos valores de P [0], Q e R utilizados na simulacao acima, se apenas
P [0] for alterado (para outro m ultiplo da identidade) o valor para o qual a
variancia de x e a vari ancia de y convergem nao e alterado (Terceira Figura).
No entanto, o valor inicial das curvas e alterada, como era de se esperar. Se
apenas Q for aumentado (ainda multiplo da identidade) o valor para o qual a
variancia de x e a variancia de y convergem tambem aumenta. Se apenas R for
aumentado (ainda multiplo da identidade) o valor para o qual a variancia de x
e a variancia de y convergem tambem aumenta, porem menos do que se apenas
Q fosse aumentado.
Se R >> P0+ , Q, o filtro de Kalman confia mais no chute inicial e nas
predic
oes iniciais (sistema dinamico) e menos nas medicoes iniciais, e portanto
vai demorar mais para convergir. Isto porque K1 EEST1 /(EEST1 + EM ED1 ),
sendo EEST1 o erro de estimacao inicial caracterizado por P1 , e EM ED1 o erro
de medic ao inicial caracterizado por R. Mas P1 depende tanto de P0+ quanto
de Q.
Se Q e pequeno e R e grande, o filtro de Kalman obedece mais o sistema
dinamico. A trajet oria estimada do robo se aproxima mais de uma linha reta,
mas a trajet oria real do rob o nao e em linha reta.
Por fim, Q e R ditam o quanto se confia mais nas predicoes (propagacao
da media e covari ancia do sistema dinamico) ou nas medicoes (estimativa por
mnimos quadrados recursivos). Q e a incerteza no sistema dinamico, e R e a
incerteza nas medic oes. Diminuir Q diz para o filtro confiar mais no sistema
dinamico, ao passa que aumentar Q diz para o filtro confiar mais nas medicoes.
Diminuir R diz para o filtro confiar mais nas medicoes, ao passo que aumentar R
diz para o filtro confiar mais no sistema dinamico. Aumentar Q indefinidamente
nao vai aumentar a covari ancia do erro de medicao (P) indefinidamente, pois a
incerteza de medic ao real e fixa (1 para um intervalo de confianca de 99.7%,
dada a distribuicao normal). Aumentar R indefinidamente vai apenas reduzir o
filtro `
a propagacao de media e covariancia do sistema dinamico.

Refer
encias
[1] G. Strang, Linear Algebra and Its Applications, 4th ed. Belmont, CA,
USA: Thomson, 2006, p. 479.

10