Filtro de Kalman Iniciantes

Prévia do material em texto

1
Filtro de Kalman
Teoria e Aplicação para Iniciantes
Prof. Dr. Marco Antonio Leonel Caetano
M&V Consultoria e Treinamento
www.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 2
1
x
x


(a) Isolando x1 na primeira equação:
(b) Substituindo na segunda equação:
3
1
39
5548
1
5
24
2
2
2
22
2
2











 
x
x
xx
x
x
(c) Substituindo x2 em x1 isolado em (a):
3
2
1 x
Solução:
3
2
1 x
3
1
2 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
anguloExemplo:
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
k Sistema 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,15
1208,0
1
k
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í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
0 1 2 3 4 5 6
1
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
i
i
k



1
Para uma medida adicional (k+1) a nova estimativa será
x
z
kk
i
i
k






1
1
1
1
Deve-se manipular os termos para deixar a estimativa sempre em função
das medidas anteriores:








 

  1
1
1
1
1
ˆ
k
k
i
ik zz
k
x








 

 1
11
1
 k
k
i
i zz
k
k
k




k
k
x
z
kk
k
1 1
1
média








 

 1
11
1
 k
k
i
i zz
k
k
k
Somando-se e subtraindo 
x
k
k
 1
   x
k
k
x
k
x
k
z
k
xk k k k k  





1 11
1
1
1
1
1
1
 kKk xz
k
x ˆ
1
1
 ˆ 1 

 
Estimação recursiva de variáveis

   . x x
k
z xk k k k   
1 1
1
1
Ganho do
Sistema
informação ainda
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
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
e
xf



•Média: 
•Variância: 2


Para duas variáveis – Distribuição Espacial
 
2
2
)(
)(
)()()(
2
),(
12
xy
yy
xx
yyxx xy
e
yxf

 











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 paraduas variáveis
 
2/1
2
)(
)(
 )()(
2
),(
1
P
e
yxf
yy
xx
Pyyxx













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
e
yxf
yy
xx
Pyyxx













Isso é desejado!
Colocando a medida na distribuição gaussiana:
Deseja-se minimizar
Ou,
 priorikk
Tpriori
priori
k
posteriori
k xHz
R
HP
xx ˆˆˆ 
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
•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
HP
PP
Tpriori
Tpriori
prioriposteriori


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 Kalman
Priori = ( - ) 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 TkTkk
TT
kk 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 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
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
k
k
P
P
K
    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 pontos
medida
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