Trabalho 6 - Filtro de Kalman

Esta é uma pré-visualização de arquivo. Entre para ver o arquivo original

6 - Filtro de Kalman/Filtro Kalman I.pdf
Engenharia de Controle e Automac¸a˜o - PUCRS
O Filtro de Kalman - I
O Filtro de Kalman e´ amplamente utilizado no processo de filtragem de
sensores. Como veremos, ele pode ser descrito na representac¸a˜o por espac¸o de
estados e utilizado como um observador de estados. Neste primeiro contato
com o Filtro de Kalman iremos falar de varia´veis estoca´sticas e da regra de
Bayes.
1 Varia´veis estoca´sticas
Sinais estoca´sticos sa˜o aqueles que possuem um ou mais componentes aleato´rios.
Em princ´ıpio, na˜o podemos determinar o valor de um sinal aleato´rio, por
isto recorremos a` uma ana´lise estat´ıstica para descreveˆ-los. Uma das repre-
sentac¸o˜es mais comuns para a descric¸a˜o de varia´veis estoca´sticas e´ atrave´s
da distribuic¸a˜o Gaussiana, que faz uso de apenas dois paraˆmetros: a me´dia
µ e a variaˆncia σ2 do sinal. Em sua forma geral uma Gaussiana e´ descrita
por,
p(x) =
1
2piσ2
· e−
(x−µ)2
2σ2 ,
e um sinal pertencente a` distribuic¸a˜o Gaussiana (ou normal) e´ representado
por X ∼ N(µ, σ2).
E´ importante entender que esta distribuic¸a˜o determina qual a probabili-
dade de uma observac¸a˜o cair entre dois valores. Por isto ela e´ utilizada para
111
0.50.50.5
0
0
0
0
0
0 -4-4-4 -2-2-2 222 444
f1 f2 f3
Figura 1: Gaussianas calculadas com me´dia µ = 0 mas com variaˆncias σ21 =
0.5, σ22 = 0.2 e σ
2
3 = 1. Qual variaˆncia representa qual sinal?
Prof. Aure´lio Salton To´picos Especiais II - ECA
Engenharia de Controle e Automac¸a˜o - PUCRS
determinar varia´veis provenientes de sensores. Por exemplo, recorde das de-
finic¸o˜es ba´sicas de probabilidade e estat´ıstica, e suponha que os gra´ficos da
Fig. 1 representam Gaussianas de treˆs sensores distintos. Qual deles repre-
senta o melhor sensor?
Naturalmente o melhor sensor e´ aquele que possui menor variaˆncia, isto
porque a variaˆncia e´ uma medida de incerteza do sinal sendo medido. A Fig.
2 demonstra a evoluc¸a˜o temporal de sinais de variaˆncias diferentes.
0.1
0.1
-0.1
-0.1
0
0
0
0
20
20
40
40
60
60
80
80
100
100
Z
Q
amostras [k]
Figura 2: Dois sinais de mesma me´dia mas variaˆncias σ2Q = 0.01 e σ
2
Z = 0.05.
Uma vez que sinais estoca´sticos sa˜o determinados por sua me´dia µ e
variaˆncia σ2, um estimador destas varia´veis tambe´m deve determinar um va-
lor me´dio e uma variaˆncia relacionada a` estimac¸a˜o. Neste caso a variaˆncia
tambe´m e´ uma medida de incerteza: quanto maior for a variaˆncia da es-
timac¸a˜o, maior a incerteza sobre a varia´vel sendo estimada.
2 Regra de Bayes
Para entender a regra de Bayes iremos recorrer a` um exemplo. Suponha que
estamos interessados em determinar a localizac¸a˜o de um ve´ıculo autoˆnomo
em apenas uma dimensa˜o (como sua distaˆncia ate´ uma parede). Em um
determinado instante t = 0 sabemos com alto grau de confianc¸a que o ve´ıculo
se encontra na posic¸a˜o x = 2 m. Portanto, representamos sua posic¸a˜o por
uma Gaussiana me´dia µx = 2 e pequena variaˆncia σ
2
x = 0.1. A probabilidade
p(x) ∼ N(2, 0.1) esta´ demonstrada na Fig 3(a).
Prof. Aure´lio Salton To´picos Especiais II - ECA
Engenharia de Controle e Automac¸a˜o - PUCRS
Suponha ainda que cada movimento realizado pelo ve´ıculo gere uma in-
certeza na sua posic¸a˜o, ou seja, damos o comando para o ve´ıculo se mover
δ metros, mas ele na˜o se move exatamente a distaˆncia desejada. Portanto
dizemos que o ve´ıculo se move com uma incerteza σ2δ = 0.1 ·δ, ou seja, quanto
maior o movimento do ve´ıculo, maior a incerteza gerada em sua posic¸a˜o. Por
fim, este sistema possui um sensor capaz de determinar sua posic¸a˜o com uma
variaˆncia σ2y = 0.2.
Predic¸a˜o
No primeiro passo, damos o comando para o nosso ve´ıculo andar quatro
metros para a direita. Sua nova Gaussiana sera´ representada por
p¯(x) ∼ N(µ¯x, σ¯
2
x)
sendo que µ¯x = 2 + 4 = 6 (dois metros de condic¸a˜o inicial mais quatro
devido ao comando dado ao ve´ıculo), e σ¯2x = σ
2
x + σ¯
2
δ , ou seja, a simples
soma da incerteza inicial σ2x = 0.1 com a incerteza σδ = 0.4, associada a` sua
movimentac¸a˜o. Neste passo foi executada uma predic¸a˜o da posic¸a˜o futura do
sistema, e, como demonstrado na Fig. 4, toda a predic¸a˜o adiciona incertezas
a` estimac¸a˜o. Achamos que o ve´ıculo se encontra perto da posic¸a˜o x = 6 m,
mas na˜o temos plena certeza disto. A func¸a˜o de densidade de probabilidade
resultante p¯(x) ∼ N(6, 0.5) e´ chamada a priori por representar a predic¸a˜o
do sinal antes da realizac¸a˜o de uma medic¸a˜o. Ela esta´ representada na Fig.
3(b).
 
 
p(x) ∼ N(2, 0.1)
1
0.5
0
0 2 4 6 8
(a)
 
 
p(x) ∼ N(2, 0.1)
p¯(x) ∼ N(6, 0.5)1
0.5
0
0 2 4 6 8
(b)
Figura 3: Func¸o˜es de densidade de probabilidade a priori. A incerteza asso-
ciada a` varia´vel x aumenta devido ao deslocamento do ve´ıculo.
Prof. Aure´lio Salton To´picos Especiais II - ECA
Engenharia de Controle e Automac¸a˜o - PUCRS
Correc¸a˜o
Neste passo calcula-se a func¸a˜o de probabilidade a posteriori do ve´ıculo,
ou seja, a probabilidade da localizac¸a˜o do mesmo apo´s uma medic¸a˜o ser
realizada. Para isto, suponha que seja feita uma medic¸a˜o e que retorne o
sinal escuro da Fig. 4(a). Agora possu´ımos duas informac¸o˜es sobre a posic¸a˜o
do ve´ıculo. A primeira, representada pela curva cinza na Fig. 4(a), e´ a
probabilidade a priori de sua posic¸a˜o, esta e´ a mesma curva da figura Fig.
3(b). A segunda informac¸a˜o que possu´ımos, curva preta da Fig. 4(a), e´ a
probabilidade de localizac¸a˜o do ve´ıculo dada pelo sensor.
 
 
p¯(x) ∼ N(6, 0.5)
p(y) ∼ N(5, 0.2)1
0.5
0
0 2 4 6 8
(a)
 
 
p(x) ∼ N(5.25, 0.15)
1
0.5
0
0 2 4 6 8
(b)
Figura 4: Func¸a˜o de densidade de probabilidade a posteriori representada na
curva preta do gra´fico (b). A incerteza associada a` varia´vel x diminui apo´s
a realizac¸a˜o de uma medic¸a˜o.
Em outras palavras, a Fig. 4(a) representa a probabilidade p¯(x) referente
a` posic¸a˜o da varia´vel x antes da medic¸a˜o ser realizada (em cinza), e a pro-
babilidade p(y) ∼ N(µy, σ
2
y) da medida proveniente do sensor (em preto).
A resposta que procuramos e´ a func¸a˜o de probabilidade p(x|y), ou seja, a
posic¸a˜o de x dada a medic¸a˜o y. A regra de Bayes surge exatamente para
resolver este problema, pois nos permite agregar as informac¸o˜es de p(x) e
p(y) para encontrar p(x|y). Em sua forma original ela e´ dada por,
p(x|y) =
p(y|x)p(x)
p(y)
.
Resultando em uma nova probabilidade de me´dia e variaˆncia dadas por,
µx =
µ¯xσ
2
y + µyσ¯
2
x
σ2y + σ¯
2
x
, σ2x =
1
1
σ¯2
x
+ 1
σ2
y
.
Ou seja, a probabilidade a posteriori da localizac¸a˜o x do ve´ıculo e´ dada
por p(x) ∼ N(5.25, 0.15).
Prof. Aure´lio Salton To´picos Especiais II - ECA
Engenharia de Controle e Automac¸a˜o - PUCRS
Portanto, o filtro de Bayes funciona em dois passos: predic¸a˜o e correc¸a˜o.
No passo de predic¸a˜o a evoluc¸a˜o da varia´vel estoca´stica e´ dada por,
p¯(x) ∼ N(µx + µδ, σ
2
x + σ
2
δ ),
aumentando a incerteza da estimac¸a˜o. Mas no passo de correc¸a˜o, a nova
estimativa e´ dada por,
p(x) ∼ N
(
µ¯xσ
2
y + µyσ¯
2
x
σ2y + σ¯
2
x
,
1
1
σ¯2
x
+ 1
σ2
y
)
,
reduzindo a incerteza de estimac¸a˜o.
2.1 Exerc´ıcio sobre o Filtro de Bayes
Vamos refazer o exemplo anterior com as seguintes suposic¸o˜es:
• posic¸a˜o inicial e variaˆncia inicial do ve´ıculo: µ0 = 0, σ
2
0 = 10
−2;
• variaˆncia de movimentac¸a˜o σ2δ = 0.5/m;
(1) Suponha que para este ve´ıculo
a seguinte sequeˆncia de movimentos foi
realizada:
µδ(k) = [0.1 0.5 0.2 0.5]
pede-se,
(a) Calcule a posic¸a˜o e variaˆncia final do ve´ıculo;
(b) Fac¸a gra´ficos similares ao da Fig. 3(b) mostrando a func¸a˜o de
densidade de probabilidade para cada instante de tempo.
Dica: supondo que µ =m e σ2 =s, pode-se plotar uma gaussiana no Matlab
da seguinte forma:
m = 0;
s = 1e-2;
x = linspace(m-5,m+5,100);
fx = 1/sqrt(2*pi*s)*exp(-(x-m).^2/(2*s));
plot(x,fx,’LineWidth’,1.5);
Prof. Aure´lio Salton To´picos Especiais II - ECA
Engenharia de Controle e Automac¸a˜o - PUCRS
(2) Para este mesmo ve´ıculo adquiriu-se um sensor que para uma distaˆncia
de um metro retornou a sequeˆncia de medic¸o˜es vista na Fig. 2.1.
1
0.5
1.5
0 20 40 60 80 100
k
y
(k
)
Figura 5: Sequeˆncia de medic¸o˜es para a distaˆncia de um metro.
Baixe os dados deste sensor, dispon´ıveis no Moodle, e calcule sua
variaˆncia σ2y .
(3) Suponha agora que para cada instante de tempo k > 0 uma medic¸a˜o
foi realizada com os seguintes resultados:
y(k) = [0.127 0.512 0.786 1.333]
pede-se,
(a) Calcule a posic¸a˜o e variaˆncia final do ve´ıculo utilizando o sensor
para corrigir as estimac¸o˜es;
(b) Fac¸a gra´ficos similares ao da Fig. 4(b) mostrando a func¸a˜o de den-
sidade de probabilidade a priori e a posteriori para cada instante
de tempo.
Apresente o trabalho ate´ a data indicada em sala de aula.
Prof. Aure´lio Salton To´picos Especiais II - ECA
Engenharia de Controle e Automac¸a˜o - PUCRS
Refereˆncias
[1] Kalman, R. E. “A New Approach to Linear Filtering and Prediction
Problems,” Transaction of the ASME Journal of Basic Engineering,
pp. 35-45 (March 1960).
[2] Thrun, S., Burgard, W. and Fox, D. “Probabilistic Robotics,” The MIT
Press, Cambridge, Massachusetts.
[3] Anderson B.D.O., Moore, J.B. “Optimal Filtering,” Prentice Hall, New
Jersey, 1979.
Prof. Aure´lio Salton To´picos Especiais II - ECA
6 - Filtro de Kalman/Filtro Kalman II.pdf
Engenharia de Controle e Automac¸a˜o - PUCRS
O Filtro de Kalman - II
Neste trabalhos veremos que o Filtro de Kalman nada mais e´ do que
uma aplicac¸a˜o da Regra de Bayes para sistemas dinaˆmicos. Ale´m disto,
como queremos estimar os diversos estados x1, . . . xn dos sistemas em espac¸o
de estados, devemos recorrer a` Gaussianas multivaria´veis, que representam
diversos sinais estoca´sticos com alguma relac¸a˜o entre si. A diferenc¸a ba´sica
esta´ apenas na notac¸a˜o. Considere um vetor de n posic¸o˜es com me´dias
µ1 . . . µn e matriz de covariaˆncia Σ ∈ R
n×n. Sua func¸a˜o de probabilidade
e´ dada por,
f(x) = 2π−n/2|Σ|−1/2 · e−
(x−µ)TΣ−1(x−µ)
2
Note que agora a variaˆncia σ2x foi substitu´ıda por uma matriz Σ, e a me´dia
µx por um vetor de me´dias descrito por µ.
1 O filtro
O filtro faz a fusa˜o de um modelo (a priori) com sensores (a posteriori) do
sistema cujos estados deseja-se estimar. Ele funciona em dois passos distintos:
a “predic¸a˜o”, e a “correc¸a˜o.” Para fazer a predic¸a˜o dos estados o filtro se
baseia no modelo do sistema, que e´ dado por um sistema linear sujeito a
ru´ıdo Gaussiano aditivo,
xk = Akxk−1 +Bkuk + ǫk
zk = Ckxk + δk
(1)
Nas equac¸o˜es acima a varia´vel x ∈ Rn representa os estados e u ∈ Rm a
entrada do sistema, respectivamente. A varia´vel z ∈ Rp representa a medic¸a˜o
dos estados (o sinal proveniente dos sensores), e as matrizes Ak, Bk, Ck des-
crevem o sistema e podem ser variantes no tempo.
O termo δk e´ uma representac¸a˜o do ru´ıdo inerente aos sensores utilizados.
Dizemos que este sinal e´ Gaussiano, possui me´dia zero e covariaˆncia Qk.
O sinal descrito por ǫk tambe´m e´ Gaussiano, com me´dia zero e covariaˆncia
Rk, mas neste caso ele representa uma medida de incerteza do modelo que
descreve o sistema. Se o modelo fosse perfeitamente conhecido ter´ıamos
R = 0, da mesma forma, um sensor sem ru´ıdo algum implicaria em Q = 0.
A Tabela 1 descreve o algoritmo do filtro de Kalman. Note que ele re-
torna uma estimativa dos estados da planta assim como a covariaˆncia desta
estimativa, que nada mais e´ do que uma medida de confianc¸a da estimac¸a˜o
de xk.
Prof. Aure´lio Salton To´picos Especiais II - ECA
Engenharia de Controle e Automac¸a˜o - PUCRS
1: Algoritmo Filtro Kalman( xk−1, Pk−1, uk, zk)
2: x¯k = Akxk−1 +Bkuk
3: P¯k = AkPk−1A
′
k +Rk
4: Kk = P¯kC
′
k(CkP¯kC
′
k +Qk)
−1
5: xk = x¯k +Kk(zk − Cx¯k)
6: Pk = (I −KkCk)P¯k
7: return Pk, xk
Tabela 1: O algoritmo do filtro de Kalman com a predic¸a˜o (linas 2 e 3) e a
correc¸a˜o (linhas 4-6).
Predic¸a˜o: As linhas 2 e 3 do co´digo sa˜o utilizadas para prever o estado
atual do sistema e a sua covariaˆncia antes de realizar uma medic¸a˜o. Note que
elas se baseiam apenas no modelo do sistema. Obviamento quanto melhor
for o modelo, melhor sera´ esta´ predic¸a˜o.
Correc¸a˜o: A correc¸a˜o acontece entre as linhas 4 e 6, onde o chamado
“ganho de Kalman” Kk e´ calculado. E´ atrave´s deste ganho que a medic¸a˜o zk
e´ incorporada na estimac¸a˜o, o que ocorre na linha 5. Note que o erro entre
a predic¸a˜o x¯k e a medic¸a˜o zk e´ corrigido por este ganho. Por fim, uma nova
covariaˆncia Pk e´ calculada e armazenada para o pro´ximo passo do algoritmo.
1.1 Escolha das matrizes Q e R
A matriz Q, que representa a covariaˆncia do sensor, pode ser adquirida expe-
rimentalmente a partir de uma se´rie de medic¸o˜es dos sensores. Por exemplo,
a func¸a˜o cov do Matlab calcula a variaˆncia de um vetor de medic¸o˜es.
A determinac¸a˜o da matriz R e´ um pouco mais complicada uma vez que
ela representa a incerteza do modelo. De forma geral esta matriz e´ deter-
minada experimentalmente: diferentes valores sa˜o testados comparando o
desempenho do filtro. Este processo de tentativa e erro e´ feito ate´ que o
filtro apresente um resultado satisfato´rio. Mas vale lembrar que R e´ uma
medida de incerteza do modelo. Portanto quanto maior for o valor de R,
mais incerto e´ o modelo utilizado.
Prof. Aure´lio Salton To´picos Especiais II - ECA
Engenharia de Controle e Automac¸a˜o - PUCRS
1.2 Modelo vs. Medic¸a˜o
Repare que o ganho Kk calculado na linha 4 da Tabela 1 e´ diretamente
proporcional a` matriz R (via o ca´lculo de P¯k) e inversamente proporcional
a` matriz Q. Portanto quando aumentamos o valor de R (ou diminu´ımos o
valor de Q), aumentamos a magnitude do ganho Kk, dando maior “peso” a`s
medic¸o˜es. Do contra´rio, quando diminu´ımos R (ou aumentamos Q), damos
maior eˆnfase ao modelo, pois o fator de correc¸a˜o Kk(zk−Cx¯k) sera´ pequeno.
Este e´ apenas mais um cobertor curto da engenharia. Para encontrar
os estados de um sistema possu´ımos apenas duas alternativa, confiar ou no
modelo ou nos sensores. O Filtro de Kalman e´ uma maneira esperta de
integrar a informac¸a˜o do modelo com a informac¸a˜o dos sensores.
1.3 Melhor Estimador Linear
O filtro de Kalman possui grande sucesso por uma se´rie de razo˜es. Certa-
mente as principais esta˜o associadas ao fato de que ele e´ de simples imple-
mentac¸a˜o, como visto na Tabela 1, e tambe´m o fato de que ele e´ o melhor
estimador linear. Em outras palavras este filtro e´ um filtro o´timo no sentido
te´cnico da palavra.
Na engenharia dizemos que um filtro ou controlador e´ o´timo quando ele
minimiza alguma quantia. Por exemplo, o Controlador de Tempo O´timo,
do ingleˆs Time Optimal Control (TOC), minimiza o tempo de estabilizac¸a˜o
de um dado sistema. Quanto a` filtragem de sinais, uma vez que estamos
lidando com processos estoca´sticos, o melhor que podemos fazer e´ minimizar
a Covariaˆncia do erro de estimac¸a˜o, que e´ exatamente o que o filtro de Kalman
minimiza.
Em outras palavras, o filtro de Kalman minimiza a covariaˆncia do erro
de estimac¸a˜o, representada por,
Pk = E {[xk − xˆk][xk − xˆk]
′} .
Ao mesmo tempo que garante que a me´dia do erro de estimac¸a˜o seja igual a`
zero [3],
E {[xk − xˆk]} = 0.
Prof. Aure´lio Salton To´picos Especiais II - ECA
Engenharia de Controle e Automac¸a˜o - PUCRS
2 Derivac¸a˜o Matema´tica
Um esboc¸o da derivac¸a˜o matema´tica do filtro de Kalman e´ descrita no que
segue. Este esboc¸o pode ser pulado em um primeiro momento, mas indica ao
leitor os passos necessa´rios para a derivac¸a˜o do algoritmo descrito na Tabela
1.
A derivac¸a˜o deste filtro segue os passos de predic¸a˜o e correc¸a˜o do filtro de
Bayes, vistos na aula passada. Formalmente estes passos sa˜o descritos com
a seguinte notac¸a˜o,
est(xk) =
∫
p(xk|uk, xk−1) est(xk−1)dxk−1
est(xk) = η p(yk|xk)est(xk)
(2)
A primeira linha determina a estimativa a priori dos estados do sistema. E´ a
linha de predic¸a˜o, que aumenta a incerteza da estimativa. Na segunda linha
esta´ descrito o passo de correc¸a˜o, em que a estimativa est(xk) e´ finalizada.
No caso de varia´veis Gaussianas escalares e esta´ticas, as equac¸o˜es em (2)
podem ser facilmente resolvidas, resultando em,
est(xk) ∼ N(µx + µδ, σ
2
x + σ
2
δ )
est(xk) ∼ N
(
µ¯xσ2y+µy σ¯
2
x
σ¯2x+σ
2
y
,
1
1
σ¯2x
+ 1
σ2y
)
.
(3)
Para sistemas dinaˆmicos representados em espac¸o de estados a soluc¸a˜o
destas equac¸o˜es na˜o e´ ta˜o simples, mas resulta no filtro de Kalman. O passo
de predic¸a˜o e´ descrito por,
est(xk) =
∫
p(xk|uk, xk−1)︸ ︷︷ ︸
∼N (Ajxk−1+Bkuk,Rk)
est(xk−1)dxk−1︸ ︷︷ ︸
∼N (µk−1,Pk−1)
,
= ηp
∫
e−Lkdxk−1
(4)
sendo que,
Lk =
1
2
(xk −Axk −Buk)
TR−1k (xk − Axk − Buk)
+1
2
(xk−1 − µk−1)
TP−1(xk−1 − µk−1)
(5)
e ηp e´ uma constante que normaliza a integral. A soluc¸a˜o desta integral na˜o
e´ trivial, mas resulta nas linhas 2 e 3 da Tabela 1.
De forma ana´loga, as linhas 4, 5 e 6 do algoritmo de Kalman referentes a`
correc¸a˜o, sa˜o encontradas resolvendo a expressa˜o:
est(xk) = ηc p(yk|xk)︸ ︷︷ ︸
∼N (yk;Ckxk,Qk)
est(xk)︸ ︷︷ ︸
∼N (xk;µk,P¯k)
,
= ηce
−Jk
(6)
Prof. Aure´lio Salton To´picos Especiais II - ECA
Engenharia de Controle e Automac¸a˜o - PUCRS
para,
Jk =
1
2
(yk − Ckxk)
TQ−1k (yk − Ckxk) +
1
2
(xk − µ¯k)
T P¯−1k (xk − µ¯k) (7)
Os passos completos da derivac¸a˜o do Filtro de Kalman podem ser en-
contrados nas refereˆncias no fim deste documento. A derivac¸a˜o a cima foi
retirada de [2].
3 Definic¸a˜o do Problema
No´s iremos implementar o filtro de Kalman em um sistema de fusa˜o de senso-
res do tipo GPS e IMU. Para tanto, nosso modelo sera´ dado por uma simples
massa pontual se movendo em uma superf´ıcie plana (um mapa). Tambe´m
iremos supor que, ale´m do GPS, podemos medir a acelerac¸a˜o do carro nos
sentidos Norte-Sul e Leste-Oeste.
A posic¸a˜o em coordenadas Leste-Oeste sera´ descrita por x1 e em Norte-
Sul por x3. Assim as equac¸o˜es dinaˆmicas deste sistema sa˜o simplificadas
para,
x˙1 = x2,
x˙2 = ux + ǫ,
x˙3 = x4,
x˙4 = uy + ǫ,
(8)
sendo que ǫ representa o ru´ıdo do sensor de acelerac¸a˜o. As medic¸o˜es prove-
nientes do GPS, i.e., os valores de z1 e z2, podem ser descritas por,
z1 = x1 + δ
z2 = x3 + δ
(9)
com δ representando o ru´ıdo do GPS, que executa duas medic¸o˜es por segundo.
4 Tarefas
A tarefa deste estudo consistem em implementar o filtro de Kalman da Tabela
1. Para tanto sera´ necessa´rio,
• Baixar os arquivos referentes ao trabalho.
• Determinar as matrizes discretas (A,B,C) que descrevem o sistema.
(3 pontos)
• Implementar o Filtro de Kalman de modo a` estimar a correta posic¸a˜o
do ve´ıculo. (7 pontos)
Prof. Aure´lio Salton To´picos Especiais II - ECA
Engenharia de Controle e Automac¸a˜o - PUCRS
• Apresente o trabalho para o professor ate´ a data estipulada em aula.
Boa sorte.
4.1 Tarefa Extra
Um ve´ıculo real na˜o pode ser representado por uma massa pontual com dois
graus de liberdade. Um bom modelo de um ve´ıculo deve determinar ale´m
da sua posic¸a˜o (x, y) a sua orientac¸a˜o θ. Tambe´m deve estar expl´ıcito neste
modelo o fato de que o ve´ıculo pode se locomover apenas na direc¸a˜o em que
esta´ apontando. Um exemplo de modelo mais preciso e´ dado por,
x˙ = v sin(θ),
y˙ = v cos(θ),
θ˙ = ω.
(10)
Onde v e´ a velocidade linear do carro e ω e´ a sua velocidade angular.
Quais seriam os maiores desafios na implementac¸a˜o do filtro de Kalman
no modelo acima?
Refereˆncias
[1] Kalman, R. E. “A New Approach to Linear Filtering and Prediction
Problems,” Transaction of the ASME Journal of Basic Engineering,
pp. 35-45 (March 1960).
[2] Thrun, S., Burgard, W. and Fox, D. “Probabilistic Robotics,” The MIT
Press, Cambridge, Massachusetts.
[3] Anderson B.D.O., Moore, J.B. “Optimal Filtering,” Prentice Hall, New
Jersey, 1979.
Prof. Aure´lio Salton To´picos Especiais II - ECA
6 - Filtro de Kalman/Parte 1/Ivan/Kalman_1.m
clear all
clc
%% Dados do problema
mi0=0;
var0=1e-2;
varDelta=0.5;
delta=[0.1 0.5 0.2 0.5]; 
%% Cálculos
mi1=mi0+delta(1);
var1=var0+delta(1)*varDelta;
mi2=mi1+delta(2);
var2=var1+delta(2)*varDelta;
mi3=mi2+delta(3);
var3=var2+delta(3)*varDelta;
mi4=mi3+delta(4);
var4=var3+delta(4)*varDelta;
%% Plot Gaussiana
x0 = linspace(mi0-5,mi0+5,100);
fx0 = 1/sqrt(2*pi*var0)*exp(-(x0-mi0).^2/(2*var0));
x1 = linspace(mi1-5,mi1+5,100);
fx1 = 1/sqrt(2*pi*var1)*exp(-(x1-mi1).^2/(2*var1));
x2 = linspace(mi2-5,mi2+5,100);
fx2 = 1/sqrt(2*pi*var2)*exp(-(x2-mi2).^2/(2*var2));
x3 = linspace(mi3-5,mi3+5,100);
fx3 = 1/sqrt(2*pi*var3)*exp(-(x3-mi3).^2/(2*var3));
x4 = linspace(mi4-5,mi4+5,100);
fx4 = 1/sqrt(2*pi*var4)*exp(-(x4-mi4).^2/(2*var4));
figure(1)
plot(x0,fx0,'LineWidth',1.5,'color',0.1*[1 1 1]);
hold on
plot(x1,fx1,'LineWidth',1.5,'color',0.3*[1 1 1]);
plot(x2,fx2,'LineWidth',1.5,'color',0.5*[1 1 1]);
plot(x3,fx3,'LineWidth',1.5,'color',0.7*[1 1 1]);
plot(x4,fx4,'LineWidth',1.5,'color',0.9*[1 1 1]);
grid on
%% Cálculos variância sensor
load Q
miSensor=mean(Q)
varSensor=var(Q)
medicoes=[0.127 0.512 0.786 1.333]
%% Cálculos novos
miPri0=0;
varPri0=1e-2;
varDeltaPri=0.5;
deltaPri=[0.1 0.5 0.2 0.5];
miSensor0=miSensor;
varSensor0=varSensor;
miSensor1=medicoes(1);
miSensor2=medicoes(2);
miSensor3=medicoes(3);
miSensor4=medicoes(4);
miPri1=miPri0+deltaPri(1);
varPri1=varPri0+deltaPri(1)*varDeltaPri;
miPost1=(miPri1*varSensor+miSensor1*varPri1)/(varSensor + varPri1);
varPost1=1/((1/varPri1)+(1/varSensor))
miPri2=miPri1+deltaPri(2);
varPri2=varPri1+deltaPri(2)*varDeltaPri;
miPost2=(miPri2*varSensor+miSensor2*varPri2)/(varSensor + varPri2);
varPost2=1/((1/varPri2)+(1/varSensor))
miPri3=miPri2+deltaPri(3);
varPri3=varPri2+deltaPri(3)*varDeltaPri;
miPost3=(miPri3*varSensor+miSensor3*varPri3)/(varSensor + varPri3);
varPost3=1/((1/varPri3)+(1/varSensor))
miPri4=miPri3+deltaPri(4);
varPri4=varPri3+deltaPri(4)*varDeltaPri;
miPost4=(miPri4*varSensor+miSensor4*varPri4)/(varSensor + varPri4);
varPost4=1/((1/varPri4)+(1/varSensor))
%% Plot Gaussianas A Priori e A Posteriori
xPri1 = linspace(miPri1-5,miPri1+5,100);
fxPri1 = 1/sqrt(2*pi*varPri1)*exp(-(xPri1-miPri1).^2/(2*varPri1));
xPost1 = linspace(miPost1-5,miPost1+5,100);
fxPost1 = 1/sqrt(2*pi*varPost1)*exp(-(xPost1-miPost1).^2/(2*varPost1));
xPri2 = linspace(miPri2-5,miPri2+5,100);
fxPri2 = 1/sqrt(2*pi*varPri2)*exp(-(xPri2-miPri2).^2/(2*varPri2));
xPost2 = linspace(miPost2-5,miPost2+5,100);
fxPost2 = 1/sqrt(2*pi*varPost2)*exp(-(xPost2-miPost2).^2/(2*varPost2));
xPri3 = linspace(miPri3-5,miPri3+5,100);
fxPri3 = 1/sqrt(2*pi*varPri3)*exp(-(xPri3-miPri3).^2/(2*varPri3));
xPost3 = linspace(miPost3-5,miPost3+5,100);
fxPost3 = 1/sqrt(2*pi*varPost3)*exp(-(xPost3-miPost3).^2/(2*varPost3));
xPri4 = linspace(miPri4-5,miPri4+5,100);
fxPri4 = 1/sqrt(2*pi*varPri4)*exp(-(xPri4-miPri4).^2/(2*varPri4));
xPost4 = linspace(miPost4-5,miPost4+5,100);
fxPost4 = 1/sqrt(2*pi*varPost4)*exp(-(xPost4-miPost4).^2/(2*varPost4));
figure(2)
%plot(x0,fx0,'LineWidth',1.5,'color',0.1*[1 1 1]);
hold on
plot(1,fxPri1,'LineWidth',1.5,'color',0.3*[1 1 1]);
plot(xPost1,fxPost1,'LineWidth',1.5,'color',[0.1 0.3 0.1]);
plot(xPri2,fxPri2,'LineWidth',1.5,'color',0.5*[1 1 1]);
plot(xPost2,fxPost2,'LineWidth',1.5,'color',[0.1 0.5 0.1]);
plot(xPri3,fxPri3,'LineWidth',1.5,'color',0.7*[1 1 1]);
plot(xPost3,fxPost3,'LineWidth',1.5,'color',[0.1 0.7 0.1]);
plot(xPri4,fxPri4,'LineWidth',1.5,'color',0.9*[1 1 1]);
plot(xPost4,fxPost4,'LineWidth',1.5,'color',[0.1 0.9 0.1]);
grid on
6 - Filtro de Kalman/Parte 1/Ivan/Q.mat
6 - Filtro de Kalman/Parte 1/Kalman.m
clear all
clc
%% Dados do problema
mi=[0];
variancia=[1e-2];
varDelta=0.5;
delta=[0.1 0.5 0.2 0.5]; 
x=[];
fx=[];
% Gausiana
figure(1)
hold on
for m = 1:5
 if(m<5)
 mi(m+1) = mi(m)+delta(m);
 variancia(m+1) = variancia(m)+delta(m)*varDelta;
 end
 
 x(m,:) = linspace(mi(m)-5, mi(m)+5, 100);
 fx(m,:) = 1/sqrt(2*pi*variancia(m))*exp(-(x(m,:)-mi(m)).^2/(2*variancia(m)));
 
 plot(x(m,:),fx(m,:),'LineWidth',1.5,'color',[0 0 0])
 
end
grid on
%% Cálculos variância sensor
load Q
miSensor=[mean(Q)];
varSensor=var(Q);
medicoes=[0.127 0.512 0.786 1.333];
miPri=[0];
varPri=[1e-2];
varDeltaPri=0.5;
deltaPri=[0.1 0.5 0.2 0.5];
miPost = [];
varPost = [];
xPri = [];
fxPri = [];
xPost = [];
fxPost = [];
figure(2)
hold on
for m = 2:5 
 miSensor(m) = medicoes(m-1) 
 
 miPri(m) = miPri(m-1) + deltaPri(m-1);
 varPri(m) = varPri(m-1) + delta(m-1) * varDeltaPri;
 
 miPost(m) = (miPri(m)*varSensor+miSensor(m)*varPri(m))/(varSensor + varPri(m));
 varPost(m) = 1/((1/varPri(m))+(1/varSensor));
 
 xPri(m-1,:) = linspace(miPri(m)-5, miPri(m)+5, 100);
 xPost(m-1,:) = linspace(miPost(m)-5,miPost(m)+5,100);
 
 fxPri(m-1,:) = 1/sqrt(2*pi*varPri(m)) *exp(-(xPri(m-1,:) -miPri(m)).^2/ (2*varPri(m))); 
 fxPost(m-1,:) = 1/sqrt(2*pi*varPost(m))*exp(-(xPost(m-1,:)-miPost(m)).^2/(2*varPost(m)));
 
 plot(xPri(m-1,:), fxPri(m-1,:), 'LineWidth',1.5,'color',[0 0 0]);
 plot(xPost(m-1,:),fxPost(m-1,:),'LineWidth',1.5,'color',[0 0 0]);
end
grid on
6 - Filtro de Kalman/Parte 1/Q.mat
6 - Filtro de Kalman/Parte 2/controle.m
function [u] = controle(x,r)
 kp = .05;
 kd =-.5;
 u = [(r(1)-x(1))*kp + x(2)*kd; (r(2)-x(3))*kp + x(4)*kd];
end
6 - Filtro de Kalman/Parte 2/Fkalman.m
function [x,P] = Fkalman(x,P,A,B,C,Q,R,z,w)
 xk=A*x+B*w;
 Pk=A*P*A'+R;
 
 K=Pk*C'*inv(C*Pk*C'+Q);
 x=xk+K*(z-C*xk);
 P=(eye(4)-K*C)*Pk;
 
end
6 - Filtro de Kalman/Parte 2/GPS.m
clear all
close all
clc
%% parametros de simulação
T = .5;
% tempo total de simulação
N = 50;
figure(1) 
mapa = imread('mapa.png');
imshow(mapa);
hold on
ruidoGPS = 50; % ruido na posição
ruidoAcc = 0.1; % ruido no acelerometro
n = [ 5 5 1 6 5 1 5 330 1000];
r = [330 340 400 405 499 513 514 30 -1000 0%x
 450 410 405 435 447 320 274 275 1000 0]; %y
 
%% sistema
A = [0 1 0 0;
 0 0 0 0;
 0 0 0 1;
 0 0 0 0];
B = [0 0;
 1 0;
 0 0;
 0 1];
C = [1 0 0 0;
 0 0 1 0];
A=A*T+eye(4);
B=B*T;
% Covariancia dos sensores (incerteza dos sensores) Matriz diagonal 2x2
Q = 5000*eye(2); 
% Covariancia do modelo (incerteza do modelo) Matriz diagonal 4x4
R = eye(4)/1000;
% Covariancia inicial da predição (incerteza da C.I.) Matriz diagonal 4x4
P = eye(4)*1000; 
 
%% I.C.
x = [0 0 0 0]'; % condições iniciais do filtro
IC= [300 0 500 0]'; % condições iniciais do sistema
u = [0 0]';
%% Simulação
j=1;k=0;
while j<length(r)
 k=k+1;
 
% carro chegou na referencia? 
 if norm([r(1,j)-IC(1,k) r(2,j)-IC(3,k)],2) < n(j)
 j=j+1;
 end
 
% simulação do sistema 
 [IC(:,k+1)] = XYmass(IC(:,k),T,u(:,k)); 
% medições 
 z(:,k) = [1 0 0 0; 0 0 1 0]*IC(:,k+1) + [ruidoGPS*(rand(1,1)-.5); % medição x
 ruidoGPS*(rand(1,1)-.5)]; % medição y
 w(:,k) = u(:,k) + ruidoAcc*(rand(1,1)-.5); % acelerometro
% filtro 
 [x(:,k+1),P] = Fkalman(x(:,k),P,A,B,C,Q,R,z(:,k),w(:,k)); % Kalman Filter
% controle
 u(:,k+1) = controle(IC(:,k+1),r(:,j)); 
 
 
 if k == 100
 R = R/1000;
 end
 
 plot(IC(1,:),IC(3,:),'k') % actual position
 plot(z(1,k),z(2,k),'r.') % position from sensors
 plot(x(1,k),x(3,k),'*') % estimated position
 drawnow 
 
 
end
%return
%% results
figure(2)
subplot(2,1,1)
plot((1:k)*T,z(:,1:k) , (1:k)*T,IC(1,1:k) , (1:k)*T,IC(3,1:k) , (1:k)*T,x(1,1:k) , (1:k)*T,x(3,1:k))
title('Real, Measured and Estimated Positions')
legend('x Meas','y Meas','x Real','y Real','x Est','y Est')
grid
subplot(2,1,2)
plot((1:k)*T,IC(2,1:k),(1:k)*T,IC(4,1:k),(1:k)*T,x(2,1:k),(1:k)*T,x(4,1:k))
title('Real and Estimated Velocities')
legend('Vx Real','Vy Real','Vx Est','Vy Est')
grid
6 - Filtro de Kalman/Parte 2/mapa.png
6 - Filtro de Kalman/Parte 2/Original/bayes.m
%% filtro bayes
clear all
clc
a=[0.1 0.5 0.2 0.5];
y=[0.127 0.512 0.786 1.333];
q=matfile('Q.mat');
delta=cov(q.Q);
m=abs(a(1));
s=1e-2;
nx=Nx(m,y(1),s,delta);
dx=Dx(s,delta);
x= linspace(nx-5,nx+5,100);
fx=1/sqrt(2*pi*dx)*exp(-(x-nx).^2/(2*dx));
plot(x,fx,0.5,1.5);
hold on
% x= linspace(m-5,m+5,100);
% fx=1/sqrt(2*pi*s)*exp(-(x-m).^2/(2*s));
% plot(x,fx,0.5,1.5);
% hold on
m=abs(a(2)-a(1));
s=s+0.4*0.5;
nx=Nx(m,y(2),s,delta);
dx=Dx(s,delta);
x= linspace(nx-5,nx+5,100);
fx=1/sqrt(2*pi*dx)*exp(-(x-nx).^2/(2*dx));
plot(x,fx,0.5,1.5);
hold on
% x= linspace(m-5,m+5,100);
% fx=1/sqrt(2*pi*s)*exp(-(x-m).^2/(2*s));
% plot(x,fx,'g',0.5,1.5);
m=abs(a(3)-a(2));
s=s+0.3*0.5;
nx=Nx(m,y(3),s,delta);
dx=Dx(s,delta);
x= linspace(nx-5,nx+5,100);
fx=1/sqrt(2*pi*dx)*exp(-(x-nx).^2/(2*dx));
plot(x,fx,0.5,1.5);
% x= linspace(m-5,m+5,100);
% fx=1/sqrt(2*pi*s)*exp(-(x-m).^2/(2*s));
% plot(x,fx,'y',0.5,1.5);
m=abs(a(4)-a(3));
s=s+0.3*0.5;
nx=Nx(m,y(4),s,delta);
dx=Dx(s,delta);
x= linspace(nx-5,nx+5,100);
fx=1/sqrt(2*pi*dx)*exp(-(x-nx).^2/(2*dx));
plot(x,fx,'g',0.5,1.5);
% x= linspace(m-5,m+5,100);
% fx=1/sqrt(2*pi*s)*exp(-(x-m).^2/(2*s));
% plot(x,fx,'r',0.5,1.5);
6 - Filtro de Kalman/Parte 2/Original/controle.m
function [u] = controle(x,r)
 kp = .05;
 kd =-.5;
 u = [(r(1)-x(1))*kp + x(2)*kd; (r(2)-x(3))*kp + x(4)*kd];
end
6 - Filtro de Kalman/Parte 2/Original/Dx.m
function d = Dx( delta0,deltay)
%DX Summary of this function goes here
% Detailed explanation goes here
d=1/(1/delta0+1/deltay);
end
6 - Filtro de Kalman/Parte 2/Original/Fkalman.m
function [x,P] = Fkalman(x,P,A,B,C,Q,R,z,w)
 xk=A*x+B*w;
 Pk=A*P*A'+R;
 
 K=Pk*C'*inv(C*Pk*C'+Q);
 x=xk+K*(z-C*xk);
 P=(eye(4)-K*C)*Pk;
 
end
6 - Filtro de Kalman/Parte 2/Original/GPS.m
clear all
close all
clc
%% parametros de simulação
T = .5;
% tempo total de simulação
N = 50;
figure(1) 
mapa = imread('mapa.png');
imshow(mapa);
hold on
ruidoGPS = 200; % ruido na posição
ruidoAcc = 0.1; % ruido no acelerometro
n = [ 5 5 1 6 5 1 5 330 1000];
r = [330 340 400 405 499 513 514 30 -1000 0%x
 450 410 405 435 447 320 274 275 1000 0]; %y
 
%% sistema
A = [0 1 0 0;
 0 0 0 0;
 0 0 0 1;
 0 0 0 0];
B = [0 0;
 1 0;
 0 0;
 0 1];
C = [1 0 0 0;
 0 0 1 0];
A=A*T+eye(4);
B=B*T;
% Covariancia dos sensores (incerteza dos sensores) Matriz diagonal 2x2
Q = 100*eye(2);
% Covariancia do modelo (incerteza do modelo) Matriz diagonal 4x4
R = eye(4)/100;
% Covariancia inicial da predição (incerteza da C.I.) Matriz diagonal 4x4
P = eye(4); 
 
%% I.C.
x = [0 0 0 0]'; % condições iniciais do filtro
IC= [300 0 500 0]'; % condições iniciais do sistema
u = [0 0]';
%% Simulação
j=1;k=0;
while j<length(r)
 k=k+1;
 
% carro chegou na referencia? 
 if norm([r(1,j)-IC(1,k) r(2,j)-IC(3,k)],2) < n(j)
 j=j+1;
 end
 
% simulação do sistema 
 [IC(:,k+1)] = XYmass(IC(:,k),T,u(:,k)); 
% medições 
 z(:,k) = [1 0 0 0; 0 0 1 0]*IC(:,k+1) + [ruidoGPS*(rand(1,1)-.5); % medição x
 ruidoGPS*(rand(1,1)-.5)]; % medição y
 w(:,k) = u(:,k) + ruidoAcc*(rand(1,1)-.5); % acelerometro
% filtro 
 [x(:,k+1),P] = Fkalman(x(:,k),P,A,B,C,Q,R,z(:,k),w(:,k)); % Kalman Filter
% controle
 u(:,k+1) = controle(IC(:,k+1),r(:,j)); 
 
 
 if k == 100
 R = R/1000;
 end
 
 plot(IC(1,:),IC(3,:),'k') % actual position
 plot(z(1,k),z(2,k),'r.') % position from sensors
 plot(x(1,k),x(3,k),'*') % estimated position
 drawnow 
 
 
end
%return
%% results
figure(2)
subplot(2,1,1)
plot((1:k)*T,z(:,1:k) , (1:k)*T,IC(1,1:k) , (1:k)*T,IC(3,1:k) , (1:k)*T,x(1,1:k) , (1:k)*T,x(3,1:k))
title('Real, Measured and Estimated Positions')
legend('x Meas','y Meas','x Real','y Real','x Est','y Est')
grid
subplot(2,1,2)
plot((1:k)*T,IC(2,1:k),(1:k)*T,IC(4,1:k),(1:k)*T,x(2,1:k),(1:k)*T,x(4,1:k))
title('Real and Estimated Velocities')
legend('Vx Real','Vy Real','Vx Est','Vy Est')
grid
6 - Filtro de Kalman/Parte 2/Original/kalman.m
6 - Filtro de Kalman/Parte 2/Original/mapa.png
6 - Filtro de Kalman/Parte 2/Original/Nx.m
function [ n ] = Nx( n0, ny, delta0, deltay )
%NX Summary of this function goes here
% Detailed explanation goes here
n=(n0*delta0+ny*deltay)/(delta0+deltay);
end
6 - Filtro de Kalman/Parte 2/Original/Q.mat
6 - Filtro de Kalman/Parte 2/Original/XYmass.m
function [dx] = XYmass(x,T,u)
A = [1 T 0 0 
 0 1 0 0 
 0 0 1 T
 0 0 0 1];
 B = [0 0 
 1 0
 0 0
 0 1];
 dx = zeros(4,1);
 dx = A*x+T*B*u;
end
6 - Filtro de Kalman/Parte 2/Q.mat
6 - Filtro de Kalman/Parte 2/XYmass.m
function [dx] = XYmass(x,T,u)
A = [1 T 0 0 
 0 1 0 0 
 0 0 1 T
 0 0 0 1];
 B = [0 0 
 1 0
 0 0
 0 1];
 dx = zeros(4,1);
 dx = A*x+T*B*u;
end