You are on page 1of 1

kalman filters

LISTING 1 Kalman filter simulation
fnto kla(uain d) ucin amndrto, t %fnto kla(uain d) ucin amndrto, t % %Kla fle smlto fravhcetael gaogara. amn itr iuain o e i l r v l in l n od %IPT NUS % % drto =lnt o smlto (eod) uain egh f iuain scns d =se sz (eod) t tp ie scns

manie=1;%psto maueetnie(e ) esos 0 o i i n e s r m n o s f et aclos =02 %aclrto nie(etsc ) cenie .; c e e a i n o s f e / e ^2 a=[ d;01;%tasto mti 1 t ] rniin arx b=[t22 d] %iptmti d^/; t; nu arx c=[ 0;%maueetmti 1 ] esrmn arx x=[;0;%iiilsaevco 0 ] nta tt etr xa =x %iiilsaeetmt ht ; nta tt siae S =manie2 %maueeterrcvrac z esos^; esrmn ro oaine S =aclos^ *[t44d^/;d^/ d^] %poesniecv w cenie2 d^/ t32 t32 t2; rcs os o P=S;%iiiletmto cvrac w nta siain oaine %Iiilz ary frltrpotn. ntaie ras o ae ltig ps=[;%tu psto ary o ] re oiin ra psa =[;%etmtdpsto ary oht ] siae oiin ra pses=[;%maue psto ary oma ] esrd oiin ra vl=[;%tu vlct ary e ] re eoiy ra vla =[;%etmtdvlct ary eht ] siae eoiy ra frt=0:d:drto, o t uain %Ueacntn cmaddaclrto o 1fo sc2 s osat omne ceeain f o t/ e ^ . u=1 ; %Smlt telna sse. iuae h ier ytm P o e s o s = a c l o s * [ d ^ / ) r n n d *r n n ; rcsNie cenie (t22*ad; t ad] x=a*x+b*u+Poesos; rcsNie %Smlt teniymaueet iuae h os esrmn MaNie=manie*rnn esos esos ad; y=c*x+MaNie esos; % E t a o a e t e m s r c n s a e e t m t t th p e e t t m . xrplt h ot eet tt siae o e rsn ie xa =a*xa +b*u ht ht ; %Fr teInvto vco. om h noain etr In=y-c*xa; n ht %Cmuetecvrac o teInvto. opt h oaine f h noain s=c*P*c +S; ’ z %Fr teKla Gi mti. om h amn an arx K=a*P*c *ivs; ’ n() %Udt tesaeetmt. pae h tt siae xa =xa +K*In ht ht n; %Cmuetecvrac o teetmto err opt h oaine f h siain ro. Lsig1cniudo p 7. itn otne n . 8

are almost too close to distinguish from one another. T he noisy-looking curve is the measured position. Figure 2 shows the error between the true position and the measured position, and the error between the true position and the Kalman filter’s estimated position. T he measurement error has a standard deviation of about 10 feet, with occasional spikes up to 30 feet (3 sigma). T he estimated position error stays within about two feet. Figure 3 shows a bonus that we get from the Kalman filter. Since the vehicle velocity is part of the state x, we get a velocity estimate along with the position estimate. Figure 4 shows the error between the true velocity and the Kalman filter’s estimated velocity. T he M atlab program that I used to generate these results is shown in L isting 1. Don’t worry if you don’t know M atlab— it’s an easy-to-read language, almost like pseudocode, but with built-in matrix operations. If you use M atlab to run the program you will get different results every time because of the random noise that is simulated, but the results will be similar to the figures shown here.

Practical issues and extensions
T he basic ideas of Kalman filtering are straightforward, but the filter equations rely heavily on matrix algebra. L isting 2 shows the Kalman filter update equations in C. T he matrix algebra listings referenced in L isting 2 can be found at www.embedded.com/ code.html. T hese listings are very general and, if the problem is small enough, could probably be simplified considerably. For example, the inverse of the 2-by-2 matrix:

 d1 D= d3

d2  d4  

is equal to:

D−1 =

 d4 1 d1d4 − d2 d3 − d3 

− d2  d1  

76

J UNE 2001

Embedded Systems Programming