Practical Filter Implementation

4.2.28 Practical Filter Implementation

A Kalman filter, EKF, ESKF, MEKF, UKF, or any other state estimator may look straightforward when written as a compact set of equations. In practice, however, a flight-ready estimator is not simply a direct transcription of those equations into software.

Real implementation must deal with:

• continuous dynamics but discrete computation,
• numerical integration,
• covariance propagation,
• changing sampling intervals,
• asynchronous measurements,
• floating-point precision,
• matrix conditioning,
• positive-semidefinite covariance,
• covariance symmetry,
• stable innovation solves,
• quaternion normalization,
• estimator scheduling,
• computational limits,
• software architecture,
• and integration with the rest of the GNC system.

The difference between a mathematically correct filter and a robust flight implementation can therefore be substantial.

The central engineering idea is

\[ \boxed{ \text{Estimator Theory} \rightarrow \text{Numerically Stable Discrete Algorithm} \rightarrow \text{Real-Time Flight Software} } \]

A practical estimator must satisfy all three levels.

4.2.28.1 From Mathematical Filter to Flight Software

Consider a continuous nonlinear spacecraft model:

\[ \dot{\mathbf x} = f(\mathbf x,\mathbf u,t) + G\mathbf w. \]

The estimator runs on a digital processor, so the continuous system must ultimately be represented at discrete times

\[ t_0,t_1,\ldots,t_k. \]

Thus the flight estimator actually performs something closer to

\[ \boxed{ \hat{\mathbf x}_{k+1}^{-} = f_d( \hat{\mathbf x}_k^{+}, \mathbf u_k, \Delta t_k ) } \]

followed by covariance propagation and, when a measurement is available, a measurement update.

This means practical implementation begins with one fundamental transformation:

\[ \boxed{ \text{continuous dynamics} \rightarrow \text{discrete estimator dynamics}. } \]

4.2.28.2 Continuous-Time State Model

A general continuous linear model is

\[ \dot{\mathbf x}(t) = F(t)\mathbf x(t) + B(t)\mathbf u(t) + G(t)\mathbf w(t). \]

The corresponding covariance dynamics are

\[ \boxed{ \dot P = FP+PF^T+GQ_cG^T } \]

where \\(Q_c\\) is the continuous-time process-noise spectral-density matrix.

For nonlinear systems, \\(F\\) is usually the Jacobian

\[ F = \frac{\partial f}{\partial x} \]

evaluated at the current nominal state.

These continuous equations are useful theoretically, but flight software requires a discrete representation.

4.2.28.3 Discrete State Propagation

The discrete linear model is

\[ \boxed{ \mathbf x_{k+1} = \Phi_k\mathbf x_k + \Gamma_k\mathbf u_k + \mathbf w_k } \]

where

\[ \Phi_k \]

is the state-transition matrix and

\[ \Gamma_k \]

is the discrete input matrix.

For a time-invariant linear system,

\[ \boxed{ \Phi = e^{F\Delta t} } \]

where \\(\Delta t\\) is the propagation interval.

Thus the discrete filter depends strongly on how accurately \\(\Phi\\) is computed.

4.2.28.4 First-Order Discretization

For sufficiently small \\(\Delta t\\),

\[ \Phi \approx I+F\Delta t. \]

This is computationally inexpensive.

The corresponding propagation becomes

\[ \hat x_{k+1} \approx (I+F\Delta t)\hat x_k. \]

However, the approximation error increases with both:

\[ \|F\| \]

and

\[ \Delta t. \]

Therefore

\[ \boxed{ \text{small timestep} \not\Rightarrow \text{first-order discretization is always adequate}. } \]

The approximation should be verified for the actual dynamics and estimator update rate.

4.2.28.5 Higher-Order State Transition Approximation

The matrix exponential can be expanded as

\[ e^{F\Delta t} = I + F\Delta t + \frac{1}{2!}F^2\Delta t^2 + \frac{1}{3!}F^3\Delta t^3 +\cdots. \]

Keeping additional terms improves accuracy.

For example,

\[ \Phi \approx I + F\Delta t + \frac{1}{2}F^2\Delta t^2. \]

This can be useful when first-order propagation is insufficient but computing a full matrix exponential every cycle is unnecessary or too expensive.

4.2.28.6 Exact Matrix Exponential

For linear time-invariant dynamics, an exact discrete transition matrix is

\[ \boxed{ \Phi=e^{F\Delta t}. } \]

Numerical methods such as scaling-and-squaring or Padé approximations can evaluate this matrix exponential reliably.

However, repeatedly computing matrix exponentials onboard may be computationally expensive for large states.

Therefore implementation choices may include:

• precomputing \\(\Phi\\) for fixed \\(\Delta t\\),
• using analytical expressions,
• using low-order approximations,
• or computing the exponential only when necessary.

4.2.28.7 Nonlinear State Integration

For nonlinear dynamics,

\[ \dot{\mathbf x}=f(\mathbf x,\mathbf u,t), \]

the state can be propagated numerically.

A simple Euler step is

\[ \mathbf x_{k+1} = \mathbf x_k + f(\mathbf x_k,\mathbf u_k,t_k)\Delta t. \]

Higher-order integrators such as Runge–Kutta methods can provide greater accuracy.

For example, classical RK4 evaluates several intermediate slopes before computing

\[ \mathbf x_{k+1}. \]

The numerical integration method used by the estimator should be consistent with the accuracy required by the navigation system.

4.2.28.8 State Propagation and Covariance Propagation Are Different Problems

One common implementation mistake is to propagate the nonlinear state accurately but approximate the covariance carelessly.

The nominal state may use

\[ \hat x_{k+1}^{-} = f_d(\hat x_k^{+},u_k), \]

while covariance propagation uses

\[ P_{k+1}^{-} = \Phi_kP_k^{+}\Phi_k^T+Q_k. \]

Thus two separate discretization problems exist:

\[ \boxed{ \text{state propagation} } \]

and

\[ \boxed{ \text{uncertainty propagation}. } \]

Both must be implemented consistently.

4.2.28.9 Discrete Process Noise

The discrete process covariance is not generally equal to the continuous spectral density multiplied blindly by the timestep.

The correct relationship is

\[ \boxed{ Q_d = \int_0^{\Delta t} \Phi(\tau) GQ_cG^T \Phi^T(\tau) \,d\tau. } \]

This integral accounts for how process noise propagates through the dynamics over the integration interval.

For simple systems, analytical forms can be derived.

For more complex systems, numerical or matrix-exponential methods may be used.

4.2.28.10 Why \\(Q_d\neq Q_c\\)

The units themselves demonstrate the distinction.

If

\[ Q_c \]

is a continuous noise spectral density, its units involve time.

The discrete covariance

\[ Q_d \]

represents uncertainty accumulated over a finite interval.

Thus,

\[ \boxed{ Q_c \neq Q_d } \]

in general.

Incorrect conversion between them is a common source of tuning inconsistency.

4.2.28.11 Van Loan Discretization

For linear systems, one systematic method for obtaining both

\[ \Phi \]

and

\[ Q_d \]

uses the Van Loan matrix exponential.

A block matrix is constructed from

\[ F, \qquad GQ_cG^T. \]

The exponential of this augmented matrix contains the quantities needed for discrete propagation.

The exact block convention varies, so implementation must follow one chosen derivation consistently.

The important idea is that state transition and process-noise discretization can be derived together rather than approximated independently.

4.2.28.12 Covariance Prediction

After discretization,

\[ \boxed{ P_{k+1}^{-} = \Phi_k P_k^{+} \Phi_k^T + Q_{d,k}. } \]

This equation appears simple but becomes numerically important because it is executed repeatedly over long periods.

Any small numerical defect can accumulate across thousands or millions of cycles.

Therefore covariance propagation should be treated as a numerically sensitive operation.

4.2.28.13 Covariance Symmetry

In theory,

\[ P=P^T. \]

Floating-point computation can gradually produce

\[ P\neq P^T. \]

Even tiny asymmetry may eventually affect decomposition, consistency checks, and matrix operations.

A common numerical safeguard is

\[ \boxed{ P \leftarrow \frac{1}{2}(P+P^T). } \]

This does not correct a fundamentally wrong covariance model, but it removes small asymmetries caused by finite numerical precision.

4.2.28.14 Positive-Semidefinite Covariance

A valid covariance must satisfy

\[ \boxed{ P\succeq0. } \]

For every vector \\(a\\),

\[ a^TPa\ge0. \]

Negative covariance eigenvalues are physically meaningless.

Yet numerical roundoff, unstable updates, or poor conditioning can produce small negative eigenvalues.

Therefore practical implementation may monitor:

• diagonal entries,
• Cholesky decomposition success,
• eigenvalues during testing,
• and numerical conditioning.

A covariance matrix that loses positive semidefiniteness should be treated as a serious estimator-health problem.

4.2.28.15 Joseph-Form Covariance Update

The theoretical covariance update is commonly written

\[ P_k^{+} = (I-K_kH_k)P_k^{-}. \]

This compact form can be numerically fragile.

A more robust expression is the Joseph form:

\[ \boxed{ P_k^{+} = (I-K_kH_k) P_k^{-} (I-K_kH_k)^T + K_kR_kK_k^T. } \]

The Joseph form more explicitly preserves covariance symmetry and positive-semidefinite structure in finite-precision arithmetic.

For practical implementations, its additional computational cost is often worthwhile.

4.2.28.16 Why the Simple Covariance Update Can Be Risky

Suppose

\[ I-KH \]

contains quantities formed through subtraction of nearly equal floating-point numbers.

Roundoff can create asymmetry and even slightly negative covariance directions.

The Joseph form avoids some of this cancellation because it retains both uncertainty contributions explicitly.

Thus,

\[ \boxed{ \text{algebraically equivalent} \not\Rightarrow \text{numerically equivalent}. } \]

This is a fundamental lesson in estimator software.

4.2.28.17 Innovation Computation

The innovation is

\[ \boxed{ \nu_k = z_k-h(\hat x_k^-). } \]

Its covariance is

\[ \boxed{ S_k = H_kP_k^-H_k^T+R_k. } \]

The Kalman gain is

\[ K_k = P_k^-H_k^TS_k^{-1}. \]

Mathematically this uses \\(S_k^{-1}\\), but practical software should usually not explicitly invert \\(S_k\\).

4.2.28.18 Avoid Explicit Matrix Inversion

Instead of calculating

\[ S^{-1}, \]

the implementation should solve the corresponding linear system.

For example,

\[ KS = PH^T \]

can be solved for \\(K\\).

If \\(S\\) is symmetric positive definite, Cholesky decomposition can be especially efficient and stable.

Therefore,

\[ \boxed{ \text{solve linear systems} \quad\text{rather than}\quad \text{forming explicit inverses} } \]

is an important numerical implementation principle.

4.2.28.19 Cholesky Decomposition

For a symmetric positive-definite matrix,

\[ S=LL^T \]

where \\(L\\) is lower triangular.

The system

\[ Sx=b \]

can then be solved using two triangular solves:

\[ Ly=b \]

followed by

\[ L^Tx=y. \]

This is generally more stable and efficient than explicitly evaluating

\[ S^{-1}. \]

Failure of Cholesky decomposition can also act as a useful diagnostic that the covariance has lost expected numerical properties.

4.2.28.20 Matrix Conditioning

A matrix can be theoretically invertible yet numerically difficult to use.

The condition number is conceptually

\[ \kappa(A) = \|A\| \|A^{-1}\|. \]

Large

\[ \kappa(A) \]

means that small numerical perturbations can produce large solution changes.

Poor conditioning can arise when states have dramatically different scales.

For example, an estimator might combine:

\[ 10^6\text{ m}, \qquad 10^{-3}\text{ rad}, \qquad 10^{-6}\text{ rad/s}. \]

State scaling can therefore improve numerical behaviour.

4.2.28.21 State Scaling

Suppose

\[ x= D\tilde x \]

where \\(D\\) is a scaling matrix.

The transformed state

\[ \tilde x \]

can be chosen so that its components have comparable numerical magnitude.

This may improve:

• covariance conditioning,
• Jacobian calculations,
• matrix factorizations,
• and numerical robustness.

Scaling should be performed systematically because all associated dynamics, measurements, and covariance matrices must be transformed consistently.

4.2.28.22 Floating-Point Precision

A practical implementation must choose numeric precision.

Common options include:

• single precision,
• double precision,
• mixed precision,
• or fixed-point arithmetic for constrained embedded systems.

Double precision provides wider dynamic range and generally greater numerical robustness.

Single precision may reduce memory and computational burden.

The correct choice should be based on:

• state scale,
• covariance dynamic range,
• processor architecture,
• estimator accuracy,
• and worst-case numerical analysis.

4.2.28.23 Single Precision Is Not Automatically Unsafe

Many real-time systems successfully use single precision.

The correct question is not:

“Is single precision acceptable?”

but rather:

“Does single precision preserve the required estimator accuracy and numerical stability for this state definition, scaling, covariance range, and mission duration?”

This should be demonstrated through testing rather than assumed.

4.2.28.24 Quaternion Normalization

Attitude estimators frequently propagate a quaternion

\[ q= \begin{bmatrix} q_0&q_1&q_2&q_3 \end{bmatrix}^T. \]

A valid quaternion satisfies

\[ \boxed{ \|q\|=1. } \]

Numerical integration and correction can cause small norm errors.

A practical implementation may therefore perform

\[ \boxed{ q \leftarrow \frac{q}{\|q\|}. } \]

For MEKF/ESKF architectures, quaternion correction and covariance reset must remain consistent with the selected multiplicative error convention.

4.2.28.25 Quaternion Sign

The quaternions

\[ q \]

and

\[ -q \]

represent the same physical orientation.

However, arbitrary sign switching can create discontinuities in logged states or naive differencing operations.

A practical implementation may enforce sign continuity, for example by selecting the representation satisfying

\[ q_k^Tq_{k-1}>0. \]

This is mainly a representation-management issue, but it prevents confusing numerical discontinuities.

4.2.28.26 Error-State Injection

For an ESKF, the estimator maintains a nominal state and an error state.

After measurement correction,

\[ \delta\hat x \]

is injected into the nominal state:

\[ \boxed{ \hat x^{+} = \hat x^{-} \oplus \delta\hat x. } \]

For additive states this may be ordinary addition.

For attitude it may involve quaternion multiplication.

After injection, the error state is reset.

Implementation must ensure that state injection, covariance transformation, and reset are consistent.

4.2.28.27 Error-State Reset

After injection,

\[ \delta\hat x\rightarrow0. \]

However, covariance cannot always simply be left unchanged.

A reset Jacobian

\[ G_r \]

may be required:

\[ \boxed{ P^{+} \leftarrow G_r P^{+} G_r^T. } \]

This is particularly important when the error representation is nonlinear, as in attitude error states.

Ignoring the reset transformation can gradually make covariance inconsistent with the error-state definition.

4.2.28.28 Covariance Initialization

The initial covariance

\[ P_0 \]

must represent actual uncertainty in the initial state.

If

\[ P_0 \]

is too small, the estimator becomes overconfident.

If it is excessively large, early gains may become unnecessarily aggressive.

Thus each diagonal element should represent a physically meaningful initial uncertainty:

\[ P_{0,ii} = \sigma_{x_i,0}^{2}. \]

Cross-covariances should also be included when justified.

4.2.28.29 Covariance Floors

Sometimes numerical or modelling behaviour causes a variance to become unrealistically small.

A covariance floor can impose

\[ P_{ii} \ge P_{ii,\min}. \]

This can prevent numerical collapse, but such floors should not be used to hide incorrect filter tuning.

A physically justified minimum uncertainty is preferable to an arbitrary software patch.

4.2.28.30 Covariance Ceilings

Similarly, large covariance can produce extremely large gains or numerical issues in some architectures.

An implementation may monitor upper bounds.

However, clipping covariance artificially can misrepresent true uncertainty.

Usually, excessive covariance should instead trigger a navigation-health or degraded-mode response.

Thus,

\[ \boxed{ \text{large uncertainty should normally be reported, not hidden}. } \]

4.2.28.31 Variable Time Steps

Real software timing is not always perfectly periodic.

The actual interval may be

\[ \Delta t_k = t_k-t_{k-1}. \]

The estimator should generally use this actual interval rather than assuming

\[ \Delta t=\text{constant} \]

if meaningful timing variation exists.

Both

\[ \Phi_k \]

and

\[ Q_{d,k} \]

should then depend on the measured timestep.

4.2.28.32 Timestamp-Driven Propagation

A robust estimator architecture uses measurement timestamps and propagation time explicitly.

Suppose a measurement belongs to time

\[ t_m. \]

The filter state may currently exist at

\[ t_f. \]

The implementation should determine whether it must:

• propagate to \\(t_m\\),
• update at \\(t_m\\),
• propagate forward afterward,
• or perform an out-of-sequence update.

This is especially important for asynchronous multi-sensor fusion.

4.2.28.33 Multi-Rate Estimator Scheduling

Suppose

\[ f_{IMU}=200\text{ Hz}, \]

\[ f_{GNSS}=10\text{ Hz}, \]

\[ f_{camera}=5\text{ Hz}. \]

A practical estimator can propagate at the high-rate inertial frequency and perform correction events whenever lower-rate measurements arrive.

Conceptually,

\[ \boxed{ \text{propagate frequently} + \text{correct when information arrives}. } \]

This architecture avoids forcing every sensor into one artificial synchronous cycle.

4.2.28.34 Event-Driven Measurement Updates

Rather than checking every sensor at every estimator tick, measurement arrival can trigger an update.

A generic event-driven loop is:

\[ \boxed{ \begin{array}{c} \text{new measurement arrives}\\ \downarrow\\ \text{validate timestamp}\\ \downarrow\\ \text{propagate to measurement time}\\ \downarrow\\ \text{validate measurement}\\ \downarrow\\ \text{update estimator} \end{array} } \]

This can be cleaner for heterogeneous sensor suites.

4.2.28.35 Handling Delayed Measurements

Suppose the estimator has propagated to

\[ t_k, \]

but receives a valid measurement generated at

\[ t_m

Possible solutions include:

• rejecting excessively old data,
• updating a stored historical state and repropagating,
• fixed-lag smoothing,
• state augmentation,
• or specialized out-of-sequence measurement processing.

Simply applying the old measurement to the current state is generally incorrect when significant dynamics occur between \\(t_m\\) and \\(t_k\\).

4.2.28.36 State History Buffer

One practical approach is to maintain a history buffer containing

\[ \{ t_i,\hat x_i,P_i,u_i \}. \]

When a delayed measurement arrives, the estimator can retrieve the appropriate historical state, apply the update, and then repropagate to the current time.

The buffer length must cover the expected measurement latency.

This trades memory and computational load for better delayed-measurement handling.

4.2.28.37 Measurement Gating Implementation

The NIS is

\[ \epsilon = \nu^TS^{-1}\nu. \]

Again, implementation should avoid explicitly computing

\[ S^{-1}. \]

Instead solve

\[ Sy=\nu \]

and evaluate

\[ \boxed{ \epsilon=\nu^Ty. } \]

This provides the same quantity more stably.

4.2.28.38 Chi-Square Thresholds

For measurement dimension \\(m\\), innovation gating can compare

\[ \epsilon \]

against a threshold

\[ \gamma_{\alpha,m}. \]

Thus,

\[ \epsilon \le \gamma_{\alpha,m} \]

may define acceptance.

The threshold should depend on measurement dimension and chosen confidence level.

It should not simply be a single arbitrary number reused for every sensor.

4.2.28.39 Sequential Versus Batch Updates

When several measurements arrive at the same epoch, they can be processed sequentially or together.

Sequentially:

\[ z_1 \rightarrow z_2 \rightarrow z_3. \]

Batch:

\[ z= \begin{bmatrix} z_1\\z_2\\z_3 \end{bmatrix}. \]

For independent linear measurements, both may be equivalent mathematically.

Sequential updates can reduce matrix size.

Batch updates can handle known cross-correlation more naturally.

The implementation choice should reflect measurement structure and computational constraints.

4.2.28.40 Measurement Dimension and Computational Cost

The innovation covariance dimension equals the measurement dimension.

Inverting or factorizing

\[ S\in\mathbb R^{m\times m} \]

costs approximately

\[ O(m^3) \]

for general dense operations.

Thus large batch measurements can become computationally expensive.

This becomes particularly important for vision systems containing many image features.

Sparse, sequential, or reduced-order update strategies may therefore be advantageous.

4.2.28.41 Computational Complexity

For a state dimension \\(n\\), dense covariance operations often scale roughly as

\[ O(n^3). \]

Memory for a full covariance matrix scales as

\[ O(n^2). \]

For small spacecraft navigation filters, this may be manageable.

For large augmented states containing many biases, calibration terms, landmarks, or feature states, computation can become substantial.

State selection therefore has direct implementation consequences.

4.2.28.42 State Dimension Should Be Justified

Adding a state can improve modelling capability.

But every additional state can increase:

• covariance size,
• propagation cost,
• measurement Jacobian complexity,
• observability requirements,
• tuning burden,
• and verification complexity.

Therefore,

\[ \boxed{ \text{estimate a parameter online only when doing so adds justified value}. } \]

A parameter that is poorly observable may degrade the filter rather than improve it.

4.2.28.43 Sparse Matrices

Some large estimation problems contain sparse Jacobians and covariance structures.

Exploiting sparsity can reduce computational cost.

Instead of storing and operating on every zero element, sparse methods retain only significant structural entries.

This becomes especially important in larger navigation architectures, mapping, multi-object estimation, or factor-graph approaches.

For ordinary small spacecraft Kalman filters, dense algebra may remain simpler and entirely adequate.

4.2.28.44 Square-Root Filters

Instead of propagating \\(P\\) directly, a square-root filter propagates a factor such as

\[ P=SS^T. \]

This can improve numerical stability and preserve positive-definite structure.

Square-root filtering is attractive when:

• covariance conditioning is difficult,
• high numerical accuracy is required,
• or long-duration propagation magnifies roundoff effects.

The implementation is more involved but can offer substantial numerical advantages.

4.2.28.45 Information Filters

An alternative representation uses the information matrix

\[ Y=P^{-1} \]

and information vector

\[ y=Y\hat x. \]

Measurement updates can become additive in information space.

This can be useful for decentralized fusion.

However, propagation can become less convenient.

Thus filter representation should be selected based on architecture rather than mathematical elegance alone.

4.2.28.46 Numerical Jacobians

For nonlinear filters, Jacobians may be obtained analytically or numerically.

A finite-difference approximation is

\[ \frac{\partial f}{\partial x_i} \approx \frac{ f(x+\epsilon e_i)-f(x) }{\epsilon}. \]

Choosing \\(\epsilon\\) is delicate.

Too large gives truncation error.

Too small gives roundoff error.

Analytical Jacobians are often preferable for flight implementations when feasible.

4.2.28.47 Automatic Differentiation

Automatic differentiation can generate accurate derivatives without symbolic hand derivation.

This can reduce implementation mistakes, particularly for complicated models.

However, flight-software suitability depends on:

• toolchain qualification,
• generated code,
• computational cost,
• memory,
• determinism,
• and verification requirements.

It can be extremely useful during development even if final flight derivatives are coded analytically.

4.2.28.48 Jacobian Verification

Analytical Jacobians should be verified numerically.

A common development check compares

\[ H_{\text{analytic}} \]

with

\[ H_{\text{finite difference}}. \]

The difference

\[ \Delta H = H_{\text{analytic}} - H_{\text{numerical}} \]

should be appropriately small over representative operating points.

This simple test can reveal sign errors, frame mistakes, and omitted terms before they become difficult estimator problems.

4.2.28.49 Angle Wrapping

Angular measurements require careful handling.

Suppose

\[ \theta_m=179^\circ \]

and

\[ \hat\theta=-179^\circ. \]

A naive residual is

\[ 358^\circ, \]

while the physically meaningful residual is

\[ -2^\circ. \]

Thus angle innovations must be wrapped to a consistent interval.

For example,

\[ \boxed{ \nu_\theta = \operatorname{wrapToPi} ( \theta_m-\hat\theta ). } \]

4.2.28.50 Unit Consistency

A practical filter may combine:

• metres,
• kilometres,
• seconds,
• radians,
• degrees,
• N,
• m/s,
• rad/s,
• and dimensionless quantities.

A single unit mismatch can produce catastrophic estimator behaviour while every equation remains syntactically correct.

Therefore interfaces should define units explicitly.

For flight software,

\[ \boxed{ \text{units are part of the data contract}. } \]

4.2.28.51 Frame Metadata

Similarly, vectors should have an explicitly defined frame.

A vector named

\[ \mathbf r \]

is insufficient.

The implementation should conceptually distinguish quantities such as

\[ {}^{I}\mathbf r, \qquad {}^{B}\mathbf r, \qquad {}^{S}\mathbf r. \]

Good naming conventions and interface documentation greatly reduce silent frame errors.

4.2.28.52 Estimator Software Layers

A clean implementation can separate:

Sensor interface layer

Receives raw sensor data.

Preprocessing layer

Applies calibration, units, timestamps, and frame conversion.

Validation layer

Checks validity and health.

Propagation layer

Advances nominal state and covariance.

Measurement-model layer

Computes predicted measurements and Jacobians.

Update layer

Performs innovation, gating, gain, correction, and covariance update.

Navigation-monitor layer

Checks estimator consistency and health.

Output layer

Publishes state, covariance, validity, and mode.

This modularity improves testability.

4.2.28.53 Separation of Truth and Estimate

In simulation, the truth state and estimated state should be strictly separated.

Truth dynamics may use

\[ x_{\text{truth}}. \]

Sensors are generated from truth.

The estimator should receive only simulated sensor outputs and allowed ancillary data.

Feeding truth directly into the estimator, even accidentally through initialization or helper blocks, produces unrealistic performance.

Thus,

\[ \boxed{ \text{truth belongs to simulation; estimates belong to navigation}. } \]

4.2.28.54 Sensor Model Separation

Each sensor model should produce realistic measurement data independently.

For example:

\[ z_i = h_i(x_{\text{truth}}) + b_i + n_i + e_i. \]

Sensor models may include:

• noise,
• bias,
• scale factor,
• misalignment,
• latency,
• quantization,
• dropout,
• saturation,
• and faults.

The estimator should not know the true injected error unless that quantity is explicitly available in the real system.

4.2.28.55 Measurement Bus

A standardized measurement structure can include

\[ \boxed{ \{ ID, t, z, R, frame, valid, quality \}. } \]

This simplifies fusion of heterogeneous sensors.

It also supports logging and traceability.

When an unexpected filter correction occurs, engineers can identify exactly which measurement, timestamp, covariance, and validity state produced it.

4.2.28.56 Estimator Output Bus

The estimator output may include

\[ \boxed{ \{ \hat x, P, t, health, mode, validity \}. } \]

Downstream GNC should generally consume both the state estimate and navigation-health information.

A state estimate without its associated uncertainty can be misleading.

4.2.28.57 Navigation Health Monitoring

Useful online checks include:

• covariance trace,
• selected standard deviations,
• NIS,
• state bounds,
• Cholesky success,
• innovation magnitude,
• update freshness,
• sensor availability,
• covariance positivity,
• and estimator-reset events.

A health monitor can convert these quantities into operational states such as

\[ \{ \text{valid}, \text{degraded}, \text{invalid} \}. \]

4.2.28.58 Watchdog Logic

A software watchdog may monitor whether estimator execution completes within its deadline.

If the filter must run every

\[ T_f \]

seconds, then execution time should satisfy

\[ \boxed{ T_{\text{exec}} < T_f } \]

with suitable margin.

Deadline overruns may require:

• skipping nonessential updates,
• reducing processing,
• entering degraded mode,
• or triggering fault-management logic.

4.2.28.59 Worst-Case Execution Time

Average runtime is not sufficient for flight software.

The relevant requirement is often closer to

\[ \boxed{ T_{\text{WCET}} < T_{\text{deadline}}. } \]

Worst-case execution time should consider:

• largest measurement batch,
• maximum active sensor count,
• delayed measurement processing,
• reset operations,
• fault logic,
• logging,
• and memory effects.

Deterministic timing is often more important than average speed.

4.2.28.60 Dynamic Memory Allocation

Dynamic memory allocation during real-time estimator execution can introduce:

• unpredictable timing,
• fragmentation,
• allocation failure,
• and verification difficulty.

Therefore embedded implementations often prefer statically allocated arrays and fixed-size buffers.

This is particularly useful when state and measurement dimensions are known in advance.

4.2.28.61 Matrix Libraries

Matrix operations should use well-tested numerical libraries where possible.

However, embedded flight software may require:

• bounded memory,
• deterministic execution,
• no hidden dynamic allocation,
• code-generation compatibility,
• and certification constraints.

Therefore desktop scientific libraries are not always directly suitable for flight use.

4.2.28.62 Fixed Dimensions Versus Variable Dimensions

A filter with fixed state and maximum measurement dimensions is easier to optimize and verify.

Variable measurement dimension is often still required because sensors may appear and disappear.

One implementation approach is to allocate for

\[ m_{\max} \]

but use only the active portion.

Another is to process sensors sequentially with small fixed update dimensions.

4.2.28.63 Embedded Processor Constraints

A flight computer may have limited:

• CPU throughput,
• RAM,
• nonvolatile memory,
• power,
• and communication bandwidth.

Therefore the most mathematically sophisticated estimator is not necessarily the best practical implementation.

The estimator must satisfy

\[ \boxed{ \text{accuracy} + \text{robustness} + \text{timing} + \text{memory} + \text{verification}. } \]

4.2.28.64 Update Rate Selection

Running a filter faster does not automatically improve performance.

If the process and measurements contain little new information between updates, excessive rate can waste processor time.

Too low a rate can introduce discretization error and miss fast dynamics.

Thus update rate should reflect:

• system dynamics,
• sensor bandwidth,
• control bandwidth,
• timing accuracy,
• and computational resources.

4.2.28.65 Sensor Rate Versus Estimator Rate

The estimator rate does not necessarily equal every sensor rate.

For example:

\[ f_{filter}=100\text{ Hz}, \]

while

\[ f_{star}=5\text{ Hz}. \]

The filter may propagate 20 times between star-tracker corrections.

This is normal.

The estimator architecture should therefore distinguish between

\[ \boxed{ \text{propagation rate} } \]

and

\[ \boxed{ \text{measurement-update rates}. } \]

4.2.28.66 Control and Estimation Rates

The controller may operate at a different rate from the estimator.

Suppose

\[ f_{est}=100\text{ Hz} \]

and

\[ f_{ctrl}=50\text{ Hz}. \]

The controller can use the most recent valid navigation estimate.

However, the age of that estimate should remain bounded and known.

Timestamping is therefore important at the estimator–controller interface as well.

4.2.28.67 Closed-Loop GNC Integration

The complete chain is

\[ \boxed{ \text{Sensors} \rightarrow \text{Estimator} \rightarrow \text{Guidance} \rightarrow \text{Control} \rightarrow \text{Actuators} \rightarrow \text{Spacecraft Dynamics} \rightarrow \text{Sensors}. } \]

Estimator latency affects control.

Control changes the spacecraft state.

The changed state alters future measurements.

Thus filter implementation cannot be validated only in isolation.

Closed-loop testing is essential.

4.2.28.68 Estimator Latency

If navigation output corresponds to time

\[ t_e \]

but the controller acts at

\[ t_c>t_e, \]

the effective delay is

\[ \tau=t_c-t_e. \]

For fast dynamics, this can degrade control performance.

A system may compensate by propagating the estimate to the control epoch when appropriate.

Thus navigation latency should be measured and included in GNC analysis.

4.2.28.69 General RPO Considerations

In general rendezvous and proximity operations, implementation becomes particularly sensitive to:

• multi-rate sensors,
• relative-navigation geometry,
• changing sensor availability,
• delayed optical processing,
• asynchronous ranging,
• covariance growth,
• estimator mode transitions,
• and strict navigation-validity requirements.

A hypothetical close-approach estimator may process different relative-navigation sensors at different times.

The fundamental implementation requirement remains:

\[ \boxed{ \text{every measurement must be fused at the correct epoch, frame, uncertainty, and validity state}. } \]

This is a general RPO principle and does not depend on any particular mission architecture.

4.2.28.70 Mode-Dependent Filter Configuration

Different operating phases may use different:

• states,
• sensors,
• \\(Q\\),
• \\(R\\),
• gating thresholds,
• propagation models,
• and update rates.

For example,

\[ \mathcal M = \{ M_1,M_2,\ldots,M_n \}. \]

Each mode can define an estimator configuration appropriate to the current navigation regime.

Transitions must preserve state and covariance consistency.

4.2.28.71 Filter Reconfiguration

Suppose a state is added or removed.

The covariance dimension changes.

Adding a state requires appropriate initialization:

\[ \hat x_{new} \]

and

\[ P_{new}. \]

Removing a state requires extracting the appropriate marginal covariance rather than simply deleting arbitrary rows and columns without understanding correlation.

Reconfiguration should therefore be designed explicitly.

4.2.28.72 Sensor Enable and Disable

When a sensor becomes unavailable, the measurement update is skipped.

The covariance naturally grows through propagation.

When it returns, measurement processing resumes after validation.

The filter should not generally reset its entire state merely because one sensor temporarily disappears.

This is a major advantage of recursive multi-sensor estimation.

4.2.28.73 Reinitialization

Sometimes reinitialization is required.

Possible triggers include:

• covariance divergence,
• invalid state,
• prolonged sensor outage,
• severe numerical failure,
• major mode transition,
• or recovery from reset.

Reinitialization should define:

\[ \hat x_0, \qquad P_0, \qquad \text{required sensors}, \qquad \text{validity criteria}. \]

It should not be an undefined emergency software action.

4.2.28.74 Soft Reset Versus Hard Reset

A soft reset may preserve some valid state components while resetting others.

A hard reset may reconstruct the entire navigation state from available measurements.

For example, attitude might be reinitialized while orbital position remains valid.

This can preserve useful information and reduce recovery time.

4.2.28.75 Numerical Failure Detection

The estimator should detect conditions such as:

• NaN,
• Inf,
• failed matrix decomposition,
• negative variance,
• invalid quaternion,
• excessive state magnitude,
• and impossible covariance.

These are not merely debugging issues.

In flight, they require defined responses.

A useful philosophy is

\[ \boxed{ \text{detect numerical failure before it propagates into GNC commands}. } \]

4.2.28.76 NaN Containment

A single NaN entering a matrix operation can contaminate the entire navigation solution.

Sensor interfaces should therefore validate data before fusion.

Estimator output should also be checked before publication.

If invalid values are detected, the system should enter a defined recovery or degraded-navigation mode rather than continuing silently.

4.2.28.77 Saturation and Overflow

Even floating-point systems can encounter extreme values.

Fixed-point implementations are especially sensitive.

Input validation should therefore check:

• range,
• physical plausibility,
• numeric limits,
• and scaling.

A saturated sensor measurement should not be treated as an ordinary Gaussian measurement.

4.2.28.78 Logging

During development, useful logged quantities include

\[ \hat x, \quad P, \quad \nu, \quad S, \quad K, \quad Q, \quad R. \]

Also log:

• raw measurements,
• processed measurements,
• timestamps,
• measurement acceptance,
• sensor health,
• mode,
• resets,
• and estimator runtime.

Without these signals, diagnosing filter problems becomes unnecessarily difficult.

4.2.28.79 Covariance Logging

Logging the entire covariance matrix can be expensive.

Possible alternatives include logging:

• diagonal variances,
• selected cross-covariances,
• covariance trace,
• determinant,
• eigenvalue diagnostics,
• or state-specific uncertainty summaries.

During detailed offline testing, full covariance logging is often worthwhile.

Flight telemetry may use a reduced set.

4.2.28.80 Innovation Logging

Innovations are among the most valuable diagnostic quantities.

For each sensor, log

\[ \nu_k \]

and ideally

\[ S_k. \]

This allows offline evaluation of:

• mean,
• variance,
• correlation,
• NIS,
• bias,
• model mismatch,
• and measurement-noise tuning.

An estimator with no innovation logging is much harder to validate statistically.

4.2.28.81 Deterministic Replay

A powerful development capability is the ability to replay recorded sensor data through the estimator deterministically.

Given identical inputs,

\[ \boxed{ \text{same input sequence} \rightarrow \text{same estimator output} } \]

should hold for deterministic software.

Replay allows developers to reproduce rare failures and compare algorithm revisions exactly.

4.2.28.82 Unit Testing

Individual estimator functions should be unit tested.

Examples include:

• quaternion multiplication,
• quaternion normalization,
• frame transformations,
• state transition calculation,
• process covariance discretization,
• Jacobians,
• innovation computation,
• Joseph update,
• gating,
• and state reset.

Testing small components makes later system integration much easier.

4.2.28.83 Analytical Test Cases

Where possible, numerical implementation should be compared against analytical solutions.

For example, a constant-velocity model has simple closed-form propagation.

If the estimator cannot reproduce a simple analytical case, more complicated nonlinear simulations should not yet be trusted.

This is an effective development principle.

4.2.28.84 Zero-Noise Tests

Setting all measurement and process noise to zero can be useful.

For a consistent deterministic scenario, estimator error should behave according to the model and initialization.

This can isolate mathematical or frame errors before stochastic noise obscures them.

Zero-noise testing is especially useful during early verification.

4.2.28.85 Single-Sensor Tests

Each measurement update should be tested independently.

For example:

\[ \text{Estimator + Sensor A only} \]

before

\[ \text{Estimator + A + B + C}. \]

This confirms whether each measurement model and Jacobian behaves correctly before interactions between sensors complicate diagnosis.

4.2.28.86 Fault-Injection Tests

Implementation should be tested with injected:

• bias,
• drift,
• dropout,
• latency,
• timestamp error,
• frame error,
• outlier,
• saturation,
• and corrupted covariance.

The objective is not only to see whether the estimator survives, but whether navigation-health and fault-management logic respond correctly.

4.2.28.87 Monte Carlo Testing

Once deterministic implementation is correct, Monte Carlo testing evaluates robustness.

Randomized quantities can include:

\[ x_0, \quad b_0, \quad Q, \quad R, \quad \Delta t, \quad \text{misalignment}, \quad \text{dropout}. \]

Performance should be assessed statistically rather than by one simulation trajectory.

4.2.28.88 SIL Testing

Software-in-the-loop testing runs estimator software against simulated dynamics and sensors.

This helps verify:

• actual software logic,
• execution ordering,
• interfaces,
• numerical implementation,
• and code-level behaviour.

SIL bridges the gap between equations and deployed software.

4.2.28.89 HIL Testing

Hardware-in-the-loop adds representative processors, communication interfaces, or sensors.

HIL can reveal:

• processor timing,
• data transport delays,
• packet loss,
• timestamping errors,
• bus contention,
• quantization,
• and actual embedded numerical behaviour.

These effects may not be visible in desktop simulation.

4.2.28.90 Processor-in-the-Loop

Processor-in-the-loop testing executes generated estimator code on the intended or representative processor while the environment remains simulated.

This is useful for validating:

• execution time,
• memory usage,
• compiler effects,
• precision,
• and code-generation behaviour.

It can be performed before full HIL infrastructure is available.

4.2.28.91 Code Generation

Model-based workflows may generate C or C++ from estimator models.

Generated code should be reviewed for:

• deterministic memory use,
• numerical equivalence,
• unsupported dynamic constructs,
• matrix dimensions,
• function call overhead,
• and timing.

Automatic code generation reduces some transcription errors but does not eliminate the need for verification.

4.2.28.92 Model–Code Equivalence

A key question is whether generated or hand-written embedded code reproduces the reference algorithm.

For identical test vectors,

\[ \hat x_{\text{model}} \]

should be compared with

\[ \hat x_{\text{code}}. \]

Likewise,

\[ P_{\text{model}} \]

should be compared with

\[ P_{\text{code}}. \]

Differences should remain within justified numerical tolerance.

4.2.28.93 Regression Testing

Every estimator modification should be tested against a regression suite.

This prevents a change intended to improve one case from silently breaking another.

Useful regression scenarios include:

• nominal propagation,
• sensor dropout,
• large initial error,
• delayed measurement,
• bias estimation,
• mode transition,
• and numerical edge cases.

4.2.28.94 Software Configuration Control

Estimator behaviour depends on:

• code version,
• state definition,
• \\(Q\\),
• \\(R\\),
• sensor configuration,
• initialization,
• thresholds,
• and timing parameters.

These should be under configuration control.

Otherwise, reproducing a simulation or flight result becomes difficult.

4.2.28.95 Parameter Management

Filter parameters should not be scattered throughout code as unexplained constants.

A structured parameter set can contain:

\[ Q, \quad R_i, \quad P_0, \quad \gamma_i, \quad f_{prop}, \quad \text{sensor limits}. \]

Each should have:

• units,
• source,
• rationale,
• allowed range,
• and version.

This improves traceability.

4.2.28.96 Calibration Database

Sensor calibration parameters may include:

• scale factors,
• biases,
• mounting matrices,
• lever arms,
• timing offsets,
• intrinsic camera parameters,
• and temperature coefficients.

These should be versioned and traceable.

Using the wrong calibration version can make an otherwise correct estimator appear defective.

4.2.28.97 Runtime Parameter Updates

Some systems allow filter parameters to be updated during operation.

This can be useful, but changes should be bounded and validated.

Unexpected modification of

\[ Q, \quad R, \quad P \]

can drastically alter estimator behaviour.

Runtime parameter management should therefore be treated as part of the navigation safety architecture.

4.2.28.98 Covariance Interpretation in Software

Each covariance element must correspond exactly to the implemented state ordering.

If

\[ x= [ r_x,r_y,r_z,v_x,v_y,v_z,b_{gx},\ldots]^T, \]

then row and column indices in \\(P\\) must match that ordering everywhere.

Changing state order without updating all Jacobians and covariance operations is a serious software risk.

A centralized state-index definition is therefore valuable.

4.2.28.99 State Index Management

Instead of hard-coded numerical indices scattered throughout software, state indices should be defined symbolically or centrally.

For example:

\[ IDX\_POS=1{:}3, \]

\[ IDX\_VEL=4{:}6. \]

The specific programming syntax varies, but the principle is universal:

\[ \boxed{ \text{state structure should be explicit and maintainable}. } \]

4.2.28.100 Sensor Interface Contracts

Each sensor interface should define:

• measurement dimension,
• units,
• frame,
• timestamp convention,
• covariance convention,
• validity flags,
• update rate,
• and failure behaviour.

Without this contract, integration errors can be misdiagnosed as estimator problems.

4.2.28.101 Time Convention

Every timestamp should have a clearly defined reference and unit.

For example:

• seconds since boot,
• GPS time,
• UTC-related system time,
• or simulation epoch.

Mixing time bases can create subtle fusion errors.

Therefore,

\[ \boxed{ \text{time reference is part of the measurement definition}. } \]

4.2.28.102 Thread Safety and Concurrency

If sensor interfaces and estimator tasks run concurrently, shared data must be protected.

Potential problems include:

• partially updated measurement packets,
• inconsistent timestamp/data pairs,
• race conditions,
• and overwritten buffers.

Double buffering, message queues, or atomic transfer structures can provide cleaner interfaces.

4.2.28.103 Measurement Queues

An asynchronous architecture may place measurements into a time-ordered queue.

Each item contains

\[ \{ t_i,z_i,R_i,ID_i \}. \]

The estimator processes the queue according to measurement timestamp.

This naturally supports different sensor rates.

However, queue depth and latency limits must be controlled.

4.2.28.104 Handling Queue Overflow

If measurements arrive faster than they can be processed, the queue may overflow.

The system must define whether it:

• drops oldest measurements,
• drops newest measurements,
• prioritizes particular sensors,
• reduces measurement rate,
• or enters a degraded mode.

Silent uncontrolled overflow is unacceptable.

4.2.28.105 Sensor Priority

Not all measurements necessarily have equal processing priority.

For example, high-rate inertial propagation may be essential for estimator continuity, while a computationally expensive secondary update may tolerate occasional delay.

Priority scheduling should therefore be related to navigation importance and timing requirements.

4.2.28.106 Data Freshness

A measurement may be physically valid but too old to be useful.

Define age

\[ \tau_i = t_{\text{current}} - t_i. \]

A measurement can be rejected if

\[ \tau_i>\tau_{i,\max}. \]

Freshness thresholds should depend on sensor type and system dynamics.

4.2.28.107 Sensor Timeout

If no valid measurement has been received for

\[ T>T_{\text{timeout}}, \]

the sensor can be marked unavailable.

The estimator should then stop expecting corrections from that source.

Timeout logic must distinguish between:

• slow nominal sensor rate,
• communication delay,
• temporary dropout,
• and actual sensor failure.

4.2.28.108 Filter Output Freshness

Downstream GNC may similarly require navigation data no older than

\[ \tau_{\text{nav,max}}. \]

Even an accurate estimate can become unsuitable if not refreshed in time.

Thus navigation validity can include both

\[ \text{uncertainty} \]

and

\[ \text{age}. \]

4.2.28.109 Numerical Assertions

During development, assertions can check properties such as

\[ P=P^T, \]

\[ P_{ii}\ge0, \]

\[ \|q\|\approx1. \]

Flight software may replace development assertions with health-monitoring logic rather than terminating execution.

These checks provide valuable early warning of implementation errors.

4.2.28.110 Covariance Symmetry Error Metric

A useful numerical metric is

\[ \epsilon_{sym} = \frac{\|P-P^T\|} {\|P\|}. \]

Small values are expected.

A growing symmetry error can indicate numerical problems or incorrect matrix updates.

This can be monitored during Monte Carlo and long-duration simulation.

4.2.28.111 Covariance Eigenvalue Monitoring

During development, examine

\[ \lambda_{\min}(P). \]

A valid covariance should satisfy

\[ \lambda_{\min}(P)\ge0 \]

within numerical tolerance.

Repeatedly negative eigenvalues usually indicate a deeper numerical or modelling issue.

Eigenvalue computation may be too expensive for continuous flight monitoring, but it is valuable during verification.

4.2.28.112 Condition Number Monitoring

Another development diagnostic is

\[ \kappa(P). \]

A rapidly increasing condition number may indicate:

• poorly scaled states,
• weak observability,
• near-deterministic covariance directions,
• or numerical degradation.

Such monitoring helps identify cases where theoretically valid filters become computationally fragile.

4.2.28.113 Covariance Regularization

If a matrix becomes nearly singular, small regularization may be introduced:

\[ S' = S+\epsilon I. \]

However, this should be used carefully.

Regularization changes the statistical model.

It is appropriate as a controlled numerical safeguard, not as a routine way to conceal poor conditioning.

4.2.28.114 Process Noise Tuning and Numerical Stability

Very small \\(Q\\) can make the estimator excessively confident and produce nearly singular covariance directions.

Very large \\(Q\\) can produce high gains and noisy estimates.

Thus process-noise tuning affects both statistical performance and numerical behaviour.

This is another reason \\(Q\\) should be physically justified rather than selected solely by trial and error.

4.2.28.115 Measurement Noise Tuning and Numerical Stability

Similarly, extremely small

\[ R \]

causes the filter to treat a sensor as nearly perfect.

Then

\[ S = HPH^T+R \]

may become poorly conditioned in certain situations.

Measurement uncertainty should therefore reflect realistic sensor performance, including irreducible calibration and modelling uncertainty.

4.2.28.116 Filter Divergence

Divergence occurs when estimation error grows while the estimator remains unaware of its true uncertainty.

Possible causes include:

• incorrect dynamics,
• wrong Jacobians,
• underestimated \\(Q\\),
• underestimated \\(R\\),
• frame errors,
• timing errors,
• unmodelled bias,
• poor initialization,
• or numerical failure.

A practical estimator must detect symptoms of divergence rather than assuming the recursion always remains valid.

4.2.28.117 Divergence Indicators

Useful indicators include:

• sustained high NIS,
• large residual mean,
• covariance inconsistency,
• state jumps,
• frequent gating failures,
• disagreement among independent sensors,
• and impossible physical states.

No single indicator proves divergence.

A combination of statistical and physical checks is more robust.

4.2.28.118 Innovation Whiteness

For a well-modelled estimator, innovations should approximately behave as white noise.

Autocorrelation can be examined:

\[ R_\nu(\tau) = E[ \nu_k\nu_{k-\tau}^{T} ]. \]

Strong temporal structure may indicate:

• missing dynamics,
• unmodelled bias,
• timing error,
• or inappropriate noise assumptions.

Whiteness analysis is therefore useful during filter tuning and implementation verification.

4.2.28.119 Long-Duration Testing

A filter that behaves correctly for 60 seconds may fail over hours or days because of:

• numerical accumulation,
• slow bias growth,
• covariance collapse,
• clock effects,
• or rare mode transitions.

Long-duration testing should therefore be included where representative of the intended mission.

4.2.28.120 Initialization Transients

Estimator performance immediately after startup is often different from steady-state performance.

The implementation should explicitly handle

\[ t\approx t_0 \]

as an initialization phase.

Possible differences include:

• larger covariance,
• stricter validity restrictions,
• limited adaptation,
• delayed control authority,
• and special sensor-acquisition logic.

4.2.28.121 Convergence Flag

A navigation system may provide

\[ \text{filter\_converged}. \]

This flag should not simply mean

“the filter has been running for 30 seconds.”

A stronger condition can use:

• covariance bounds,
• sufficient sensor availability,
• innovation consistency,
• bias convergence,
• and stable state estimates.

Thus convergence should be evidence-based.

4.2.28.122 Navigation Validity Flag

Similarly,

\[ \text{nav\_valid} \]

may require:

\[ P

acceptable measurement freshness,

\[ H_{\text{sensor}}=\text{healthy}, \]

and no critical numerical faults.

The specific condition depends on system requirements.

4.2.28.123 GNC Interface

The estimator should present a clearly defined interface to guidance and control.

Typical outputs include:

\[ \hat r, \quad \hat v, \quad \hat q, \quad \hat\omega, \]

together with uncertainty and status.

Guidance and control should not need to know the internal details of EKF, UKF, or ESKF implementation.

This encapsulation makes the architecture easier to maintain.

4.2.28.124 Mode-Transition Interface

When GNC changes mode, estimator configuration may need to change.

For example,

\[ M_A\rightarrow M_B. \]

The transition interface should define:

• required sensor set,
• required covariance,
• state continuity,
• filter configuration,
• and failure response.

Estimator transitions should not be hidden side effects of unrelated software logic.

4.2.28.125 Embedded Implementation Philosophy

A practical embedded estimator should generally be:

\[ \boxed{ \text{deterministic} + \text{bounded} + \text{observable} + \text{testable} + \text{recoverable}. } \]

Deterministic: execution behaviour is predictable.

Bounded: memory and computation are controlled.

Observable: internal health and diagnostic quantities can be inspected.

Testable: algorithm components can be verified independently.

Recoverable: numerical or sensor failures have defined responses.

4.2.28.126 Practical Development Workflow

A strong development sequence is:

1. derive the continuous-time model,
2. verify dynamics independently,
3. derive measurement models,
4. verify analytical Jacobians,
5. derive discrete propagation,
6. derive \\(Q_d\\),
7. implement deterministic zero-noise tests,
8. implement covariance propagation,
9. verify symmetry and positivity,
10. add individual sensor updates,
11. verify innovation statistics,
12. add gating,
13. add multi-rate scheduling,
14. add sensor dropout and recovery,
15. add navigation-health logic,
16. perform Monte Carlo,
17. perform SIL,
18. perform processor-in-the-loop,
19. perform HIL,
20. perform closed-loop GNC verification.

This progression reduces debugging complexity.

4.2.28.127 Common Implementation Mistakes

Some errors appear repeatedly.

Explicit matrix inversion

Computing

\[ S^{-1} \]

directly when a linear solve would be more stable.

Incorrect \\(Q\\) discretization

Using continuous noise covariance directly as discrete covariance.

Ignoring covariance symmetry

Allowing small numerical asymmetry to accumulate.

Using the simplified covariance update

when numerical robustness requires the Joseph form.

Incorrect timestamp handling

Applying delayed measurements at the current epoch.

Wrong frame convention

Mixing body, inertial, orbital, or sensor-frame vectors.

Quaternion convention mismatch

Mixing scalar-first/scalar-last or left/right multiplicative conventions.

Hard-coded state indices

causing errors when the state vector changes.

Ignoring units

particularly degrees versus radians.

Treating simulation truth as estimator input

which creates unrealistically good navigation.

No runtime health monitoring

allowing NaNs or invalid covariance to propagate to GNC.

4.2.28.128 Practical Checklist

Before an estimator is considered implementation-ready, verify:

Dynamics

• state equations verified,
• state transition verified,
• integration timestep justified.

Noise

• continuous/discrete distinction correct,
• \\(Q_d\\) derivation verified,
• \\(R\\) units correct.

Numerics

• no unnecessary matrix inversion,
• covariance symmetry preserved,
• positive-semidefinite behaviour checked,
• matrix conditioning acceptable.

Attitude

• quaternion convention documented,
• normalization implemented,
• error injection/reset verified.

Timing

• actual timestamps used,
• asynchronous updates supported,
• delayed measurements handled consistently.

Software

• state indices centralized,
• interfaces documented,
• memory bounded,
• execution time measured.

Health

• innovation monitoring,
• covariance monitoring,
• invalid-value detection,
• reset strategy defined.

Verification

• unit tests,
• analytical cases,
• Monte Carlo,
• SIL,
• HIL,
• closed-loop tests.

4.2.28.129 Complete Practical Filter Execution Cycle

A practical estimator cycle can be summarized as

\[ \boxed{ \begin{array}{c} \text{Receive Time / Inputs}\\ \downarrow\\ \text{Compute }\Delta t\\ \downarrow\\ \text{Propagate State}\\ \downarrow\\ \text{Propagate Covariance}\\ \downarrow\\ \text{Restore Numerical Symmetry}\\ \downarrow\\ \text{Process Available Measurements}\\ \downarrow\\ \text{Validate / Gate}\\ \downarrow\\ \text{Solve for Kalman Gain}\\ \downarrow\\ \text{Correct State}\\ \downarrow\\ \text{Joseph Covariance Update}\\ \downarrow\\ \text{Normalize / Reset Manifold States}\\ \downarrow\\ \text{Check Numerical Health}\\ \downarrow\\ \text{Publish State + Covariance + Health} \end{array} } \]

This is much closer to a real navigation implementation than the short textbook expression

\[ \text{predict}\rightarrow\text{update}. \]

4.2.28.130 Key Takeaways

Practical filter implementation is the engineering discipline that converts estimator mathematics into reliable real-time navigation software.

The most important principles are:

• Continuous spacecraft dynamics must be discretized carefully.
• State and covariance propagation should be treated as separate numerical problems.
• Continuous process spectral density and discrete process covariance are not the same quantity.
• Covariance propagation must preserve physically meaningful uncertainty.
• Covariance should remain symmetric and positive semidefinite.
• Joseph-form covariance updates provide improved numerical robustness.
• Explicit matrix inversions should generally be replaced by linear solves.
• Matrix conditioning and state scaling strongly influence numerical stability.
• Quaternion normalization and convention management are essential for attitude estimators.
• ESKF/MEKF error injection and covariance reset must be implemented consistently.
• Actual timestamps should be used when propagation intervals vary.
• Multi-rate and asynchronous measurements should be processed according to measurement time.
• Delayed measurements require explicit handling.
• Measurement gating should use statistically meaningful innovation covariance.
• State dimension directly affects computational and verification cost.
• Large estimators may benefit from sparse, square-root, or alternative formulations.
• Units, coordinate frames, sensor alignment, and lever arms are part of the estimator implementation.
• Truth data must remain separated from the navigation estimator in simulation.
• Measurement and estimator-output buses improve integration and traceability.
• Navigation output should include covariance, health, freshness, and validity.
• Real-time estimator design must consider worst-case execution time, not only average runtime.
• Memory use should be predictable and bounded.
• Estimator numerical failures must have defined detection and recovery behaviour.
• Logging innovations and covariance is fundamental for debugging and validation.
• Deterministic replay is extremely valuable for reproducing navigation problems.
• Unit tests, analytical tests, zero-noise tests, and single-sensor tests should precede full Monte Carlo campaigns.
• SIL, processor-in-the-loop, HIL, and closed-loop GNC verification progressively increase implementation realism.
• Filter configuration, calibration, and tuning parameters should be version controlled.
• Practical estimator software should be deterministic, bounded, observable, testable, and recoverable.

The central principle is

\[ \boxed{ \begin{array}{c} \text{A mathematically correct estimator is not automatically a reliable navigation system.}\\[4pt] \text{Reliable implementation requires correct discretization, stable numerical algebra,}\\ \text{consistent timing and frames, controlled computation, covariance integrity,}\\ \text{software health monitoring, and rigorous verification.} \end{array} } \]

The natural next section is 4.2.29 Simulink Implementation of Sensor Fusion, where these implementation principles move into a structured model-based environment: truth dynamics, sensor-model blocks, measurement buses, estimator subsystems, scheduling, logging, covariance outputs, and closed-loop GNC integration.

Next: 4.2.29 Simulink Implementation of Sensor Fusion

Practical filter implementation has established how estimator mathematics is converted into numerically stable, real-time software with correct discretization, covariance handling, timing, scheduling, interfaces, health monitoring, and verification.

The next section moves these implementation principles into a structured model-based environment, where truth dynamics, sensor models, measurement buses, estimator subsystems, prediction and correction scheduling, logging, and closed-loop GNC interfaces can be assembled and tested systematically.