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:

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

Loop:
x +
k = Fk xk1 + Gk uk
Pk = Fk Pk1
+
FkT + Qk
Kk = Pk HkT (Hk Pk HkT + Rk )1

x+
k = xk + Kk (yk Hk xk )
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
variancias 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: Comparacao da estimativa vs a posicao real (data.xy est e

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

4
1.2 Segunda Figura

Figura 5: Erro de estimacao (data.xy real data.xy est) ao longo do tempo

(data.t).

Figura 6: Mesmos graficos da Figura 5, porem com a regiao esta-

cionariaampliada.

5
1.3 Terceira Figura

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

Figura 8: Mesmos graficos da Figura 7, porem com parte da regiao estacionaria

ampliada.

6
1.4 Quarta Figura

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

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

Figura 11: Mesmos graficos da Figura 9, porem com o primeiro grafico ampliado
na regiao estacionaria.

8
1.5 Relacione o grafico das variancias (terceira figura) com
o erro de estimacao (segunda figura). Lembre-se do
intervalo de confianca e sua relacao com a variancia.
Voce acredita que sua estimativa e consistente com a
variancia obtida?
Conforme se observa na Terceira Figura ampliada (Figura 8), a variancia de x
e a variancia 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 covariancia se a

camera nao encontrasse a porta por alguns instantes?
O que voce faria?
Se a camera nao encontrasse a porta por alguns instantes, as medicoes dei-
xariam de ser confiaveis 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 predicao (sistema dinamico) enquanto a camera nao encontrasse a porta.
Depois eu retornaria R[k] para seu valor habitual.

9
1.7 Espera-se que voces realizem varias simulacoes 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 multiplo da identidade) o valor para o qual a
variancia de x e a variancia 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
predicoes 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 medicao 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 trajetoria estimada do robo se aproxima mais de uma linha reta,
mas a trajetoria real do robo nao e em linha reta.
Por fim, Q e R ditam o quanto se confia mais nas predicoes (propagacao
da media e covariancia do sistema dinamico) ou nas medicoes (estimativa por
mnimos quadrados recursivos). Q e a incerteza no sistema dinamico, e R e a
incerteza nas medicoes. 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 covariancia do erro de medicao (P) indefinidamente, pois a
incerteza de medicao 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.

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

10