1

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 1940 1960
Filtro de Wiener
(contnuo)
Filtro de Kolmogorov
(discreto)
Filtro de Kalman
(linear e estendido)
Sinal no tempo
Estatstica
(varivel
aleatria)
Processos Estocsticos
Probabilidade
O que caracteriza um sinal?
Mdia
Desvio Padro ( volatilidade )
Sinal com alta volatilidade
Sinal com baixa volatilidade
Resolvendo Sistema Linear
Observar o seguinte sistema linear:

=
= +
1 2
4 2 5
2 1
2 1
x x
x x
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.
Primeira Resoluo

=
= +
1 2
4 2 5
2 1
2 1
x x
x x
5
2 4
2
1
x
x

=
(a) Isolando x1 na primeira equao:
(b) Substituindo na segunda equao:
3
1
3 9
5 5 4 8
1
5
2 4
2
2
2
2 2
2
2
=

= |
.
|

\
|
x
x
x x
x
x
(c) Substituindo x2 em x1 isolado em (a):
3
2
1
= x
Soluo:
3
2
1
= x
3
1
2
= x
Segunda Resoluo

=
= +
1 2
4 2 5
2 1
2 1
x x
x x
(a) Transformar o sistema em representao matricial:
|
|
.
|

\
|
=
|
|
.
|

\
|
|
|
.
|

\
|
1
4
1 2
2 5
2
1
x
x
A B
(b) Resolve-se agora o sistema : AX = B
(c) A soluo deve envolver a inversa da matriz A, ou seja, A
-1
B A AX A
1 1
=
(d) Como A
-1
.A matriz identidade a soluo ser: X = A
-1
B
(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.

= +
= +
= +
3
2 2 3
1 2
2 1
2 1
2 1
x x
x x
x x
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.
X
Y
reta de regresso linear
b=tangente(angulo)
a
angulo
Exemplo:
Reta que mais se
aproxima dos
pontos amostrados
Os resduos dessa
diferena so os
menores possveis
quando elevados ao
quadrado.
A Estimativa do Mtodo dos Mnimos Quadrados (MMQ)
( ) ( ) ( ) B A A A x A A A A
B A x A A
B x A
Seja
T T T T
T T
. . .
1 1
=

=
,
,
,
identidade
Logo, para encontrar o vetor x mais prximo possvel da soluo:
( ) B A A A x
T T
1
=
,
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:
2 1
medida k medida =
Como estimar k?
Soluo via MMQ
36 28 , 0
22 18 , 0
13 10 , 0
=
=
=
k
k
k
Sistema com 3 equaes e 1 incgnita
Neste caso as matrizes A e B sero:
|
|
|
.
|

\
|
=
28 , 0
18 , 0
10 , 0
A
|
|
|
.
|

\
|
=
36
22
13
B
Ento, lembrando que o x nesse problema o valor de k e:
( ) B A A A x
T T
1
=
,
Qual a soluo?
Soluo
( ) 1208 , 0
28 , 0
18 , 0
1 , 0
28 , 0 18 , 0 1 , 0 =
|
|
|
.
|

\
|
= A A
T
(a)
( ) 34 , 15
28 , 0
18 , 0
1 , 0
28 , 0 18 , 0 1 , 0 =
|
|
|
.
|

\
|
= B A
T
(b)
Assim,
127 34 , 15
1208 , 0
1
~ = k
A Soluo Numrica
( ) B A A A x
T T
1
=
,
Estimativa de parmetros - Ajuste de Funo
Dada uma tabela de dados
(

3 2 1
3 2 1
:
:
y y y Y
x x x X
Deseja-se encontrar a melhor funo linear que ajuste y aos valores de x:
x c c y
1 0
+ =
Observando que os dados so inseridos na funo da seguinte forma:
) ( ) (
1 0
i x c c i y + =
O sistema a ser resolvido :
|
|
|
|
|
.
|

\
|
=
|
|
.
|

\
|
|
|
|
|
|
.
|

\
|
n n
y
y
y
c
c
x
x
x
. . .
2
1
1
0 2
1
.
1
1
1
A X B
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 0 3 6
Y 1 4 5
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
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
Estimao Recursiva
Considere o problema da estimativa de uma constante escalar no
Aleatria baseada em k medidas anteriores, corrompidas por rudo:
z x v
i i
= + 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!
`
x
z
k
k
i
i
k
=
=

1
Para uma medida adicional (k+1) a nova estimativa ser
`
x
z
k
k
i
i
k
+
=
+
=
+

1
1
1
1
Deve-se manipular os termos para deixar a estimativa sempre em funo
das medidas anteriores:
|
.
|

\
|
+
+
=
+
=
+ 1
1
1
1
1

k
k
i
i k
z z
k
x
|
.
|

\
|
+
+
=
+
=
1
1
1
1

k
k
i
i
z z
k
k
k
=
+
+
+
+
k
k
x
z
k
k
k
1 1
1
`
mdia
|
.
|

\
|
+
+
=
+
=
1
1
1
1

k
k
i
i
z z
k
k
k
Somando-se e subtraindo
`
x
k
k
+1
` ` ` `
x
k
k
x
k
x
k
z
k
x
k k k k k + +
=
+
+
+
+
+

+
1 1
1
1
1
1
1
1
1
( )
k K k
x z
k
x

1
1

1

+
+ =
+
Estimao recursiva de variveis

( )
` `
.
`
x x
k
z x
k k k k + +
= +
+

1 1
1
1
Ganho do
Sistema
informao ainda
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
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
NORMALIZAO
Por que usar a distribuio gaussina?
Segundo o teorema do limite central, todas convergem para a normal
para um nmero 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 varivel
2
2
) (
2
) (
2
2
to
o
x x
e
x f

=
Mdia:
Varincia:
2
o
o
o +
Para duas variveis Distribuio Espacial
| |
2
2
) (
) (
) ( ) ( ) (
2
) , (
1 2
xy
y y
x x
y y x x
xy
e
y x f
to
o
(

=
Mdia de x:
Mdia de y
Desvio Padro de x:
Desvio padro de y:
Covarincia de x e y:
Correlao de x e y:
x
y
2
x
o
2
y
o
2
xy
o

Mas a Covarincia uma matriz ( P )

|
|
.
|

\
|
= =
2
2
2
y y x
y x x
xy
P
o o o
o o o
o
A funo gaussina corrigida para duas variveis
| |
2 / 1
2
) (
) (
) ( ) (
2
) , (
1
P
e
y x f
y y
x x
P y y x x
t
(

=
onde | P | o determinante da matriz de covarincia
necessrio uma correo na frmula pois,
Problema
E se as variveis forem vetores de dados com sinais do tipo:
(
(
(
(

=
n
x
x
x
x
.
,
2
1
A covarincia seria:
|
|
|
|
|
.
|

\
|
=
2
2
2 2 1 12
1 1 2 1 12
2
1
n
n n
P
o
o o o
o o o o o
.
.
Impraticvel! Todos os sinais teriam que serem armazenados
em gigantescos bancos de dados para o calculo no fim.
Soluo: Filtro de Kalman
tempo
x

( )
n
x x x f , , ,
2 1

x
1
x
n
(
(
(
(

=
n
x
x
x
x
.
,
2
1
Como o filtro de Kalman estima variveis?
sinal
Algoritmo Esquemtico
Trabalha com dados
Equao de Propagao
Equao de Atualizao
Equao de Covarincia
tempo
Equao de
Propagao

+1

k
x
k
k+1
Equao de
Atualizao
+
+1

k
x
Bia
Satlite
dados
Equao de
Propagao

+2

k
x
k+2
Equao de
Atualizao
+
+2

k
x
Equao de
Propagao

+3

k
x
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
.
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 v
k
do sensor.
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.
k k k
C Ax x e + =
+1
k k k
Hx z u + =
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).
2. A medida do sensor supostamente corrompida por rudo
branco gaussiano com mdia zero e covarincia R.
|
|
|
|
|
.
|

\
|
=
2
2
2
2
1
0 0 0
0
0 0 0
0 0
nx
x
x
Q
o
o
o
.
.
|
|
|
|
|
.
|

\
|
=
2
2
2
2
1
0 0 0
0
0 0 0
0 0
nz
z
z
R
o
o
o
.
.
Propagaes
Propagao do Estado ( priori)
Qual a melhor estimativa? A mdia! As barras representam a
mdia.
k k
k k k
x A x
C Ax x

estado do branco rido o

1
1
=

+ =
+
+
e
e
Propagao da covarincia do estado ( priori)
( )( )
( ) | | ( ) | |
T T
k k
T
k k k k k k k
T
k k k k k
CQC A AP P
C x x A C x x A P
x x x x P
+ =

+ + =
=
+
+
+ + + + +
1
1
1 1 1 1 1

e e
Atualizaes
Atualizao do estado
- A melhor estimativa de x
k
quando se tem medida z
k
aquela que
minimiza o termo do expoente da distribuio gaussiana.
| |
2 / 1
2
) (
) (
) ( ) (
2
) , (
1
P
e
y x f
y y
x x
P y y x x
t
(

=
Isso desejado!
Colocando a medida na distribuio gaussiana:
Deseja-se minimizar
Ou,
( )
priori
k k
T
priori
priori
k
posteriori
k
x H z
R
H P
x x

+ =
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
Atualizao da covarincia
A melhor covarincia P quando se tem medida z
k
aquela que
zera o erro entre medida e varivel:
A melhor estimativa conseguida substituindo a relao de atualizao
do estado na equao acima. A relao linear que surge :
R H HP
H P
P P
T priori
T priori
priori posteriori
+
=
A matriz R a inversa da matriz de covarincia do erro
do sensor de medida.
Cuja medida estatstica est na matriz de covarincia (a barra significa mdia):
posteriori
k
x
`
.
`
x A x
k k

+
=
1
P A P A C Q C
k k
T T

+
= + . . . .
1
( ) K P H H P H R
k k
T
k
T
= +


. . . .
1
P P K H P
k k k k
+
= . .
| |
` `
. .
`
x x K z H x
k k k k k
+
= +
Equao de Propagao:
propagao do estado
propagao da covarincia do estado
ganho do filtro
atualizao da matriz de covarincia do estado
atualizao do estado
Equao de Atualizao:
O Filtro de Kalman
Priori = ( - ) Posteriori = ( + )
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?
Modelo Medida
( )
1

+ = R H HP H P K
T
k
T
k k
T T
k k
CQC A AP P + =
+

1
+
~ + + +
k k k k
x x K P Q

( )
+
+ =
k k k k k
x H y K x x


+
+ =
k k k k k
x H y AK x A x

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
x
1
0 1 2 3 4 5 6 7 8 9 10
-10
0
10
20
x
2
Resultado
Sinal real
Sinal estimado
Errado!
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:
| + K R
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:
+ + K Q
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
P(0) ideal
demora para aprender
aprende rpido
Caracterstica ideal da covarincia do estado P
k
tempo
P(0) alto
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)
observado
Modelos
Modelo Utilizado
k k k
C Ax x e + =
+1
1. Fez se uma hiptese que A = 1.
2. Fez-se a suposio que C =1.
3. O valor de H =1 pois s tem um sensor de temperatura.
4. A perturbao aleatria no estado (temperatura) foi Q=
0,5
o
C.
5. O rudo no sensor foi suposto de 10
o
C, ou varincia
R=100.
6. A matriz de covarincia inicial da perturbao do estado
P(0)=40.
7. Finalmente, foi dito ao filtro que a temperatura inicial foi
x(0)=0
o
C, para obrigar a ter uma alta impreciso e fazer
um aprendizado rpido.
8. O tempo final de simulao para aprendizado e filtragem
foi de t = 100.
Filtro
+

=
1

k k
x x
5 , 0
1
+ =
+

k k
P P
100 +
=

k
k
k
P
P
K
( )
+
=
k k k
P K P 1
| |
+
+ =
k k k k k
x z K x x .
Equao de Atualizao:
Equao de Propagao:
x(0)=0
P(0)=40
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)
O filtro de Kalman aprendeu com 20 dados!!
P(k)
Cenrio 2: Sensor com mais rudo Sinal mais
filtrado
erro = 22,36
o
C
R=erro
2
= 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