STATISTICAL ORBIT DETERMINATION Satellite Tracking Example of SNC and DMC ASEN 5070 LECTURE 6 4.08.011 1
We will develop a simple state noise compensation (SNC) algorithm. This algorithm adds process noise to the acceleration equations of the system. Process-noise is added to the differential equation of motion given by Eq. (4..1), i.e., t F, t Bt t where u is white Gaussian process noise with where ij is the kronicker delta. X X u (1) T t 0, ti t j u Qti ij u u () The matrix, B, maps the process noise into the state derivatives.
Expanding Eq. (1) in a Taylor series about a reference trajectory, X * t, yields t t F X X X X X u X * * * t t t t B t t (3) define * t t t and At x X X F X X t t * (4) Thus the state deviation propagation equation including process noise for a linear system is given by (see Eq. (4.9.1)) t t t t t x x u (5) 3
We will further assume that process noise is only being added to the acceleration components of the state; hence, x y z u X 3x3 x, u u, Y x (6) 3x3 u Z y z The time update for the estimation error covariance matrix is given by Eq. (4.9.44). t k1,,, ( ) ( ), t t t t t B QB t d (7) k1 k1 k k k1 k k1 k1 t k 4
In this case is a constant given by Eq. (6) and t r r tk tk r tk tk 1 1 tk 1, r 1 r 1 r r r is given by, k1. (8) Each element of is a 3x3 matrix, so for simplicity define. (9) 1 t k 1, 3 4 Then 1 t k 1, 3 4 4 (10) 5
One could substitute values of and 4 into Eq. (7) and carry out the integration as a quadrature. Alternatively one can approximate the values of and 4 and carry out the integration analytically. We will do the latter. From Eq. (8) we see that X t X t X t X Y Z r t Y t Y t Y t k1 k1 k1 k 1 k 1 k 1 k 1 r X Y Z Z t Z t Z t X Y Z k1 k1 k1. (11) 6
Because we are generally dealing with dense tracking data, the time between observations is usually 10 seconds or less. Therefore, we can assume that X () is constant over this interval X t by and approximate and k 1 X t X t (1) k1 k1 X t X k1 t k1. (13) 7
X t will be negligibly affected by Z. The same arguments apply to Yt and, hence Here we have assumed that for t k1 small, k 1 k 1 Z tk 1 Y and Also, tk1 0 0 0 k 1 0. (14) 0 0 tk 1 4 tk 1, r tk1 r. (15) Again the off diagonal terms are small and the diagonal terms are Aproximately=1. We assume that the velocity is constant over the interval so (, ) 1 4 t k 1 8
Substituting Eqs. (14) and (16) into Eq. (7) and assuming that Q is given by Define Q 0 0 X 0 Y 0 0 0 Z k1 t t t (18) k (17) 9
then the process noise contribution to the estimation error covariance matrix time update at tk 1 is given by t k1 t k, ( ) ( ), t B QB t d k1 k1 3 t t 0 0 0 0 X X 3 3 t t 0 0 0 0 Y Y 3 t t 0 0 0 0 3 t 0 0 t 0 0 X X t 0 0 0 t 0 Y Y t 0 0 0 0 t 3 Z z Z Z. (19) 10
Eq. (19) represents the contribution to the estimation error covariance matrix from uncertainty in the accelerations acting on the system. The values chosen for, and should X Y Z correspond to the magnitude of the uncertainty of the acceleration acting on the system. For example, if we were trying to compensate for atmospheric drag uncertainty this would primarily be along track. In the RTN frame one might assume that R N 0 and T corresponds to the uncertainty in the drag acceleration. A transformation to the X,Y,Z frame, which is generally ECI, would be made at each observation time so that (see Eq. (4.16.19)) Q ECI T QRTN (0) where is the transformation matrix from ECI to RTN (see Eq. (4.16.)). Note that Q ECI is not diagonal nor is it constant and Eq. (19) must be reevaluated with the proper value of Q ECI at each observation time. 11
The J 3 Example An ephemeris was generated for a satellite in an orbit similar to that of the Quikscat spacecraft (sun sync, 800 km, circular orbit). The propagator used in the ephemeris generation used a gravitational potential that included J and J 3 as well as atmospheric drag. Observations of range and range rate from three tracking spacecraft in Geostationary orbit were generated for a five hour data arc. Using the simulated observations containing Gaussian noise in a Kalman Filter, the position and velocity were estimated for the satellite and compared with the true values. 1
Tracking Configuration 13
The J 3 Problem Gravitational Potential Function for the J 3 Model U r 1 J R r 3sin 1 J 3 R r 3 5sin 3 3sin where sin 1 z r r x y z 14
J 3 Equations of Motion Taking the partials of the Gravitational Potential to get x,y,z accelerations 3 3 U x 3 Re z 5 Re z z x 1 J 3 5 1 J 3 3 7 3 x r r r r r r 3 3 U y 3 Re z 5 Re z z y 1 J 3 5 1 J 3 3 7 3 y r r r r r r 3 3 U z 3 Re z 3 Re z 35 z r z 1 J 3 3 5 J 3 10 3 z r r r r r 3 r z 15
Conventional Kalman Filter (CKF) y hxˆ y hx 16
Conventional Kalman Filter (CKF) RSS=4.18 m/s Velocity RSS = 4.934 x 10 - m/s Position RSS = 50.66 m RSS errors are based on X T (X * + ) at each stage ˆx 17
Extended Kalman Filter (EKF) No State Noise Added EKF implemented after 154 minutes y Hxˆ y Hx Note that pre-fit residuals for the CKF are much larger that the EKF but post-fit residuals are identical indicating that the reference trajectory for the CKF is in the linear range 18
Extended Kalman Filter (EKF) Velocity RSS = 4.33 x 10 - m/s Position RSS = 50.79 m RSS position and velocity errors for CKF and EKF are nearly identical 19
Band Diagonal State Noise Matrix 3 t t 0 0 0 0 X X 3 3 t t 0 0 0 0 Y Y 3 t t 0 0 0 0 T Q 3 t 0 0 t 0 0 X X t 0 0 0 t 0 Y Y t 0 0 0 0 t 3 Z Z Z Z See web page handouts or previous slides for derivation 0
Conventional Kalman Filter with SNC Band Diagonal State Noise Matrix results for optimum value: = 10-13 y Hxˆ y hx y hx 1
Conventional Kalman Filter with SNC Band Diagonal State Noise Matrix results for optimum value: = 10-13 Velocity RSS = 1.6 x 10 - m/s Position RSS = 8.43 m
Conventional Kalman Filter with SNC Band Diagonal State Noise Matrix results for optimum value: = 10 +13 Kalman gain is so large that data is fit almost perfectly well below noise levels of 1cm and 1mm/s This is what happens if an input error inject a huge amount of process noise i.e. = 10 + 13 instead of 10-13 3
Conventional Kalman Filter with input error Band Diagonal State Noise Matrix results for optimum value: = 10 +13 standard deviations of estimation error covariance are red lines Velocity RSS = 1. x 10 - m/s Position RSS = 1.084 m Small residuals do not a good orbit Make RSS errors are larger than Previous results 4
Extended Kalman Filter with State Noise (SNC) Band Diagonal State Noise Matrix 3 t t 0 0 0 0 X X 3 3 t t 0 0 0 0 Y Y 3 t t 0 0 0 0 T Q 3 t 0 0 t 0 0 X X t 0 0 0 t 0 Y Y t 0 0 0 0 t 3 Z Z Z Z 5
Extended Kalman Filter with State Noise (SNC) Band Diagonal State Noise Matrix Looked for optimum values of by analyzing residuals and errors Optimum value to minimize position errors: = 10-13 6
Extended Kalman Filter with State Noise (SNC) Band Diagonal State Noise Matrix results for optimum value: = 10-13 y Hxˆ y Hx y ( recall that x 0) 7
Extended Kalman Filter with State Noise (SNC) Band Diagonal State Noise Matrix results for optimum value: = 10-13 Velocity RSS = 1.484 x 10 - m/s Position RSS = 8.50 m Position RSS = 8.50 m The CKF and EKF results are Nearly identical indicating that Initial conditions are in the linear range 8
Extended Kalman Filter with Fading Adds a fading term to the time update this downweights earlier data by Keeping the Kalman gain elevated P i s(t i,t i1 )P i1 T (t i,t i1 ) where t s e >1 t = time between measurements = 0 seconds = age-weighting time constant 9
Extended Kalman Filter with Fading Looked for optimum value of s to minimize the residuals and errors Optimum value of s ~ 1.0339 This corresponds to a of 10 minutes 30
Extended Kalman Filter with Fading Optimum values of s to minimize position and velocity errors : s = 1.0339 y Hxˆ x y 31
Extended Kalman Filter with Fading Optimum values of s to minimize position and velocity errors : s = 1.0339 Velocity RSS = 1.545 x 10 - m/s Position RSS = 8.688 m Position RSS=8.688 m 3
Kalman Filter Results Comparison Conventional Kalman Extended Kalman Filter State Noise Matrix State Noise Matri Fading N/A 10-13 N/A 10-13 N/A Ra nge Residual (m) 3.98 0.834 3.97 0.834 0.653 Ra nge Rate Residua l (m) 0.087 6.64E-03 0.087 6.64E-0 6.37E-03 X Position Erro r (m) 1.10 5.618 11.743 5.317 4.967 Y Position Erro r (m) 45.98.35 45.491 3.7 3.370 Z Position Erro r (m) 19.178 5.84 17.908 5.798 6.81 Position RSS (m) 50.66 8.43 50.79 8.50 8.688 X Velocity Error (m/s) 1.41E-0 6.19E-03 1.39E-0 5.7E-03 6.1E-03 Y Velocity Error (m/s) 3.0E-0 8.89E-03 3.19E-0 8.13E-03 7.33E-03 Z Velocity Error (m/s).66e-0 1.1E-0.58E-0 1.10E-0 1.1E-0 Velocity RSS (m/s) 4.394E-0 1.6E-0 4.33E-0 1.484E-0 1.545E-0 Conclusions: The CKF and EKF produce comparable results indicating that the reference orbit for the CKF is in the linear range for this example. Fading produces comparable results to SNC 33
Dynamic Model Compensation (DMC) DMC accounts for unmodeled or inaccurately modeled accelerations acting on the spacecraft - J 3 in this problem. The state vector was augmented to the following: X x, y, z, x, y, z, x, y, z where x, y, and z are the accelerations A Gauss-Markov process is used to account for these accelerations: ( t) ( t) u( t) where u(t) is white Gaussian noise with E(u) 0 Eu(t)u() (t ) and 1 Where is a time constant 34
Dynamic Model Compensation and were optimized to give the lowest position and velocity errors: 10 orbital period = 10-16 35
Dynamic Model Compensation Optimized Results Velocity RSS = 1.49 x 10 - m/s Position RSS = 8.195 m 36
Dynamic Model Compensation Optimized Results Actual vs estimated accelerations Errors in acceleration estimates in x,y, And z directions Note: the DMC did a poor job of recovering accelerations. More work is needed on optimizing and.we should do a better job of recovering accelerations and the state while reducing tracking residuals. 37
Filter Comparisons Conventional Kalman Extended Kalman Filter DMC State Noise Matrix State Noise Matri Fading N/A 10-13 N/A 10-13 N/A 10-16 Ra nge Residual (m) 3.98 0.834 3.97 0.834 0.653 0.6764 Ra nge Rate Residual (m) 0.087 6.64E-03 0.087 6.64E-0 6.37E-03 6.10E-03 X Position Error (m) 1.10 5.618 11.743 5.317 4.967 3.744 Y Position Error (m) 45.98.35 45.491 3.7 3.370 3.151 Z Position Error (m) 19.178 5.84 17.908 5.798 6.81 6.574 Position RSS (m) 50.66 8.43 50.79 8.50 8.688 8.195 X Velocity Error (m/s) 1.41E-0 6.19E-03 1.39E-0 5.7E-03 6.1E-03 5.65E-03 Y Velocity Error (m/s) 3.0E-0 8.89E-03 3.19E-0 8.13E-03 7.33E-03 7.61E-03 Z Velocity Error (m/s).66e-0 1.1E-0.58E-0 1.10E-0 1.1E-0 1.071E-0 Velocity RSS (m/s) 4.394E-0 1.6E-0 4.33E-0 1.484E-0 1.545E-0 1.49E-0 All filters achieved comparable results; however we should do better with the DMC 38
Added deviations to the initial conditions so that they are outside linear range to show Convergence for EKF with SNC Divergence for CKF with SNC Original Initial Conditions [757700.301, 5606.566, 4851499.77, 13.50611, 4678.37741, -5371.314404] m and m/s Deviation [8, 5, 5, 8, 5, 5] m and m/s Perturbed Initial Conditions [757708.301, 5611.566, 4851504.77, 1.50611, 4683.37741, -5366.314404] m and m/s 39
Conventional Kalman Filter Band Diagonal State Noise Matrix results for optimum value: = 10-13 Deviated Initial Conditions y hxˆ y Hx y hx 40
Conventional Kalman Filter Band Diagonal State Noise Matrix results for optimum value: = 10-13 Deviated Initial Conditions Velocity RSS =.88 m/s Position RSS = 353.49 m 41
Extended Kalman Filter Band Diagonal State Noise Matrix results for optimum value: = 10-13 Deviated Initial Conditions y Hxˆ y Hx y 4
Extended Kalman Filter Band Diagonal State Noise Matrix results for optimum value: = 10-13 Deviated Initial Conditions Velocity RSS =.53 x 10 - m/s Position RSS = 17.79 m 43
Filter Comparisons CKF EKF Initial Co nditions Linear R ange Nonlinear R ange Linear R ange Nonlinear R ange Ra nge Residual (m) 0.834 18.035 0.834 1.93 Ra nge Rate Residual (m) 6.64E-03 7.66E-01 6.64E-0 1.04E-0 X Position Erro r (m) 5.618 537.948 5.317 5.363 Y Position Erro r (m).35 131.617 3.7 6.66 Z Position Erro r (m) 5.84 193.008 5.798 15.758 Position RSS (m) 8.43 353.49 8.50 17.785 X Velocity Error (m/s) 6.19E-03 6.73E-01 5.7E-03 6.17E-03 Y Velocity Error (m/s) 8.89E-03 1.833 8.13E-03 1.64E-0 Z Velocity Error (m/s) 1.1E-0.1 1.10E-0 1.81E-0 3D Velocity RSS (m/s) 1.6E-0.883 1.484E-0.58E-0 While the CKF does not diverge, its solution is significantly In error 44
Added deviations to the initial conditions to show Convergence for EKF with SNC Divergence for the Batch Processor Original Initial Conditions [757700.301, 5606.566, 4851499.77, 13.50611, 4678.37741, -5371.314404] m, m/s Deviation [1000, 1000, 1000, 500, 500, 500] m,m/s Perturbed Initial Conditions [758700.301, 53606.566, 485499.77, 713.50611, 5178.37741, -4871.314404] m, m/s 45
Batch Processor Pass 1 Range RMS = 367.36 km Range rate RMS = 4.304 km/s Pass 3 Range RMS = 4457.33 km Range rate RMS = 4.710 km/s Note that the batch processor is not converging and successive Iterations show divergence 46
Extended Kalman Filter Band Diagonal State Noise Matrix results for various values of Deviated Initial Conditions Trajectory updated after 30 minutes Note: Values of from 10-5 to 10-0 were tested. However, the residuals and errors were orders of magnitude higher for value of between 10-5 to 10-9. Therefore, those values are not shown on the plots. Note that the optimal value for is the same as for small initial condition errors. 47
Extended Kalman Filter Band Diagonal State Noise Matrix results for = 10-13 Deviated Initial Conditions Trajectory updated after 30 minutes y Hxˆ y y 48
Extended Kalman Filter Band Diagonal State Noise Matrix results for = 10-13 Deviated Initial Conditions Position Errors after 50 minutes Position Errors Position RSS = 8.643 m 49
Extended Kalman Filter Band Diagonal State Noise Matrix results for = 10-13 Deviated Initial Conditions Velocity Errors after 50 minutes Velocity Errors Velocity RSS = 1.58 x 10 - m/s Note that position and velocity RSS are comparable to those on slide 40. If IC errors are large it may be Colorado Center Advantageous for Astrodynamics to use the Research EKF to The University Obtain of Colorado ICs for the batch 50
Transformation of State and Covariance Matrix to Alternate Frames Sometimes it is desirable to transform the state vector and the estimation error covariance into alternate coordinate systems. It may be of interest to view these quantities in a radial, transverse, and normal (RTN) system. The general transformation between any two coordinate frames (say prime to unprimed) for a position vector is given by r r a, (4.16.15) where is an orthogonal transformation matrix. r is the vector in the unprimed frame, and a is the vector offset of the origin of the two systems. 51
Transformation of State and Covariance Matrix to Alternate Frames The velocity transforms according to r rr (4.16.16) Generally will be zero unless we are transforming from a rotating to a nonrotating frame or vice versa; for example, ECEF to ECI. It can be shown that r r, where is the angular velocity of the rotating frame. The transformation we want is ECI to RTN. We assume that the RTN frame is fixed to the osculating orbit at each point in time; hence 0 and r 0 r 0 v v RTN ECI. (4.16.17) 5
Transformation of State and Covariance Matrix to Alternate Frames The covariance of the estimation error is transformed as follows: xˆ x xˆ x, RTN ECI where 0 0 rˆ r 0 0 and ˆ ˆ x x v v, (4.16.18) 0 0 I ˆ and r, v, and represent the true values of the position, velocity, and all other quantities in the state vector, respectively. It is assumed that none of the elements of are affected by the coordinate transformation. 53
Transformation of State and Covariance Matrix to Alternate Frames The desired covariance is given by P RTN xˆ xxˆ x E x xx x RTN T T E ˆ ˆ. T ECI (4.16.19) The elements of for the ECI to RTN transformation are given by R T N r * u i j k r * r* v* u i j k, r* v* X Y Z u u u i j k N R X Y Z X Y Z (4.16.0) where ur, ut, u N are unit vectors in the RTN frame, i, j, and k are unit vectors in the ECI frame, and r* and v* are the position and velocity vectors of the reference orbit. 54
Transformation of State and Covariance Matrix to Alternate Frames Equation (4.16.0) may be written ur X Y Z i u j. (4.16.1) T X Y Z u N X Y Z k Hence, the transformation matrix relating the RTN and ECI frame is X Y Z X Y Z. (4.16.) X Y Z The transformation relating the covariance matrix in two different frames is P P. B B A A B A T 55
Transformation of State and Covariance Matrix to Alternate Frames Assume we wish to transform Φ(t i, t j ) from the A frame to the B frame r t i x( ti ) v ti Let Recall that and x( ti ) ( ti, t j ) x( t j ) A A A A B x( t ) x( t ) i B T 0 T 0 A B x( t ) A B i A i A 56
Transformation of State and Covariance Matrix to Alternate Frames But Hence, x( ti ) B ti, t j ( t j ) B x A A B B ti, t j A x( t j ) x( ti ) ( ti, t j ) x( t j ) B B B t, t A t, t B i j B B i j A A A A A B where B A A B T 57