Observation and Maneuver Detection for Cislunar Vehicles

Abstract

International interest in the sustained development of cislunar space will generate traffic and debris in the region which requires monitoring; similar to how current space situation awareness is necessary for the traffic and debris near Earth. There are many challenges associated with developing a cislunar situation awareness program, but 2 primary issues addressed by this paper are observational strategies and maneuver detection methods. This work proposes an observational strategy that utilizes the ballistic Optimal Control Based Estimator (OCBE) to filter measurements from cislunar optical observers. To reduce numerical issues associated with filtering, new modifications to the ballistic Optimal Control Based Estimator (OCBE) are introduced that preserve the OCBE update equations in Square Root Information (SRI) space. This new derivation produces a more stable version of the ballistic OCBE which is beneficial for filtering larger data sets with non-linear motion and measurements. Applying the SRI OCBE to the estimation problem it was found that only a single L2 observer with angle and angle-rate measurements provided sufficient information for consistent estimation. Then a newly developed maneuver detection method is presented to statistically identify maneuvers. The method applies a binary hypothesis test to the optimal control policy of the ballistic OCBE to quantify mismodeling. This method was tested given a impulsive maneuver policy with a mean of 50 mm/s and standard deviation of 15 mm/s, and 194 out of 200 tests correctly identified if a maneuver occurred. The OCBE control policy also provided appropriate impulse estimates of mismodeling, which may be used to reconstruct maneuvers in future work. Together, the proposed observation and maneuver detection methodology yields reliable tracking and provides a statistical framework to detect maneuvers.

Introduction

There is significant international interest in developing cislunar space for future exploration and exploitation. First and foremost, NASA’s Artemis plan is exemplary of this growing interest since they plan to develop an outpost and highway for deep space exploration in cislunar space [15]. This vision suggests that there will be a myriad of spacecraft flying through this zone; everything from crewed vehicles to robotic explorers. NASA is not alone in its vision for developing cislunar space however. National and commercial organizations alike have all publicly expressed interest, including oranizatoin such as: JAXA, ESA, CNSA, Roscosmos, ISA, ISRO, SpaceX, Blue Origin, and Dynetics [4, 13, 16]. The growing focus on this area of space will lead to a surge in traffic, and inevitably, debris in the region. In order to ensure long-term use and safety for all cislunar vehicles the congestion on this interplanetary highway will need to be monitored carefully, creating a cislunar space situation awareness need.

Traditional estimation schemes for cislunar spacecraft are predicated on Earth-based radar such as the Deep Space Network (DSN). The DSN has persisted since the Apollo era and it is still a primary source of high accuracy information for current studies and missions [2, 3, 10, 11]. While Earth-based radar provides accurate solutions for cooperative vehicles, it also leads to several issues for persistent observation of all vehicles. First, scheduling DSN time is difficult because it is already heavily subscribed. Alleviating DSN resources has become important enough that it has resulted in various onboard autonomous navigation studies for vehicles planning to use the network [1, 19]. Second, the DSN is not optimized for non-cooperative vehicles and therefore cannot provide the same level of accuracy in those cases. Third, taking measurement from Earth severely limits viewing geometry of cislunar orbits, particularly ones out of the Earth-Moon orbit plane [11].

To alleviate Earth-based resources, cislunar optical observation platforms are examined as an alternative measurement source. Cislunar optical observers are an appealing solution because they utilize existing technology and generally yield an improved viewing geometry over Earth-based sensors. Optical measurements also provide a key benefit by supporting the same degree of accuracy for cooperative and non-cooperative systems. Ideally only a single observer placed near the L2 equilibrium point would be necessary to enable robust estimation, however a variety of observer locations can be explored if a single observer is not sufficient. Thus, this work explores cislunar optical observers to establish a feasible strategy that achieves both persistent and accurate state estimation. Note that for this study the observer state is assumed to be deterministic relative to the target state uncertainty, and future research will investigate the implications of this assumption.

It is important to understand that having cislunar observers does not guarantee an accurate state solution since estimating cislunar spacecraft is exacerbated by the regions of chaos found in cislunar models. Chaotic dynamics means that small perturbations and estimation inaccuracies lead to large predictive errors. These in turn may cause filter divergence if insufficient information is ingested into the estimation algorithm. Beyond filtering issues, these chaotic dynamics also underpin the necessity of a station keeping scheme for spacecraft to maintain its orbit [2, 10, 11]. If station keeping and other events are not accounted for, then the spacecraft will deviate from its nominal trajectory and the track may be lost. This necessitates some form of maneuver compensation, or ideally, detection for long-term estimation purposes.

To account for the expected station keeping maneuvers, a maneuver detection method based on the Optimal Control Based Estimator (OCBE) is presented. The OCBE is a state estimator that provides an estimated optimal control policy that is sensitive to unmodeled forces. An approximation of the distributions of the OCBE control estimate is built via Monte-Carlo simulation. These distributions are then used for a binary hypothesis test to evaluate if a maneuver occurred. Because control estimate is sensitive to unmodeled forces, system health can be tracked to a much finer level than from measurement residuals alone. Together, this maneuver detection method with optical observers is capable of providing consistent and robust tracking for cooperative and non-cooperative systems alike to enhance the safety of all vehicles in cislunar space.

This paper begins by detailing the modeling choices and assumptions for all subsequent simulations. This covers the dynamics, reference orbits, maneuver modeling, and measurements. Next, the state estimation strategy is outlined as a result of subsequent filter behavior given modeling choices from the previous section. This includes newly introduced modifications for the ballistic OCBE, observation strategy results, and filter tuning schemes. Note that while the modifications to the OCBE are presented in the body of this text the full algorithm is given in the appendix. Finally, the maneuver detection methodology and results are presented. This encompasses the binary hypothesis test based on the OCBE control policy, OCBE control policy quantification, and detection results from a Monte-Carlo analysis. The paper concludes by summarizing the work presented and discusses possible future work.

Modeling Definitions

Dynamics

The Circular Restricted 3 Body Problem (CR3BP) approximates cislunar dynamics and trajectories found in the CR3BP hold with small deviations in higher fidelity models [18]. The CR3BP is used here to prove the fundamental application of filtering cislunar because it provides useful insight on cislunar trajectories. Even in higher fidelity models it is generally beneficial to do analysis in a similarly rotating frame. The equations of motion for the CR3BP in the synodic frame are given in their dimensional form in Eq. 2. The dimensional form is used since all of the simulation parameters are given by dimensional parameters, but the non-dimensional equations may be used and converted to the Earth-Moon system if desired.

$$ \begin{array}{@{}rcl@{}} \boldsymbol{n} &=& \begin{bmatrix} 0 \\ 0 \\ \sqrt{\frac{G (m_{1} +m_{2})}{||\boldsymbol{r}_{2}-\boldsymbol{r}_{1}||^{3}}} \end{bmatrix} \end{array} $$

(1)

$$ \begin{array}{@{}rcl@{}} \ddot{\boldsymbol{r}} &=& -2\boldsymbol{n}\times\dot{\boldsymbol{r}} -\boldsymbol{n}\times(\boldsymbol{n}\times \boldsymbol{r}) -\frac{G m_{1}}{||\boldsymbol{r} -\boldsymbol{r}_{1}||^{3}}(\boldsymbol{r} -\boldsymbol{r}_{1}) -\frac{G m_{2}}{||\boldsymbol{r}-\boldsymbol{r}_{2}||^{3}}(\boldsymbol{r} -\boldsymbol{r}_{2}) \end{array} $$

(2)

The synodic frame is a frame that rotates such that the primary and secondary body always lie on the first axis $\hat {\boldsymbol {i}}$. The tertiary axis $\hat {\boldsymbol {k}}$ is aligned with the angular momentum of the system and $\hat {\boldsymbol {j}} = \hat {\boldsymbol {k}} \times \hat {\boldsymbol {i}}$ completes the frame. The origin of this frame is the center of mass of the system. This reference frame is displayed in Fig. 1. There are many periodic orbits in the CR3BP that do not exist in the 2 body problem including Near Rectilinear Halo Orbits (NRHOs).

Fig. 1

CR3BP reference frame

There are 2 southern L2 NRHO reference orbits of interest; an approximate 9:2 resonance orbit which NASA is targeting for its Lunar Gateway and a stable NRHO with a stability index of 1. Both NRHOs are found using a standard single shooting algorithm with initial conditions at apoapsis. Because the initial condition is at apoapsis the initial y position, x velocity, and z velocity are 0. The details of these orbits including the stability index, radius of periapsis, and non-dimensional non-zero initial conditions are given in Table 1.

Table 1 Reference orbit details

The 2 reference orbits, as well as other family members, are displayed in Fig. 2 with respect to the moon. The 9:2 resonance orbit is highlighted in blue and the stable orbit is highlighted in red. Note the axes in Fig. 2 are centered at the Moon’s origin and not the CR3BP origin.

Fig. 2

Southern L2 NRHO family, with the 2 reference orbits highlighted in blue and red

The station keeping model used here is based on previous NRHO station keeping work [2]. The proposed station keeping model functions by burning at apoapsis nearly every orbit. Due to the small station keeping costs it may be impractical, or impossible, to execute the minor burns every revolution. The station keeping costs vary with navigation errors but are capped by 5 m/s impulses and are often on the order of 10’s of mm/s. To approximate these station keeping maneuvers for uncertainty quantification purposes a randomly pointing impulse is added at apoapsis with a magnitude given by a 3-sigma truncated Gaussian. This policy is formally described by Eqs. 3–5.

$$ \begin{array}{@{}rcl@{}} u_{\theta} &=& \mathscr{U}[-\pi,\pi] \end{array} $$

(3)

$$ \begin{array}{@{}rcl@{}} u_{\psi} &=& \mathscr{U}[-\pi/2,\pi/2] \end{array} $$

(4)

$$ \begin{array}{@{}rcl@{}} u_{mag} &=& \mathscr{N}(\mu_{u},\sigma_{u},\mu_{u}-3\sigma_{u},\mu_{u}+3\sigma_{u}) \end{array} $$

(5)

Within the station keeping framework there are 2 policies that are examined. First, a noisy policy with a mean of 1 m/s and standard deviation of 0.3 m/s. Second, a quiet policy with a mean of 50 mm/s and a 15 mm/s standard deviation. This information is summarized in Table 2. Since station keeping maneuvers are generally pointed along the stable manifold this policy encapsulates additional maneuver possibilities since the stable manifold direction is a subset of the fully random pointing. Ensuring the observational system can handle the boarder set includes additional robustness for state estimation purposes.

Table 2 Station keeping policy detail

Measurements

The optical measurements are commonly represented by azimuth and elevation angles provided by the observer. It is assumed that these angles are given in the CR3BP frame when being sent to the filter. The equations for azimuth and elevation are given in Eqs. 7 and 8 respectively. Note that these equations are only a function of the relative position vector from the observer to the object being tracked, which is defined by Eq. 6.

$$ \begin{array}{@{}rcl@{}} \boldsymbol{\rho} &=& \boldsymbol{r} - \boldsymbol{r}_{obs} \end{array} $$

(6)

$$ \begin{array}{@{}rcl@{}} \theta &=& \tan^{-1}(\rho_{j},\rho_{i}) \end{array} $$

(7)

$$ \begin{array}{@{}rcl@{}} \psi &=& \sin^{-1}\left( \frac{\rho_{k}}{||\boldsymbol{\rho}||}\right) \end{array} $$

(8)

The rate of change of these angles are also introduced as measurements for filtering purposes and these rates are analytically derived in Eqs. 10 and 11. These angular rates contain velocity information which is useful since orbits in the CR3BP, particularly NRHOs, are highly sensitive to velocity deviations. Specifically, constraining velocity information around perilune of NRHOs is critical to maintaining custody of targets [2, 12].

$$ \begin{array}{@{}rcl@{}} \dot{\boldsymbol{\rho}} &=& \dot{\boldsymbol{r}} - \dot{\boldsymbol{r}}_{obs} \end{array} $$

(9)

$$ \begin{array}{@{}rcl@{}} \dot{\theta} &=& \frac{1}{(\rho_{j}/\rho_{i})^{2} +1} \left( \frac{\dot{\rho}_{j}}{\rho_{i}} - \frac{\rho_{j} \dot{\rho}_{i}}{{\rho_{i}^{2}}} \right) \\ &=& \frac{\rho_{i} \dot{\rho}_{j} - \rho_{j} \dot{\rho}_{i}}{{\rho_{j}^{2}} +{\rho_{i}^{2}}} \end{array} $$

(10)

$$ \begin{array}{@{}rcl@{}} \dot{\psi} &=& \frac{1}{(1 - (\rho_{k}/||\boldsymbol{\rho}||)^{2})^{1/2}} \left( \frac{\dot{\rho}_{k}}{||\boldsymbol{\rho}||} - \frac{\rho_{k} \dot{||\boldsymbol{\rho}||}}{||\boldsymbol{\rho}||^{2}} \right) \\ &=& \frac{1}{(1 - (\rho_{k}/||\boldsymbol{\rho}||)^{2})^{1/2}} \left( \frac{\dot{\rho}_{k}}{||\boldsymbol{\rho}||} - \frac{\rho_{k} (\boldsymbol{\rho} \cdot \dot{\boldsymbol{\rho}})/||\boldsymbol{\rho}|| }{||\boldsymbol{\rho}||^{2}} \right) \\ &=& \frac{\dot{\rho}_{k} ||\boldsymbol{\rho}||^{2} - \rho_{k} (\boldsymbol{\rho} \cdot \dot{\boldsymbol{\rho}})}{||\boldsymbol{\rho}||^{2}(||\boldsymbol{\rho}||^{2} - {\rho_{k}^{2}})^{1/2}} \end{array} $$

(11)

Angular rate information can be provided by differencing subsequent optical observations, or if the exposure times are long enough, a light streak. The simplest way to incorporate angular rate information would be to apply a numerical differentiation scheme, such as the midpoint rule, over several successive observations. Given that the measurements are independent identically distributed zero mean Gaussian measurements, then the noise of numerical derivative calculated using Eq. 12 is derived following Eq. 13. This derivation holds for $\sigma _{\dot {\psi }}^{2}$ as well.

$$ \begin{array}{@{}rcl@{}} \dot{\theta} &=& \frac{\theta_{2}-\theta_{1}}{t_{2} - t_{1}} \end{array} $$

(12)

$$ \begin{array}{@{}rcl@{}} \sigma_{\dot{\theta}}^{2} &=& \frac{1}{\Delta t^{2}} \left( \sigma^{2}_{\theta} + \sigma^{2}_{\theta}\right) \\ &=& \frac{2}{\Delta t^{2}} \sigma^{2}_{\theta} \end{array} $$

(13)

For the purpose of this paper it is assumed that numerical errors associated with calculating the angular rate are negligible in comparison to the angular measurement noise, and therefore the angular rate noise is only a product of the angular measurements used for the angular rate calculation as shown in Eq. 13. The angular rate measurements are assumed to be taken 1 second apart to conservatively preserve a higher noise estimate. These measurements are also assumed to be processed outside the state estimator and made available to the filter with no correlation to the incoming angular measurements. In a higher fidelity case with real data correlation could be added in the measurement uncertainty matrix.

Note, the presence of angular rate measurements is not meant to add significant information to the system. This is reflected by the fact that chosen rate measurement uncertainty is roughly equivalent to angular measurements. The purpose of explicitly adding the rates as measurements is to stabilize filter updates. It is known that linearized filters struggle with the strongly non-linear dynamics near periapsis and can cause filter failure [2, 12]. The strongly non-linear dynamics cause time updates to produce poor covariance propagation estimates, which in turn causes filter failure. Adding the rate as an explicit measurement forces the filter back to the true solution after the poor time update.

The measurement noise for all following simulations is shown in Table 3. Note that the angular uncertainty is chosen to match previous studies that were based on current optical equipment aimed at autonomous optical navigation, which is likely conservative compared to a dedicated optical observation system [1].

Table 3 Measurement noise

The measurements are taken from a host of potential observer locations in the lunar vicinity. A natural placement for a cislunar observer would be in the vicinity of the L2 equilibrium point for its uninterrupted view of L2 NRHOs. Other attractive observer locations include the lunar surface or on a Distant Retrograde Orbit (DRO). The spacecraft observers, such as the DRO vehicle, include lunar occultations. It was found that while the L2 observers do not experience line-of-sight interruptions, the DRO experiences limited outages that are infrequent and short. The lunar surface observatories are limited by their horizon and therefore require 3 observatories to view the full orbit 9:2 orbit due to the low periapsis coming so close to the lunar surface. All observer locations and the NRHOs are displayed with respect to the Moon’s center in Fig. 3. It is assumed that the state of these observers is known well enough to be considered deterministic for simulation purposes.

Fig. 3

Observer locations in red, and NRHOs in blue

State Estimation

The ballistic Optimal Control Based Estimator (OCBE) [8] is used to produce all state estimate tracks since it is the foundation of the maneuver detection method introduced in the next section. The ballistic OCBE is a generalized Kalman Filter (KF) that provides an estimated optimal control policy that dynamically links state estimates at adjacent measurement epochs. This estimated control policy is highly sensitive to perturbations and thus is a valuable metric for detecting mismodeling in the filter [7]. Control based metrics have been used in previous works to measure the dynamic distance between discrete state estimates, and it was found that they provide effective event detection properties in general [5].

All simulations initialize the OCBE with a random initial error and fixed uncertainty dictated by Earth-radar observing in order to approximate SSA hand-off from Earth-based resources to an optical observer. To be approximately consistent with previous navigation studies the 3-sigma position uncertainty is 10 km and the 3-sigma velocity uncertainty is 10 cm/s [18, 19]. With the filter selected and initialization specified, modifications to the ballistic OCBE are now presented to aid state estimation in the CR3BP. While this section focuses on the modification the full algorithm is given in the appendix for reference. Subsequently, an effective observation scheme is sought to provide consistent and robust estimation given no mismodeling. Finally, a filter tuning strategy to handle maneuvers is presented and its expected results are shown.

Ballistic Optimal Control Based Estimator Modifications

Because the OCBE is a generalization of the KF it can be modified to adhere to common KF techniques and practices. The first enhancement is to adopt a Square-Root Information (SRI) space implementation to reduce numerical instability. This SRI modification is particularly useful for larger data sets with strongly non-linear dynamics and measurements as it conditions the update equations to be more numerically stable [14]. The second modification is the implementation of an extended KF re-linearization scheme to improve convergence. This is extremely important as nominal trajectories diverge quickly due to the presence of hyperbolic instabilities in the CR3BP that can cause filter failure within a single orbit.

To make the SRI adaptation to the ballistic OCBE, it is important to note that both the OCBE time and measurement updates are functions of the state and not dependent on the co-state. Hence, only the state needs to be tracked in SRI space and the co-state can be calculated in state space without loss of accuracy. This also means the OCBE can be converted to SRI space via direct substitution into standard SRI update equations. The base SRI filtering method used here is from Statistical Orbit Determination by Tapley, Schutz, and Born [14], and utilizes orthogonal transformations computed via the Householder transformation which is represented by the matrix T.

Implementing the SRI algorithm for the OCBE begins with the time update equations. The OCBE time update equations are nearly identical to the standard KF time update equations except for the KF process noise term in the covariance update. The OCBE does not include a process noise term explicitly, but instead calculates a process noise-like term based on the OCBE State Transition Matrix (STM). The OCBE STM is unique in that it propagates both state and co-state at once. The OCBE STM from time t_n to t_n+ 1 is broken down in parts for update purposes as described in Eq. 14. The time dependence is dropped moving forwards for notational simplicity. The sub-matrix Φ_xx is the standard KF STM as it adheres to the same dynamics.

$$ \boldsymbol{\Phi}(t_{n+1},t_{n}) = \begin{bmatrix} \boldsymbol{\Phi}_{xx}&\boldsymbol{\Phi}_{xp}\\\boldsymbol{\Phi}_{px}&\boldsymbol{\Phi}_{pp}\end{bmatrix} \\ $$

(14)

The STM is used to write the covariance time update equation which is given by Eq. 15. This is the same generalized form of a standard KF update which is given by Eq. 16. Equating these update functions leads to the definition of an OCBE process noise term given by Eq. 17. Realizing this demonstrates an important property of the OCBE; the OCBE calculates a process noise like term through state and co-state dynamics and thus removes the need to pre-define a process noise covariance. Note that subscripts on filter estimates like P_n+ 1|n read as P at time t_n+ 1 given information up to time t_n.

$$ \begin{array}{@{}rcl@{}} \boldsymbol{P}_{n+1|n} &=& \boldsymbol{\Phi}_{xx} \boldsymbol{P}_{n|n} \boldsymbol{\Phi}_{xx}^{T} - \boldsymbol{\Phi}_{xp} \boldsymbol{\Phi}_{xx}^{T} \end{array} $$

(15)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{P}_{n+1|n} &=& \boldsymbol{\Phi}_{xx} \boldsymbol{P}_{n|n} \boldsymbol{\Phi}_{xx}^{T} + \boldsymbol{Q} \end{array} $$

(16)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{Q} &=& - \boldsymbol{\Phi}_{xp} \boldsymbol{\Phi}_{xx}^{T} \end{array} $$

(17)

To transition to SRI space the SRI update equation needs to be addressed. The SRI dynamics include a process noise state, δq, as well as a process noise state transition matrix Γ(t_n+ 1,t_n). Again, the time dependence is dropped for notational simplicity. Given these dynamics the time updated uncertainty takes the form Eq. 18. For this equation to be consistent with the KF definition in Eq. 16, the trivial solution is given by Eqs. 19–20.

$$ \begin{array}{@{}rcl@{}} \boldsymbol{P}_{n+1|n} &=& \boldsymbol{\Phi}_{xx} \boldsymbol{P}_{n|n} \boldsymbol{\Phi}_{xx}^{T} + \boldsymbol{\Gamma} \hat{\boldsymbol{Q}} \boldsymbol{\Gamma}^{T} \end{array} $$

(18)

$$ \begin{array}{@{}rcl@{}} \hat{\boldsymbol{Q}} &=& \boldsymbol{Q} \end{array} $$

(19)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{\Gamma} &=& \boldsymbol{I} \end{array} $$

(20)

Given there is never any a priori knowledge of the process noise the term b_q,n|n is always 0. Similarly, since the filter is re-linearized every step b_x,n|n is also always set to 0. Then plugging in Eqs. 17, 19, and 20 to the standard SRI update equations, the extended SRI OCBE state update is defined by Eqs. 21–23. The co-state and estimated control can be calculated in state space given the original OCBE equations after the time update.

$$ \begin{array}{@{}rcl@{}} \boldsymbol{P}_{n|n} &=& \boldsymbol{S}_{x,n|n}^{-1} \boldsymbol{S}_{x,n|n}^{-T} \end{array} $$

(21)

$$ \begin{array}{@{}rcl@{}} - \boldsymbol{\Phi}_{xp} \boldsymbol{\Phi}_{xx}^{T} &=& \boldsymbol{S}_{q,n|n}^{-1} \boldsymbol{S}_{q,n|n}^{-T} \end{array} $$

(22)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{T} \begin{bmatrix} \boldsymbol{S}_{q,n|n} & \boldsymbol{0} & \boldsymbol{0} \\ -\boldsymbol{S}_{x,n|n} \boldsymbol{\Phi}_{xx}^{-1} & \boldsymbol{S}_{x,n|n} \boldsymbol{\Phi}_{xx}^{-1} & \boldsymbol{0} \end{bmatrix} &=& \begin{bmatrix} \boldsymbol{S}_{q,n+1|n} & \boldsymbol{S}_{qx,n+1|n} & \boldsymbol{b}_{q,n+1|n} \\ \boldsymbol{0} & \boldsymbol{S}_{x,n+1|n} & \boldsymbol{b}_{x,n+1|n} \end{bmatrix} \end{array} $$

(23)

The measurement update equations are formulated identically between the OCBE, KF, and SRI filters and thus no changes need to be made to apply the SRI update to the OCBE. To complete the re-linearization the delta state is added to the nominal state and so the process can be repeated for the next measurement. The measurement update is described by Eq. 24–28. Note that the measurement at time t_n+ 1 is denoted y_n+ 1 and that h(t_n+ 1,x_n+ 1|n) is the non-linear estimate of the measurement given the current state estimate.

$$ \begin{array}{@{}rcl@{}} \delta \boldsymbol{y}_{n+1} &=& \boldsymbol{y}_{n+1} - h(t_{n+1},\boldsymbol{x}_{n+1|n}) \end{array} $$

(24)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{H}_{n+1} &=& \frac{\partial h}{\partial \boldsymbol{x}}(t_{n+1},\boldsymbol{x}_{n+1|n}) \end{array} $$

(25)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{S}_{y}^{-1} \boldsymbol{S}_{y}^{-T} &=& \boldsymbol{R}_{n+1} \end{array} $$

(26)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{T} \begin{bmatrix} \boldsymbol{S}_{x,n+1|n} & \boldsymbol{b}_{x,n+1|n}\\ \boldsymbol{S}_{y}^{-1} \boldsymbol{H}_{n+1} & \boldsymbol{S}_{y}^{-1} \delta \boldsymbol{y}_{n+1} \end{bmatrix} &=& \begin{bmatrix} \boldsymbol{S}_{x,n+1|n+1} & \boldsymbol{b}_{x,n+1|n+1} \\ \boldsymbol{0} & \boldsymbol{e}_{x,n+1} \end{bmatrix} \end{array} $$

(27)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{x}_{n+1|n+1} &=& \boldsymbol{x}_{n+1|n} + \boldsymbol{S}_{x,n+1|n+1}^{-1} \boldsymbol{b}_{x,n+1|n+1} \end{array} $$

(28)

In order to address the OCBE co-state update, the single step smoothing property of the OCBE is leveraged [8]. First, a single step SRI smoother is applied to get the smoothed state. Then the co-state is calculated as a function of the difference from the forward and smoothed state. This process is given by Eqs. 29–32.

$$ \begin{array}{@{}rcl@{}} \boldsymbol{T} &&\begin{bmatrix} \boldsymbol{S}_{q,n+1|n} + \boldsymbol{S}_{qx,n+1|n} & \boldsymbol{S}_{qx,n+1|n} \boldsymbol{\Phi}_{xx} & \boldsymbol{b}_{q,n+1|n} \\ \boldsymbol{S}_{x,n+1|n+1} & \boldsymbol{S}_{x,n+1|n+1} \boldsymbol{\Phi}_{xx} & \boldsymbol{S}_{x,n+1|n+1} (\boldsymbol{x}_{n+1|n+1} -\boldsymbol{x}_{n+1|n}) \end{bmatrix} \end{array} $$

(29)

$$ \begin{array}{@{}rcl@{}} &&\qquad= \begin{bmatrix} \boldsymbol{S}_{q,n|n+1} & \boldsymbol{S}_{qx,n|n+1} & \boldsymbol{b}_{q,n|n+1} \\ \boldsymbol{0} & \boldsymbol{S}_{x,n|n+1} & \boldsymbol{b}_{x,n|n+1} \end{bmatrix}\\ &&\boldsymbol{x}_{n|n+1} = \boldsymbol{x}_{n|n} + \boldsymbol{S}_{x,n|n+1}^{-1} \boldsymbol{b}_{x,n|n+1} \end{array} $$

(30)

$$ \begin{array}{@{}rcl@{}} &&\boldsymbol{p}_{n|n+1} = -\boldsymbol{P}_{n|n}^{-1} (\boldsymbol{x}_{n|n+1} -\boldsymbol{x}_{n|n}) \end{array} $$

(31)

$$ \begin{array}{@{}rcl@{}} &&\hat{\boldsymbol{u}}(t) = -\tilde{\boldsymbol{Q}} \boldsymbol{B}^{T} \boldsymbol{\Phi}_{pp}(t,t_{n}) \boldsymbol{p}_{n|n+1} \end{array} $$

(32)

With these modifications in place the SRI smoothing algorithms can be directly applied to OCBE state smoothing without any changes. The full extended SRI OCBE filter and smoother algorithms are given in the appendix. With the filter modifications complete a viable observation strategy can now be sought.

Observation Strategy

The objective of the observation strategy analysis is to determine the requirements for an optical measurement scheme to provide robust and consistent state estimation for the given reference orbits using the linearized filters. To begin, a simulation with no mismodeling or maneuvers is instantiated to determine if a measurement scheme provides feasible estimation. These simulations are started at apoapsis to allow the filter to decrease its uncertainty before periapsis. Decreasing uncertainty prior to periapsis is desirable because it is the region most sensitive to perturbations and therefore the most likely location for filter divergence.

First, a simple angle only measurement approach is examined. Given a single L2 observer and varied measurement frequencies ranging from 1 minute to 12 hours, every strategy leads to filter divergence before periapsis for both reference orbits. In an attempt to constrain the system with additional geometric information the L2 observer was supplemented with a DRO observer and 3 lunar stations. Even with this additional information the unstable and 9:2 NRHO filter nearly always fail. In an all-out attempt to enable filter convergence, both the measurement accuracy was increased and initial covariance reduced by an order of magnitude. The filter estimate still diverged from the true solution.

An example of the 9:2 NRHO filter divergence is shown in Fig. 4. This example includes all 5 observers taking measurements every hour, as well as reduced measurements and initial uncertainty by an order of magnitude. The figures display the true error in blue and the 3-sigma filter uncertainty in red. The plots are also split into position and velocity space by taking the norm error and uncertainty respectively, with the top being position and bottom being velocity. A filter that produces a consistent solution has the true error bounded below the filter uncertainty. Normally the smoothed solution is also shown, but since the forwards filter fails the smoothed solution is completely erroneous.

Fig. 4

Typical 9:2 NRHO filter divergence with angle only measurements from L2, DRO, and lunar surface observers. This particular example has improved measurements and initial uncertainty, and the filter estimate still diverged from the true solution

Next, angle and angle rate measurements are examined. This time a single L2 observer with measurement frequencies from 1 minute to 12 hours lead to consistent filtering solutions in every scenario. As expected higher measurement frequencies lead to greater filter certainty over a single orbit. Final uncertainty levels given various measurement rates on both reference orbits are given in Table 4.

Table 4 Idealistic final filter covariance levels for various measurement rates given no mismodeling

As anticipated, the stable NRHO filter produces qualitatively similar but quantitatively better results than the 9:2 NRHO filter. All rates produce consistent filters that properly track the true state so the measurement rate can be used to set the desired filter accuracy. A typical successful 9:2 NRHO filter result is plotted in Fig. 5. Since only 1 observer is necessary to produce a working system the other observers are ignored.

Fig. 5

A typical working 9:2 NRHO filter with angular and angular rate measurements with measurements every 2 hours from a single L2 observer. The forwards filter is on the left and the smoothed filter is on the right

These results exemplify the fact that severe non-linearity needs to be addressed for linearized filtering in the CR3BP. This is most notably apparent by the fact taking angular measurements at a high enough frequency should lead to equivalent information ingested into the filter as taking angular rate measurements, but as found in the results it still led to filter failure. This is because the issue is not insufficient information content. The issue is the fact that the 1st-order linearized filter cannot accurately represent the strongly non-linear dynamics. Adding angular rate measurements assuages this issue enough to produce working linearized filters. This negates the need for higher order filters and maintains the speed and simplicity of a linearized filter.

Given these results, the chosen observation strategy used for the rest of the paper is a single L2 observer with angle and angular rate measurements taken every 2 hours. This rate is chosen as a low demand requirement that balances filter accuracy and algorithm run-time, and appears to be a sweet spot for the 9:2 NRHO. With the observation strategy outlined the filter method of maneuver processing can now be addressed.

Filter Tuning Strategy

The OCBE handles mismodeled dynamics by including a fictitious control policy that absorbs errors and represents the errors as an optimal control policy. This control policy is similar to a process noise term and needs to be tuned in a similar manner. A primary difference between the control policy and process noise is that the control policy is constrained to follow system dynamics and is calculated via the STM. While this removes the need to explicitly define a process noise term, it does not remove the tuning process since the control uncertainty term determines the OCBE performance.

Adaptive OCBE Tuning

When picking a tuning method for the OCBE there are several options. The most attractive option is the entirely autonomous adaptive OCBE algorithm, which identifies and tunes filter uncertainty automatically. The principle behind the adaptive OCBE is that it defines a statistical metric derived from the estimated control, measurement, and a priori uncertainty to determine if a maneuver occurs. If the metric remains above a probable limit, set to 99% for this paper, for 3 consecutive epochs then a maneuver is detected at the first epoch. If a maneuver is detected, then the filter solves for a state covariance that sets the metric at that epoch equal to its mean value. This algorithm produces near optimal filtering results for impulsive system with large maneuvers as it automatically adjusts the control uncertainty to reflect the true dynamics.

To demonstrate the adaptive OCBE performance on a sufficiently larger maneuver a simulation was created with a 3 m/s maneuver at apoapsis on the 9:2 NRHO reference orbit. The simulation is started at periapsis to allow the filter to reduce its uncertainty before the station keeping maneuver. All other simulation parameters remain the same as previously defined. The state estimation results for the forwards and smoothing process are displayed in Fig. 6. The norm of the estimated control policy and the normalized OCBE metric are displayed in Fig. 7. The normalized OCBE metric is the OCBE metric divided by the maneuver detection limit. The OCBE metric figures plot the calculated metric in blue, and the statistical metric mean in red.

Fig. 6

Adaptive tuning state estimate performance given a 3 m/s impulse at apoapsis. The filter automatically increases its uncertainty during the maneuver in the forwards process, which is at the 1/2 orbit period, to maintain a correct filter solution

Fig. 7

Filter control estimates are plotted on top, and OCBE metrics on the bottom for the adaptively tuned system. Again, the forwards filter is on the left and the smoothed filter is on the right

The filter was able to autonomously identify the maneuver and adjust its uncertainty to reflect the control uncertainty necessary to reduce the OCBE metric. This appears as an instantaneous increase in filter uncertainty in the forwards solution, and spike in control estimates in the forwards and smoothed solutions. It also appears as an outlier in the forwards metrics, which is subsequently removed in the smoothed metrics. This is the preferred tuning method given maneuvers over 1 m/s, but unfortunately this method doesn’t perform as well for the smaller station keeping maneuvers.

Constant OCBE Tuning

As the maneuver magnitude decreases the detection becomes delayed as it takes time for the information to transition into the metric. This is demonstrated with a simulation that replaced the 3 m/s with a 50 mm/s maneuver. Filter results given this reduced maneuver are displayed in Figs. 8 and 9.

Fig. 8

Filter performance for adaptively tuned system given a 50 mm/s impulse at apoapsis. The filter doesn’t catch the deviation until nearly periapsis where it adjusts its uncertainty to reconverge to the true solution

Fig. 9

Filter control estimates and metrics for adaptive system given a 50 mm/s maneuver which goes undetected for a long time

The filter is unable to immediately detect the minute change in trajectory, and error builds as the true system drifts from the estimate. Eventually the error builds to a point where the filter can detect a modeling error and makes a significant correction to its uncertainty bringing the filter back to the true solution. This does fix the filter for the later epochs but leaves a large portion of the estimated trajectory to be erroneous. If the user only cares about a forwards solution at the final time this may be acceptable, but if navigation during that arc was necessary then this leaves much to be desired.

Instead of adaptively tuning the filter the system can simply contain a larger control uncertainty that would account for the maneuver. This produces a filter with a greater overall uncertainty but also results in a consistent state estimate for the entire observation window. An example of taking this approach is displayed in Figs. 10 and 11.

Fig. 10

Filter performance with increased control uncertainty given a 50 mm/s maneuver. Both the forwards and smoothed filters produce consistent, but less certain, solutions

Fig. 11

Filter control estimates and metrics with increased control uncertainty given a 50 mm/s maneuver. The maneuver appears as a spike in estimated control only in the smoothed system

Given this approach the event is obscured in the forwards solution, but it is still visible as a control effort spike in the smoothed solution. This control spike does not appear if there is no maneuver, and thus control policy can be used to distinguish maneuvers. This is the basis for the maneuver detection methodology proposed in the next section.

Proposed OCBE Tuning

For this study the filter mismodeling is limited to the station keeping maneuvers, and thus it makes sense to limit the control uncertainty to around apoapsis. Therefore, the final tuning strategy is to increase the control uncertainty for an 8 hour window around apoapsis to account for the unknown station keeping maneuver. A typical filter result given this approach is displayed in Figs. 12 and 13. This produces a consistent filter with a smoothed control spike around apoapsis that is indicative of a maneuver.

Fig. 12

Filter performance for appropriately tuned system given a small maneuver which still performs well and identifies maneuver

Fig. 13

Filter control estimates and metrics for appropriately tuned system given a small maneuver which still performs well and identifies maneuver

This limited control uncertainty approach exemplifies the estimation strategy for all future simulations. An Earth-based radar estimate is handed off to the optical observer at periapsis, where the system is observed for a single orbit period by a L2 observer with angular and angular-rate measurements. A measurement cadence of 1 observation per 2 hours is adopted, and control uncertainty is increased over an 8 hour window at apoapsis to account for the expected station keeping maneuver. An observer can use this strategy to maintain custody of the craft for an indefinite time as it performs station keeping maneuvers on a NRHO.

Maneuver Detection

The methodology proposed for maneuver detection is a binary hypothesis test on the integral of OCBE control policy, which represents the total estimated control on the system. This approach is chosen because the sensitivity to accelerations of the control estimate makes it an ideal indicator of mismodeling, while the OCBE’s filtering properties act to minimize the effects of noisy measurements and initial errors to provide low variance results. The overall process for maneuver detection is as follows: apply the OCBE to the estimation problem, integrate the estimated control policy from the OCBE, and preform a hypothesis test to determine if a maneuver has occurred.

Hypothesis Testing Using Optimal Control Policies

The null hypothesis (${\mathscr{H}}_{0}$) is that the observed system did not maneuver, and the alternative (${\mathscr{H}}_{1}$) is that it did maneuver. The test statistic, the integral of the OCBE control profile, is a random value denoted by Z. A realization of the random variable, z, is calculated from the OCBE control policy by Eq. 33.

$$ z(u) = {\int}_{t_{0}}^{t_{f}} ||u(t)|| dt $$

(33)

There are many approaches to binary hypothesis tests, but the approach taken here is maximum likelihood decision rule. This rule is a conservative approach that assumes an equivalent cost for incorrect decisions and equally likely a-priori hypothesis probabilities. Taking this approach yields the decision function Eq. 34. The derivation for this approach is found in many texts, but it is cleanly derived from a Bayesian minimum error sense in [6].

$$ D(z) = \left\{ \begin{array}{@{}ll@{}} \mathscr{H}_{0} & \text{if } PDF_{\mathscr{H}_{0}}(z) \ge PDF_{\mathscr{H}_{1}}(z)\\ \mathscr{H}_{1} & \text{otherwise} \end{array} \right. $$

(34)

In order to perform this hypothesis test the distributions for Z need to be determined or numerically approximated via an uncertainty quantification method. The uncertainty quantification method of choice for this paper is a Monte-Carlo analysis. Beyond being necessary for the hypothesis test, approximating these distributions also allows for a better understanding of OCBE control policy which is often non-intuitive.

Control Policy Distributions

The test statistic Z is random variable dependent on the stochastic inputs to the filter which are: a station keeping maneuver, measurements, and initial filter error. For the case where the system does not maneuver then it is only dependent on the measurements and initial error. Thus, when numerically approximating the non-maneuvering PDF the input dimensionality is lower and requires less samples to return the same level of accuracy. As a result, the non-maneuvering Monte-Carlo analysis only contains 500 sample simulations, while maneuvering case contains 3000 sample simulations.

Beginning with the active station keeping policy with a mean maneuver size of 1 m/s and standard deviation of 0.3 m/s, it is clear that non-maneuvering systems produce statistically smaller control policies than maneuvering systems. The histograms of the control policy integrals for the 9:2 NRHO Monte-Carlo are displayed in Fig. 14. PDFs for these distributions are numerically calculated and plotted in Fig. 15. The maneuvering system appears to have an approximately Gaussian shape which is reassuring since the station keeping magnitude comes from a truncated Gaussian.

Fig. 14

Histogram of Monte-Carlo results for 1 m/s mean non-maneuvering and maneuvering case

Fig. 15

Numerical PDF of Monte-Carlo results for 1 m/s mean non-maneuvering and maneuvering case

From examining these figures it is clear that $PDF_{{\mathscr{H}}_{0}}(z)$ only crosses with $PDF_{{\mathscr{H}}_{1}}(z)$ once. Therefore, the maximum likelihood rule can be simplified to Eq. 35. For the active case presented here z_lim = 0.4106 m/s. These PDFs and z_lim values are used to calculate the probability that ${\mathscr{H}}_{0}$ is incorrectly rejected or that ${\mathscr{H}}_{0}$ is incorrectly accepted. This is done by integrating the PDF of ${\mathscr{H}}_{0}$ above z_lim, and ${\mathscr{H}}_{1}$ below z_lim. These rates are more commonly called false positive and missed detection rates respectively. The expected hypothesis test results for the active policy are given in Table 5.

$$ D(z) = \left\{ \begin{array}{ccc} \mathscr{H}_{0} & \text{if } z(u) \le z_{lim}\\ \mathscr{H}_{1} & \text{otherwise} \end{array} \right. $$

(35)

Table 5 Expected 9:2 NRHO maneuver detection probability for the active station keeping policy based on Monte-Carlo results

To further investigate the control policy’s sensitivity to maneuvers, the Monte-Carlo results are plotted against the station keeping maneuver parameters in Fig. 16. The results show a bi-modal distribution with peaks corresponding to large maneuvers pointing in the $\pm \hat {j}$ direction, which corresponds to an azimuth of ± 90 deg. This makes sense since maneuvers in this direction are aligned with the measurement sensitivity matrix at apoapsis. Given these results it may be possible to roughly estimate the maneuver parameters that result in a given z value, but that is future work. The stable NRHO produced similar results to the 9:2 NRHO, so the plots are omitted.

Fig. 16

Scatter results of the control integral over station keeping parameters from the Monte-Carlo analysis given the active policy

The control policy estimate was also plotted against the station keeping maneuver magnitude and the results are in Fig. 17. It can be observed from the figures that the optimal control policy returns an appropriate representation of the true mismodeling and could be used to estimate maneuver magnitude because of the apparently linear dependence.

Fig. 17

Scatter results of the control integral over station keeping magntidue only from the Monte-Carlo analysis given the active policy

The quiet station keeping policy with a mean maneuver size of 50 mm/s and standard deviation of 15 mm/s produced similar distributions, excepting the correlation between azimuth and elevation of the station keeping policy appears less bi-modal and more uniform. The histograms of the control policy integrals for the 9:2 NRHO are in Fig. 18 and the numerical PDFs are in in Fig. 19. Given these results z_lim = 17.22 mm/s and the expected hypothesis test results are given in Table 6.

Fig. 18

Histogram of Monte-Carlo results for 50 mm/s mean non-maneuvering and maneuvering case

Fig. 19

Numerical PDF of Monte-Carlo results for 50 mm/s mean non-maneuvering and maneuvering case

Table 6 Expected 9:2 NRHO maneuver detection probability for the quiet station keeping policy based on Monte-Carlo results

A scatter plot of the maneuvering Monte-Carlo results is displayed in Fig. 20 to show how the control policy varies with maneuver parameters, and Fig. 21 shows just the relation between magnitudes. Once again, the stable NRHO produced similar results to the 9:2 NRHO so the plots are omitted.

Fig. 20

Scatter of Monte-Carlo results for 50 mm/s mean maneuvering case over input parameters

Fig. 21

Scatter results of the control integral over station keeping magnitude only from the Monte-Carlo analysis given the active policy

It is important to note that 2 different distributions are examined here to demonstrate the flexibility of the algorithm on ’larger’ and ’smaller’ expected maneuvers. When applying this to a real system the level of station keeping should be readily available since there are numerous studies on station keeping for NRHOs which detail expected maneuver sizes [2, 9, 10, 17, 18]. If, for some reason, this information is not readily available then one simply needs to run a bank of filters with various levels of expected maneuvers. The filter that performs the best should then be selected. Performance can be based on standard metrics such as residual levels, normalized innovation squared testing, or by selecting the filter that returns the most likely control policy.

Maneuver Detection

With the OCBE control policy distributions approximated, test data with unknown maneuvers is now examined. 100 maneuvering and 100 non-maneuvering data sets were generated for every policy and reference orbit, and the integrated control policy from the unknown data was used to test for maneuvers given Eq. 35.

For the 9:2 NRHO with active station keeping, the maneuver tests returned an accuracy of 100%, and with quiet station keeping the maneuver tests returned an accuracy of 97%. The full results are in Table 7. The stable NRHO had a marginally better performance and the results for the stable NRHO are in Table 8.

Table 7 9:2 NRHO Maneuver Detection Results

Table 8 Stable NRHO Maneuver Detection Results

Control as a Health Metric

In addition to being able to identify, and potentially characterize, stochastic maneuvers the integral of the control policy can be utilized as a fast acting health metric for the OCBE. The fast acting nature of this metric is due to the control metrics sensitivity to mismodeling, which often shows significant deviations before measurement residuals do. This behavior is demonstrated in the following simulation where the OCBE is tuned for the quiet station keeping policy, but the observed system undergoes a larger maneuver of 0.5 m/s. The smoothed state estimate and measurement residuals are plotted in Fig. 22.

Fig. 22

Filter performance given a larger maneuver than expected. The filter estimate diverges (left) but the measurement residuals appear benign (right) missing the maneuver. The optimal control integral z was approximately 30σ though which does catch the maneuver

x Because the system is not tuned to compensate for such a large maneuver the filter diverges, but this divergence does not appear evident in the measurement residuals. Most common filters rely on measurement residuals to indicate filter deterioration, and therefore would miss this filter divergence. However, when the control policy is examined for this system z = 0.4761 m/s. This is approximately at 30σ of the maneuvering distribution of Z, thus is strongly indicating mismodeling. Not only is it indicative of mismodeling, but is also produces an appropriate estimate of the maneuver size. This demonstrates the value of control policy as a fast acting health metric for filtering.

Conclusion

Cislunar SSA is crucial to maintain the safety of spacecraft going to the Moon and beyond. This task comes with a variety of challenges and is exacerbated by limited Earth-based resources and chaotic dynamics. First, the results in “State Estimation” reformulated the ballistic OCBE into SRI space to help reduce numerical instabilities related to filtering. Then the results went on to show that including angular rate information as a measurement in linearized filters is critical to the success of optical cislunar observation and, given angular rate measurements, only a single observer is necessary to provide accurate state estimation.

Next, to ensure an accurate solution for cislunar vehicles then some form of maneuver detection or compensation needs to be implemented to handle the necessary station keeping maneuvers for cislunar vehicles. In this paper this is accounted for via the newly presented hypothesis testing utilizing the OCBE’s control policy. Results from “Maneuver Detection” show that maneuvers with a mean of 50 mm/s and a 15 mm/s standard distribution where correctly identified 194 out of 200 times for the 9:2 NRHO and 200 out of 200 for the stable NRHO. The value of OCBE control as a health metric is also demonstrated as it experiences large deviations to its expected value before measurement residuals do.

This work opens the door for various future developments. First, the demonstrated methodology should be implemented in a high fidelity model subject to additional dynamic, observer state, and measurement uncertainty to verify the results remain as accurate. Second, the Monte-Carlo approximation of the control policy distribution can be used to reconstruct the unknown maneuver. Lastly, the filter and maneuver detection performance should be examined given multiple unmodeled events in a single orbit period. Expanding on the presented work in this way would lead to improved tracking performance for cislunar vehicles.

Appendix:

This implementation of the ballistic OCBE is in SRI space and thus SRI variables such as the SRI measurement uncertainty, information matrix, and SRI state are introduced in Eqs. 36–38 respectively.

$$ \begin{array}{@{}rcl@{}} \boldsymbol{R}&=& \boldsymbol{S}_{y}^{-1} \boldsymbol{S}_{y}^{-T} \end{array} $$

(36)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{P} &=& \boldsymbol{S}_{x}^{-1} \boldsymbol{S}_{x}^{-T} \end{array} $$

(37)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{S}_{x} \delta \boldsymbol{x} &=& \boldsymbol{b}_{x} \end{array} $$

(38)

This implementation makes the assumption that the unmodeled perturbations are forces and the uncertainty of these forces are equal in every direction. This implementation is consistent with being given no a priori knowledge about that events which is commonly the case for state estimation problems. This assumption is enforced by the B and $\tilde {\boldsymbol {Q}}$ matrices. The level of control uncertainty is defined as $\tilde {q}$ and this term is used to tune the filter. This logic is also reflected in the SRI time update equations by setting the noise state b_q to zero as well.

To define the ballistic OCBE, the OCBE STM must be defined. The ballistic STM is given by Eqs. 39–43. The STM is integrated via a numerical ODE solver given the nominal state, start time, and final time.

$$ \begin{array}{@{}rcl@{}} \boldsymbol{B} &=& \begin{bmatrix} \boldsymbol{0} \\ \boldsymbol{I} \end{bmatrix} \end{array} $$

(39)

$$ \begin{array}{@{}rcl@{}} \tilde{\boldsymbol{Q}} &=& \tilde{q} \boldsymbol{I} \end{array} $$

(40)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{\Phi}(n,n) &=& \boldsymbol{I} \end{array} $$

(41)

$$ \begin{array}{@{}rcl@{}} \dot{\boldsymbol{\Phi}} &=& \begin{bmatrix} \frac{\partial f}{\partial \boldsymbol{x}} & -\boldsymbol{B} \tilde{\boldsymbol{Q}} \boldsymbol{B}^{T} \\ \boldsymbol{0} & -\frac{\partial f}{\partial \boldsymbol{x}}^{T} \end{bmatrix} \boldsymbol{\Phi} \end{array} $$

(42)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{\Phi}(t_{n+1},t_{n}) &=& \begin{bmatrix} \boldsymbol{\Phi}_{xx}&\boldsymbol{\Phi}_{xp}\\\boldsymbol{\Phi}_{px}&\boldsymbol{\Phi}_{pp}\end{bmatrix} \end{array} $$

(43)

Given the OCBE STM the SRI OCBE time update from time t_n to t_n+ 1 is calculated as given in Eqs. 44–45. The co-state does not need to be updated as it is assumed to be zero in the ballistic OCBE.

$$ \begin{array}{@{}rcl@{}} \boldsymbol{S}_{q,n|n}^{-1} \boldsymbol{S}_{q,n|n}^{-T} &=& - \boldsymbol{\Phi}_{xp} \boldsymbol{\Phi}_{xx}^{T} \end{array} $$

(44)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{T} \begin{bmatrix} \boldsymbol{S}_{q,n|n} & \boldsymbol{0} & \boldsymbol{0} \\ -\boldsymbol{S}_{x,n|n} \boldsymbol{\Phi}_{xx}^{-1} & \boldsymbol{S}_{x,n|n} \boldsymbol{\Phi}_{xx}^{-1} & \boldsymbol{0} \end{bmatrix} &=& \begin{bmatrix} \boldsymbol{S}_{q,n+1|n} & \boldsymbol{S}_{qx,n+1|n} & \boldsymbol{b}_{q,n+1|n} \\ \boldsymbol{0} & \boldsymbol{S}_{x,n+1|n} & \boldsymbol{b}_{x,n+1|n} \end{bmatrix} \end{array} $$

(45)

After the time update the measurement state update is performed given Eqs. 46–49. This only updates the state and covariance.

$$ \begin{array}{@{}rcl@{}} \delta \boldsymbol{y}_{n+1} &=& \boldsymbol{y}_{n+1} - h(t_{n+1},\boldsymbol{x}_{n+1|n}) \end{array} $$

(46)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{H}_{n+1} &=& \frac{\partial h}{\partial \boldsymbol{x}}(t_{n+1},\boldsymbol{x}_{n+1|n}) \end{array} $$

(47)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{T} \begin{bmatrix} \boldsymbol{S}_{x,n+1|n} & \boldsymbol{b}_{x,n+1|n}\\ \boldsymbol{S}_{y}^{-1} \boldsymbol{H}_{n+1} & \boldsymbol{S}_{y}^{-1} \delta y_{n+1} \end{bmatrix} &=& \begin{bmatrix} \boldsymbol{S}_{x,n+1|n+1} & \boldsymbol{b}_{x,n+1|n+1} \\ \boldsymbol{0} & \boldsymbol{e}_{x,n+1} \end{bmatrix} \end{array} $$

(48)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{x}_{n+1|n+1} &=& \boldsymbol{x}_{n+1|n} + \boldsymbol{S}_{x,n+1|n+1}^{-1} \boldsymbol{b}_{x,n+1|n+1} \end{array} $$

(49)

After the state update the co-state and control is now calculated via Eqs. 50–53. These equations leverage the fact that the OCBE contains single step smoothing and that the co-state can be calculated as a function of the nominal and smoothed state.

$$ \begin{array}{@{}rcl@{}} \boldsymbol{T}&& \begin{bmatrix} \boldsymbol{S}_{q,n+1|n} + \boldsymbol{S}_{qx,n+1|n} & \boldsymbol{S}_{qx,n+1|n} \boldsymbol{\Phi}_{xx} & \boldsymbol{b}_{q,n+1|n} \\ \boldsymbol{S}_{x,n+1|n+1} & \boldsymbol{S}_{x,n+1|n+1} \boldsymbol{\Phi}_{xx} & \boldsymbol{S}_{x,n+1|n+1} (\boldsymbol{x}_{n+1|n+1} -\boldsymbol{x}_{n+1|n}) \end{bmatrix} \\ &&\qquad= \begin{bmatrix} \boldsymbol{S}_{q,n|n+1} & \boldsymbol{S}_{qx,n|n+1} & \boldsymbol{b}_{q,n|n+1} \\ \boldsymbol{0} & \boldsymbol{S}_{x,n|n+1} & \boldsymbol{b}_{x,n|n+1} \end{bmatrix} \end{array} $$

(50)

$$ \begin{array}{@{}rcl@{}} &&\boldsymbol{x}_{n|n+1} = \boldsymbol{x}_{n|n} + \boldsymbol{S}_{x,n|n+1}^{-1} \boldsymbol{b}_{x,n|n+1} \end{array} $$

(51)

$$ \begin{array}{@{}rcl@{}} &&\boldsymbol{p}_{n|n+1} = -\boldsymbol{P}_{n|n}^{-1} (\boldsymbol{x}_{n|n+1} -\boldsymbol{x}_{n|n}) \end{array} $$

(52)

$$ \begin{array}{@{}rcl@{}} &&\hat{\boldsymbol{u}}(t) = -\tilde{\boldsymbol{Q}} \boldsymbol{B}^{T} \boldsymbol{\Phi}_{pp}(t,t_{n}) \boldsymbol{p}_{n|n+1} \end{array} $$

(53)

Once the forwards filter is complete, it is often desirable to smooth the solution to return a better estimate for the entire trajectory. Given information up to time t_l the smoothing process is performed by iteratively applying Eqs. 54–57.

$$ \begin{array}{@{}rcl@{}} &&\boldsymbol{T} \begin{bmatrix} \boldsymbol{S}_{q,n|n-1} + \boldsymbol{S}_{qx,n|n-1} & \boldsymbol{S}_{qx,n|n-1} \boldsymbol{\Phi}_{xx} & \boldsymbol{b}_{q,n|n-1} \\ \boldsymbol{S}_{x,n|l} & \boldsymbol{S}_{x,n|l} \boldsymbol{\Phi}_{xx} & \boldsymbol{S}_{x,n|l} (\boldsymbol{x}_{n|l} -\boldsymbol{x}_{n|n-1}) \end{bmatrix} \\&&~~~~~~~~~~~~~~~~~~= \begin{bmatrix} \boldsymbol{S}_{q,n-1|l} & \boldsymbol{S}_{qx,n-1|l} & \boldsymbol{b}_{q,n-1|l} \\ \boldsymbol{0} & \boldsymbol{S}_{x,n-1|l} & \boldsymbol{b}_{x,n-1|l} \end{bmatrix} \end{array} $$

(54)

$$ \begin{array}{@{}rcl@{}} &&\qquad\boldsymbol{x}_{n-1|l} = \boldsymbol{x}_{n-1|n-1} + \boldsymbol{S}_{x,n-1|l}^{-1} \boldsymbol{b}_{x,n-1|l} \end{array} $$

(55)

$$ \begin{array}{@{}rcl@{}} &&\qquad\boldsymbol{p}_{n-1|l} = -\boldsymbol{P}_{n-1|n-1}^{-1} (\boldsymbol{x}_{n-1|l} -\boldsymbol{x}_{n-1|n-1}) \end{array} $$

(56)

$$ \begin{array}{@{}rcl@{}} &&\qquad \hat{\boldsymbol{u}(t)} = -\tilde{\boldsymbol{Q}} \boldsymbol{B}^{T} \boldsymbol{\Phi}_{pp}(t,t_{n-1}) \boldsymbol{p}_{n-1|l} \end{array} $$

(57)