Uncertainty Propagation and Nonlinear Filtering for Space Navigation using Differential Algebra

Transcription

1 Uncertainty Propagation and Nonlinear Filtering for Space Navigation using Differential Algebra M. Valli, R. Arellin, P. Di Lizia and M. R. Lavagna Departent of Aerospace Engineering, Politecnico di Milano Taylor Model Methods VII, Deceber 14-17, 2011, Keywest, Florida

2 2 Outline Motivations of the Work Introduction to Space Navigation DA-based higher order Taylor series approach - Nonlinear Uncertainty Propagation - Nonlinear Filtering Siulations and Results Conclusions and Developent Axes

3 3 Motivations and Background Increase of interplanetary/deep space/saple and return exploration issions - Long duration - Large heliocentric/geocentric distances - Unknown and hostile environents - Coplex dynaics

4 3 Motivations and Background Increase of interplanetary/deep space/saple and return exploration issions - Long duration - Large heliocentric/geocentric distances - Unknown and hostile environents - Coplex dynaics Strong need to study the navigation proble in space A navigation syste is a state estiation filter that, starting fro sensors easureents, can estiate the spacecraft state variables - The state of a space vehicle can include a large nuber of paraeters, first of all satellite orbital position and velocity

5 4 Introduction The orbit odel can be written in the following general equation for: x k+1 = f(x k, t k )+w k y k = h(x k, t k )+v k

6 Introduction The orbit odel can be written in the following general equation for: x k+1 = f(x k, t k )+w k y k = h(x k, t k )+v k The on-board navigation syste is ade up by two ain steps: 1. PREDICTION STEP Use of the dynaics equation to predict the state of the vehicle in a future tie 2. CORRECTION STEP Use the easureents fro the on-board sensors (autonoous navigation) or fro the ground station (ground-based navigation) to correct the predicted state 4

7 5 Introduction Autonoous navigation syste requireents - Real tie estiation - High accuracy - Need to fully account for nonlinearities in the syste

8 6 Introduction Present day orbit uncertainty propagation and filtering - Linearized odels - Full nonlinear Monte Carlo siulations - Higher order Taylor series approaches

9 6 Introduction Present day orbit uncertainty propagation and filtering - Linearized odels LOW ACCURACY - Full nonlinear Monte Carlo siulations - Higher order Taylor series approaches CONSISTENT COMPUTATIONAL COST

10 6 Introduction Present day orbit uncertainty propagation and filtering - Linearized odels LOW ACCURACY - Full nonlinear Monte Carlo siulations - Higher order Taylor series approaches CONSISTENT COMPUTATIONAL COST Differential Algebra can provide powerful tools to face the proble - Possibility to consider nonlinearities - Strong reduction of coputational costs - Opportunity to directly evaluate the ipact of changes in soe physical variables on the estiated state Ipact on the estiated state of errors in the initial state Ipact on the estiated state of easureent noises (sensors architecture definition)

11 7 Differential Algebra Given any function f of v variables, DA delivers its Taylor expansion up to the arbitrary order n - Any integration schee is based on algebraic operations, involving the evaluation of f at several integration points Replacing x 0 with [x 0 ]=x 0 + δx 0 and carrying out all the operations within the DA fraework à Taylor expansion of the ODE flow [x k ]=x k + M(δx 0, δα, δβ, δγ,...) à Pointwise integration can be replaced by fast evaluation of polynoials à The derivatives of the function up to the order of the Taylor expansions are available

12 8 Higher order Taylor series approaches Nonlinear uncertainty propagation Nonlinear filtering

13 Nonlinear Mapping of the Syste Dynaics Consider the spacecraft dynaics governed by the equations of otion: ẋ i = f i [t, x(t)] x i (t 0 )=x 0 i Park R. and Scheeres D., Nonlinear apping of Gaussian state covariance and orbit uncertainties, AAS Paper , January,

14 Nonlinear Mapping of the Syste Dynaics Consider the spacecraft dynaics governed by the equations of otion: ẋ i = f i [t, x(t)] x i (t 0 )=x 0 i Perforing a Taylor series expansion of the deviation of the current state fro the noinal trajectory in ters of the initial deviation: δx i (t) = Φ i,k1...k p δx 0 k 1...δx 0 k p p=1 Park R. and Scheeres D., Nonlinear apping of Gaussian state covariance and orbit uncertainties, AAS Paper , January,

15 Nonlinear Mapping of the Syste Dynaics Consider the spacecraft dynaics governed by the equations of otion: ẋ i = f i [t, x(t)] x i (t 0 )=x 0 i Perforing a Taylor series expansion of the deviation of the current state fro the noinal trajectory in ters of the initial deviation: δx i (t) = Φ i,k1...k p δx 0 k 1...δx 0 k p p=1 The current ean and covariance atrix can be written as: δ i (t) = P ij (t) = p=1 p=1 q=1 1 p! Φ i,k 1...k p E[δx 0 k 1...δx 0 k p ] 1 p!q! Φ i,k 1...k p Φ j,γ1...γ q E[δx 0 k 1...δx 0 k p δx 0 γ 1...δx 0 γ q ] Park R. and Scheeres D., Nonlinear apping of Gaussian state covariance and orbit uncertainties, AAS Paper , January,

16 10 Nonlinear Mapping of the Syste Dynaics δ i (t) = P ij (t) = p=1 p=1 q=1 1 p! Φ i,k 1...k p E[δx 0 k 1...δx 0 k p ] 1 p!q! Φ i,k 1...k p Φ j,γ1...γ q E[δx 0 k 1...δx 0 k p δx 0 γ 1...δx 0 γ q ]

17 10 Nonlinear Mapping of the Syste Dynaics δ i (t) = P ij (t) = p=1 p=1 q=1 1 p! Φ i,k 1...k p E[δx 0 k 1...δx 0 k p ] 1 p!q! Φ i,k 1...k p Φ j,γ1...γ q E[δx 0 k 1...δx 0 k p δx 0 γ 1...δx 0 γ q ] Φ i,a = f i,α Φ α,a Φ i,ab = f i,α Φ α,ab + f i,αβ Φ α,aφ β,b Φ i,abc = f i,α Φ α,abc + f i,αβ (Φ α,aφ β,bc + Φ α,ab Φ β,c + Φ α,ac Φ β,b )+f i,αβγ Φ α,aφ β,b Φ γ,c...

18 10 Nonlinear Mapping of the Syste Dynaics δ i (t) = P ij (t) = p=1 p=1 q=1 1 p! Φ i,k 1...k p E[δx 0 k 1...δx 0 k p ] 1 p!q! Φ i,k 1...k p Φ j,γ1...γ q E[δx 0 k 1...δx 0 k p δx 0 γ 1...δx 0 γ q ] Φ i,a = f i,α Φ α,a Φ i,ab = f i,α Φ α,ab + f i,αβ Φ α,aφ β,b Φ i,abc = f i,α Φ α,abc + f i,αβ (Φ α,aφ β,bc + Φ α,ab Φ β,c + Φ α,ac Φ β,b )+f i,αβγ Φ α,aφ β,b Φ γ,c... The higher order derivatives can be directly extracted fro the expansion ap The integration of this syste is substituted with ONE integration in DA More flexible approach Very general approach (applying nonlinear transforations to ean and covariance estiates)

19 11 Nonlinear Mapping of the Syste Dynaics The second nuerical operation is the higher-order oent coputation 1 δ i (t) = p! Φ i,k 1...k p E[δx 0 k 1...x 0 k p ] P ij (t) = p=1 p=1 q=1 1 p!q! Φ i,k 1...k p Φ j,γ1...γ q E[δx 0 k 1...x 0 k p δx 0 γ 1...x 0 γ q ] δ i (t) j (t)

20 11 Nonlinear Mapping of the Syste Dynaics The second nuerical operation is the higher-order oent coputation 1 δ i (t) = p! Φ i,k 1...k p E[δx 0 k 1...x 0 k p ] P ij (t) = p=1 p=1 q=1 1 p!q! Φ i,k 1...k p Φ j,γ1...γ q E[δx 0 k 1...x 0 k p δx 0 γ 1...x 0 γ q ] δ i (t) j (t) à Calculated using the Joint Characteristic Function of the initial distribution: E[x i ]= i E[x i x j ]= i j + P ij E[x i x j x k ]= i j k +( i P jk + j P ik + k P ij ) E[x i x j x k x l ]= i j k l +( i j P kl + i k P jl + j k P il + i l P jk + j l P ik + k l P ij )+P ij P kl + P ik P jl + P il P jk NOTE: If the initial ean is zero à Further seplification: all the odd oent are zero

21 12 Nonlinear Mapping of the Syste Dynaics Consider the two-body proble of a Earth graviting satellite Initial state defined as DA variable (lengths units scaled by the seiajor axis and tie units by the factor a 3 ) µ x 0 = r0 v 0 = δx δy δz δv x δv y δv z P = Carry out the integration of the otion in differential algebra Calculate the estiation of the ean and covariance

22 13 Nonlinear Mapping of the Syste Dynaics DA-1st order DA-2nd order DA-3rd order r[%] r[%] r[%] 0.1 orbit orbit orbit orbit

23 13 Nonlinear Mapping of the Syste Dynaics DA-1st order DA-2nd order DA-3rd order r[%] r[%] r[%] 0.1 orbit orbit orbit orbit

24 14 Higher order Taylor series approaches Nonlinear uncertainty propagation Nonlinear filtering

25 15 Higher Order Taylor Series Filtering Methods The application of higher order Taylor series ethods to nonlinear filtering is the Higher-Order Nuerical Extended Kalan Filter (HNEKF) [Park&Scheeres2007]

26 15 Higher Order Taylor Series Filtering Methods The application of higher order Taylor series ethods to nonlinear filtering is the Higher-Order Nuerical Extended Kalan Filter (HNEKF) [Park&Scheeres2007] HNEKF PREDICTION EQUATION ( k+1 )i = E[Φ i (t k+1 ; + k + δx k,t k )+w i k]=φ i (t k+1 ; + k,t k)+ p=1 1 p! Φi,γ 1...γ p (t k+1,t k ) E[δxγ 1 k...δxγ p k ] x 1 ref 0 x 0 ref (δ 0 = 0) δ 1 =0 1

27 15 Higher Order Taylor Series Filtering Methods The application of higher order Taylor series ethods to nonlinear filtering is the Higher-Order Nuerical Extended Kalan Filter (HNEKF) [Park&Scheeres2007] HNEKF PREDICTION EQUATION ( k+1 )i = E[Φ i (t k+1 ; + k + δx k,t k )+w i k]=φ i (t k+1 ; + k,t k)+ p=1 1 p! Φi,γ 1...γ p (t k+1,t k ) E[δxγ 1 k...δxγ p k ] x 1 ref 0 x 0 ref (δ 0 = 0) δ 1 =0 1 (P k+1 )ij =( p=1 q=1 1 p!q! Φi,γ 1...γ p (t k+1,t k ) Φj,ζ 1...ζ q (t k+1,t k ) E[δxγ 1 k...xγ p k xζ1 k...xζq k ]) δi k+1(δx k )δ j k+1 (δx k)+q ij k (n k+1 )i = E[h i (t k+1 ; + k + δx k,t k )+v i k+1] =h i (t k+1 ; + k,t k)+ p=1 1 p! hi,γ 1...γ p t k+1,t k E[δx γ 1 k...δxγ p k ]

28 16 Higher Order Taylor Series Filtering Methods HNEKF UPDATE EQUATION (Pk+1) zz ij = R ij k (P xz k+1) ij = p!q! hi,γ 1...γ p t k+1,t k p=1 q=1 1 p!q! Φi,γ 1...γ p t k+1,t k p=1 q=1 h i,ζ 1...ζ q t k+1,t k E[δx γ 1 k...δxγ p k δxζ 1 k...δxζ q k ] (δn k+1 )i (δn k+1 )j h i,ζ 1...ζ q t k+1,t k E[δx γ 1 k...δxγ p k δxζ 1 k...δxζ q k ] (δ k+1 )i (δn k+1 )j K k+1 = P xz k+1(p zz k+1) 1 + k+1 = k+1 + K k+1(z k+1 n k+1 ) P + k+1 = P k+1 K k+1p zz k+1k T k+1

29 16 Higher Order Taylor Series Filtering Methods HNEKF UPDATE EQUATION (Pk+1) zz ij = R ij k (P xz k+1) ij = p!q! hi,γ 1...γ p t k+1,t k p=1 q=1 1 p!q! Φi,γ 1...γ p t k+1,t k p=1 q=1 h i,ζ 1...ζ q t k+1,t k E[δx γ 1 k...δxγ p k δxζ 1 k...δxζ q k ] (δn k+1 )i (δn k+1 )j h i,ζ 1...ζ q t k+1,t k E[δx γ 1 k...δxγ p k δxζ 1 k...δxζ q k ] (δ k+1 )i (δn k+1 )j K k+1 = P xz k+1(p zz k+1) 1 + k+1 = k+1 + K k+1(z k+1 n k+1 ) P + k+1 = P k+1 K k+1p zz k+1k T k+1 The state and the easureents have been defined as DA variables [x k+1 ]= x k+1 + M(δx k ) [z k+1 ]= z k+1 + M(δx k+1 )= z k+1 + M(δx k )

30 16 Higher Order Taylor Series Filtering Methods HNEKF UPDATE EQUATION (Pk+1) zz ij = R ij k (P xz k+1) ij = p!q! hi,γ 1...γ p t k+1,t k p=1 q=1 1 p!q! Φi,γ 1...γ p t k+1,t k p=1 q=1 h i,ζ 1...ζ q t k+1,t k E[δx γ 1 k...δxγ p k δxζ 1 k...δxζ q k ] (δn k+1 )i (δn k+1 )j h i,ζ 1...ζ q t k+1,t k E[δx γ 1 k...δxγ p k δxζ 1 k...δxζ q k ] (δ k+1 )i (δn k+1 )j K k+1 = P xz k+1(p zz k+1) 1 + k+1 = k+1 + K k+1(z k+1 n k+1 ) P + k+1 = P k+1 K k+1p zz k+1k T k+1 The state and the easureents have been defined as DA variables [x k+1 ]= x k+1 + M(δx k ) [z k+1 ]= z k+1 + M(δx k+1 )= z k+1 + M(δx k ) à Higher order derivatives are provided fro one DA integration of the state and easureent coputation

31 17 HNEKF: Halo orbit about the Sun-Earth L1 point State: position and velocity Measureent odel: linear Initial guess: off by 100 k and 0.1 /s fro the true state (boundary of initial ellipsoid) No process noise: errors in the initial state and in the easureents

32 18 HNEKF: Halo orbit about the Sun-Earth L1 point State: position and velocity Measureent odel: linear Initial guess: off by 100 k and 0.1 /s fro the true state (boundary of initial ellipsoid) No process noise: errors in the initial state and in the easureents

33 19 HNEKF: Halo orbit about the Sun-Earth L1 point Without using DA: - Two syste of differential equations up to the th (order of the Taylor expansion) order ust be solved - If soething changes in the dynaical or easureent equations the whole solving syste ust be rewritten Using DA: - At each tie step only one integration in DA is required - More flexibility - The sae accuracy of the standard HNEKF is guaranteed

34 Higher Order Taylor Series Filtering Methods Missions with predeterined reference trajectories à copute the reference trajectory over a tie span before filtering x 0 x 1 = M 10 (δx 0 ) x 1 x i Predeterined trajectory! integrated offline! DA with high accuracy 0 ref 1 = M 10 ( 0 x 0 ) 1 = ref 1 + δ 1 (δx 0 ) 20

35 HAEKF: Halo orbit about the Sun-Earth L1 point 21

36 HNEKF: 2-body Proble - Nonlinear Measureents DA-based HNEKF behavior in case of low frequency estiation and nonlinear easureents - Measuring the velocity fro S/C to Earth, the right ascension and declination of the Earth - Better perforance of higher orders wrt first order filter 22

37 Conclusions and Developent Axes CONCLUSIONS New ethods for nonlinear uncertainty propagation and filtering have been presented The ethods are based on Taylor differential algebra ipleented in COSY-Infinity The ethods have good perforance in ters of coputational costs and flexibility FURTHER DEVELOPMENTS Study and developent of Monte Carlo-based filters using DA Use of DA to define a sensors architecture that increase estiation precision and reduce the coputational load 23

38 Uncertainty Propagation and Nonlinear Filtering for Space Navigation using Differential Algebra M. Valli, R. Arellin, P. Di Lizia and M. R. Lavagna Departent of Aerospace Engineering, Politecnico di Milano Taylor Model Methods VII, Deceber 14-17, 2011, Keywest, Florida