Kalman Filter Applications - Cornell University

1 Subject MI63: Kalman Filter Tank FillingKalman Filter ApplicationsThe Kalman Filter (see Subject MI37) is a very powerful toolwhen it comes to controlling noisy systems. The basic idea of aKalman Filter is: Noisy data in hopefully less noisy data Applications of a Kalman Filter are numerous: Tracking objects ( , missiles, faces, heads, hands) Fitting Bezier patches to (noisy, moving, ..) point data Economics Navigation Many computer vision Applications Stabilizing depth measurements Feature tracking Cluster tracking Fusing data from radar, laser scanner andstereo-cameras for depth and velocity measurements Many moreThis lecture will help you understand some direct applicationsof the Kalman Filter using numerical examples that will be outlined are:1. Simple 1D example, tracking the level in a tank (this pdf)2. Integrating disparity using known ego-motion (in MI64)Page 1 September MI63: Kalman Filter Tank FillingPredict-Update EquationsFirst, recall the time discrete Kalman Filter equations (see MI37)and the predict-update equations as below:Predict: xt|t 1=Ft xt 1|t 1+BtutPt|t 1=FtPt 1|t 1 FTt+QtUpdate: xt|t= xt|t 1+Kt(yt Ht xt|t 1)Kt=Pt|t 1 HTt(HtPt|t 1 HTt+Rt) 1Pt|t= (I KtHt)Pt|t 1where x:Estimated :State transition matrix ( , transition between states).

2 U:Control :Control matrix ( , mapping control to state variables).P:State variance matrix ( , error of estimation).Q:Process variance matrix ( , error due to process).y:Measurement :Measurement matrix ( , mapping measurements onto state).K: Kalman :Measurement variance matrix ( , error from measurements).Subscripts are as follows:t|tcurrent time period,t 1|t 1previous time period, andt|t 1are intermediate 2 September MI63: Kalman Filter Tank FillingModel Definition ProcessThe Kalman Filter removes noise by assuming a pre-definedmodel of a system. Therefore, the Kalman Filter model must bemeaningful. It should be defined as the situation:Look at the problem. Break itdown to the mathematical basics. If you don t do this, youmay end up doing unneeded the state process:Start with a basic model. It maynot work effectively at first, but this can be refined the measurement process:Analyze how you aregoing to measure the process.

3 The measurement space maynot be in the same space as the state ( , using an electricaldiode to measure weight, an electrical reading does noteasily translate to a weight). the noise:This needs to be done for both thestateandmeasurementprocess. The base Kalman Filter assumesGaussian (white) noise, so make the variance andcovariance (error) meaningful ( , make sure that the erroryou model is suitable for the situation). the Filter :Often overlooked, use synthetic data ifnecessary ( , if the process is not safe to test on a liveenvironment). See if the Filter is behaving as it Filter :Try to change the noise parameters ( Filter ), asthis is the easiest to change. If necessary go back further,you may need to rethink the 3 September MI63: Kalman Filter Tank FillingExample: Water level in tank1. Understanding the situationWe consider a simple situation showing a way to measure thelevel of water in a tank. This is shown in the are trying to estimate the level of water in the tank, which isunknown.

4 The measurements obtained are from the level of the float . This could be an electronic device, or a simplemechanical device. The water could be: Filling, emptying or static ( , the average level of the tankis increasing, decreasing or not changing). Sloshing or stagnant ( , the relative level of the float to theaverage level of the tank is changing over time, or is static). welch/ Kalman / 4 September MI63: Kalman Filter Tank FillingFirst Option: A Static Model2. Model the state processWe will outline several ways to model this simple situation,showing the power of a good Kalman Filter first is the most basic model, the tank is level ( , the truelevel is constantL=c).Using the equations from Page 2, the state variable can bereduced to a scalar ( , x=xwherexis the estimate ofL).We are assuming a constant model, thereforext+1=xt, soA= 0andFt= 1, for anyt variablesBanduare not used ( , both= 0).3. Model the measurement processIn our model, we have the level of the float.

5 This is representedbyy= value we are measuring could be a scaled measurement( , a 1 cm measurement on a mechanical dial could actuallybe about 10 cm in the true level of the tank). Think of yourpetrol gauge on your car, 1 cm can represent 10 L of petrol!For simplicity, we will assume that the measurement is the exactsame scale as our state estimatex( ,H= 1).Page 5 September MI63: Kalman Filter Tank Filling4. Model the noiseFor this model, we are going to assume that there is noise fromthe measurement ( ,R=r).The process is a scalar, thereforeP=p. And as the process isnot well defined, we will adjust the noise ( ,Q=q).We will now demonstrate the effects of changing these Test the filterYou can now see that you can simplify the equations from Page2. They simplify as follows:Predict:xt|t 1=xt 1|t 1pt|t 1=pt 1|t 1+qtUpdate:xt|t=xt|t 1+Kt(yt xt|t 1)Kt=pt|t 1(pt|t 1+r) 1pt|t= (1 Kt)pt|t 1 Page 6 September MI63: Kalman Filter Tank FillingThe Filter is now completely defined.

6 Let s put some numbersinto this model. For the first test, we assume the true level of thetank isL= initialize the state with an arbitrary number, with anextremely high variance as it is completely unknown:x0= 0andp0= 1000. If you initialize with a more meaningfulvariable, you will get faster convergence. The system noise wewill choose will beq= , as we think we have an accuratemodel. Let s start this :x1|0= 0p1|0= 1000 + hypothetical measurement we get isy1= (due to noise).We assume a measurement noise ofr= :K1= ( + ) 1= |1= 0 + ( 0) = |1= (1 ) = you can see that Step 1, the initialization of 0, has beenbrought close to the true value of the system. Also, the variance(error) has been brought down to a reasonable 7 September MI63: Kalman Filter Tank FillingLet s do one more step:Predict:x2|1= |1= + = hypothetical measurement we get this time isy2= (dueto noise).Update:K2= ( + ) 1= |2= + ( ) = |2= (1 ) = can notice that the variance is reducing each time.

7 If wecontinue (with hypotheticalyt-values) this we get the followingresults:PredictUpdatetxt|t 1pt|t 1ytKtxt|tpt| 8 September MI63: Kalman Filter Tank FillingAnother reading sequence; try this:PredictUpdatetxt|t 1pt|t 1ytKtxt|tpt| 9 September MI63: Kalman Filter Tank FillingYou can see (Page 8) that the model successfully works. Afterstabilization (aboutt= 4) the estimated state is true value, even though the measurements are and ( , the true value).Over time we will get the following graph:Page 10 September MI63: Kalman Filter Tank FillingSo what have we established so far?If we create a model based on the true situation, our estimatedstate will be close to the true value, even when themeasurements are very noisy ( , a 20% error, only produced a5% inaccuracy).This is the main purpose of the Kalman what happens if thetruesituation is different?We will keep the same model ( , a static model).

8 This time, thetruesituation is that the tank isfillingat a constant rate:Lt=Lt 1+fLet s assume that the tank is filling at a rate off= timeframe, and we start with an initialization ofL0= 0. We willassume that the measurement and process noise remain thesame ( ,q= ).Let s see what happens here:PredictMeasurement and UpdateTruthtxt|t 1pt|t 1ytKtxt|tpt|tL0 11 September MI63: Kalman Filter Tank FillingWe can now see that over time the estimated state stabilizes ( ,the variance gets very low). We can easily see this on thefollowing graph:Can you see the problem?The estimated state is trailing behind the true level. This ofcourse is not desired, as a Filter is supposed to remove noise, notgive an inaccurate reading. In this case the estimated state has amuch greater error (compared to the truth) than the noise fromthe measurement 12 September MI63: Kalman Filter Tank FillingWhat is causing this?

9 There are two contributions: The model we have chosen. The reliability of our process model ( , our chosenqvalue).The easiest thing to change is ourqvalue. What was the reasonwe choseq= It was because we thought our model wasa good estimation of the true process. However, in this case ourmodel was not a good why not relax this? Let s assume there is a greater error withour process model, and setq= :Page 13 September MI63: Kalman Filter Tank FillingSo the benefits of increasing the error were beneficial. Howeverthe estimated state still has more error than the actual s increase this value again! Letq= :This is getting close to the true value, and has less error than themeasurement 14 September MI63: Kalman Filter Tank FillingLet s tryq= 1to see what happens:Now there is almost no difference to the measured value. Thereis minor noise removal, but not much. There s almost no pointin having the what has been learnt here?

10 If you have a badly defined model, you will not get a goodestimate. But you canrelaxyour model by increasing yourestimated error. This will let the Kalman Filter rely more on themeasurement values, but still allow some noise your own time, try playing with the measurement errorrandsee what the effect 15 September MI63: Kalman Filter Tank FillingSecond Option: A Filling Model2. Model the state processTo get better results, we need to change our Kalman s redefine it , the actual model is:Lt=Lt 1+fThis translates to acontinuousprocess transition of:x= (xl,xf)>A= 0 10 0 wherexlis the levelL, andxf=dxldtis the estimated filling rate,andArepresents the tank continuously filling at ratexf(defined byfand the used time scale).However, we want adiscreteprocess. SoAneeds to be madetime-discrete. Just apply the infinite sum as given in MI37 (notethatAiis zero in all four components, for alli >1):Ft= 1 t01 for allt 0.