Kalman predictor and filter

Page 1

Kalman predictor and filter February 4, 2017 1 Problem statement We have a system { x(t + 1) = F x(t) + Gu(t) + v1(t) y(t) = Hx(t) + v2(t) with v1 ∼ W N(0, V1), v2 ∼ W N(0, V2) and E[v1(t1)v2(t2)] = { 0 ∀t1̸= t2 V12 if t1 = t2 . and also the following initial conditions x0 = E[x(0)] P0 = E[(x(0)− x0)(x(0)− x0)T ] 2 Kalman predictor The Kalman 1-step predictor is the following:    ˆx(t + 1|t) = F ˆx(t|t− 1) + K(t)e(t) + Gu(t) e(t) = y(t)− ˆy(t|t− 1) ˆy(t + 1|t) = H ˆx(t + 1|t) K(t) = [ F P(t)H T + V12 ][ HP (t)H T + V2 ]−1 P (t + 1) = F P(t)F T + V1− K(t) [ HP (t)H T + V2 ] K(t)T where P (t) is the covariance of the state prediction error P (t) = E[(x(t)− ˆx(t|t− 1))(x(t)− ˆx(t|t− 1))T ] The Kalman k-step predictor is the following: { ˆx(t + k|t) = F k−1ˆx(t + 1|t) Pk(t + k) = F Pk−1(t + k− 1)F T + V1 1 Figure 1: Block diagram of the Kalman predictor 3 Kalman filter The Kalman filter is the following: { ˆx(t|t) = F −1ˆx(t + 1|t) if V12 = 0 and F is invertible ˆx(t|t) = ˆx(t|t− 1) + KD(t)e(t) otherwise with KD(t) = P (t)H T[ HP (t)H T + V2 ]−1 The covariance of the filter is the following: PF (t) = P (t)− KD(t) [ HP (t)H T + V2 ] KD(t)T where P (t) is the covariance of the state prediction error of the Kalman predictor 4 Convergence theorems Theorem (I theorem of convergence). If V12 = 0 and F is stable, then • ∀P (0)≥ 0, P(t)→ ¯P , where ¯P is the solution of the algebraic Riccati equation 2 • K(t)→ ¯K where ¯K is the gain of the steady state Kalman predictor F− H ¯K, which is stable Theorem (II theorem of convergence). If V12 = 0 and (F, H) is observable and (F, Gv) is reachable, then • ∀P (0) > 0, P(t)→ ¯P , where ¯P is the solution of the algebraic Riccati equation • K(t)→ ¯K where ¯K is the gain of the steady state Kalman predictor F− H ¯K, which is…