Sensorifuusio Jorma Selkäinaho

Juna ja GPS dx1/dt=x2 (tarkka malli) dx2/dt=w2 y1=x1+v1 y2=x2+v2 E(v12)=(5m)2 E(v22)=(0.02m/s)2

Kalman suodin yhdistää sensoritietoa suodattaa häiriöitä laskee ei-mitattavia tiloja laskee tilan virheen varianssin tilat yhdistävä malli mittaukset ja tilat yhdistävä yhtälö

jatkuva-diskreetti suodin differentiaaliyhtälöryhmä kuvaa tilan dynamiikkaa dx/dt=A*x+B*u+w mittaukset diskreetteinä ajanhetkinä y=C*x+v E(w)=0 ja E(v)=0

juna ja GPS A=(0 1; 0 0) B=0 C=I w=(0 w2)T v=(v1 v2)T

mittausten välillä dx=(A*x+B*u)*dt dP=(A*P+P*AT+Q)*dt alkuarvot x(0) ja P(0) mallivirheen kovarianssi E(w2)=Q

mittauksen hyödyntäminen tilan korjaus x+=x-+K*(y-C*x-) tilan virheen kovarianssi P P+-1=P--1+CT*R-1*C mittausvirheen kovarianssi E(v2)=R

Riccatin yhtälö kun dim(x) >dim(y) P+=P--P-*CT*(C*P-*CT+R)-1*C*P- estimoinnin vahvistus K=P-*CT*(C*P-*CT+R)-1 tilan virheiden varianssit > 0 kovarianssi P symmetrinen

diskretointi malliyhtälö dx/dt=A*x+B*u diskretointi dx/dt=(x(k+1)-x(k))/t x(k+1)=x(k)+t*(A*x(k)+B*u(k)) u(k) vakio välillä (k,k+1)

diskreetti suodin mittausten välillä x=Ad*x+B*u , Ad=I+t*A P=Ad*P*AdT+Q mittauksen hyödyntäminen x=x+K*(y-C*x) P=P-P*CT*(C*P*CT+R)-1*C*P

kiinteän vahvistuksen suodin x=Ad*x+B*u x=x+K*(y-C*x) K lasketaan etukäteen K=P*CT*(C*P*CT+R)-1 P=Ad*(P- P*CT*(C*P*CT+R)-1*C*P)AdT+Q P on tasapainotilan arvo

suotimen viritys Iso Q/R kasvattaa vahvistusta, lisää nopeutta ja häiriöitä suotimen viive kompensoidaan termillä B*u virityksen tarkistus (1/n)(yk-Cxk,k-1)(yk-Cxk,k-1)TC*P*CT+R

2 DOF kulkuneuvo dx1/dt=x4*cos(x3) dx2/dt=x4*sin(x3) y1=x1+v1 y2=x2+v2

Laajennettu Kalman suodin epälineaarisille malleille dx/dt=f(x)+w y=h(x)+v tilan laskenta epälineaarisilla malleilla kovarianssin laskenta linearisoiduilla matriiseilla

Laajennettu Kalman suodin A=f(x)/x C=h(x)/x mittausten välillä dx=f(x)*dt mittausten hyödyntäminen x=x+K*(y-h(x))

2 DOF kulkuneuvo A=( 0 0 -x4*sin(x3) cos(x3) ; 0 0 x4*cos(x3) sin(x3) ; 0 0 0 0 ; 0 0 0 0 ) dx3/dt=0 dx4/dt=0 C=I

mittaus/ohjaus y1 = paikan mittaus y2 = nopeuden mittaus y3 = kiihtyvyyden mittaus dx1/dt=x2 dx2/dt=u u=y3

parametrien määritys Expectation maximization (EM) soveltuu lineaarisille malleille suodatetaan tila ajassa eteenpäin käyttäen mallin parametrejä A ja C aika 1 -> n xk,k=xk,k-1+Gk*(yk-Cxk,k-1) xk+1,k=Axk,k

parametrien määritys tasoitetaan tila ajassa taaksepäin aika n -> 1 xk-1=xk-1,k-1+Jk-1(xk-Axk-1,k-1) Jk-1=Pk-1,k-1AT(Pk,k-1)-1 EM on raskas off-line menetelmä

parametrien määritys ratkaistaan A, C, Q ja R regressiolla käyttäen tasoitettuja estimaatteja xk A= Pk,k-1 ( Pk-1,k-1)-1 C= ykxkT (Pk,k)-1 R= (1/n)(yk-Cxk)ykT Q= (1/(n-1))(Pk,k-APk-1,k) toistetaan vuorotellen näitä kahta vaihetta