GN&C | Modeling and Simulation

Orbit Determination with an EKF and LQR Control

I built a 2D orbit simulator that estimates a spacecraft state from noisy ground station measurements using an Extended Kalman Filter (EKF). The estimate drives an LQR state feedback controller with a fixed acceleration limit. I evaluate the filter with Monte Carlo runs using NEES and NIS consistency checks, and I track control effort and saturation.

Fall 2025
ASEN 5044 (Estimation) + ASEN 5014 (Control)
MATLAB with a browser based visualization
Closed loop flow
Truth propagation → EKF estimate → LQR control
Handles measurement gaps and actuator saturation
Estimator validation
NEES / NIS
Monte Carlo chi square consistency tests
Key interfaces
Dynamics / sensors / actuation
Discrete time measurements with noise
View demo Read reports

Overview

This project couples orbit determination and feedback control in one loop. The dynamics use a simple planar two body gravity model so the focus stays on measurement availability, noise modeling, and estimator consistency.

Navigation: EKF based orbit determination
Estimate position and velocity in 2D using noisy range, range rate, and angle measurements from ground stations. When multiple stations are in view, stack the measurements and use a block diagonal measurement noise matrix R.
Control: LQR stabilization using the EKF estimate
Design an LQR gain using a linearized in plane model and apply u = −K x̂ with saturation. Initialize weights with Bryson’s rule, then tune the balance between response speed and control effort.
Verification: Monte Carlo testing and consistency statistics
Run many trials and compare NEES and NIS to chi square bounds. Use the results to spot process and measurement noise mismatch and to see how the filter behaves during measurement gaps.
Team context and my contributions
The course reports were submitted as a team and include work from other students. This page focuses on what I implemented and tested.

Estimator summary

EKF with nonlinear propagation (ode45), Jacobians evaluated at the current estimate, covariance propagation with acceleration process noise, stacked measurement updates, and a measurement size that changes with station visibility.

Control summary

LQR designed about a nominal circular orbit using a linear state space model, gain synthesis, Bryson’s rule weight selection, and saturation to a fixed maximum acceleration used in this demo.

Simulation approach

Separate truth noise from the noise assumed by the filter, include time varying station visibility, run Monte Carlo trials, and log errors, innovations, and control saturation.

Setup

Truth dynamics in a planar two body model

The truth model propagates nonlinear two body dynamics and injects disturbances as additive acceleration noise. Disturbances and actuation share the same acceleration interface so the truth inputs can be compared directly to the model used inside the EKF.

\[ \mathbf{x}= \begin{bmatrix} X\\ \dot X\\ Y\\ \dot Y \end{bmatrix}, \qquad r=\sqrt{X^2+Y^2} \] \[ \begin{aligned} \ddot X &= -\mu \frac{X}{r^3} + u_1 + w_1\\ \ddot Y &= -\mu \frac{Y}{r^3} + u_2 + w_2 \end{aligned} \]

The truth trajectory is propagated with numerical integration and sampled at a fixed update period for EKF updates.

Ground station measurements

Each visible station returns range, range rate, and angle. Visibility changes over time, so the measurement size changes. When no stations are visible, the filter only predicts forward and uncertainty grows. When measurements return, uncertainty shrinks again.

\[ \begin{aligned} \Delta X &= X - X_s(t), & \Delta Y &= Y - Y_s(t)\\ \Delta \dot X &= \dot X - \dot X_s(t), & \Delta \dot Y &= \dot Y - \dot Y_s(t) \end{aligned} \] \[ \begin{aligned} \rho &= \sqrt{\Delta X^2 + \Delta Y^2}\\ \dot \rho &= \frac{\Delta X\,\Delta \dot X + \Delta Y\,\Delta \dot Y}{\rho}\\ \phi &= \operatorname{atan2}(\Delta Y,\Delta X) \end{aligned} \]
Diagram of Earth, ground stations, and the line of sight region that defines visibility.
Station measurement geometry with shown line of sight. Range and angle are shown.
Diagram of radial and tangential control axes along the orbit.
Control axes used by the in plane control model. Radial and tangential forcing shown.

Extended Kalman Filter

1

Predict step

Propagate the nonlinear state estimate forward with the dynamics. Evaluate the dynamics Jacobian at the most recent corrected estimate.

\[ \hat{\mathbf{x}}^-_{k+1}=f_d\!\left(\hat{\mathbf{x}}^+_k\right), \qquad A_k=\left.\frac{\partial f}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}^+_k} \]
2

Covariance propagation

Propagate covariance using the linearized transition and an acceleration noise mapping. Use NEES and NIS trends across Monte Carlo trials to tune noise levels.

\[ P^-_{k+1}=\tilde F_k P^+_k \tilde F_k^{\mathsf{T}} + \tilde\Omega_k Q_{KF}\tilde\Omega_k^{\mathsf{T}} \]
3

Update step

Stack measurements from all visible stations, build a block diagonal R, and apply the EKF update.

\[ \tilde{\mathbf{y}}_k=\mathbf{y}_k-h\!\left(\hat{\mathbf{x}}^-_k\right) \] \[ \begin{aligned} S_k &= H_k P^-_k H_k^{\mathsf{T}} + R_k\\ K_k &= P^-_k H_k^{\mathsf{T}} S_k^{-1}\\ \hat{\mathbf{x}}^+_k &= \hat{\mathbf{x}}^-_k + K_k\tilde{\mathbf{y}}_k\\ P^+_k &= (I-K_k H_k)P^-_k \end{aligned} \]

Implementation details

Stacking: If m stations are visible, the measurement vector has length 3m. The ordering must stay consistent across y, H, and R.

Linearization: If the Jacobians do not match the model used in propagation, the EKF becomes unreliable fast. I evaluate Jacobians at the corrected estimate each step.

Dropouts: Visibility gaps are common, especially during lower orbits. During gaps, uncertainty grows. After gaps, measurements pull the estimate back toward the truth.

Noise assumptions: Truth noise is separate from the noise assumed by the filter so mismatch can be tested and its impact on NEES and NIS can be measured.

Linear Quadratic Regulator

Linearized perturbation model about a circular orbit

The controller is designed by linearizing about a nominal circular orbit of radius r₀. The state is expressed as small in plane perturbations with a two axis control input.

\[ \mathbf{x}= \begin{bmatrix} \delta r\\ \delta \dot r\\ \delta \theta\\ \delta \dot\theta \end{bmatrix}, \qquad \mathbf{u}= \begin{bmatrix} \delta u_r\\ \delta u_t \end{bmatrix} \]

With mean motion n = sqrt(μ / r₀³), one common in plane linearization is:

\[ \begin{aligned} \dot{\delta r} &= \delta \dot r\\ \dot{\delta \dot r} &= 3n^2 \delta r + 2 r_0 n\,\delta \dot\theta + \delta u_r\\ \dot{\delta \theta} &= \delta \dot\theta\\ \dot{\delta \dot\theta} &= -\frac{2n}{r_0}\delta \dot r + \frac{1}{r_0}\delta u_t \end{aligned} \]

Cost function, weights, and saturation

LQR minimizes state error and control effort and returns a stabilizing gain K.

\[ J=\int_{0}^{\infty}\left(\mathbf{x}^{\mathsf{T}}Q\mathbf{x}+\mathbf{u}^{\mathsf{T}}R\mathbf{u}\right)\,dt, \qquad \mathbf{u}=-K\hat{\mathbf{x}} \]

I initialize Q and R with Bryson’s rule and tune the trade between tracking and effort with a scalar multiplier. The commanded acceleration is saturated to a fixed limit used for this demo, 0.01 g.

\[ Q=\operatorname{diag}\!\left(\frac{1}{x_{\max}^2}\right), \qquad R=\rho\,\operatorname{diag}\!\left(\frac{1}{u_{\max}^2}\right) \]

Controller evaluation approach

I compare designs using time response and integrated control effort under the same acceleration limit. Once saturation engages, performance is limited by available acceleration rather than by the feedback gain.

Verification and testing

NEES and NIS consistency checks

I evaluate filter consistency using NEES for state error and NIS for innovation error, and compare them to chi square bounds. These checks reveal underconfident or overconfident covariance tuning across many trials.

Monte Carlo setup

I randomize the initial truth state around the filter initial estimate using the initial covariance, then run repeated trials. This exposes failure modes that a single run can hide.

Bounded actuation

The controller command is saturated to a fixed acceleration capability, 0.01 g in this demo. This keeps comparisons consistent across controller designs.

Implementation summary

What I implemented

This page documents a coupled estimation and control loop with time varying measurement availability. The algorithms were developed in MATLAB. This site presents results and links to supporting materials.

  • Planar two body truth propagation with injected acceleration disturbances
  • Multi station measurement model with visibility driven dropouts
  • EKF predict and update with stacked measurements
  • LQR control with saturation

Interactive demo

Orbit Navigation & Sensor Fusion Demo
2D inertial frame · RK4 · EKF · 12 rotating ground stations · LQR controller
Disturbance OFF
Saturation OK
Orbit View
t (s)
0.00
step
0
Δt (s)
10
visible
0
mode
Paused
speed
100×
NEES
–
NIS
–
Pan: drag · Zoom: scroll · Space: Run/Pause · R: Reset · S: Step
Relative position and covariance
–
Centered at the EKF estimate. The dot shows the position error at the most recent measurement update, truth minus estimate. Ellipses show 1 sigma, 2 sigma, and 3 sigma position uncertainty from the EKF covariance.
Diagnostics
X position
Y position
X velocity
Y velocity
X error ±2σ
Y error ±2σ
Ẋ error ±2σ
Ẏ error ±2σ
Range ρ (km): measurements as dots, prediction as a line
Range rate ρ-dot (km/s): measurements as dots, prediction as a line
Angle phi (rad): measurements as dots, prediction as a line
Stations
Each box is one station. ACTIVE means the station produced a measurement at the last EKF update.
NEES (df=4) + 95% bounds
NIS (df=3m) + 95% bounds
NEES checks state consistency: e = xtrue − x̂, and NEES = eᵀP⁻¹e. NIS checks measurement consistency using the innovation and its covariance. If your filter is well tuned, these curves should usually stay inside the 95% bounds (occasional violations are expected).
u_r, u_t (mm/s²)
‖u‖ (mm/s²) + u_max
Controller state: Δr (km)
Thrusters
Saturation
OK
‖u‖ / umax
0.00
The thrust arrow in the orbit view shows the applied control acceleration. It disappears automatically when control is off or zero.
Tip: when paused, hover on plots to see values. Drag to pan. Scroll to zoom.
Reports GitHub

Improvements to make next

Thruster like control using pulsing and delta V

The demo applies continuous acceleration, which helps compare controllers but does not match many real spacecraft thrusters. A more realistic actuation layer would convert commands into discrete firings with a minimum impulse bit and would track propellant use and delta V. Deadband logic would also reduce chatter driven by estimator noise.

  • Pulse-width or pulse-frequency modulation with a minimum impulse bit
  • Deadband + hysteresis to reduce chatter near the target
  • Report delta V, duty cycle, and missed command cases alongside state response

Constraint aware outer loop

LQR is a strong baseline, but it only handles limits through saturation. A more operational setup adds an outer layer that trades performance against propellant and delta V, and that enforces constraints directly.

  • Per-axis constraints and optional rate/settling constraints
  • Optional MPC or QP layer for constraint handling and allocation
  • Extend Monte Carlo tests to longer dropouts and larger initial errors

Reports and supporting material

ASEN 5044: Statistical Estimation of Dynamic Systems report (PDF)

Orbit determination with nonlinear filtering, station geometry, stacked measurement updates, and NEES/NIS testing.

Open PDF

ASEN 5014: Linear Control Systems report (PDF)

Linearized orbital perturbation model, LQR synthesis and tuning, and performance comparison against a manual controller.

Open PDF

Code

Source code for the demo and supporting scripts.

GitHub

Contact

Interested in discussing GN&C roles or this project? I'd love to chat.
Please reach out by email or LinkedIn below.

View Other Projects Email Me LinkedIn
Copied to Clipboard