Kalman Filter Example

Rudolf E. Kalman b. 1930 Hungary Kalman Filter NASA Ames 1960 National Medal of Science (2009)

Actions and Observations Through Time Belief(xt) (using all evidence to date) Get Measurement Elapse Time Belief'(xt) (without latest evidence)

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

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

Initial Estimate

Elapse Time

Get Measurement

New Belief

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)

Kalman: Measurements zt = Htxt + vt zt = measurements Ht = Transformation matrix vt = observational noise vt ~ N(0, Rt)

Kalman: Belief Gaussian Distribution x0 = initial mean P0 = Variance N(x0, P0)

Kalman Example Action: ft/m State: xt vt

Kalman Update State: Action: xt = xt-1 + vt-1△t + 0.5 (ft/m) * △t2 vt = vt-1 + (ft/m)△t x't v't = + 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

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

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)

t=0 x0=4.5 P2=2

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

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

Kalman Filters Must be linear system Stored as a gaussian