ElltrodeKalman

JosAntonioCamarenaIbarrola

QueesunFiltrodeKalman?
Esunalgoritmorecursivop?modeprocesamientodedatos
Haymuchasformasdedenirp?mo
ElltrodeKalmanesp?morespectoacasicualquiercriterio
quetengasen?do
ElltrodeKalmantomaencuentatodalainformacinquese
tenga.
Procesatodaslasmedicionessinimportarsuprecisinpara
es?marelvalordelasvariablesdeinters
Haceusode:
1. Conocimientodelsistemaydeladinmicadelos
instrumentosdemedicin,
2. Ladescripcinestads?cadeloserroresdemedicinydela
incer?dumbredelosmodelosdinmicos.
3. Cualquierinformacindisponibledelascondicionesiniciales
delasvariablesdeinters

Ejemplo
Paradeterminarlavelocidaddeunaaeronave
podramosdisearunltrodeKalmanque
tomeencuenta:
1. UnradardeefectoDoppler
2. Velocmetrodenavegacininercial
3. Lapresinest?ca
4. Vientorela?vo
5. Loquedigaelpiloto

Filtrorecursivo?
Esrecursivoporquenorequierealmacenar
todoslosdatospreviosyreprocesarloscada
vezquellegaunanuevainformacin
Esunltroenelsen?dodequeesun
programaquetrabajaconmuestrastomadas
en?empodiscretoenlugardeentradasen
?empocon?nuo

UsodelltrodeKalman
Lasvariablesdeintersusadas
paradescribirelestadodel
sistemanosepuedenmedir
directamenteysedebenes?mar
apar?rdelainformacin
disponible

PorqunecesitamosalltrodeKalman?
Lossistemasestnnormalmentecontroladospormas
variablesquelasqueeldiseadordelossistemas
proveencomovariablesdecontrol
Larelacinentrelasvariablesdeestadoylasvariables
quesemidenseconocesoloconciertogradode
incer?dumbre
Lasmedicionessoncorrompidasenciertogradocon
ruido,desviacioneseimprecisionesdedisposi?vos
Lasdiferentesdinmicasdelosdiferentes
instrumentosdemedicinu?lizadossepueden
confundirconladinmicadelsistema
ElltrodeKalmanproduceunaes?macinp?ma
estads?camente
Tomaencuentatodoslasmedicionesmasla
informacinaprioriquesetengadelsistemaydelos
disposi?vosdemedicin

EnfoqueBayesiano
Elltrodebepropagarladensidaddeprobabilidad
condicionaldelascan?dadesdeseadascondicionado
alconocimientodelosdatosqueprovienendelos
disposi?vosdemedicin.
Considereladensidaddeprobabilidadcondicionaldel
valordeunavariablexenuninstantei(x(i)),
condicionadaalconocimientodequeunavariable
medidaenelinstante1(z(1))tomelvalorz1yal
medirlaenelinstante2(z(2))tomelvalorz2,etc.
Porejemplox(i)pudieraserlaposicindeunvehculo
enunasoladimensinenelinstanteiyz(j)pudieraser
laposicindelvehculoendosdimensionesreportada
pordosradaresseparadosenelinstantej

Ladensidadcondicional

Ladensidad
condicionalcon?ene
todalainformacin
disponibleacercade
x(i).
Indicalaprobabilidad
dex(i)deasumir
cualquiervalor
especco,parael
valordadodetodas
lasmediciones
tomadashastael
instantei.
Escondicional
porqueelvalor
reportadodepende
detodaslas
medicionestomadas
Sifueraunpico
tendramoscerteza
delvalordex(i)

Restricciones
UnltrodeKalmanrealizalapropagacindela
densidaddeprobabilidadparaproblemasenlasqueel
sistemapuedeserdescritomedianteunmodelolineal
ylosruidosenlasmedicionessongaussianos
Bajoestasrestricciones,elltrodeKalmanesp?mo
Alhacerlapropagacinpodemosu?lizarlamedia,la
medianaolamoda
Enladistribucingaussianaestas3medidascoinciden
Siseeliminalapresuncindegaussianidad,elltrode
Kalmanesunltroconelmenorerrordelaclasede
ltrosnosesgados
Paramuchasaplicacionesestasrestriccionesse
cumplen

Presuncionesbsicas

Unmodelolineal.Sepuedeajustaralinealcuandohayno
linealidades.
Lossistemaslinealessonmuydeseablespueshaymuchas
herramientasyteoramascompletayprc?ca
Ruidoblancosignicaqueno?enecorrelacinconel?empo(en
nadaayudaconocerel?empoenlaprediccindelvalordelruido)
Alconsiderarqueelruidoesblancosevuelvetratableel
problema,detodosmodosalsistemaledalomismoelruido
blancoqueelruidodeanchodebandaampliaparaelanchode
bandaalquerespondeelsistema

Presuncionesbsicas
Unadistribucingaussianaqueda
determinadasoloconlosprimerosdos
momentosdeladistribucinloscualesson
fcilesdeaveriguarnormalmente
Lastrespresuncionesjuntasconviertenel
problemaentratable

Ejemplo
Supongaqueno?eneideadesuposicinenelmar
Localizaunaestrellaenelcieloparatratardeubicarse
Supongaporsimplicidadqueelsistemaesdeunasoladimensin
Enelinstantet1,determinasquetuposicinesz1
Debidoalasimprecisionesinherenteslaposicinesincierta
Supongamosquedecidesquetuprecisinestalqueladesviacin
estndares z1
Entoncespodemosestablecerquelaprobabilidadcondicional
de,condicionadaalvalorobservadodelamedicines
x(t1 )
z1

Ejemplo(cont)

x(t1 ) = z1
Hastaestemomentolamejores?macindelaposicines
ylavarianzaes x2 (t1 ) = z2
1
Enseguida,unmarinoconmasexperienciatomaotramedicin
enelinstantet2muypocodespusdet1demaneraquela
z2
posicinrealnohacambiadoyob?enez2convarianza
menorpuestoqueesmasexperimentado

Actualizandoladistribucin

Ylamejores?macines:

x(t2 ) =
Observeque

<z

<z

EstructurapredictorcorrectordelltrodeKalman

Puedereescribirsecomo:

donde:

Lamejores?macinenelinstantet2esigualalamejor
prediccinantesdequellegaraz2masuntrminode
correccinformadoporunfactordepesoop?mizadoque
ponderaladiferenciaentrez2ylamejorprediccinque
habaantesdelallegadadez2

EstructurapredictorcorrectordelltrodeKalman
Basadaentodalainformacinpreviahacemosuna
prediccindelvalorquelasvariablesdeseadastendrn
laprximavezquesehagalamedicin
Cuandolasiguientemedicinsehace,ladiferencia
entreestaylaprediccinseusaparacorregirla
prediccindelasvariablesdeseadas
Lavarianzatambinpuedereescribirsecomo:

x
(
t
)

Observequeyencierrantodala
x (t 2 )
2
f x (t2 )| z (t1 ), z (t2 ) ( x | z1 , z2 )
informacinde

Propagandoestasdosvariables,ladensidad
condicionaldetuposicinenelinstantet2dadosz1y
z2quedacompletamenteespecicada

Incorporandodinmica
Supongaqueviajaporalgn?empoantesde
tomarotramedicin
Asumaqueelmodelodelmovimiento?enela
formadx/dt=u+w
dondeueslavelocidadywesruidoquerepresenta
incer?dumbresobrelavelocidadrealdebidaa
disturbiosycondicionesnocontrolables
Asumaquewsemodelamedianteunagaussiana
conmediaceroyvarianza w2

Propagacindeladensidad
condicional
Amedidaqueel?empoavanza,ladensidad
condicionalviajaalolargodelejexalavelocidadu
mientrasqueseensanchaalrededordesumedia
debidoalaconstanteadicindeincer?dumbre

Primerolaprediccin

t
Enelinstante 3 ,justoantesdequesetomelamedicinen t3

ladensidad

f x (t3 )| z (t1 ), z (t2 ) ( x | z1 , z 2 )

esunagaussianaconmediayvarianzadadaspor:

x (t3 ) eslaprediccinp?madelvalorquetendrxen t3

x2 (t3 )

eslaincer?dumbredelaprediccin

Yluegolacorreccin
2
Ahorasetomaunamedicinyes z3 ysuvarianzaseasumees z3

Comoantes,tenemosdosgaussianasquecon?eneninformacin
acercadelaposicin,unaconlainformacinantesdelamedicin
ylaotraeslaqueconlainformacinquetraeconsigolamedicin
Combinamosladensidadconmedia x (t3 ) yvarianza x2 (t3 )
conladensidadconmedia z3 yvarianza z23

donde

Sinconanzaenlamedicin
Si
Si

esgrande,entonces
entonces

serpequeo
serceroyentonces x (t3 ) = x (t3 )

Ylamedicinnuevainnitamenteruidosaescompletamenteignorada

Sinconanzaenladinmicadel
sistema
2

Si w esgrande,entoncestambinloser

Estosignicaqueno?enesmuchaconanzaenelmodelolinealy
seledarimportanciaalamedicin,enuncasoextremosi
y

Entoncesenelcasoextremodenotenerningunaconanzaenel
modelo,lasolucinp?maesignorarlaprediccinytomarla
medicincomolaes?macinp?ma

Certezaenlaprediccin
Si x2 (t3 ) = 0 entonces

K (t3 ) = 0

Signicaquese?eneabsolutaseguridadenlaprediccinyse
descartarlaprximamedicin

Ejemplo
UntanquedeaguallenndoseavelocidadCteLt = Lt!1 + f
Elestadodelsistemasedenemedianteelvector
! x $
l
&
x =#
# xf &
"
%

Elniveldellquidoes xl
Larapidezconlaquesellenaeltanquees x f = dxl / dt
Observeque!!
xl = x! f = 0 Puestoqueeltanquesellenaa
Velocidadconstante
x! = Ax
! x! $ !
! x $
$
# l & = # 0 1 &# l &
# x! f & " 0 0 %# x f &
"
%
"
%

TransformandoAenF
Necesitamosunprocesodiscreto,entoncesla
matrizAdebeser?empodiscre?zada
!
i
A!t
x
F = e
x
Sabemosque: e = 1+

" i!

"

Entonces:

!
F =#
"
"
F =$
#

1
0
1
0

A !t
F = I +#
i!
i=1

i=1

2 !
$
!
$
't
0
0 1
0 0 $
& + 't #
&+
#
& +...
1 %
" 0 0 % 2 " 0 0 %
!t % Si t = 1 Entonces: F = 1 1
'
0 1
1 &

Modelandoelruidodelamedicin
Asumiendoqueelruidoocurresoloenla
partedemedicinasociadaalllenado

Dondeeselruidodelllenado

Modelandoelruidodelamedicin
Elprocesocon?nuosepuedeaproximar
medianteunprocesodiscretoen?empo
mediante:
e A
e A

Ai i
= I +
i!
i =1

1 0 0 1 2 0 0
1
+
+
+ ... =

=
0 1 0 0 2 0 0
0 1

Modelandoelruidodelamedicin
Demanerasimilar
T

eA
A

e Qe

AT

1 0 0 0 2 0 0
1 0
+
+
+ ... =

=
0 1 1 0 2 0 0
1

1 0 0 1 0 0 q f

=
=

0 1 0 q f 1 0 q f

qf
1 0

=
1 q
f

2 q f
=
q f
0
t

t 3

qf
3
= 2
t
qf

t 2
qf
2

qf

Si

t = 1

q f

d
q f

q f

q f

Modelandoelprocesodemedicin
Asumiendoquesolomedimoselnivelynoel
rgimendellenado

Ejemplo
Es?macininicial x
0|0
Prediccin

0
1000
0

= P0|0 =
0 1000
0

q f = 0.00001

1 1 0 0
=
xt|t 1 =
0 1 0 0
0.0001 0.0001
2000.00000333 1000.000005
0 1 0 3
1 11000
2
=

Pt|t 1 =
0
.
0001
1000 1 1
1000.00001
0 1 0
0.0001 1000.000005

Correccin

2000.000003 1000.000005 1
2000.000003 1000.000005 1

(1 0)
+ 1
K t =
1000.000005 1000.00001 0
1000.000005 1000.00001 0

0.9995

K t =
0.49975

Supongamosunamedicinde yt = 2.462
(Ruidorealde5)

0 0.9995
0 2.46079
2.462 (1 0) =

xt|t = +
0 0.49975
0 1.2304

1 0 0.9995
2000.000003 1000.000005 0.9995
0.49975

(1 0)
=

pt|t =
0 1 0.49975
1000.000005 1000.00001 0.49975 500.24988

EnMatlab

F=[11;01];
H=[10];%Matrizquerelacionamedicionesconelestadodelsistema
q=0.00001;%ruidodelasmediciones;
Q=[q/3q/2;q/2q];%Matrizdecovarianzasderuidodemediciones;
r=1;%ruidodelasmediciones
I=[10;01]%Matriziden?dad
x(:,1)=[00]';%Estadoinicial(nivelceroyregimendellenadodecero)
P=[10000;01000];%Matrizdecovarianzasdedistribuciondeedoinicial
fori=1:1:40
z(i)=i*0.1;
end
y=random('norm',0,1,1,40);%Ruidogaussianoconmediaceroyvarianza1
y=z+y;
fort=1:1:39
%Prediccion
x(:,t+1)=F*x(:,t)%observequenohayentradas(B=0;u=0)
P=F*P*F'+Q
%Correccion
K=P*H'/[H*P*H'+r]%K=P*H'*inv([H*P*H'+r])Paraesteejemploeslomismo
x(:,t+1)=x(:,t+1)+K*(y(t+1)H*x(:,t+1))
P=(IK*H)*P
t=t+1;
end