Você está na página 1de 63

Filtro de Kalman

Teoria e Aplicao para Iniciantes

Prof. Dr. Marco Antonio Leonel Caetano

M&V Consultoria e Treinamento


www.mudancasabruptas.com.br

A Histria da Filtragem de Sinais

1930
Filtro de Wiener
(contnuo)

1940
Filtro de Kolmogorov
(discreto)

1960
Filtro de Kalman
(linear e estendido)

Processos Estocsticos
Sinal no tempo

Estatstica
(varivel
aleatria)

O que caracteriza um sinal?


Mdia
Desvio Padro ( volatilidade )

Probabilidade

Sinal com baixa volatilidade

Sinal com alta volatilidade

Resolvendo Sistema Linear


Observar o seguinte sistema linear:

5 x1 2 x2 4

2 x1 x2 1
Para resolver seguem-se duas maneiras diferentes:
(1) Isolar x1 da primeira equao e substituir na segunda.
(2) Transformar o sistema em matrizes e vetores.

5 x1 2 x2 4

2 x1 x2 1

Primeira Resoluo

(a) Isolando x1 na primeira equao:

x1

4 2 x2
5

(b) Substituindo na segunda equao:


4 2 x2
2
x2 1
5

8 4 x2 5 x2 5

9 x 2 3

x2

1
3

(c) Substituindo x2 em x1 isolado em (a):

Soluo:

x1

2
3

x2

1
3

x1

2
3

5 x1 2 x2 4

2 x1 x2 1

Segunda Resoluo

(a) Transformar o sistema em representao matricial:

5 2 x1 4

x2 1

(b) Resolve-se agora o sistema : AX = B

(c) A soluo deve envolver a inversa da matriz A, ou seja, A-1

A1 AX A1B
(d) Como A-1.A matriz identidade a soluo ser: X = A-1B
(e) No matlab basta :

x = inv(A)*B

No Matlab

B tem que ser transposto! Ou B = [ 4 ; 1]

Problema com dimenso de matrizes


Resolver sistema linear com mesmo nmero de linhas e colunas
fcil usando matlab. Mas quando se tem MAIS equao que incgnita
a inversa da matriz no possvel.

2 x1 x2 1

3x1 2 x2 2
x x 3
1 2

3 equaes
2 incgnitas (x1,x2)

????

Mtodo dos Mnimos Quadrados


A soluo para o problema anterior encontrar o vetor x mais prximo
possvel tal que o sistema AX = B seja o mais verdadeiro possvel!
Deve-se encontrar o vetor x cujo erro do sistema seja o menor possvel
ao quadrado. Por isso o mtodo se chama MNIMOS QUADRADOS.
Exemplo:
Reta que mais se
aproxima dos
pontos amostrados

reta de regresso linear

b=tangente(angulo)

angulo

a
X

Os resduos dessa
diferena so os
menores possveis
quando elevados ao
quadrado.

A Estimativa do Mtodo dos Mnimos Quadrados (MMQ)

Seja

Ax B

AT Ax AT B

A A .A A.x A A
T

. AT B

identidade

Logo, para encontrar o vetor x mais prximo possvel da soluo:

1 T

T
x A A A B

Aplicao dos MMQ medidas


Imaginar que duas variveis foram acompanhadas por 3 dias e tiveram
seus valores relacionados na tabela a seguir:

medida1

medida 2

0,10

13

0,18

22

0,28

36

Suponha que exista a seguinte relao entre as medidas:

medida1 k medida2
Como estimar k?

Soluo via MMQ


0,10k 13
0,18k 22

Sistema com 3 equaes e 1 incgnita

0,28k 36
Neste caso as matrizes A e B sero:
0,10

A 0,18
0,28

13

B 22
36

Ento, lembrando que o x nesse problema o valor de k e:

1 T

T
x A A A B
Qual a soluo?

Soluo
(a)

0,1

AT A 0,1 0,18 0,28 0,18 0,1208


0,28

(b)

0,1

AT B 0,1 0,18 0,28 0,18 15,34


0,28

Assim,
1
k
15,34 127
0,1208

A Soluo Numrica

1 T

T
x A A A B

Estimativa de parmetros - Ajuste de Funo


Dada uma tabela de dados

X : x1
Y : y
1

x2
y2

x3
y3

Deseja-se encontrar a melhor funo linear que ajuste y aos valores de x:

y c0 c1 x
Observando que os dados so inseridos na funo da seguinte forma:

y(i) c0 c1 x(i)

O sistema a ser resolvido :

1 x1
y1


1 x2 c0 y2
. c

1
1 x
y
n

Exerccio
Fazer um programa para entrar com n valores de x e de y e no final
o programa deve ajustar a funo linear pelo mtodo dos mnimos
quadrados. Use como exemplo a tabela a seguir:
X

Soluo

Exerccio
Modificar o programa anterior para fazer o grfico dos pontos da
Tabela e dos pontos da reta ajustada y(i) = c0 + c1* x(i)

Soluo
5.5
5
4.5
pontos da tabela
4
3.5
3
RETA AJUSTADA
2.5
2
1.5
1

Estimao Recursiva

Considere o problema da estimativa de uma constante escalar no


Aleatria baseada em k medidas anteriores, corrompidas por rudo:

zi x vi

i = 1,2,3,..., k

onde vi rudo branco gaussiano ( mdia zero e desvio padro fixo).

Qual a melhor estimativa para x?

A mdia a melhor estimativa para x!


k

x k

z
i 1

Para uma medida adicional (k+1) a nova estimativa ser


k 1

x k 1

z
i 1

k 1

Deve-se manipular os termos para deixar a estimativa sempre em funo


das medidas anteriores:

x k 1

1 k

z i z k 1
k 1 i 1

1 k k

z i z k 1
k 1 k i 1

mdia

1 k

z i z k 1
k 1 k i 1

k
z k 1

x k
k 1
k 1

Somando-se e subtraindo

x k 1

x k
k 1

k
1
1
1

x k
x k
z k 1
x k
k 1
k 1
k 1
k 1

xk

1
zK 1 xk

k 1

Estimao recursiva de variveis

x k 1

1
x k
. z k 1 x k
k 1

Ganho do informao ainda


Sistema no utilizada

Exemplo
Estimar a constante a=10 com rudo de medida v (mdia = 0, desvio=5)
para 100 medidas de a.

Programando a frmula do mnimos quadrados recursivo:

Resultado

Para n simulaes

Comparando com as medidas

Por que usar a distribuio gaussina?


Segundo o teorema do limite central, todas convergem para a normal
para um nmero grande de pontos.
580

22
20
540

18
16

500

14
12

460

10
8

420

6
4

380
0.3

0.5

0.7

0.9

1.1

1.3

2
0

Teorema do Limite Central


22
20
18
16
14
12
10
8
6
4
2
0

NORMALIZAO

Para uma nica varivel


Mdia:
2

Varincia:

4
3
2
1
0
-1
-2
-3
-4

f ( x)

( x x )2
2 2

2 2

11

13

15

17

19

Para duas variveis Distribuio Espacial


( x x )

f ( x, y )
Mdia de x:

Mdia de y

2 xy2

Desvio Padro de x: x

Desvio padro de y: y

2
Covarincia de x e y: xy

Correlao de x e y:

2 1 ( x x )
( y y ) ( xy
)

( y y )
2

Mas a Covarincia uma matriz ( P )


necessrio uma correo na frmula pois,

2
xy

x2


x y

x y

2
y

A funo gaussina corrigida para duas variveis


( x x )

f ( x, y )

( x x )
( y y ) P 1

( y y )
2

2 P

1/ 2

onde | P | o determinante da matriz de covarincia

Problema
E se as variveis forem vetores de dados com sinais do tipo:

A covarincia seria:

12

12 1 2
P

x1

x2
x


xn

12 1 2 1n 1 n

2
2

n2

Impraticvel! Todos os sinais teriam que serem armazenados


em gigantescos bancos de dados para o calculo no fim.
Soluo: Filtro de Kalman

Como o filtro de Kalman estima variveis?


f x1 , x2 ,, xn

x1
x1

x2
x


xn

xn

sinal

tempo

Algoritmo Esquemtico
Trabalha com dados
Equao de Propagao
Equao de Atualizao
Equao de Covarincia

Satlite
dados
Bia

Equao de
x
Atualizao k 1

k
Equao de
x
Propagao k 1

k+1

Equao de
x
Propagao k 2

Equao de
x
Atualizao k 2

k+2
Equao de
x
Propagao k 3

tempo

Algumas Definies

Varivel de Estado (x)


- Representa a varivel de estudo do modelo matemtico para a
previso dos dados futuros. Ex: temperatura, salinidade.
- O filtro linear usa modelo linear.
- O futuro o presente com alguma correo e corrompido por rudo w k.

xk 1 Axk Ck

Varivel de Medida (z)


- Varivel que simula a aquisio dos dados de um sensor.
- Para o filtro linear o modelo de medida linear com rudo vk do sensor.

zk Hxk k

Propagao ( do estado ou da covarincia )


- a simulao do que se espera de dados futuros baseados em k dados.
- Utiliza um modelo para a previso.

Atualizao ( do estado ou da covarincia )


- a correo da previso usando os novos dados coletados pelo sensor.

Assumir
1.

O estado corrompido por perturbao do tipo rudo


branco gaussinano com mdia zero e covarincia Q (matriz
para todos os valores propagados e atualizados).
12x
0 0

0
0 22x 0
Q

2
0
0
0 nx

2.

A medida do sensor supostamente corrompida por rudo


branco gaussiano com mdia zero e covarincia R.
12z
0 0

2
0

0
0

2z
R

2
0
0
0 nz

Propagaes
Propagao do Estado ( priori)
Qual a melhor estimativa? A mdia! As barras representam a
mdia.

xk 1 Axk Ck

o rido branco do estado

xk 1 Axk
Propagao da covarincia do estado ( priori)

Pk 1 xk 1 xk 1 xk 1 xk 1

Pk 1 Axk xk Ck Axk xk Ck


Pk 1 APk AT CQC T

Atualizaes
Atualizao do estado

- A melhor estimativa de xk quando se tem medida zk aquela que


minimiza o termo do expoente da distribuio gaussiana.

( x x )

f ( x, y )

( x x )
( y y ) P 1

( y y )
2

2 P

1/ 2

Colocando a medida na distribuio gaussiana:

Deseja-se minimizar

Ou,

Isso desejado!

Isso leva seguinte relao

Isolando o estado x para sua estimativa

Usando a mesma idia do mnimos quadrados recursivos para novas medidas


chega-se a seguinte equao de atualizao

xk

posteriori

xk

priori

priori

priori

zk Hxk

Atualizao da covarincia
A melhor covarincia P quando se tem medida zk aquela que
zera o erro entre medida e varivel:

Cuja medida estatstica est na matriz de covarincia (a barra significa mdia):

A melhor estimativa conseguida substituindo a relao de atualizao


posteriori
xk
do estado na equao acima. A relao linear que surge :

priori T
P
H
posteriori
priori
P
P

HP prioriH T R

A matriz R a inversa da matriz de covarincia do erro


do sensor de medida.

O Filtro de Kalman
Priori = ( - )

Posteriori = ( + )

Equao de Propagao:

x k A. x k 1

propagao do estado

Pk A. Pk1. AT C. Q.CT

propagao da covarincia do estado

Equao de Atualizao:

Kk Pk . H T . H. Pk . H T R

Pk Pk Kk . H. Pk

x k x k Kk . z k H. x k

ganho do filtro

atualizao da matriz de covarincia do estado

atualizao do estado

Quando o Filtro Falha


1.

Modelamento errado do estado


- Se a matriz de perturbao Q do estado for mal dimensionada em
relao ao problema real o que acontece?
x k 1 Ax k AKk yk Hx k

Modelo

Medida

Pk APk1 A T CQCT

1
T
T
Kk Pk H HPk H R
x k x k Kk yk Hx k

Q Pk Kk x k x k

Resultado

-5

10

Sinal estimado 0
20
Errado!

10

10

x1

2
0
-2

x2

10
0
-10

Sinal real

2. Se o rudo de medida muito menor que a perturbao no estado,


ou seja, se R<<<Q.

Significa que o sensor totalmente confivel, pois:

R K

O filtro no filtra, acompanha as medidas


de perto, pois so confivies!

3. Se o rudo de estado muito menor que o rudo de medida,


ou seja, se Q<<<R.

Significa que o sensor no confivel, pois:


Q K

O filtro s usa o modelo e filtra tudo.


Abandona as Medidas, pois K0 !

4. Se a matriz de covarincia da perturbao do estado inicial P(0) for


muito baixa.

Ela significa a memria do filtro. Se P(0) baixa, o filtro no


tem memria e demora a enxergar acontecimentos futuros.

P(0) muito baixa


demora para aprender

P(0) ideal
aprende rpido

Caracterstica ideal da covarincia do estado Pk


P(0) alto

tempo

Covarinica do erro de localizao horizontal


(Takemasa, Shozo, 2007, pag 3849)
ERRO NO MODELO

O problema da estimativa da temperatura SIMULAO

Dados de temperaturas na localidade dos sensores (satlites, PCD,etc)


Dados com rudo.
O que se desejava?
- possvel estimar os dados em tempo real?
- possvel fazer uma previso confivel com
boa antecedncia?

Soluo: Filtro de Kalman

Sensores radiosondas (Takemasa, 2010)

Modelos

observado

Modelo Utilizado
xk 1 Axk Ck
1.
2.
3.
4.
5.
6.
7.

8.

Fez se uma hiptese que A = 1.


Fez-se a suposio que C =1.
O valor de H =1 pois s tem um sensor de temperatura.
A perturbao aleatria no estado (temperatura) foi Q=
0,5oC.
O rudo no sensor foi suposto de 10oC, ou varincia
R=100.
A matriz de covarincia inicial da perturbao do estado
P(0)=40.
Finalmente, foi dito ao filtro que a temperatura inicial foi
x(0)=0 oC, para obrigar a ter uma alta impreciso e fazer
um aprendizado rpido.
O tempo final de simulao para aprendizado e filtragem
foi de t = 100.

Filtro
Equao de Propagao:

xk xk1

x(0)=0

Pk Pk1 0,5

P(0)=40

Equao de Atualizao:

Pk
Kk
Pk 100

Pk 1 K k Pk

xk xk K k . zk xk

Simulando os dados do sensor

onde w rudo branco com mdia zero e desvio padro 1

Inicializando o filtro

A caixa do Filtro de Kalman

100 pontos

medida
estimado

A Covarincia do estado (temperatura)

P(k)

O filtro de Kalman aprendeu com 20 dados!!

Cenrio 2: Sensor com mais rudo Sinal mais


filtrado

erro = 22,36oC
R=erro2 = 500

Valoriza o modelo pois


medidas menos
Confiveis !

Cenrio 3: Covarincia com maior incerteza

P(0) = 400
R = 100

Ajusta mais rpido


s medidas. Elas
tem mais valor do
que o modelo!

Cenrio 4: Rudo de sensor super-baixo

P(0) = 40
R=1

Esquece o modelo pois


a melhor estimativa a
medida do sensor

BIBLIOGRAFIA