Imprecise Filtering for Spacecraft Navigation

Transcription

1 Imprecise Filtering for Spacecraft Navigation Tathagata Basu Cristian Greco Thomas Krak Durham University Strathclyde University Ghent University

2 Filtering for Spacecraft Navigation

3 The General Problem Dynamical system with state x t X, t 0 For simple orbital mechanics: X [0, 1] 6 General statement given by (stochastic) differential equation d x t d t = f (x t, t θ) + }{{} system dynamics d W t d t }{{} Brownian motion noise with stochastic initial condition x 0 P(X 0 θ). σ(x t, t θ) }{{} diffusion coefficient Collect all parameters in θ For given θ induces a stochastic process {X t θ} t 0 This process is a Markov chain

4 The General Problem Realisations of {X t θ} t 0 cannot be observed directly I.e. it is a hidden Markov chain But, noisy measurement model: Y t ψ( x t, θ) (Just writing θ for the parameters again) Filtering Problem Given collection y t1:n y t1,..., y tn, compute conditional probability P θ (X t y t1:n ) More generally: conditional expectation of function h : X R: E θ [ h(xt ) y t1:n ]

5 Imprecise Filtering What if we don t know θ exactly? If we can assume it belongs to a set ϑ: Induces an imprecise stochastic process I.e. a set of stochastic processes Imprecise Filtering Problem Compute lower- and upper expectation E [ h(x t ) ] yt1:n = inf E [ θ h(xt ) ] yt1:n θ ϑ E [ h(x t ) ] [ y t1:n = sup E θ h(xt ) ] y t1:n θ ϑ provide robust bounds on quantity of interest

6 Some Immediate Difficulties Even for fixed θ, no analytical solution for {X t θ} t 0 Let alone for E θ [ h(xt ) yt1:n ] Convergence of numerical approaches? Uniformly w.r.t ϑ? The optimisation to compute E or E is difficult Multi-modal objective surface,... Semantic issues in problem statement Physical interpretation of certain relaxations,...

7 Some Existing Approaches Precise case: Kalman Filter (/variants) Gaussian distr., linear dyn. P(X t y t1:n ) is Gaussian Extended Kalman Filter: Just re-linearise over time Particle Filters Essentially a Monte Carlo method Imprecise case: Special cases: propagation of p-boxes, set-valued observations,... Imprecise Kalman Filter [1] Provides general solution but so far only solved in specific cases E.g. Linear Gaussian-Vacuous Mixture [1] A. Benavoli, M. Zaffalon, E. Miranda: Robust filtering through coherent lower previsions, IEEE Transactions on Automatic Control, 2011

8 The Formal Setup Basically we just need to apply Bayes rule: E [ h(x t ) y t1:n ] = E [ h(x t ) n i=1 ψ(y t i X ti ) ] E [ n i=1 ψ(y t i X ti ) ] In the imprecise case (let s say E [ n i=1 I {y ti }(Y ti ) ] > 0): E [ [ ] (h(xt h(x t ) y t1:n = µ E ) µ ) n ] I {yti }(Y ti ) = 0 i=1 So, we need to compute a joint expectation.

9 Iterated (Lower) Expectation Write g(x t1:n, X t, Y t1:n ) = ( h(x t ) µ ) n i=1 I {y ti }(Y ti ) Use epistemic irrelevance imprecise Markov property Recursively decompose the joint: E [ g( ) ] = E [ E [ g( ) X 0 ]] = E [ E [ E [ ] ]] g( ) X 0, X t1 X 0 }{{} ] =E [g( ) X t1 = E [ E [ E [ g( ) X t1 ] X0 ]] = E [ E [ E [ E [ g( ) Y t1, X t1 ] Xt1 ] X0 ]]. = E [ E [ E [ E [ E [ E [ g( ) X tn, Y tn ] Xtn ] Yt1, X t1 ] Xt1 ] X0 ]]

10 Iterated (Lower) Expectation Now resolve the remaining conditionals inward-outward : E [ E [ E [ E [ E [ E [ ] ] ] ] ]] g( ) X tn, Y tn X tn Yt1, X t1 Xt1 X0 }{{} start here Fix all variables except X t : [ (h(xt E ) µ ) ] n i=1 I y ti (γ ti ) X tn = x tn, Y tn = γ tn = E [( h(x t ) µ ) X tn = x tn, Y tn = γ tn ] n i=1 I y ti (γ ti ) = E [( h(x t ) µ ) X tn = x tn ] n i=1 I y ti (γ ti ) =: g ( )

11 Iterated (Lower) Expectation We have a new function g that no longer depends on X t. After substitution E [ E [ E [ E [ E [ g ] ] ] ]] ( ) X tn Y t1, X t1 Xt1 X0 }{{} this part next Fix all variables except Y tn, E [ g ] ( ) X tn = x tn [ = E E [( h(x t ) µ ) ] X tn = x tn Iytn (Y tn ) ] n 1 i=1 I y ti (γ ti ) X tn = x tn = n 1 i=1 I y ti (γ ti )E [( h(x t ) µ ) ] X tn = x tn { [ E Iytn (Y ] t n ) X tn = x tn if E [( h(x t ) µ ) ] X tn = x tn 0 E [ ] I ytn (Y tn ) X tn = x tn otherwise =: g ( )

12 Iterated (Lower) Expectation We have E [ E [ E [ E [ g ( ) Y t1, X t1 ] Xt1 ] X0 ]] g ( ) depends on X t1,..., X tn 1, X tn and Y t1,..., X tn 1 This is what we started with, but reduced Now just recurse E [ E [ E [ E [ g ] ] ]] ( ) Y t1, X t1 Xt1 X0 }{{} =:g ( ) = E [ g (X 0 ) ]

13 Remaining Problem, in Summary Lower- and upper likelihoods of states given measurements: E [ I yt (Y t ) X t = x t ] and E [ Iyt (Y t ) X t = x t ] For arbitrary h : X R, need to evaluate E [ h(x 0 ) ] (okay) E [ h(x tn ) X tn 1 ] (not okay)

14 Some Simplifying Assumptions Assume dynamics are precise and time-homogeneous: d x t d t = f (x t) + d W t d t σ(x t ) Let X [0, 1] d and h t : x E[h(X t ) X 0 = x]. Then d h t (x) d t = d i=1 f i (x) h t(x) x i d d i=1 j=1 (σ(x)σ(x) ) ij 2 h t (x) x i x j with h 0 = h. To get h t (x) = E[h(X t ) X 0 = x] we need to solve this PDE. In general no analytical solution.

15 Some Further Simplifying Assumptions Assume the dynamics are also deterministic I.e. the process is degenerate and described by f (x t ) We keep stochastic (and imprecise) initial distribution and measurements We get with h 0 = h. d h t (x) d t = d i=1 f i (x) h t(x) x i Still need to solve a PDE, and still no analytical solution We re going to approximate this

16 Approximate Solution Easy enough to solve pointwise: for any x 0 X, E[h(X t ) X 0 = x 0 ] = h(x t ), with t x t = x 0 + f (x τ ) dτ 0 Just do this for a lot of different x 0 Then extend pointwise estimates to entire domain X

17 Dynamics Propagation Collection of starting points Z = z 1,..., z m Compute Z = z (t) 1,..., z(t) m, with t z (t) i = z i + 0 f ( z (τ) ) i dτ, i = 1,..., m This is just an ODE use your favourite numerical integrator I.e. Explicit/implicit Euler, Runge-Kutta schemes,... Note: this could be embarrassingly parallelized A modern GPU can solve hundreds to thousands in parallel

18 Propagated Dynamics We now know h t (z i ) = h ( x (t) ) i = E[h(Xt ) X 0 = z i ] for i = 1,..., m Now extend this to ĥ t on entire X

19 Radial Basis Function Interpolation Training points Z = z 1,..., z m X. Radial basis functions φ : X X R, φ(z, c) = φ( z c ) Basis expansion Φ z R m of any z X : Φ z = [φ(z, z 1 ) φ(z, z m )] Can find w h R m to approximate any ( nice ) function h : X R: h(z) ĥ(z) = m w i φ(z, z i ) = Φ z w h i=1

20 N. Flyer, G.B. Wright: A radial basis function method for the shallow water equations on a sphere, Proceedings Royal Soc A, 2009

21 Radial Basis Function Interpolation Construct design(/kernel/feature/...) matrix Φ z1 φ(z 1, z 1 ) φ(z 1, z m ) Φ Z =. =..... φ(z m, z 1 ) φ(z m, z m ) Φ zm Then Φ Z is usually invertible if all z 1,..., z m are unique. Let h(z) = [h(z 1 ) h(z m )] Find w h that interpolates h on Z: Φ Z w h = h(z) In other words, we get w h = Φ 1 Z h(z)

22 Radial Basis Function Interpolation Pros: Straightforward (conceptually and to implement) Meshless (i.e. arbitrary Z), so no need to partition X Just solve a linear system Well-developed theory Good performance for approximating PDE solutions Near spectral (exponential) convergence Many parameters to fine tune performance Basis functions, hyperparameters, regularisation,... Cons: Many parameters to fine tune Numerical instability (system is typically ill-conditioned)

23 Approximation in Summary We want to compute ĥ t (X 0 ) h t (X 0 ) := E[h(X t ) X 0 ] We know that h t (x 0 ) = h(x t ), where We proceed as follows 1 Choose some Z 2 Compute Z for time t t x t = x 0 + f (x τ ) dτ 0 3 Compute design matrix Φ Z 4 Compute w ht = Φ 1 Z h(z ) 5 Let ĥt(x) := Φ x w ht for any x X

24 Iterated Expectation, Again E[h(X tn )] = E [ E [ h(x tn ) X 0 ]] X 0 X tn

25 Iterated Expectation, Again E[h(X tn )] = E [ E [ E [ E[h(X tn ) X tn 1 ] X t1 ] X0 ]] X 0 X t1 X tn 1 X tn

26 Iterated Expectation, Again E[h(X tn )] = E [ E [ E [ E[h(X tn ) X tn 1 ] X t1 ] X0 ]] h tn := h X 0 X t1 X tn 1 X tn

27 Iterated Expectation, Again E[h(X tn )] = E [ E [ E [ E[h(X tn ) X tn 1 ] X t1 ] X0 ]] h tn := h X 0 X t1 X tn 1 X tn Z

28 Iterated Expectation, Again E[h(X tn )] = E [ E [ E [ E[h(X tn ) X tn 1 ] X t1 ] X0 ]] h tn := h X 0 X t1 X tn 1 X tn Z Z f ( )

29 Iterated Expectation, Again E[h(X tn )] = E [ E [ E [ E[h(X tn ) X tn 1 ] X t1 ] X0 ]] w htn = Φ 1 Z h t n (Z ) h tn := h X 0 X t1 X tn 1 X tn Z Z f ( )

30 Iterated Expectation, Again E[h(X tn )] = E [ E [ E [ E[h(X tn ) X tn 1 ] X t1 ] X0 ]] w htn = Φ 1 Z h t n (Z ) ĥ tn 1 h tn := h X 0 X t1 X tn 1 X tn Z Z f ( )

31 Iterated Expectation, Again E[h(X tn )] E [ E [ E [ ĥt n 1 (X tn 1 ) X t1 ] X0 ]] ĥ tn 1 X 0 X t1 X tn 1

32 Iterated Expectation, Again E[h(X tn )] E [ E [ E [ ĥt n 1 (X tn 1 ) X t1 ] X0 ]] ĥ tn 1 X 0 X t1 X tn 1 f ( )

33 Iterated Expectation, Again E[h(X tn )] E [ E [ E [ ĥt n 1 (X tn 1 ) X t1 ] X0 ]] ĥ t1 ĥ tn 1 X 0 X t1 X tn 1 f ( ) f ( )

34 Iterated Expectation, Again E[h(X tn )] E [ E [ ĥ t1 (X t1 ) X 0 ]] ĥ t1 X 0 X t1

35 Iterated Expectation, Again E[h(X tn )] E [ E [ ĥ t1 (X t1 ) X 0 ]] ĥ 0 ĥ t1 X 0 X t1 f ( )

36 Iterated Expectation, Again E[h(X tn )] E [ ĥ 0 (X 0 ) ] ĥ 0 X 0

37 Iterated Expectation, Again E[h(X tn )] E [ ĥ 0 (X 0 ) ] = X ĥ 0 (x 0 ) dp(x 0 = x 0 )

38 The Crucial Part How to make this tractable: 1 Make 0 =: t 0, t 1,..., t n a uniform partition of [0, t] So t i t i 1 = for all i = 1,..., n 2 Choose the same Z at every step

39 The Crucial Part Z is the same at each step (by homogeneity) You can precompute Z, Z, Φ Z, Φ Z, Φ 1 Z, (Φ Z Φ 1 Z ) =: K Only once, offline, and O( m) + O(m 3 )

40 The Crucial Part Z is the same at each step (by homogeneity) You can precompute Z, Z, Φ Z, Φ Z, Φ 1 Z, (Φ Z Φ 1 Z ) =: K Only once, offline, and O( m) + O(m 3 ) Remains to solve the recursion: ĥ tn (x) = h(x) ĥ tn 1 (x) = Φ x wĥtn ĥ tn 2 (x) = Φ x wĥtn 1 = Φ x Φ 1 Z h(z ) = Φ x Φ 1 Z K0 h(z ) = Φ x Φ 1 Z Φ Z Φ 1 Z h(z ) = Φ x Φ 1 Z K1 h(z ). ĥ tn l (x) = Φ x Φ 1 Z Kl 1 h(z ) for l = 1,..., n After precomputing, can find ĥ 0 for any h in O(nm 2 )

41 But does it work?

42 Dynamics of a Spacecraft mirquurius

43 Dynamics of a Spacecraft Pendulum mirquurius

44 Pendulum Setup Pendulum dynamics model: d x t = d [ ] [ ] αt α = t dt dt α t g/l sin α t ρ α t With l = 1, g 9.8 and ρ {0, 0.5}. Known true initial position x 0 = [ π /2 0] Sequential reinitialisation with best guess after 1 second

45 No Friction, Known Start, No Measurements RMSE Number of training points Angle Time

46 With Friction, Known Start, No Measurements RMSE Number of training points Angle Time

47 Pendulum Setup, With Measurements Model thinks friction term ρ = 0 Measurement model: Y t N ( l sin α t, ) Unknown initial position with uniform P(X 0 ) = U( 2, 2) U( 5, 5) Sequential reinitialisation with same uniform after 1 second

48 No Friction, Uniform Start, 20 Measurements/s RMSE EKF=0.171 Observations sd= Number of training points Angle Time

49 Friction, Uniform Start, 20 Measurements/s RMSE EKF=0.349 EKF'=0.200 Observations sd= Number of training points Angle Time

50 Pendulum Setup, Imprecise Imprecise measurement model with lower- and upper likelihood: ψ(y t x t ) = 0.5N ( y t l sin α t, ) ψ(y t x t ) = 1.5N ( y t l sin α t, ) Unknown initial position with vacuous on [ 2, 2] [ 5, 5] Sequential reinitialisation with same vacuous after 1 second

51 Vacuous Start, 20 Measurements/s, Imprecise Measurement Models Angle Time Angle Time

52 Open Questions Does this scale?? Numerical stability issues? Preconditioning Smart selection/pruning of training points? E.g. regularisation, LASSO Generalisation to stochastic (and/or imprecise) dynamics? Find informative sequential prior? Interpretation of K = Φ Z Φ 1 Z?