The Kalman Filter. x k+1 F k x k.

Transcription

1 The Kalman Filter The RLS algorithm for updating the least squares estimate given a series of observations vectors looked like a filter : new data comes in, and we use it (along with collected knowledge of the old data) to produce a new output In the previous section, x was fixed for the entire sequence of observations The Kalman filter incorporates dynamics for the unknown vector x into our estimation framework It addresses the case where x changes from observation to observation in a manner which we can model The classic example is trying to estimate the position and velocity of an airplane If the plane is in motion, these will of course change over time But the path of the plane is somewhat predictable, and there are real physical constraints on how quickly it can turn and/or accelerate Our dynamical models will be linear That is, we will assume that we can approximate the solution at the next time step x k+1 by applying a known N N matrix F k to the current solution: x k+1 F k x k Here is a quick example of how this might work Suppose that we are trying to estimate the position p and velocity v of a particle traveling in one dimension: 54

2 p In this case, our unknowns are a two-vector: [ ] pk x k v k v If we expect the velocity to be almost the same from measurement to measurement, our dynamics equations would look like p k+1 p k + α k v k v k+1 v k (+ error) (+ error), where α k would be proportional to the time elapsed between k + 1 and k In terms of the x k, we can write [ ] 1 αk x k+1 F k x k, where F k 1 Setting up the system As before, we are making noisy observations of an unknown vector: y k A k x k + e k, e k measurement error There can be a different number of measurements at each step, but all of the x k have length N We will say that y k R M k and the A k are M k N matrices The x k are dynamic; we model the transition from step k to k + 1 using x k+1 F k x k + ɛ k, ɛ k state error 55

3 The F k are all N N It is possible to incorporate the correlation structure of the e k and ɛ k into our estimates (using the BLUE), but we will start with the standard least-squares framework Notation: Since the x k are linked together through the dynamical model, the measurements y k A k x k + e k also give us indirect information about x,, x k As such, our estimate of all of x,, x k will change at time k We will use the notation ˆx l k least-squares estimate of x l at time k Step k We have observed y A x + e We form the least-squares estimate ˆx (A T A ) A T y Step k 1 We have the following system of equations y A x + e F x x 1 + ɛ y 1 A 1 x 1 + e 1 We can write this compactly as y A 1 x + e 1, where y A [ ] x e F I x 1 + ɛ y }{{ 1 A 1 e 1 }}{{}}{{}}{{} y 1 A 1 x 1 e 1 56

4 This system is (M + N + M 1 ) 2N; we can form the least-squares estimate using ] [ˆx 1 (A ˆx T 1 A 1 ) A T 1 y Step k 2 Now we have the following system of equations y A x + e F x x 1 + ɛ y 1 A 1 x 1 + e 1 F 1 x 1 x 2 + ɛ 1 y 2 A 2 x 2 + e 2, which we can rewrite as y A e F I x 1 ɛ y 1 A 1 x 2 + e 1 F 1 I ɛ 1 y }{{ 2 } } {{ A 2 }}{{} e 2 }{{} y 2 A 2 x 2 e 2 This system is (M + M 1 + M 2 + 2N) 3N; we can form the leastsquares estimate using ˆx 2 ˆx 1 2 ˆx 2 2 (A T 2 A 2 ) A T 2 y 2 57

5 In general, we have A k A F I A 1 F 1 F k A k y y 1, y k ; (1) I this system is (M + + M k + kn) kn, and we can for the least-squares estimate using ˆx k ˆx 1 k ˆx k k The kn-vector A T k y k is (AT k A k ) A T k y k A T k y k A T y A T 1 y 1, A T k y k while the kn kn matrix A T k A k we have to invert looks like A T A + F T F F T F I + A T 1 A 1 + F T 1 F 1 F T 1 F 1 I + A T 2 A 2 + F T 2 F 2 F T 2 I + A T ka k + F T kf k y k F k (2) Notice that this matrix is block tridiagonal; this structure is precisely what we will exploit in deriving the efficient update equations later on F T k I + A T k A k 58

6 Example: Multiple measurements of a pulse The following example illustrates the essential different between the standard least-square framework and the Kalman filter We make three measurements of a patient s pulse, and want an (updated) estimate of its true value after each one Our measurements are scalars, and have the form: y x + noise We record the three values y, y 1, y 2 In the standard least-square framework, we solve the three problems [ ] [ [ ] y 1 e y 1 x +e, x y 1 1] y 1 e +, and y e 1 1 x + e If we call ˆx k the least-squares estimate after recording y k, a simple calculation shows that the estimate is simply the average of the measurements we have seen to date: ˆx y, ˆx 1 y + y 1 2 y 2, ˆx 2 y + y 1 + y 2 3 e 2 Now suppose the we use the Kalman filtering framework to explicitly recognize that the pulse can drift over time Our dynamical model is simple: x k+1 x k + ɛ k This is saying that our best guess for the pulse at the next time step is that it takes the same value as before, but we are incorporating the fact that it will in fact not be exactly the same as this best guess 59

7 In the Kalman framework, the three systems we solve look like y 1 x + e, y y 1 y y 1 y 2 1 [ ] 1 x + x x x 1 x 2 e ɛ e 1, + e ɛ e 1 ɛ 1 e 2 After the first measurement, our estimate for x is the same: ˆx y After the second measurement, we have ] [ˆx 1 ˆx 1 1 ] [ 1 [ ] y y 1 ([ ]) [ ] 2 y 2 y [ ] [ ] 1 2/3 1/3 y 1/3 2/3 y 1 ] [ 2y +y 1 3 y +2y 1 3 6

8 So the best guess for where the pulse was is now a weighted average between the two samples; the estimate for x 1 weighs the most recent measurement more heavily than the first one This is natural, since we are saying the pulse is changing between measurements After the third measurement, we have ˆx 2 ˆx 1 2 ˆx y +2y 1 +y 2 8 y +2y 1 +y 2 4 y +2y 1 +5y 2 8 y y 1 y 2 Again, we see that as a direct result of allowing the pulse to drift, recent measurements are weighed more heavily in the current estimate 61

9 Block tridiagonal factorization To find the least-squares estimate of x,, x N, we need to solve the kn kn system given by (2) a few pages back In this section, we will show how this this type of system allows a nice factorization which makes it clear how to solve it in a streaming manner Consider the following general symmetric block tridiagonal system: D C T x b C D 1 C T 1 x 1 b 1 C 1 D 2 C T k x k b k C k D k x k b k The system is (k + 1)N (k + 1)N, and each of the D i and C i are N N Likewise, x i R N and b i R N The matrix on the left-hand side above as the product of block lower triangular and block upper triangular matrices: Q I U C Q 1 I U 1 C 1 Q 2, U k C k Q k I where the C i are the same as in the original system, and the Q i and U i are also all N N By inspecting the blocks, we see that we can compute the Q i and U i using the following iterative algorithm: Q D for i 1, 2,, k 62

10 end U i Q ic T i Q i D i C i U i, meaning that Q i D i C i Q ic T i If the factorization is already in place, we can solve for the x i by using forward substitution followed by back substitution First, we solve Q w b C Q 1 w 1 C 1 Q 2 b 1 C k Q k w k b k working from the top down: w Q b for i 1, 2,, k w i Q i (b i C i w i ) end With the w i in hand, we can now solve I U I U 1 U k I x x 1 x k w w 1 w k for the x i by working bottom up: x k w k for i k 1, k 2,, 63

11 x i w i U i x i+1 end Of course, we can combine the factorization with the forward step of the solve above to yield the following algorithm for solving symmetric 1 block tridiagonal systems: Symmetric Block Tridiagonal Solve Input: N N matrices D,, D k and C,, C k ; Right-hand side N-vectors b,, b k Initialize: Q D w Q b for i 1, 2,, k do U i Q ic T i Q i D i C i U i w i Q i (b i C i w i ) end for x k w k for i k 1, k 2,, do x i w i U i x i+1 end for 1 This is easily adapted to non-symmetric block tridiagonal, but we only need the symmetric case for what we do below 64

12 Kalman filter update equations From (2) previously in this notes, we can find the least-squares solution ˆx k, ˆx 1 k,, ˆx k k at step k using the tridiagonal solver from the previous section with C i F i, i,, k 1 A T A + F T F, i D i I + A T i A i + F T i F i, i 1,, k 1 I + A T k A k, i k That is great, but what is even better is that we can quickly move from the current best estimate ˆx k k to the estimate at the next time step ˆx k+1 k+1 very quickly Suppose that we have in our hands the solution to (2), consisting of the estimates ˆx k, ˆx 1 k,, ˆx k k (3) When the signal evolves (x k+1 F k x k + ɛ k ) and new measurements come in (x k+1 A k+1 x k+1 + e k+1 ), we break the update into two stages The first is the predict stage, where we add the evolution equations we solve, Ã k+1 A F I A 1 F 1 F k y y 1, ỹ k+1 I A k y k F k I 65

13 This system has another block column, but since the last block column in non-zero only in the k + 1 block, the x k+1 variables are essentially decoupled The solution to ÃT k+1ãk+1 x ÃT k+1ỹk+1 will be exactly the same as before, ie (3), but with an additional component given by ˆx k+1 k F k ˆx k k We can interpret this as a prediction for ˆx k+1 k+1 which we will update when we incorporate the measuremets y k+1 The factorization is updated with D k D k + F T k F k I + A T k A T k + F T k F k C k F k D k+1 I How exactly this works is thoroughly detailed in the Technical Details section below In the second stage, the update stage, we incorporate the measurements and solve A y F I A 1 y 1 F 1 A k+1, y F k I k+1 A k y k F k I A k+1 y k+1 This update works by changing the last block diagonal element to D k+1 D k+1 + A T k+1a k+1 I + A T k+1a k+1 66

14 we can then quickly compute ˆx k+1 k+1 w k+1 as ˆx k+1 k+1 ˆx k+1 k + G k+1 (y k+1 A k+1 ˆx k+1 k ), where G k+1 is the Kalman gain matrix (this is computed below) Notice that if the measurement y k+1 match our prediction ˆx k+1 k, then we don t update this prediction at all otherwise we pass the difference through the gain matrix G k+1 and add it to ˆx k+1 k Here are the updating equations, which are carefully derived in the Technical Details section: Kalman Filter Initialize: ˆx (A T A ) A T y P (A T A ) for k, 1, 2, do State update extrapolation: ˆx k+1 k F k ˆx k k Info matrix extrapolation: P k+1 k F k P k k F T k + I Kalman gain: G k+1 P k+1 k A T k+1(a k+1 P k+1 k A T k+1 + I) State update: ˆx k+1 k+1 ˆx k+1 k + G k+1 (y k+1 A k+1 ˆx k+1 k ) Info matrix update: P k+1 k+1 (I G k+1 A k+1 )P k+1 k end for So the cost of moving from ˆx k k to ˆx k+1 k+1 is O(N 3 ) + O(M k+1 N 2 ), which is about the same as it would cost to simply compute the standard least-squares solution for x k+1 using only the measurements y k+1 67

15 Kalman Filter and the BLUE Just as with recursive least squares, we can incorporate covariance information about the measurement and state errors into the Kalman filter Suppose the measurement errors e i have covariance matrices R i E[e i e T i ] and the state errors ɛ i have covariances matrices S i E[ɛ i ɛ T i ] In addition, we need to assume that from time step to time step, all the errors are uncorrelated with one another: E[e i e T j ], E[ɛ i ɛ T j ], i j i j E[e i ɛ T j ], for all i, j Following the derivation above using the BLUE in place of standard least square results in two changes to the update equations: Info matrix extrapolation : P k+1 k F k P k k F T k + S k Kalman gain : P k+1 k A T k+1(a k+1 P k+1 k A T k+1 + R k ) 68

16 Kalman Filter Updating: Technical Details For the first stage of the update, the predict stage, the system of equations moves from A k in (1) to à k+1 A F I A 1 F 1 F k y y 1, ỹ k+1 I A k y k F k I The least-squares solution to this system will be unchanged, except for an additional term in the k + 1st block, which we call ˆx k+1 k With our previous factorization in hand, we have P k k : Q k computed already The addition of the F k above changes the D k in our block tridiagonal system to D k D k + F T k F k, and so the factorization is updated with Q k Q k + F T k F k w k Q k (b k + F k w k ) This new system also has k + 1 blocks, so we need to compute the new terms in our running factorization: U k, Q k+1, and w k+1 69

17 By the matrix inversion lemma Q k Q k Q k F T k (I + F k Q F T k ) F k Q k k, so we can rewrite w k as w k w k Q k F T k (I + F k Q k F T k ) F k w k We now have C k F k and D k+1 I, and b k+1, so we move forward with the running factorization by solving U k Q k F T k Q k+1 I F Q k k w k+1 F T k Q k+1f k w k Q k+1(i F k Q k F T k (I + F k Q k F T k ) )F k w k Q k+1(i + F k Q k F T k ) F k w k (4) F k w k (5) where the second to last step (4) follows from the identity for symmetric matrices I A(I + A) (I + A), (6) (just multiply both sides on the right by (I + A)), and the last step (5) follows from an application of the matrix inversion lemma (I + F k Q k F T k ) I F k (Q k + F T k F k ) F T k I F Q k k F k Q k+1 7

18 Since w k+1 ˆx k+1 k and w k ˆx k k, we can rewrite the conclusion above as ˆx k+1 k F k ˆx k k We will also denote P k+1 k : Q k+1 I + F k Q k F k I + F k P k k F k We now go to the second stage of the update, where we add the measurements y k+1 A k+1 x x+1 + e k+1 We add a block row to our system of equations; we now want to solve A k+1 A F I A 1 F 1 F k, y I k+1 A k F k I A k+1 y y 1 y k y k+1 This means that we are replacing D k+1 I with D k+1 I + A T k+1a k+1 and b k+1 with b k+1 A T k+1y k+1 We update the factor-and-solve as follows: D k+1 I + A T k+1a k+1 Q k+1 Q k+1 + A T k+1a k+1 w k+1 Q k+1(a T k+1y k+1 + F k w k ) (7) 71

19 Using the matrix-inversion lemma, we can write Q k+1 Q k+1 Q k+1a T k+1(i + A k+1 Q k+1a T k+1) A k+1 Q k+1, (8) and so using the fact from above that w k+1 Q k+1f k w k Q k+1 ˆx k+1 k, the two terms in the expression for w k+1 ˆx k+1 k+1 becomes Q k+1f k w k ˆx k+1 k Q k+1a T k+1(i + A k+1 Q k+1a T k+1) A k+1 ˆx k+1 k (9) and Q k+1a T k+1y k+1 Q k+1a T k+1 (I (I + A k+1 Q Q k+1a T k+1 k+1a T k+1) A Q k+1 k+1a T k+1 ) y k+1 (I + A k+1 Q k+1a T k+1) yk+1, (1) where we have again used the identity (6) Combining (9) and (1) with (7), we have where w k+1 ˆx k+1 k+1 ˆx k+1 k + G k+1 ( yk+1 A k+1 ˆx k+1 k ), ) G k+1 Q k+1a T k+1 (I + A Q k+1 k+1a T k+1 ( P k+1 k A T k+1 I + A k+1 P k+1 k Ak+1) T Finally, (8) above also gives us the update P k+1 k+1 : Q k+1 (I G k+1 A k+1 ) P k+1 k 72