The Nonlinear Projection Filter

Submitted to the Journal of Guidance, Control, and Dynamics

The Nonlinear Projection Filter Randal Beard Jacob Gunther Jonathan Lawton Wynn Stirling Department of Electrical and Computer Engineering Brigham Young University Provo, Utah 84602 Abstract The conditional probability density function of the state of a stochastic dynamic system represents the complete solution to the nonlinear ltering problem because, with the conditional density in hand, all estimates of the state, optimal or otherwise, can be computed. It is well known that, for systems with continuous dynamics, the conditional density evolves, between measurements, according to Kolmogorov's forward equation. At a measurement, it is updated according to Bayes formula. Therefore, these two equations can be viewed as the dynamic equations of the conditional density and, hence, the exact nonlinear lter. In this paper, Galerkin's method is used to approximate the nonlinear lter by solving for the entire conditional density. Using an FFT to approximate 

Corresponding Author, email:

[email protected].

1

the projections required in Galerkin's method leads to a computationally ecient nonlinear lter. The implementation details are given and performance is assessed through simulations.

Nomenclature (A)ij The (i; j )th element of the matrix A. FFT[f ( )] The fast Fourier transform of a function f at N uniform samples between the limits a and b.

I The identity matrix. IFFT[f ( )] The inverse fast Fourier transform of a function f at N uniform samples between the limits a and b.

p(t; xjYt) The conditional density of the state of the state of the system, given the measurement sequence Yt = fyk : tk 

tg pN (t; xjYt) The Galerkin approximation of p(t; xjYt ). p^ The truncation of p(t; xjYt) to the closed and bounded set .

p(yk+1jx)

T ?1

e? 2 [yk ?h(x;tk )] Rk [yk ?h(x;tk )] (2)m jRk j 1

p

.

p(t?k+1; xjYtk ) The solution of the Kolmogorov equation at time t?k+1.

0, and any  > 0, there exists a K such that for N > K , kp(t; xjYt ) ? pN (t; xjYt)kL <  for all t < t^. 2

The proof to this theorem is made transparent by the following three lemmas.

Lemma 4.2 Under the hypothesis of Theorem 4.1, for any  > 0, there exists a positive integer K and a k > 0 such that if N > K and kbN (tk ) ? c(tk )k < k , then kbN (t) ? c(t)k < , for all t 2 [tk ; t?k+1). Proof: Since the dierential equation _ = ?AN  is globally Lipschitz with

Lipschitz constant kAN k, the theorem on the continuous dependence of ordinary dierential equations on initial conditions and bounded disturbances,21 14

and equation (15), imply that

kbN (t) ? cN (t)k  k ekAN k(t?tk ) + kAN k (ekAN k(t?tk ) ? 1): N

(16)

The rst term can be made less than =2 by making ? k < 2 e?kAN k(tk

+1

?tk ) :

The hypothesis of Theorem 4.1 implies that there exist a K such that N > K implies that the second term is less than =2. Q.E.D.

Lemma 4.3 Under the hypothesis of Theorem 4.1, for any  > 0, there exists

a positive integer K and a k?+1 > 0 such that if N > K and

bN (t?k+1 ) ? cN (t?k+1 )

< k?+1, then kbN (tk+1) ? cN (tk+1)k < . Proof: Equation (11) shows that ? ) ?1 c(tk+1) = M(y(y)kT+1c)(ct?(tk+1 ) : k+1

k+1

(17)

If we truncate equation (4) to and integrate we obtain ?1 (yk+1)b(t? ) + 1N M + 3N ; b(tk+1) = (y )T b(t? k+1 k+1 k+1 ) + 2N

where

1N = 2N = 3N =

Z X 1

`=N

Z X 1

`=N

Z X 1

`=N

b`p(yk+1j )`( )N ( )d; b`p(yk+1j )`( )d; b`( )d: 15

(18)

Since the Fourier series converges, k1N k, j2N j and k3N k can be made less than any arbitrary positive number for N suciently large. Subtracting equation (18) from equation (17) we obtain

kbN (tk+1) ? cN (tk+1)k

?1

M b (t?k+1) M?1 c(t?k+1)



T b(t? ) +  ? T c(t? )

k+1

k+1 2N

+ k3N k : +

T ?1N  b(t ) +  k+1

2N

De ne

 =4 T b(t? 1 ) +  k+1

N =4 T c(1t? ) : k+1

2N

Using this notation we have the following sequence of inequalities:

?1

M b(t?k+1) ? M?1 c(t?k+1)

T b(t?k+1) + 2N T c(t?k+1 )

=

M?1b(t?k+1) ? N M?1c(t?k+1)





M?1 



b(t?k+1) ? N c(t?k+1)



=

M?1 



(b(t?k+1) ? c(t?k+1))

+ ( ? N )c(t?k+1)

h



M?1 

jj

b(t?k+1) ? c(t?k+1)

i

+ j ? N j

c(t?k+1)

:

Since p(yk+1jx) is a Gaussian density we know that kM?1 (yk+1)k = B1 < 1 16



and jj = B2 < 1. We also know that

c(t?k+1)

= B3 < 1, where B1 , B2 and B3 are nite numbers. Also note that

1 1 j ? N j = T b(t? ) +  ? T c(t? ) 2N k+1 k+1







(T b(t? ) +  )(T c(t? ))

2N k+1 k+1

?


bN (tk+1) ? cN (t?k+1)

 2N : + T ? ? T ( b(tk+1) + 2N )( c(tk+1))

The rst quantity is bounded above by a nite number B4 , and the second quantity is a sequence, say 4N that converges in N . Therefore



j ? N j  B2

bN (t?k+1) ? cN (t?k+1)

+ 4N : Collecting the above results we get that

kbN (tk+1) ? cN (tk+1)k

h

 B1 B2

bN (t?k+1) ? cN (t?k+1)



+ (B4

bN (t?k+1) ? cN (t?k+1)

+ 4N )B2

i + 3N + 5N ;

where



 1N

5N =

T b(t? ) + 

: 2N k+1

The lemma follows from this formula. Q.E.D. 17

Lemma 4.4 If at a particular instant of time t, kbN (t) ? cN (t)k ! 0 as N ! 1, then

kp^ (t; xjYt) ? pN (t; xjYt)kL ! 0: 2

Proof:

Z

kp^ ? pN k2L = jp^ ? pN j2 dx 2

Z

 j(bN ? cN )N j2 dx X 2 1 + b` ` dx

`=N Z

=(bN ? cN )T M(bN ? cN )

2 Z X 1 + b` ` dx:

`=N

By the mean value theorem of calculus, there exists a  2 such that

X 2 1 2 kp^ ? pN L2  kbN ? cN k + ( ) b` `( ) ; `=N

k2

where ( ) is the Lebesgue measure of . Since the second term on the right hand side is the tail of a convergent Fourier series, it converges to zero. The lemma then follows from the hypothesis on kbN ? cN k. Q.E.D. Proof of Theorem 4.1: First note that

kp ? pN kL  kp ? p^ kL + kp^ ? pN kL : 2

2

2

Since an appropriate choice of guarantees that kp ? p^ kL < =2 for all t < t^, 2

18

we need to show that kp^ ? pN kL < =2 for all t < t^. Given tk < t^  tk+1 and , Lemma 4.4 indicates that there exists a ^



such that if

bN (t^) ? cN (t^)

< ^, then

p(t^; xjYt^) ? pN (t^; xjYt^)

< =2. 2

L2

From Lemma 4.2, there exists a Kk and a k such that if N > Kk and

kbN (tk ) ? c(tk )k < k then

bN (t^) ? cN (t^)

< ^. From Lemma 4.3, there



exist a Kk? and a k? such that if N > Kk? and

bN (t?k ) ? cN (t?k )

< k? then

kbN (tk ) ? c(tk )k < k . Another application of Lemma 4.2 implies that there exist a Kk?1 and a k?1 such that if N > Kk?1 and if kbN (tk?1) ? c(tk?1)k <

k?1 then

bN (t?k ) ? cN (t?k )

< k?. The above argument is repeated k times, resulting in the requirement that

kbN (0) ? cN (0)k < 0 . If we assume that the basis functions are orthonormal, then at time t = 0 there is zero approximation error since

p^ (0; xjY0) = p0(x) and

Z

pN (0; tjY0)N dx =

Z

p0 (x)N dx

implies that

kbN (0) ? cN (0)k = 0: Therefore letting

K = 0max fK ; K ?g `k ` ` 19

proves the theorem. Q.E.D.

Remark 1. The bound derived in the above proof is given qualitatively by Figure 1.

Remark 2. The requirement in the hypothesis of Theorem 4.1 that (N = kAN k)ekAk ! 0 is somewhat unsatisfactory since it is not clear at this stage how to guarantee that this is true a priori. The requirement, however, can be tested for any given system. Further research could focus on deriving conditions under which this hypothesis holds.

Remark 3. The convergence proof given in this section does not give explicit bounds on the approximation error. In other words, given a desired , we cannot say what K must be to guarantee that an approximation error of  is achieved. We can only say that such a K does in fact exist.

Remark 4. The convergence proof guarantees a small approximation error for any nite time. The arguments given in this section do not say anything about the steady state error.

5 Fourier Basis In this section we show how to implement Equations (10) and (11) using a Fourier basis and the FFT algorithm. Since the general n dimensional case is notationally messy and the basic ideas are contained in one dimension, we will restrict ourselves to this case. 20

Suppose we choose = [a; b] and a Fourier basis

f `(x); !`(x)g

N ?1 `=0

!

(

= p 1 cos b2?`a (x ? a) ; p 1 sin b2?`a (x ? a) b?a b?a

Note that the complex exponential basis is ` =

`

!)N ?1

+ j!`. If a` and b` are the

Fourier coecients, and c` = a` ? jb`, then

pN (x; tjYt) =

X

N ?1

a (t) `(x) + b`(t)!`(x)

` `=0 N ?1

= 21

X

c`(t)`(x) + c` (t)` (x) (`=0NX ) ?1 =< c`(t)`(x) : `=0

We will therefore work with a complex exponential basis function, taking the real part when we require an output of the lter. The FFT algorithm can be used to approximate the inner product of two complex functions of x as follows:

Zb a

(x) (x)dx 

p

b ? a FFT [( )] ; k N

k

(19)

where the input to the FFT algorithm (denoted by  ) is N uniform samples of

 over the interval [a; b]. Therefore, the FFT returns N weighted integrals of . Using Equation (19), we can use the FFT to approximate Equations (7){(9) and (12){(13) as

M =I p

! ! # " b ? a 2 ` @f [A1(t)]` = N FFT j b ? a f ( ) + @x ( ) `( ) 21

`=0

:

p

" 2 [`G2 ] ! # b ? a @ Q [A2(t)]` = N FFT 2 @x2 ( ) p s = bN? a FFT [p(t0 ;  )] p [k ]` = b ? a FFT [p(yk j )`( )] N  (yk ) =

p

b ? a IFFT [p(yk j )] :

The IFFT is used in the last equation since Equation (13) is an inner product with  instead of  (inner product is de ned with a conjugation). These equations show that all of the quantities needed to propagate c(t) between measurements and to update it at a measurement can be computed quickly using the FFT. Furthermore, (yk ) as computed above is a real symmetric Toeplitz matrix. Therefore, we only need to compute its rst column. Furthermore, because of the structure of (yk ), the product (yk )c is a convolution and can be implemented in O(N log N ) operations rather than O(N 2) for the full matrix vector product. Hence, the measurement update is an O(N log N ) operation. As the output of our lter we may wish to compute the conditional mean, the conditional covariance or the density function itself. These quantities can also be computed eciently using the FFT as shown below. The Conditional Mean:

By de nition the conditional mean of the state

at time t based on observations up to and including time   t is

Z

x = p(; tjY )d  t

22

We may compute the mean of our approximate conditional density as follows

Z x^t = pN (; tjY )d ) Z (NX ?1 = < c`(t)`( ) d `=0 (NX ) Z ?1  =< c`(t) x` dx

`=0

=