FiltrodeKalmanparalafusinde

datosdelaIMU.
Teora+Implementacin

Definiciones
Probabilidadyvariablesaleatorias

Probabilidad
Laprobabilidadqueelresultadodeuneventodiscreto(porej.ellanzamientodeuna
moneda)favoreceruneventoparticular,definidopor:

Enelcasodeeventosindependientes,laprobabilidadqueocurraAoBestdadapor:

silaprobabilidaddedosresultados,esdecirunanoafectaaotra,entonceslaprobabilidad
dequeambosocurranestdadapor:

SiAyBsondossucesos,laprobabilidaddequeocurraBdadoAsedenotaporp(B|A)y
esllamada
probabilidadcondicional
.Estdadapor:

Variablesaleatorias
Unavariablealeatoriaesbsicamenteunafuncinquemapeatodoslospuntosenel
espaciodemuestraalosnmerosreales.Lasvariablesaleatoriaspuedenserdiscretas,
cuandosepuedencontarelconjuntodesusresultadosposibles,ocontinuas,cuandotoma
valoresenunaescalacontinua.
PorejemplolavariablealeatoriacontinuaX(t)puedemapeartiempoaposicin.En
cualquierpuntoeneltiempo,X(t)nosdirlaposicinesperada.Enelcasodevariables
aleatoriascontinuas,laprobabilidaddecualquiereventosimplediscretoAesdehecho0.
Estoes,P(A)=0.Unafuncincomnquerepresentalaprobabilidaddeunavariable
aleatoria,esdefinidacomola
funcindedistribucinacumulativa
.Estafuncin
representalaprobabilidadacumulativadelasvariablesaleatoriascontinuasXparatodos
(losnocontables)eventoshastax.

Mscomnmenteusadaenformadesuderivadayselaconocecomola
funcinde
densidaddeprobabilidad
:

stafuncindedensidadtienepropiedades:

MediayVarianza
La
mediadeunavariablealeatoriaX
eselpromediodeunasecuenciadenmerospara
algnespaciomuestralN(
Sedefineespaciomuestralcomotodoslosresultadosposibles
deunexperimientoestadstico)
deunavariablealeatoriadiscretaX.Seexpresaconlaletra

griega
mu
.Nospodemosreferiraestamediacomoelvaloresperadodelavariable
aleatoriaX,yseexpresacomoE(X).Lamediaestdadapor

SiXesunavariablealeatoriadiscretacondistribucindeprobabilidadf(x)la
mediaovalor
esperado
deXes:

silavariablealeatoria
X
escontinua:

SiXesunavariablealeatoriadiscretacondistribucindeprobabilidadf(x)ymedia ,la
varianza
deXseexpresacomo 2 ysedefinedelasiguienteforma:

cuandolavariablealeatoria
X
escontinua:

SiXescontinualarazcuadradadelavarianza 2 = ,sellama
desviacinestndar
de
X.

DistribucinNormaloGaussiana
Muchosprocesosdelanaturalezaparecentenerunadistribucinnormalomuycercanaa
esta.Enefecto,bajociertascondiciones,sepuedeprobarqueunasumadevariables
aleatoriasconcualquierdistribucintiendehaciaunadistribucinnormal.Elteoremaque
defineestapropiedadsellamaelteoremadellmitecentral.Ladistribucinnormaltiene
algunaspropiedadesquelahacenmatemticamentemanejableyhastainteresante.

UnavariablealeatoriacontinuaXquetieneladistribucinenformadecampanadelafigura
sellama
variablealeatorianormal
.Laecuacinmatemticaparaladistribucinde
probabilidaddelavariablenormaldependedelosparmetros y .Porlotantose
representanlosvaloresdedensidaddeXporn(x , ).
LafuncindedensidaddeprobabilidadparaunavariablealeatorianormalX,conmediamu
yvarianzasigma^2estdadapor:

Cualquierfuncinlinealdeunprocesoaleatorionormalmentedistribuidotambinesun
procesoaleatorionormalmentedistribuido.EnparticularsiX~N( , 2 )yY=aX+b,
entonces

SiX1yX2sonindependientes,X1~N( 1 , 21 )yX2~N( 2 , 22 )entonces.

Independenciacontnuayprobabilidadcondicional
DosvariablesaleatoriascontinuasXyYsonllamadasestadsticamenteindependientessi
suprobabilidadconjunta f xy(x, y) esigualalasumadesusprobabilidadesmarginales,es
decir,seconsideraindependientesi f xy(x, y) = f x(x)f y(y) .
Adicionalmentela
RegladeBayes,
tomandolaecuacinde
probabilidadcondicional,

ofreceunamaneradeespecificarla
densidaddeprobabilidadcondicional
deuna
variablealeatoriaXdada(enlapresenciade)Y(Variablealeatoria).EnestecasolaRegla
deBayesestdadapor:

DadounprocesodiscretoXyunprocesocontinuoY,lafuncindiscretademasade
probabilidadparaXcondicionadosobreY=yestdadapor:

EspaciovsEspectro(seales)
Sepuededecirquelamagnituddelavarianzadeunasealindicacuntoruidotienela
seal.Sinembargo,lavarianzadeunasealnodicenadaacercadelespaciamientoola
tasadecambioconelpasodeltiempo.
Unacaractersticatiltiemporelacindeunasealaleatoriaessu
funcinde
autocorrelacinRx
,su
relacinconsmismaatravsdeltiempo
.Formalmentesepuede
definirlafuncindeautocorrelacindeunasealaleatoriaX(t)delasiguienteforma:

Paralostiemposdemuestrat1yt2.Sielprocesoes
estacionario,
esdecirquelafuncinde
densidadnovaraconeltiempo,laecuacinslodependedeladiferencia = t1 t2 .En
estecasolaautocorrelacinseescribecomo:

Dosfuncionesdeautocorrelacinsemuestranenlafigurasiguiente,seobservaqueen
comparacinconlasealaleatoriaX2,lasealaleatoriaX1esrelativamentemspequea
ymsancha.Mientras| |aumenta(cuandosemuevelejosde = 0 enelcentrodela
curva)lasealdeautocorrelacinparaX2bajarelativamenterpido.EstoindicaqueX2
estmenoscorrelacionadaconsmismaqueX1.

Caractersticaseneldominiodefrecuencia.

Sepuedeverquela
autocorrelacinesunafuncindeltiempo.
Estosignificaque
tambintieneunainterpretacinespectraleneldominiodelafrecuencia.Nuevamentepara
unprocesoestacionario,existeunaimportanterelacintemporalespectralconocidacomo
larelacinoel
teoremaWienerKhinchine:

queexpresaqueparaunprocesoestacionario,lafuncin SX(j) (llamadala

funcin
densidadespectraldepotencia
(P

SD:powerspectraldensityfunction)

delaseal
aleatoriapuedeserobtenidaporlatransformadadeFourier(
F[Rx(t)]
)delafuncinde
autocorrelacindelasealaleatoria,donde indicaelnmerodeciclosporsegundo(2 f).
Comosepuedever,estarelacinenlazalasrepresentacionesdelespectrodetiempoyde
frecuenciadelamismaseal.
EstafuncinPSDmuestracuntapotenciaestalmacenadaencadacomponentedel
espectro.Porejemplo,paraunaondasinusoidalconfrecuenciafija,elgrficodePSD
contendrslouncomponentedelespectropresenteenlafrecuenciadada.

Unprocesoaleatorio(oseal)conunafuncinPSDconstanteesunprocesoderuido
blanco
.

RuidoBlanco.
Unasealderuidoblanco(proceso)estconstituidaporun
conjuntodevariables
aleatoriasindependienteseidnticamentedistribuidas
(IID).Ensentidodiscreto,la
sealderuidoblancoconstituyeunaseriedemuestrasquesonindependientesygenerada
apartirdelamismadistribucindeprobabilidad.
Unimportantecasodeunasealaleatoriaescuandolafuncindeautocorrelacinesuna
funcinDiracdelta () quetienevalorceroentodaspartesexceptocuando = 0 .Enotras
palabrasdonde:

paraalgunamagnitudconstanteA.Enestecasoespecialdondelaautocorrelacinesun
pulso,latransformadadeFourierresultaenunespectrodefrecuenciaconstantecomose
muestraenlafigura.

Estaesunadescripcinde
ruidoblanco
,dondetodaslasfrecuenciassonigualmente
importantesotienenlamismapotenciaespectral,yestncompletamenteno
correlacionadas,exceptoeneltiempoactualcuando = 0 .Estaporestoquesedenominan
independientes
alassealesderuidoblanco.Cualquiermuestradelasealenuninstante
tiempoescompletamenteindependiente(sincorrelacin)deotramuestraenotroinstante
detiempo.
Aunquenoesposiblevernialcanzarenlaprctica(ningnsistemapuedeexhibirenerga
infinitaatravsdeunespectroinfinito),elruidoblancoesunaparteimportanteeneldiseo
yelanlisis.Lassealesaleatoriaspuedensermodeladascomoruidoblancofiltrado.
Literalmenteestoquieredecirqueunopodrafiltrarlasalidadeuna(hipottica)fuentede
ruidoblancoparalograrunafuentederuidonoblancao
colorida
queesdelimitadaenel
dominiodelafrecuencia,ymscorrelacionadaconeldominiodetiempo.

Porejemplo,sepuedegenerarunasealderuidoblancoutilizandoungeneradorde
nmerosaleatoriosdondetodaslasmuestrassiguenunadistribucindeGaussdada.Esto
seconocecomoruidoblancogaussianooruidoblancocondistribucinnormal.Delmismo
modo,unasealderuidoblancogeneradoapartirdeunadistribucinuniformese
denominaruidoblancouniforme.Ruidoblancogaussianoyruidoblancouniformeseutilizan
confrecuenciaenelmodeladodelsistema.Ruidoblancogaussianosepuedegenerar
utilizandolafuncin"randn"enMatlabquegeneranmerosaleatoriosquesiguenuna
distribucindeGauss.Delmismomodo,funcin"rand"sepuedeutilizarparagenerarruido
blancouniformeenMatlabquesigueunadistribucinuniforme.
Porejemplo,generamosunruidoblancocondistribucingaussianadelongitudL=100000,
media0ydesviacinestndar=1(enMatlabconlafuncinrandn).
Paracalcularlafuncindedensidaddelavariablealeatoria:

Enelgrficodeautocorrelacin,seobservaqueelpulsoalcanzadoessigma^2=1ent=0
loculseverificalafuncindeautocorrelacin.
ElgrficodePSDnosmuestracasiunapotenciafijaentodaslasfrecuencias.Oseaque
paraunasealderuidoblancolaPSDesconstanteentodaslasfrecuencias(infinito
+infinito).Elejeyestexpresadoen[dB/Hz].

Funcin/MatrizCovarianza.
La
funcindecovarianza
sedefinecomo:

Seutilizaporqueestamostratandoconprocesosaleatorioscompuestospornvariables
aleatorias(10enelejanterior).
Dichoprocesoesvistocomovectoraleatoriomultivariadoovariablealeatoriomultivariada.
Paraestasvariablesaleatoriasmultivariadas,lafuncindecovarianzaespecificacmo
cadaunadelasnvariablesenelprocesoaleatoriosecomportarespectoalaotra.La
funcindecovarianzageneralizaelconceptodevarianzaenmltiplesdimensiones.

Larepresentacinmatricialsellamala
matrizdecovarianza
delasealaleatoriaderuido
blanco:

Dadoquelasvariablesaleatoriasenlasealderuidoblancosonindependientes(no
correlacionadas)lafuncindecovarianzacontienevaloresalolargodesudiagonal.La
matrizanteriormuestraquesloexistelafuncindeautocorrelacinparacadavariable
aleatoria.Losvaloresfueradeladiagonalsoncero.Loselementosdeladiagonalsonigual
alavarianza.Lafuncindelconjuntoautocorrelacionadoderuidoblancoestdadapor:

Estoindicaquelafuncindeautocorrelacindelasealderuidoblancoesceroentodas
partesexceptoenelretardo = 0 .

Estimacinestocstica

ModelosEspacioEstado

Problemadediseodelobservador

Ruidodemedicionesyproceso.

FiltrodeKalman
Definiciones
ElfiltrodeKalman,peseaserllamadofiltro,esunalgoritmodeestimacin.Fueinventado
porRudolphE.Kalman,quienen1960publicunartculodescribiendounasolucin
recursivaparaelproblemadefiltradolinealdedatosdiscretos(Kalman1960).
Esbsicamenteunconjuntodeecuacionesmatemticasqueimplementanunestimadordel
tipopredictorcorrector,ptimoenelsentidoqueminimizalacovarianzadeerrorestimado
(cuandoalgunascondicionessupuestassecumplen).

ElfiltrodeKalmansehautilizadoampliamenteparaelseguimientointeractivopor
computadora.Tambinutilizadoparapredecirmovimientoyparalafusindemltiples
sensores(inerciales).
EsunmodelomatemticoentrminosdeEspacioEstado.
ElfiltrodeKalmanesunaampliacindelalgoritmoLMSdeMnimoscuadrados,peroms
sofisticado.
Esunasolucinrecursiva,vlidatantoparaambientesestacionariosynoestacionarios,que
tericamenteesidealparafusionarlosdatosconruido.
Sirvecomobaseparaungrannmerodealgoritmosdeestimacinutilizadosen
navegacin.Susfuncionesincluyenlademantenerunasolucindenavegacinporsatlite
suavizada,elalineamientoycalibracindeINS.Estaintegracinesclaveenlaobtencin
deunasolucindenavegacinptimaapartirdelasmedicionesdisponiblesenelsistema
denavegacin.

Realizaestimacionesentiemporealdedistintosparmetrosdeunsistema,comopor
ejemploposicinyvelocidad,quepuedencambiarcontinuamente.Lasestimacionesson
actualizadasapartirdedistintasmedicionesrealizadassobreelsistema,quepueden
contenerruidos.Estasmedicionessonrealizadasporsensores,yhandeserfuncindelos
parmetrosestimados.Noesnecesarioquelasmedicionescontenganentodoslospasos
detiemposuficienteinformacinparadeterminarlosvaloresdelosparmetros.

ElFKutilizaelconocimientodelaspropiedadesdeterminsticasyestadsticasdelos
parmetrosdelsistemaydelasmedicionesparaobtenerestimacionesptimasdesus
estadosapartirdelainformacindisponible.ElFKincluyeunaincertidumbresobrelas
estimacionesrealizadasyunamedidadelascorrelacionesentreloserroresenlas
estimacionesdelosdistintosparmetros.

ElFKesunatcnicadeestimacionesBayesiana.Seleproporcionaungrupode
estimacionesinicialesyesteoperadeformarecursiva,actualizandolasestimacionesen
funcindelosvalorespreviosydelosnuevosobtenidosdelasltimasmediciones.Este
asumequeelsistemaenestudioeslineal,loquellevaarealizarciertasasunciones
descritasposteriormente.Paraaplicacionesentiemporeal,comoeselcasodela
navegacin,elenfoquerecursivodeestealgoritmoesmseficiente,dadoquesolose
debenprocesarlosdatosdelasnuevasmedicionesencadaiteracin.

Principalmenteminimizaelerror,perolohacebasndoseenmsparmetros.Adems
prediceculdeberaserlasalidaparaelestadosiguienteydaunaestimacinapartirde
lasmedidasenelmismoinstantedetiempo.Todasestasmedidastienensupropia
estadsticaydependiendodecualseamsfiable,seotorgamspesoalaprediccinoala
estimacin.Enelfondoseintegranestadosredundantesparaobtenerinformacinms
precisa.

Paranuestroproyecto,seutilizarelfiltrodeKalmanparaobtenerlaposicin,lavelocidady
elrumbodelmvil(en3D).PorunladoestnlasmedidasdelaIMU,quesonmuyprecisas
enunespaciodetiemporeducidoyporotrolasmedidasdelmagnetmetroydelGPS(en

casodeusaruno)cuyoserroresnoaumentanconeltiemposinoqueesconstante,pero
tienenunerrormsgrande.

ElementosyfasesdelFK

Estcompuestopor5elementosprincipales:
vectordeestadosysucovarianza
modelodelsistema
vectordemedicionesysucovarianza
modelodemediciones
algoritmo

El
vectordeestados
esungrupodeparmetrosquedescribenelsistema,conocidas
comoestados,queelfiltrodeKalmanutilizaparaestimarunasolucinptima.Unestado
puedeserconstanteovariableeneltiempo.

x = [x1x2x3...xn] T

Elvectordeestadoincluyenoslolosparmetrosdeseados,sinoademsaquellosquese
hanaadidoalfiltrocontaldeintroducirredundanciaalproblema.
Asociadaaestevectordeestadosvala
matrizdecovarianzadelerror
,querepresentala
incertidumbresobrelasestimacionesdelosestadosdelFK,yelgradodecorrelacinentre
loserroresenesasestimaciones.Conocerestascorrelacionesesdevitalimportanciaen
algunoscasos,dadoqueenocasionesnoexistesuficienteinformacinparaestimarcada
unodelosestadosdeformaindependiente.DadoqueelFKesunprocedimientoiterativo,
losvaloresinicialesdelvectordeestadoylosdelamatrizdecovarianzahandeser
proporcionadosporelusuarioodeterminadosporunprocesoexternoalFK.

El
modelodelsistema
,omodelodetransicineneltiempo,describelavariacinenel
tiempodelvectordeestadosydesumatrizdecovarianza.Aplicaelefectodecada
parmetrodeestadodelsistemadeiteracionespreviasalestadoactualdelsistema.Por
ejemplo,unestadodeposicinvaraconeltiempocomolaintegraldelestadodevelocidad,
laincertidumbredelaposicinaumentaconeltiempocomolaintegraldelaincertidumbre
delavelocidad.Conesto,sepuedeverqueloserroresenlaestimacindelavelocidadyla
posicinestncorrelacionados.Elmodelodelsistemaesdeterministaparalosestados,
dadoqueestbasadoenpropiedadesdelsistemaconocidas.Asociadoaestemodeloest
elruidodelsistema,quehacequelaincertidumbredelosestadospuedaaumentarconel
tiempodebidoacambiosdesconocidosenelsistema,quehacenquelaestimacindelos
estadosseaerrnea.Esteruidosuelevenirdelossensoresodeparmetrosdinmicosde
loscualesnotenemosinformacin.Esteruidoysuspropiedadesestadsticashandeser
definidosyestimadospreviamentealaimplementacindelFiltrodeKalman(porel
diseador).

El
vectordemediciones
estformadoporungrupodevariablescuyamedidaest
disponible,realizadasenelmismopasodetiempo,quesonfuncindelvectordeestados.A
partirdelainformacindeestevectorsecalculanlasestimacionesdelosestadosdespus
delainicializacin.

z = [z1z2z3...zm]

Sesueledarelcasodequeladimensin(
n
)delvectordeestados
x
seamsgrandequela
dimensin(
m
)delvectordemedidas
z
.Ambosvectoresestnrelacionadosdemanera
linealtalqueapartirdelvectordeestados,secalculaelvectordevariables.

Asociadaaestevectorestla
matrizdecovarianzadelruido
enlasmediciones,que
definelaspropiedadesestadsticasdelruidoenlasmedicionesrealizadas.

El
modelodemediciones
ode
observacin
describelavariacindelvectorde
medicionescomofuncindelvectordeestadosreal(noelestimado)enausenciaderuido
enlasmediciones.Aligualqueelmodelodelsistema,esteesdeterminista,dadoquese
basaenpropiedadesconocidasdelsistema.Mapealosparmetrosdelvectordeestadoen
eldominiodelamedicin.

ElalgoritmodelKFempleaelvectordemediciones,elmodelodemediciones,yelmodelo
delsistemaparacalcularlasestimacionesptimasdelvectordeestados.Cuentacondos
etapas:

1) Prediccin
Actualizacindeltiempo
2) Correccin
Actualizacindelamedicin

Encadaiteracin,elalgoritmorealiza10pasos,dondelos4primerosformanlafasede
prediccin,ylos6siguienteslafasedeactualizacin.

FiltrodeKalmanDiscreto
Sedescribeacontinuacinelfiltro,segnlaformulacinoriginalenKalman1960,enelque
seproducenlasmedicionesyelestadoesestimadoenpuntosdiscretoseneltiempo.

Elprocesoaestimar
Modelos
Modelodelsistema
Modelodelamedicin
Modeloapriori

OrigencomputacionaldelFK
OrigenprobabilsticodelFK
ElAlgoritmodelFKD

FiltrodeKalmanExtendido(FKE)