1

Filtro de Kalman

Teoria e Aplicação para Iniciantes

Prof. Dr. Marco Antonio Leonel Caetano

M&V Consultoria e Treinamentowww.mudancasabruptas.com.br

A História da Filtragem de Sinais

1930 1940 1960

Filtro de Wiener

(contínuo)Filtro de Kolmogorov

(discreto)

Filtro de Kalman

(linear e estendido)

Sinal no tempo

Estatística

(variável

aleatória)

Processos Estocásticos

Probabilidade

O que caracteriza um sinal?

•Média

•Desvio Padrão ( volatilidade )

Sinal com alta volatilidade

Sinal com baixa volatilidade

Resolvendo Sistema Linear

Observar o seguinte sistema linear:

12

425

21

21

xx

xx

Para resolver seguem-se duas maneiras diferentes:

(1) Isolar x1 da primeira equação e substituir na segunda.

(2) Transformar o sistema em matrizes e vetores.

Primeira Resolução

12

425

21

21

xx

xx

5

24 21

xx

(a) Isolando x1 na primeira equação:

(b) Substituindo na segunda equação:

3

1

39

5548

15

242

2

2

22

22

x

x

xx

xx

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

21 x

Solução:3

21 x

3

12 x

Segunda Resolução

12

425

21

21

xx

xx

(a) Transformar o sistema em representação matricial:

1

4

12

25

2

1

x

x

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

(c) A solução deve envolver a inversa da matriz A, ou seja, A-1

BAAXA 11

(d) Como A-1.A é matriz identidade a solução 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 dimensão de matrizes

Resolver sistema linear com mesmo número de linhas e colunas é

fácil usando matlab. Mas quando se tem MAIS equação que incógnita

a inversa da matriz não é possível.

3

223

12

21

21

21

xx

xx

xx

3 equações

2 incógnitas (x1,x2)

????

Método dos Mínimos Quadrados

A solução para o problema anterior é encontrar o vetor x mais próximo

possível tal que o sistema AX = B seja o mais verdadeiro possível!

Deve-se encontrar o vetor x cujo erro do sistema seja o menor possível

ao quadrado. Por isso o método se chama MÍNIMOS QUADRADOS.

X

Y

reta de regressão linear

b=tangente(angulo)

a

angulo

Exemplo:

Reta que mais se

aproxima dos

pontos amostrados

Os resíduos dessa

diferença são os

menores possíveis

quando elevados ao

quadrado.

A Estimativa do Método dos Mínimos Quadrados (MMQ)

BAAAxAAAA

BAxAA

BxA

Seja

TTTT

TT

...11

identidade

Logo, para encontrar o vetor x mais próximo possível da solução:

BAAAx TT 1

Aplicação dos MMQ à medidas

Imaginar que duas variáveis 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 relação entre as medidas:

21 medidakmedida

Como estimar k?

Solução via MMQ

3628,0

2218,0

1310,0

k

k

kSistema com 3 equações e 1 incógnita

Neste caso as matrizes A e B serão:

28,0

18,0

10,0

A

36

22

13

B

Então, lembrando que o x nesse problema é o valor de k e:

BAAAx TT 1

Qual a solução?

Solução

1208,0

28,0

18,0

1,0

28,018,01,0

AAT(a)

34,15

28,0

18,0

1,0

28,018,01,0

BAT(b)

Assim,

12734,151208,0

1k

A Solução Numérica

BAAAx TT 1

Estimativa de parâmetros - Ajuste de Função

Dada uma tabela de dados

321

321

:

:

yyyY

xxxX

Deseja-se encontrar a melhor função linear que ajuste y aos valores de x:

xccy 10

Observando que os dados são inseridos na função da seguinte forma:

)()( 10 ixcciy O sistema a ser resolvido é:

nn y

y

y

c

c

x

x

x

2

1

1

02

1

.

1

1

1

A X B

Exercício

Fazer um programa para entrar com n valores de x e de y e no final

o programa deve ajustar a função linear pelo método dos mínimos

quadrados. Use como exemplo a tabela a seguir:

X 0 3 6

Y 1 4 5

Solução

ExercícioModificar o programa anterior para fazer o gráfico dos pontos da

Tabela e dos pontos da reta ajustada y(i) = c0 + c1* x(i)

Solução

0 1 2 3 4 5 61

1.5

2

2.5

3

3.5

4

4.5

5

5.5

pontos da tabela

RETA AJUSTADA

Estimação Recursiva

Considere o problema da estimativa de uma constante escalar não

Aleatória baseada em k medidas anteriores, corrompidas por ruído:

z x vi i i = 1,2,3,...,k

onde vi é ruído branco gaussiano ( média zero e desvio padrão fixo).

Qual a melhor estimativa para x?

A média é a melhor estimativa para x!

x

z

kk

ii

k

1

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

x

z

kk

ii

k

1

1

1

1

Deve-se manipular os termos para deixar a estimativa sempre em função

das medidas anteriores:

1

1

11

1ˆ

k

k

i

ik zzk

x

1

11

1 k

k

i

i zzk

k

k

k

kx

z

kk

k

1 1

1

média

1

11

1 k

k

i

i zzk

k

k

Somando-se e subtraindo x

k

k

1

xk

kx

kx

kz

kxk k k k k

1 11

1

1

1

1

1

1

kKk xzk

x ˆ1

1 ˆ 1

Estimação recursiva de variáveis

. x x

kz xk k k k

1 1

1

1Ganho doSistema

informação aindanão utilizada

Exemplo

Estimar a constante “a=10” com ruído de medida v (média = 0, desvio=5)

para 100 medidas de “a”.

Programando a fórmula do mínimos quadrados recursivo:

Resultado

Para “n” simulações

Comparando com as medidas

380

420

460

500

540

580

0.3 0.5 0.7 0.9 1.1 1.3

0

2

4

6

8

10

12

14

16

18

20

22

0

2

4

6

8

10

12

14

16

18

20

22

Teorema do Limite Central NORMALIZAÇÃO

Por que usar a distribuição gaussina?•Segundo o teorema do limite central, todas convergem para a normal

para um número grande de pontos.

-4

-3

-2

-1

0

1

2

3

4

1 3 5 7 9 11 13 15 17 19

Para uma única variável

2

2

)(

2)(

2

2

xx

exf

•Média:

•Variância:2

Para duas variáveis – Distribuição Espacial

2

2

)(

)()()()(

2),(

12

xy

yy

xxyyxx xy

eyxf

Média de x:

Média de y

Desvio Padrão de x:

Desvio padrão de y:

Covariância de x e y:

Correlação de x e y:

x

y

2

x

2

y

2

xy

Mas a Covariância é uma matriz ( P )

2

2

2

yyx

yxx

xyP

A função gaussina corrigida para duas variáveis

2/1

2

)(

)( )()(

2),(

1

P

eyxf

yy

xxPyyxx

onde | P | é o determinante da matriz de covariância

•É necessário uma correção na fórmula pois,

Problema

• E se as variáveis forem vetores de dados com sinais do tipo:

nx

x

x

x

2

1

•A covariância seria:

2

2

22112

112112

2

1

n

nn

P

Impraticável! Todos os sinais teriam que serem armazenados

em gigantescos bancos de dados para o calculo no fim.

Solução: Filtro de Kalman

tempo

x

nxxxf ,,, 21

x1

xn

nx

x

x

x

2

1

Como o filtro de Kalman estima variáveis?

sinal

Algoritmo Esquemático

•Trabalha com dados

•Equação de Propagação

•Equação de Atualização

•Equação de Covariância

tempo

Equação de

Propagação

1ˆ

kx

k k+1

Equação de

Atualização

1ˆ

kx

Bóia

Satélite

dados

Equação de

Propagação

2ˆ

kx

k+2

Equação de

Atualização

2ˆ

kx

Equação de

Propagação

3ˆ

kx

Algumas Definições

•Variável de Estado (x)

- Representa a variável de estudo do modelo matemático para a

previsão dos dados futuros. Ex: temperatura, salinidade.

- O filtro linear usa modelo linear.

- O futuro é o presente com alguma correção e corrompido por ruído wk.

•Variável de Medida (z)

- Variável que simula a aquisição dos dados de um sensor.

- Para o filtro linear o modelo de medida é linear com ruído vk do sensor.

•Propagação ( do estado ou da covariância )

- É a simulação do que se espera de dados futuros baseados em k dados.

- Utiliza um modelo para a previsão.

•Atualização ( do estado ou da covariância )

- É a correção da previsão usando os novos dados coletados pelo sensor.

kkk CAxx 1

kkk Hxz

Assumir

1. O estado é corrompido por perturbação do tipo ruído

branco gaussinano com média zero e covariância Q (matriz

para todos os valores propagados e atualizados).

2. A medida do sensor é supostamente corrompida por ruído

branco gaussiano com média zero e covariância R.

2

2

2

2

1

000

0

000

00

nx

x

x

Q

2

2

2

2

1

000

0

000

00

nz

z

z

R

Propagações

•Propagação do Estado (à priori)

Qual a melhor estimativa? A média! As barras representam a

média.

kk

kkk

xAx

CAxx

ˆˆ

estado do branco rúido o é

ˆ

1

1

•Propagação da covariância do estado (à priori)

TT

kk

T

kkkkkkk

T

kkkkk

CQCAAPP

CxxACxxAP

xxxxP

1

1

11111

Atualizações

•Atualização do estado- A melhor estimativa de xk quando se tem medida zk é aquela que

minimiza o termo do expoente da distribuição gaussiana.

2/1

2

)(

)( )()(

2),(

1

P

eyxf

yy

xxPyyxx

Isso é desejado!

Colocando a medida na distribuição gaussiana:

Deseja-se minimizar

Ou,

priori

kk

Tpriori

priori

k

posteriori

k xHzR

HPxx ˆˆˆ

Isso leva à seguinte relação

Isolando o estado x para sua estimativa

Usando a mesma idéia do mínimos quadrados recursivos para novas medidaschega-se a seguinte equação de atualização

•Atualização da covariância

A melhor covariância P quando se tem medida zk é aquela que

zera o erro entre medida e variável:

A melhor estimativa é conseguida substituindo a relação de atualização

do estado na equação acima. A relação linear que surge é:

RHHP

HPPP

Tpriori

Tprioriprioriposteriori

A matriz R é a inversa da matriz de covariância do erro

do sensor de medida.

Cuja medida estatística está na matriz de covariância (a barra significa média):

posteriori

kx̂

. x A xk k

1

P A P A C Q Ck k

T T

. . . .1

K P H H P H Rk k

T

k

T

. . . .1

P P K H Pk k k k

. .

. . x x K z H xk k k k k

Equação de Propagação:

propagação do estado

propagação da covariância do estado

ganho do filtro

atualização da matriz de covariância do estado

atualização do estado

Equação de Atualização:

O Filtro de KalmanPriori = ( - ) Posteriori = ( + )

Quando o Filtro Falha

1. Modelamento errado do estado

- Se a matriz de perturbação Q do estado for mal dimensionada em

relação ao problema real o que acontece?

Modelo Medida

1 RHHPHPK Tk

Tkk

TTkk CQCAAPP

1

kkkk xxKPQ ˆˆ

kkkkk xHyKxx ˆˆˆ

kkkkk xHyAKxAx ˆˆˆ

1

0 1 2 3 4 5 6 7 8 9 10-5

0

5

y

0 1 2 3 4 5 6 7 8 9 10-2

0

2

4

x1

0 1 2 3 4 5 6 7 8 9 10-10

0

10

20

x2

Resultado

Sinal real

Sinal estimado

Errado!

2. Se o ruído de medida é muito menor que a perturbação no estado,

ou seja, se R<<<Q.

Significa que o sensor é totalmente confiável, pois:

KR O filtro não filtra, acompanha as medidas

de perto, pois são confiávies!

3. Se o ruído de estado é muito menor que o ruído de medida,

ou seja, se Q<<<R.

Significa que o sensor não é confiável, pois:

KQ O filtro só usa o modelo e filtra tudo.

Abandona as Medidas, pois K0 !

4. Se a matriz de covariância da perturbação do estado inicial P(0) for

muito baixa.

Ela significa a memória do filtro. Se P(0) é baixa, o filtro não

tem “memória” e demora a “enxergar” acontecimentos futuros.

P(0) muito baixa

P(0) ideal

demora para aprender

aprende rápido

Característica ideal da covariância do estado Pk

tempo

P(0) alto

Covariânica do erro de localização horizontal

(Takemasa, Shozo, 2007, pag 3849)

ERRO NO MODELO

O problema da estimativa da temperatura SIMULAÇÃO

•Dados de temperaturas na localidade dos sensores (satélites, PCD,etc)

•Dados com ruído.

•O que se desejava?

- É possível estimar os dados em tempo real?

- É possível fazer uma previsão confiável com

boa antecedência?

•Solução: Filtro de Kalman

Sensores – radiosondas (Takemasa, 2010)

observado

Modelos

Modelo Utilizado

kkk CAxx 1

1. Fez –se uma hipótese que A = 1.

2. Fez-se a suposição que C =1.

3. O valor de H =1 pois só tem um sensor de temperatura.

4. A perturbação aleatória no estado (temperatura) foi Q=

0,5oC.

5. O ruído no sensor foi suposto de 10oC, ou variância

R=100.

6. A matriz de covariância inicial da perturbação do estado

P(0)=40.

7. Finalmente, foi dito ao filtro que a temperatura inicial foi

x(0)=0 oC, para obrigar a ter uma alta imprecisão e fazer

um aprendizado rápido.

8. O tempo final de simulação para aprendizado e filtragem

foi de t = 100.

Filtro

1ˆˆ

kk xx

5,01

kk PP

100

k

kk

P

PK

kkk PKP 1

kkkkk xzKxx ˆ.ˆˆ

Equação de Atualização:

Equação de Propagação:

x(0)=0

P(0)=40

Simulando os dados do sensor

onde w é ruído branco com média zero e desvio padrão 1

Inicializando o filtro

A caixa do Filtro de Kalman

100 pontosmedida

estimado

A Covariância do estado (temperatura)

O filtro de Kalman “aprendeu” com 20 dados!!

P(k)

Cenário 2: Sensor com mais ruído – Sinal mais

filtrado

erro = 22,36oC

R=erro2 = 500

Valoriza o modelo pois

medidas menos

Confiáveis !

Cenário 3: Covariância com maior incerteza

P(0) = 400

R = 100

Ajusta mais rápido

às medidas. Elas

tem mais valor do

que o modelo!

Cenário 4: Ruído de sensor super-baixo

P(0) = 40

R = 1

Esquece o modelo pois

a melhor estimativa é a

medida do sensor

BIBLIOGRAFIA