Kalman Filter for Statistical Orbit Determination

The final project in my State Estimation class was involved developing and implementing a Kalman filter for a simple statistical orbit determination problem. I worked with a partner to do initial data analysis, Monte Carlo simulation of a ground truth state, then implementation of Linear and Extended Kalman Filters to estimate the perturbed orbit. We used MATLAB for the development of our solution.

This project was completed for ASEN 5044: Statistical Estimation of Dynamical Systems in Fall 2020 with Dr. Nisar Ahmed at CU Boulder. My partner was Laura Fisher Clark.

Simulated Noisy Monte-Carlo Measurements

Discretization of the Continuous Time Model

Given the non-linear Cartesian equations of motion for a spacecraft subject to gravitational force due to a point mass at the origin in state space form:

the continuous time system can be linearized by computing the Jacobian:

Jacobians are similarly found for the gravitational acceleration and process noise terms, u and w.

The discrete-time Jacobians are found with a Taylor series expansion with a delta-∆t of 10 seconds:

Observability

To evaluate the observability, 𝕆, of the initial perturbation 𝛿𝑥(0), we can use the fact that without process noise or control inputs, the perturbation state is

Since the difference between the actual and expected output for the nominal orbit can be approximated as 𝛿𝑦(𝑘) ≅ 𝐻(𝑘)𝛿𝑥(𝑘) where 𝐻(𝑘) = 𝛿𝐻/𝛿𝑥, we can construct a stacked equation

If 𝕆 is full rank, then this equation can be solved for 𝛿𝑥(0) to reconstruct the original perturbation and the system is observable. To find the contribution to 𝐻 for any particular station, we calculate the partials of

and the analogous partials with respect to 𝑌. Putting them together gives

which can show the system is observable and that the initial perturbation can be estimated after only two time steps of measurements, 𝛿𝑦(1) and 𝛿𝑦(2).

Linearized Kalman Filter

The LKF is computed by linearizing the nonlinear state dynamics around a nominal trajectory, then estimating the state perturbations, 𝛿𝑥(k+1) and 𝛿𝑦(k+1), in discrete time steps. The total state is then computed by adding the estimated perturbations back to the nominal trajectory. The LKF Typical State Estimation Error plot above shows that the model tracks reasonably well for a number of time steps, a number that varies from iteration to iteration, but then begins to lose coherence as the perturbations become too great for the filter to manage.

Truth model testing, utilizing the statistical Normalized Estimations Error Squared (NEES) and Normalized Innovation Squared (NIS) tests, further supported the conclusion that the LKF is only a reasonable filter over a very short period and minimal perturbations from the nominal orbit.

Extended Kalman Filter

The EKF differs from the LKF by numerically integrating the nonlinear, noise-free ODE online, rather than continuing to refer back to a nominal trajectory.

Process noise is added in the form of a covariance matrix to account for the higher order terms:

The EKF state estimation errors plot demonstrates this filter’s ability to account for perturbations, even as they accumulate over time.

In general, the LKF can perform well as long as the deviations between the nominal trajectory and ground truth remain small. But as errors accumulate in an actual orbital path, this is unlikely to stay the case. Once deviations become significant, the LKF is no longer able to accurately rely on the linearized approximation and begins to fail. With our selected initial conditions and parameters, the LKF estimator tended to build up unrecoverable errors between 2000 and 4000 seconds into the simulation. The EKF is better equipped to react to these perturbations by updating the nominal trajectory online, making it a stronger choice for addressing this kind of nonlinear dynamics problem.