advertisement

INTRODUCTION TO KALMAN FILTER Rudolf Emil Kalman - Born 1930 in Hungary - BS and MS from MIT - PhD 1957 from Columbia - Filter developed in 1960-61 - Now retired Why not the deterministic system and control theories do not provide a totally sufficient means of performing the analysis and design? - No mathematic model is perfect, only a limited number of parameters are used in modeling. - Deterministic systems are driven not only by our own signals, but also by disturbances/noises. These factors are beyond our control and capability of analysis (Example: measure the temperature, pilot control the fly angle, etc.). - Sensors themselves do not provide perfect and complete date about a system. Sensors may behave according to their own dynamic characteristics, and may be corrupted by noises. What is a Kalman Filter?? •- Just some applied math. •- A linear system:: ff((a+b)) = ff((aa)) + ff((b)).. •- Noisy data in hopefully less noisy out. •- But delay is the price for filtering...... •- Pure KF does not even adapt to the data. What is it used for?? ••- Tracking missiles ••- Tracking heads/hands/drumsticks ••- Extracting lip motion from video ••- Lots of computer vision applications • - Economics • - Navigation A really simple example Review: conditional probability density. It indicates that for the given values of all measurement taken up through time instant i, what probability would be of x(i) assuming a certain particular value or range of values. What will happen if we suppose the probability distributes in Gaussian pattern (normal distribution and white)? Description: Suppose you are lost at sea at night and want to obtain your location x by star sighting. At time instant t1, your measurement is z1. Due to some device inaccuracies and other uncertainties, you decide the precision is such that its variance is This way, you can set up your conditional probability of your position x(t1) at time instant t1, conditioned on the measurement (observation) of z1 . 2 z1 . Based on this CPD, your best (optimal) estimation of your position is x(t1 ) z1 (t1 ) 2 x 2 z1 Now suppose an more experienced navigator takes another measurement right after you do at time t2= t1 (so the ship potion is not changed), similarly, obtained z2 2 and z 2 . Note 2 z2 < 2 z1 . Likewise, the CPD is as follows: At this point, you have two data points for estimating your position. Question: How to combine these data? Based on the assumptions made, the conditional density of your position x(t2) at time t2= t1 , given z1 and z2, is a Gaussian density below. and variance 2 , as indicated in the figure [ /( )]* z1 [ /( )]* z2 2 z2 2 z1 2 z2 2 z1 2 z1 2 z2 1/ (1/ ) (1/ ) 2 2 z1 2 z2 Note that, is less either or which is to say that the uncertainty in your estimate of position has been decreased by combining the two pieces of information. 2 2 z1 2 z2 , Given this density, the best estimate of your position is x(t2 ) with associated variance 2 . ^ x(t2 ) The equation of estimate can be rewritten as x(t2 ) [ /( )]* z1 [ /( )]* z2 2 z2 2 z1 2 z2 2 z1 2 z1 2 z2 z1 [ z21 /( z21 z22 )]*[ z2 z1 ] In final form that is used in Kalman filter implementation, it is expressed as x(t2 ) x(t1 ) K (t2 ) *[ z2 x(t1 )] ^ Note that x(t1 ) z1 , and K (t2 ) /( ) 2 z1 2 z1 2 z2 The above equations say that the best estimate at time t2, ^ x(t2 ) , is equal to the best prediction of its value before z2 is ^ taken, x(t1 ) , plus a correction term of an optimal weighting value times the difference between z2 and its best prediction ^ x(t1 ) . So the filter takes the structure of “prediction correction”. From the equation of the variance expressed afore, let us rewrite it as (t2 ) (t1 ) K (t2 ) * (t1 ) 2 x 2 x 2 x ^ This indicates that the value of x(t2 ) and f x (t2 )|z (t1 ), z (t2 ) x2 (t 2 ) embody ( x| z1 , z2 ) all the information in . State differently, by propagating these two values, the conditional density of your position at time t2, given z1 and z2, is completely specified. Thus far, we have solved the static estimation problem, next time let us incorporate the dynamics in the system. (To be described).