optimal transport based filtering with nonlinear state

1/26

Optimal Transport Based Filtering withNonlinear State Equality Constraints

Niladri Das & Dr. Raktim Bhattacharya

Dept. of Aerospace Engineering,Intelligent Systems Research Laboratory,

Texas A&M University,College Station, TX

June 13, 2020

2/26

Table of Contents

1. Introduction to the problem

2. Contributions

3. Filter model

4. Optimal Transport Filtering

5. Problem formulation

6. Constrained Filtering

7. Numerical example

8. Conclusion

3/26

Section 1

1. Introduction to the problem

2. Contributions

3. Filter model

4. Optimal Transport Filtering

5. Problem formulation

6. Constrained Filtering

7. Numerical example

8. Conclusion

4/26

Introduction

We consider the problem of Optimal Transport based Bayesianfiltering (OTF) in presence of state dependent nonlinearequality-constraints.

5/26

Section 2

1. Introduction to the problem

2. Contributions

3. Filter model

4. Optimal Transport Filtering

5. Problem formulation

6. Constrained Filtering

7. Numerical example

8. Conclusion

6/26

Contributions

Our main contribution is in proposing a framework to extend theOTF, where nonlinear state equality constraints are present. Tothe best of our knowledge, there is no prior work on OTF withstate constraints.

7/26

Section 3

1. Introduction to the problem

2. Contributions

3. Filter model

4. Optimal Transport Filtering

5. Problem formulation

6. Constrained Filtering

7. Numerical example

8. Conclusion

8/26

Filter model

The recursion starts from the initial distribution p(x0). In the predictionstep, we evaluate the prior distribution of xk, given y1, · · · ,yk−1 as

p(xk|y1, · · · ,yk−1) =

∫p(xk|xk−1)p(xk−1|y1, · · · ,yk−1)dxk−1 (1)

where p(xk|y1, · · · ,yk−1) is the posterior distribution at kth time step,p(xk|xk−1) encapsulates the state transition model from k − 1 to k, andp(xk−1|y1, · · · ,yk−1) is the posterior state distribution at (k − 1)th timestep.In the next update step, given the new measurement yk at time step k,the updated posterior distribution of the state xk is computed using theBayes rule as

p(xk|y1, · · · ,yk) =p(yk|xk)p(xk|y1, · · · ,yk−1)∫p(yk|xk)p(xk|y1, · · · ,yk−1)dxk

. (2)

9/26

Section 4

1. Introduction to the problem

2. Contributions

3. Filter model

4. Optimal Transport Filtering

5. Problem formulation

6. Constrained Filtering

7. Numerical example

8. Conclusion

10/26

Optimal Transport Filtering I

I Equally weighted prior ensemble at time step k is denoted byX−

k ∈ [x−1,k,x

−2,k, ...,x

−N,k] and the equally weighted posterior

ensemble by X+k ∈ [x+

1,k,x+2,k, ...,x

+N,k].

I The matrix T := [tij ] is a coupling between X−k and X+

k ,

N∑i=1

tij = 1/N,

N∑j=1

tij = wi, and tij ≥ 0; (3)

where wi ∝ likelihood-function(yi,x−i,k).

11/26

Optimal Transport Filtering II

Optimal matrix T is solved by optimizing the following linearprogramming problem,

T ∗ = argminT

N∑i=1

N∑j=1

tijD(x−i,k, x

+j,k) (4)

subjected to conditions in (3), where D(x−i,k,x

+j,k) is a distance metric

between x−i,k and x+

j,k. The x+j,k denotes posterior samples with sample

weights equal to wi. The equally weighted prior and unequally weighted(weighted with wi) posterior have the same sample locations.

Posterior samples:

X+k = X−

k NT (5)

(interpreted as a re-sampling).

12/26

Initial Sample Generation

Based upon the OT filtering step: X+k = X−

k NT we present analternative sampling technique.

Figure 1: Uniform samplesgenerated from an annulus

Figure 2: Histogram generatedfrom Bi-modal distribution

13/26

Section 5

1. Introduction to the problem

2. Contributions

3. Filter model

4. Optimal Transport Filtering

5. Problem formulation

6. Constrained Filtering

7. Numerical example

8. Conclusion

14/26

Problem formulation

Assume that for all k ≥ 1, the state vectors xk satisfies the equalityconstraint

g(xk) = dk (6)

where g : Rn → Rs and dk ∈ Rs are known. The objective of a nonlinearequality constrained Bayesian filtering is to evaluate (5), such that it alsosatisfies (6).

15/26

Section 6

1. Introduction to the problem

2. Contributions

3. Filter model

4. Optimal Transport Filtering

5. Problem formulation

6. Constrained Filtering

7. Numerical example

8. Conclusion

16/26

Constrained Filtering

Common techniques1:

I Nonlinear equality-constrained filtering (NLeq)

I Projected filtering (Proj)

I Measurement-Augmented filtering (MA)

We adapt these techniques for OTF and name them: OTNLeq, OTProj,and OTMA respectively.

We reinforce the nonlinear equality constraints using the concepts ofmeasurement-augmentation and projection feedback together. This isessentially OTNLeq combined with OTMA, which is named asOTNLeqMA.

1Bruno O. Soares Teixeira et al. “Unscented filtering for equality-constrainednonlinear systems”. In: 2008 American Control Conference. IEEE, 2008. doi:10.1109/acc.2008.4586463. url:https://doi.org/10.1109%2Facc.2008.4586463.

17/26

Section 7

1. Introduction to the problem

2. Contributions

3. Filter model

4. Optimal Transport Filtering

5. Problem formulation

6. Constrained Filtering

7. Numerical example

8. Conclusion

18/26

Numerical example

We consider a simple pendulum with a point mass at the end.

I We use a non-minimal co-ordinates x = [x, y] to denote the locationof the point mass on a 2-dimensional plane.

I The positive x-direction point towards the right, while the positivey-direction points downwards.

The kinematics of this pendulum is modeled as:

x =1

L2(−gxy − x(x2 + y2)) (7a)

y =1

L2(gx2 − y(x2 + y2)) (7b)

19/26

Experimental Setup I

I L is the length of the pendulum taken to be 1 meter, and g is thegravity term, which is taken to be 9.8 m/sec2.

I Since the length of the pendulum is fixed we have x(t)2 + y(t)2 = L2

for t ≥ 0 as our state dependent equality constraint.

I We set real initial location of the pendulum point massx0 = [x(0), y(0)] as x0 = [Lcos(30o), Lsin(30o)].

I The samples are generated by sampling uniformly from ±5o aboutthe mean position.

I We assume no process noise in this numerical study.

20/26

Experimental Setup II

We assume that we can measure the location of the point mass usingvisual measurement with measurement noise and that we do not haveaccess to the angular measurements. The measurement model is:

yk = h(xk) + vk, (8)

where h(xk) := Hxk with H =

[1 0 0 00 1 0 0

]. The stochastic noise

variable , vk ∼ N(0,R), where R = diag([0.01 0.01])m2. Themeasurements are available at every discrete time step of ∆t = 0.05 secs.

Constraint — g(xk) = x2k + y2k = 1, where xk and yk denotes the

position estimates of the point mass.

21/26

Results I

-1 -0.5 0 0.5 1

X axis [m]

-1.2

-1

-0.8

-0.6

-0.4

-0.2

0

Y a

xis

[m

]

0 1 2 3 4 5

Time [secs]

-1

-0.5

0

0.5

1

X p

ositio

n [m

]

real

filtered

0 1 2 3 4 5

Time [secs]

0

0.05

0.1

0.15

0.2

0.25

Length

err

or

in the p

endulu

m

[m]

0 1 2 3 4 5

Time [secs]

0

0.01

0.02

0.03

0.04

0.05 E

rror

in X

positio

n [m

]

0 1 2 3 4 5

Time [secs]

0

0.2

0.4

0.6

0.8

1

1.2

Y p

ositio

n [m

]

real

filtered

0 1 2 3 4 5

Time [secs]

0

0.02

0.04

0.06

0.08

0.1

Err

or

in Y

positio

n [m

]

Figure 3: OT filter responses for a simple pendulum without compensatingfor the length constraint, with 10 samples

22/26

Results II

-1 -0.5 0 0.5 1

X axis [m]

-1

-0.8

-0.6

-0.4

-0.2

Y a

xis

[m

]

0 1 2 3 4 5

Time [secs]

0

1

2

3

4

5

6

Length

err

or

in the p

endulu

m

[m]

10-3

0 1 2 3 4 5

Time [secs]

-1

-0.5

0

0.5

1

X p

ositio

n [m

]

real

filtered

0 1 2 3 4 5

Time [secs]

0.2

0.4

0.6

0.8

1

Y p

ositio

n [m

]

real

filtered

0 1 2 3 4 5

Time [secs]

0

0.01

0.02

0.03

0.04

0.05

Err

or

in X

positio

n [m

]

0 1 2 3 4 5

Time [secs]

0

0.02

0.04

0.06

0.08

0.1

Err

or

in Y

positio

n [m

]

Figure 4: OT filter responses for a simple pendulum after compensating forthe length constraint using OTNLeqMA

23/26

Result III

0 0.5 1 1.5 2 2.5

Pendulum Period [=2.0071 secs]

10-10

10-5

100

105

log

10

(Avg. R

MS

E)

OT

OTProj

OTMA

OTNLeq

OTNLeqMA

Figure 5: Average RMS constraint error over 100 Monte Carlo runs

24/26

Section 8

1. Introduction to the problem

2. Contributions

3. Filter model

4. Optimal Transport Filtering

5. Problem formulation

6. Constrained Filtering

7. Numerical example

8. Conclusion

25/26

Conclusion

I We addressed the nonlinear equality-constrained filtering problem fornonlinear systems when OT filtering is used for its state estimation.

I Three existing methodologies for equality-constrained filteringproblems using KF and UKF, were coupled with OT filtering andtheir performances were evaluated using a numerical example.

I We further showed numerically, that the proposed OTNLeqMA filterprovided the least constraint error compared to others.

26/26

Acknowledgment

Research sponsored by Air Force Office of Scientific Research, DynamicData Driven Applications Systems grant FA9550-15-1-0071

Thank You