Filtro de Kalman

Teoria e Aplicação para Iniciantes

Prof. Dr. Marco Antonio Leonel Caetano

M&V Consultoria e Treinamento

www.mudancasabruptas.com.br

1
 A História da Filtragem de Sinais

1930 1940 1960

Filtro de Wiener Filtro de Kolmogorov Filtro de Kalman
(contínuo) (discreto) (linear e estendido)
Processos Estocásticos
Sinal no tempo

Estatística
(variável
aleatória)
 O que caracteriza um sinal?
•Média
•Desvio Padrão ( 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 equação e substituir na segunda.

(2) Transformar o sistema em matrizes e vetores.
Primeira Resolução 5 x1  2 x2  4

 2 x1  x2  1
(a) Isolando x1 na primeira equação: 4  2 x2
x1 
5
(b) Substituindo na segunda equação:
 4  2 x2 
2   x2  1
 5 

8  4 x2  5 x2  5

 9 x 2  3

1
x2 
3
2
(c) Substituindo x2 em x1 isolado em (a): x1 
3
2 1
Solução: x1 
3
x2 
3
Segunda Resolução 5 x1  2 x2  4

 2 x1  x2  1
(a) Transformar o sistema em representação matricial:

 5 2  x1   4 
     
 2  1  x2   1 

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

A1 AX  A1B
(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.

 2 x1  x2  1

3x1  2 x2  2 3 equações
 x x 3
 1 2 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.

Exemplo:
reta de regressão linear Reta que mais se
aproxima dos
pontos amostrados
Y

angulo
b=tangente(angulo) Os resíduos dessa
diferença são os
menores possíveis
a
quando elevados ao
X
quadrado.
A Estimativa do Método dos Mínimos Quadrados (MMQ)

Seja

Ax  B


AT Ax  AT B

A A .A A.x  A A
T 1 T T 1
. AT B
identidade

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



1 T
x A A A B
T

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:

medida1  k  medida2
Como estimar k?
Solução via MMQ
0,10k  13
Sistema com 3 equações e 1 incógnita
0,18k  22
0,28k  36

Neste caso as matrizes A e B serão:

 0,10   13 
   
A   0,18  B   22 
 0,28   36 
   
Então, lembrando que o x nesse problema é o valor de k e:



1 T
x A A A B
T

Qual a solução?
Solução
 0,1 
 
(a) AT A  0,1 0,18 0,28 0,18   0,1208
 0,28 
 

 0,1 
 
(b) AT B  0,1 0,18 0,28 0,18   15,34
 0,28 
 

Assim,
1
k 15,34  127
0,1208
A Solução Numérica


 
1 T
x A A A B
T
Estimativa de parâmetros - Ajuste de Função

Dada uma tabela de dados  X : x1 x2 x3 

Y : y y3 
 1 y2

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

y  c0  c1 x
Observando que os dados são inseridos na função 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  n
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ício
Modificar 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
5.5

4.5
pontos da tabela
4

3.5

3
RETA AJUSTADA
2.5

1.5

1
0 1 2 3 4 5 6
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:

zi  x  vi 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!

z
i 1
i
x k 
k

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

k 1

z
i 1
i
x k 1 
k 1
Deve-se manipular os termos para deixar a estimativa sempre em função
das medidas anteriores:

1  k 
xˆ k 1    z i  z k 1 
k  1  i 1 

1 k k 
   z i  z k 1 
k  1  k i 1 
média
1 k 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
k 1

k 1 1 1
x k 1  x k  x k  z k 1  x k
k 1 k 1 k 1 k 1

 xˆk 
1
zK 1  xˆk 
k 1
Estimação recursiva de variáveis

. z k 1  x k 
1
x k 1  x k 
k  1 

Ganho do informação ainda
Sistema nã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
 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.
580
22

20
540
18

16
500
14

12
460
10

8
420
6

4
380 2
0.3 0.5 0.7 0.9 1.1 1.3
0

Teorema do Limite Central NORMALIZAÇÃO

22

20

18

16

14

12

10

0
Para uma única variável
•Média: ( x x )2

•Variância: 2
e 2 2
f ( x) 
2 2

4
3
2 
1
0
-1 1 3 5 7 9 11 13 15 17 19
-2
-3

-4
Para duas variáveis – Distribuição Espacial
2 1  ( x  x ) 
( x  x ) ( y  y ) ( xy )  
 ( y  y ) 
2
e
f ( x, y ) 
2 xy2

Média de x: x
Média de y y
Desvio Padrão de x:  x
2

Desvio padrão de y: y

2

Covariância de x e y: xy
2

Correlação de x e y: 
Mas a Covariância é uma matriz ( P )
•É necessário uma correção na fórmula pois,
  x2  x y 
P  2
 
xy     y 
2
 x y

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

 ( x x ) 
( x  x ) ( y  y )  P 1  
 ( y  y ) 
2
e
f ( x, y ) 
2 P
1/ 2

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

Problema
• E se as variáveis forem vetores de dados com sinais do tipo:
 x1 
 
  x2 
x

 
 xn 
•A covariância seria:
  12 12 1 2  1n 1 n 
 
 12 1 2 22

P 
   
  n2 
 
Impraticável! Todos os sinais teriam que serem armazenados
em gigantescos bancos de dados para o calculo no fim.
Solução: Filtro de Kalman
Como o filtro de Kalman estima variáveis?

f x1 , x2 ,, xn 

x

x1

 x1 
 
  x2 
x
 xn sinal
 
 xn 
tempo
 Algoritmo Esquemático

Satélite
•Trabalha com dados
dados
•Equação de Propagação
•Equação de Atualização
Bóia
•Equação de Covariância

Equação de 
xˆ Equação de 
xˆ
Atualização k 1 Atualização k 2

k k+1 k+2 tempo

Equação de  Equação de 
xˆ xˆ Equação de 
xˆ
Propagação k 1 Propagação k 2 Propagação k 3
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 w k.
xk 1  Axk  Ck
•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.
zk  Hxk  k
•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.
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).
  12x 0  0 
 
 0  22x 0 0 
Q 
  0  
 0 2 
0  nx 
 0

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

branco gaussiano com média zero e covariância R.
  12z 0  0 
 
 0  2
0 0 
R 2z

  0  
 0 2 
0  nz 
 0
Propagações

•Propagação do Estado (à priori)

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

média.
xˆk 1  Axk  Ck
 é o rúido branco do estado

xˆk 1  Axˆk
•Propagação da covariância do estado (à priori)

Pk 1  xk 1  xk 1 xk 1  xk 1 
T

Pk 1  Axk  xk   Ck Axk  xk   Ck 

T

 
Pk 1  APk AT  CQC T
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.

 ( x x ) 
( x  x ) ( y  y )  P 1  
 ( y  y ) 
2
e Isso é desejado!
f ( x, y ) 
2 P
1/ 2

Colocando a medida na distribuição gaussiana:

Deseja-se minimizar

Ou,
 Isso leva à seguinte relação

Isolando o estado x para sua estimativa

Usando a mesma idéia do mínimos quadrados recursivos para novas medidas

chega-se a seguinte equação de atualização

 
priori T
P H
xˆk  xˆk  zk  Hxk
ˆ
posteriori priori priori

R
 •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:

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

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

posteriori
x̂k do estado na equação acima. A relação linear que surge é:
priori T
P H
P posteriori
P priori

HP prioriH T  R
A matriz R é a inversa da matriz de covariância do erro
do sensor de medida.
 O Filtro de Kalman
Priori = ( - ) Posteriori = ( + )

Equação de Propagação:

x k  A. x k 1 propagação do estado

Pk  A. Pk1. AT  C. Q.CT propagação da covariância do estado

Equação de Atualização:

Kk  Pk . H T . H. Pk . H T  R
1
ganho do filtro

Pk  Pk  Kk . H. Pk atualização da matriz de covariância do estado


x k  x k  Kk . z k  H. x k  atualização do estado
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?


xˆ k  1  Axˆ k  AKk yk  Hxˆ k 
Modelo Medida

Pk  APk1 A T  CQCT


 T
Kk  Pk H HPk H  R  T

1

 
xˆ k  xˆ k  Kk yk  Hxˆ k

Q   Pk   Kk   xˆ k  xˆ k
 Resultado

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

2
x1

-2
Sinal estimado 0 1 2 3 4 5 6 7 8 9 10
Errado! 20

10
x2

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

Sinal real
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:

R  K   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:

Q  K   O filtro só usa o modelo e filtra tudo.

Abandona as Medidas, pois K0 !
 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

demora para aprender

P(0) ideal
aprende rápido
Característica ideal da covariância do estado Pk

P(0) alto

tempo
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)

Modelos

observado
Modelo Utilizado
xk 1  Axk  Ck

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
Equação de Propagação:

xˆk  xˆk1 x(0)=0

Pk  Pk1  0,5 P(0)=40

Equação de Atualização:

Pk
Kk  
Pk  100
Pk  1  K k Pk


xˆk  xˆk  K k . zk  xˆk 
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
 medida
100 pontos estimado
A Covariância do estado (temperatura)

P(k)

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

 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