# Kalman Filter Example - Center for Automation Research Kalman Filter Example Mobile Robotics Spring 2015 Rudolf E. Kalman b. 1930 Hungary Kalman Filter NASA Ames 1960

National Medal of Science (2009) Mobile Robotics Spring 2015 Actions and Observations Through Time Belief(xt) (using all evidence to date) Get

Measurement Elapse Time Belief'(xt) (without latest evidence) Mobile Robotics Spring 2015

Kalman Data: Our estimate: x1..t = x1, x2, x3...xt Our measurement: z1..t = z1, z2, z3...zt Our action: u1..t = u1, u2, u3...ut Mobile Robotics Spring 2015 Kalman Example

"Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation" Ramsey Faragher https://www.cl.cam.ac.uk/~rmf25/papers/Understanding%20the %20Basis%20of%20the%20Kalman%20Filter.pdf Mobile Robotics Spring 2015 Initial Estimate

Mobile Robotics Spring 2015 Elapse Time Mobile Robotics Spring 2015 Get Measurement Mobile Robotics

Spring 2015 New Belief Mobile Robotics Spring 2015 Kalman: Assume a Linear System xt = Ft xt-1 + Bt ut + wt xt = state (estimate)

ut = action Ft = state transition matrix Bt = control input matrix wt = noise wt ~ N(0,Qt) Mobile Robotics Spring 2015 Kalman: Measurements zt = Htxt + vt

zt = measurements Ht = Transformation matrix vt = observational noise vt ~ N(0, Rt) Mobile Robotics Spring 2015 Kalman: Belief Gaussian Distribution x0 = initial mean P0 = Variance

N(x0, P0) Mobile Robotics Spring 2015 Kalman Example State : Mobile Robotics

Spring 2015 xt vt Action: ft/m Kalman Update State

: xt vt Action: ft/m xt = xt-1 + vt-1t + 0.5 (ft/m) * t2

vt = vt-1 + (ft/m)t x't v't Mobile Robotics Spring 2015 = 1 t 0 1

xt-1 vt-1 + t2/2 t ft/m

Kalman: Elapse Time Update x't = Ftxt-1 + Btut P't = FtPt-1 FTt + Qt Mobile Robotics Spring 2015 Kalman: Measurement Update Kalman Gain (how much to correct estimate) Kt = P'tHTt (HtP'tHTt + Rt)-1 New Belief:

xt = x't + Kt(zt-Htx't) Pt = P't - KtHtP't Mobile Robotics Spring 2015 Simple Kalman Filter Example xt = just position (meters) ut = 1 m/s wt ~ N(0,0.1) vt ~ N(0,1.0)

x0 = 4.5 P2 = 2.0 x't = xt-1 + ut + wt (F=1, B=1) zt = xt + vt (H=1) Mobile Robotics Spring 2015 t=0 x0=4.5 P2=2

Mobile Robotics Spring 2015 t=1 x1 = x0 + 1 = 5.5 P'1 = F0P0 FT0 + Q0 = 1 * 2 * 1 + 0.1 = 2.1 Mobile Robotics

Spring 2015 t=1 z1=5.673 K1 = P'tHTt (HtP'tHTt + Rt)-1 = (2.1 * 1)/(1*2.1*1+1.0) = .677 x1 = x'1 + K1(z1-H1x'1) = 5.5 + .677 (5.673-1*5.5) = 5.617121 Pt = P't - KtHtP't = 2.1 - .677 * 1 * 2.1

= 0.6783 Mobile Robotics Spring 2015 Kalman Filters Must be linear system Stored as a gaussian Mobile Robotics Spring 2015