Cost Analysis of Square Root Information Filtering and Smoothing with a Mixed Real-Integer State

Transcription

1 Cost Analysis of Square Root Information Filtering and Smoothing with a Mixed Real-Integer State December 8, 013 The following analysis provides an analysis of the transformation undergone by the maximum a posteriori cost function as a priori information, process noise, and measurements are ingested by a square root information lter and smoother with a mixed real-integer state. The analysis supports claims made in [1]. 1 Filtering Analysis The initial cost function at time i, embedded with square root information equations that encode the a priori information about the real-valued state and the process noise and with square root information equations relating the new measurements to the real- and integer- valued state can be written as follows: R xxi x i z xi A priori real-valued state information R nni n i z ni A priori integer-valued state information R ww w k 0 H xk x k H nk n k z k k=i A priori process noise information k=i } {{ } Measurements Note that the nonhomogeneous measurement term for the a priori process noise is set to 0 since the process noise is assumed to be zero-mean. Stacking the a priori terms at time i we can rewrite the cost as: R ww 0 0 w i 0 0 R xxi 0 x i z xi n i z ni k=i1 R nni H xk x k H nk n k z k k=i1 To form the big block matrix above, we used the following identity: ] a b = a [ b R ww w k (1) 1

2 x i and n i can be written in terms of x i1 and n i1 according to their respective dynamics models: x i = 1 [x i1 w i ] Φ { n i = n i1 (1 : m) if a new integer becomes present between time i and i 1 n i1 otherwise wherem = numel(n i1 ) 1. Substituting x i with x i1 and n i with n i1 into the above cost function, we arrive at: R ww 0 [0 0] R xxi Φ 1 R xxi Φ 1 [R xni 0] 0 0 [R nni 0] k=i1 H xk x k H nk n k z k k=i1 w i x i1 n i1 0 z xi z ni R ww w k where [x 0] represents the augmentation of the square-root information matrices with a column of 0 s to represent the introduction of a new integer between time i and i 1. Zeros are used to indicate that no knowledge is known about this new integer. By QR factorizing the big block matrix and left multiplying the insides of the rst term by Q T, one can propagate the state elements forward to i 1 and arrive at the following equivalent cost function: R wwi R wx,i1 R wn,i1 w i z wi 0 R xx,i1 R xn,i1 x i1 z x,i1 0 0 R nn,i1 n i1 z n,i1 k=i1 H xk x k H nk n k z k k=i1 R ww w k Next, by unpacking the process noise data equation from the big block matrix (see (1)) and replacing

3 it with the latest measurement at time i 1, one can rewrite the cost as: R xx,i1 R xn,i1 [ ] 0 R nn,i1 xi1 z x,i1 z n n,i1 H i1 H i1 n,i1 z i1 Rwwi w i R wx,i1 x k1 R wn,i1 n i1 z wi H xk x k H nk n k z k R ww w k k=i Next QR factorize this new big matrix and left multiply by Q T to perform a measurement update to get : R xx,i1 R xn,i1 [ ] 0 R nn,i1 xi1 z x,i1 z n n,i1 0 0 i1 z r,i1 Rwwi w i R wx,i1 x k1 R wn,i1 n i1 z wi H xk x k H nk n k z k R ww w k k=i The cost function can now be unpacked from its big block matrix form and written as follows: R xx,i1 x i1 R xn,i1 n i1 z x,i1 R nn,i1 n i1 z n,i1 z r,i1 Rwwi w i R wx,i1 x i1 R wn,i1 n i1 z w,i H k x k H nk n k z k R ww w k k=i k=i1 The above steps can be reiterated for i i 1. After incorporating all measurements up to time i = K, the cost function can be written as: R xx,k x K R xn,k n K z x,k R nn,k n K z n,k Rwwk w k R wx,k1 x k1 R wn,k1 n k1 z w,k z r,k = 0 R nn,k ˆn K z n,k 0 z r,k when minimized over n K, x k 's and w k 's () 3

4 Smoothing Analysis Next, we can derive the above mixed integer-real cost function after smoothing. We rst dene the smoother's initial nonhomogeneous term z x,k, initial square-root information matrix R xx,k, and the integer-valued vector estimate ˆn K at time K. The new quantities are derived from the variables output after running the lter: z x,k = z x,k R xn,k ˆn K R xx,k = R xx,k ˆn K = min n i Z K R nn,k n i z n,k These variables can be substituted into the cost function in () to arrive at: R xx,k x K z x,k R nn,k ˆn K z n,k Rwwk w k R wx,k1 x k1 R wn,k1ˆn k1 z w,k z r,k We can use the following dynamics equation to eliminate x k in favor of x and ˆn K in favor of ˆn : (3) x k = Φx w {[ ] ˆn ˆn ik ˆn K = ˆn if a new integer becomes present between time K 1 and K otherwise where ˆn ik is the last component of ˆn K, the integer that arose between time K 1 and K, if there was one. Stacking the rst term and part of the third term of (3) and applying these substitutions, we arrive at [ ] [ ] [ ] Rww, R wx,k R wx,k Φ w zw, R wn,k ˆn K x K R xx,k R xx,kφ z x,k Rww,k w k R wx,k1 x k1 z w,k Rnn,K ˆn K z n,k z r,k After QR factorization and left multiplying, we propagate the smoother from time K to K 1: [ R ww, R ] [ ] [ ] wx, w z 0 R w, xx, x z x, K R ww,k w k R wx,k1 x k1 R wn,k1ˆn k1 z w,k R nn,k ˆn K z n,k z r,k 4

5 Unpacking the cost function, decrementing i by 1, we can repeat the last two steps of subsitution and QR factorization until i = 0 and the cost function looks as follows: R xx,0 x 0 z x,0 R nn,k ˆn K z n,k R ww,k w k R wx,kx k z w,k z r,k = 0 0 R nn,k ˆn K z n,k z r,k when minimized over x k 's and w k 's From this, it is clear that the minimum cost for both the lter and the smoother are the same, however the minimizing values of x k s and w ks will be dierent. The only dierence is that the dynamics are enforced going backward. A good analogy here is that between the ltering and smoothing stage, the minimization landscape is changing, however, the minimum value remains the same. References [1] K. M. Pesyna Jr., Z. M. Kassas, R. W. Heath Jr., and T. E. Humphreys, A phase-reconstruction technique for low-power centimeter-accurate mobile positioning, IEEE Transactions on Signal Processing, 013, submitted for review. 5