Simulink Implementation of Sensor Fusion

Truth dynamics, realistic sensor-model blocks, structured measurement buses, estimator prediction and correction, multi-rate and asynchronous scheduling, timing and latency, validity and innovation gating, state and covariance outputs, logging, numerical safeguards, mode logic, fault injection, closed-loop GNC integration, and verification.

4.2.29 Simulink Implementation of Sensor Fusion

Sensor-fusion theory becomes practically useful only when it can be implemented as an executable estimation system. In MATLAB/Simulink, this means going beyond individual equations for a complementary filter, observer, Kalman filter, EKF, MEKF, UKF, or other estimator. A realistic model must represent the complete flow of information from the physical system to the sensors, through the estimator, and finally to the guidance and control algorithms.

A useful high-level architecture is

\[ \boxed{ \text{Truth Dynamics} \rightarrow \text{Sensor Models} \rightarrow \text{Measurement Processing} \rightarrow \text{Estimator} \rightarrow \text{Navigation Solution} \rightarrow \text{GNC} } \]

The Simulink implementation should therefore reproduce not only the estimator mathematics but also the conditions under which the estimator will operate: different sensor rates, measurement delays, noise and bias, asynchronous measurements, invalid data, initialization errors, coordinate transformations, actuator inputs, and closed-loop feedback.

The purpose of the Simulink model is ultimately to answer a practical question:

\[ \boxed{ \text{Can the estimator reliably reconstruct the required spacecraft state under realistic operating conditions?} } \]

4.2.29.1 Overall Simulink Sensor-Fusion Architecture

A well-structured sensor-fusion simulation should separate the physical truth model from the onboard estimator.

This distinction is fundamental.

The truth model represents what the spacecraft is actually doing:

\[ \mathbf{x}_{true} \]

whereas the estimator produces only its best estimate:

\[ \hat{\mathbf{x}} \]

based on the information that would actually be available onboard.

A representative architecture is

\[ \boxed{\text{Truth Dynamics}} \rightarrow \boxed{\mathbf{x}_{true}} \]

followed by

\[ \boxed{\mathbf{x}_{true}} \rightarrow \boxed{\text{Sensor Models}} \rightarrow \boxed{\mathbf{z}} \]

and

\[ \boxed{\mathbf{z},\mathbf{u}} \rightarrow \boxed{\text{Estimator}} \rightarrow \boxed{\hat{\mathbf{x}}} \]

The estimated state may then be supplied to guidance and control:

\[ \boxed{\hat{\mathbf{x}}} \rightarrow \boxed{\text{Guidance}} \rightarrow \boxed{\text{Controller}} \rightarrow \boxed{\mathbf{u}} \]

and the resulting control action is applied back to the truth dynamics.

This creates a complete closed-loop GNC simulation.

The estimator should not have direct access to the truth state. Truth signals are retained only for simulation, verification, error calculation, and plotting.

4.2.29.2 Truth Dynamics

The truth-dynamics subsystem represents the simulated physical spacecraft or vehicle.

For a general nonlinear system,

\[ \boxed{ \dot{\mathbf{x}}_{true} = f_{true} ( \mathbf{x}_{true}, \mathbf{u}_{actual}, \mathbf{d}, \mathbf{p}_{true} ) } \]

where

Depending on the application, the state can include

\[ \mathbf{x}_{true} = \begin{bmatrix} \mathbf r & \mathbf v & \mathbf q & \boldsymbol{\omega} & \mathbf b_g & \mathbf b_a & \cdots \end{bmatrix}^{T}. \]

For spacecraft simulation, truth dynamics may include translational orbital dynamics, relative dynamics, rigid-body rotational dynamics, environmental disturbances, actuator dynamics, and other relevant physical effects.

An important verification principle is that the truth model should generally be more complete than the estimator model.

If exactly the same equations, parameters, and assumptions are used in both places, the simulation can become unrealistically favorable.

For example, the truth model might use

\[ J_{true} \]

while the estimator assumes

\[ J_{est}=J_{nom}. \]

The difference

\[ \Delta J = J_{true}-J_{nom} \]

then creates realistic model mismatch.

Similarly, truth dynamics may contain disturbances that the estimator does not know exactly.

This allows robustness to be evaluated rather than merely verifying identical mathematical models against each other.

4.2.29.3 Truth State Bus

A spacecraft model may contain many truth quantities. Passing each as an individual Simulink signal quickly becomes difficult to manage.

A structured truth bus is therefore useful.

For example:

TruthBus

may contain

\[ \mathbf r_{true}, \quad \mathbf v_{true}, \quad \mathbf q_{true}, \quad \boldsymbol{\omega}_{true}, \quad \mathbf a_{true}, \quad \mathbf b_{g,true}, \quad \mathbf b_{a,true}, \quad \mathbf F_{true}, \quad \mathbf T_{true}. \]

This creates a clean interface between the physical model and sensor models.

However, the entire truth bus should not be connected to the estimator.

Instead,

\[ \boxed{\text{TruthBus}} \rightarrow \boxed{\text{Sensor Models}} \]

while only simulated sensor outputs are passed onward.

This preserves the distinction between simulation truth and flight-available information.

Why this matters: the TruthBus is an architectural boundary, not merely a convenience for cleaner wiring. It makes it possible to expose rich internal truth data to sensor models and verification blocks while preventing those same signals from leaking into the estimator or controller. That separation is essential if the Simulink model is intended to represent the information actually available in flight.

4.2.29.4 Sensor-Model Layer

The next layer converts physical truth into realistic measurements.

A generic sensor model is

\[ \boxed{ \mathbf z_k = h(\mathbf x_{true,k}) + \mathbf b_k + \mathbf v_k } \]

with possible additional effects such as scale factor, misalignment, quantization, saturation, latency, dropout, and finite bandwidth.

A more complete representation may be written as

\[ \boxed{ \mathbf z_k = \mathcal Q \left[ \mathbf S \mathbf M h(\mathbf x_{true,k-\tau}) + \mathbf b_k + \mathbf v_k \right] } \]

where

A realistic sensor subsystem should therefore reproduce the measurement available to flight software, not simply output a selected component of the truth state.

4.2.29.5 IMU Sensor Blocks

An IMU subsystem can contain separate gyroscope and accelerometer models.

A gyroscope can be represented as

\[ \boxed{ \boldsymbol{\omega}_m = (\mathbf I+\mathbf S_g) \mathbf M_g \boldsymbol{\omega}_{true} + \mathbf b_g + \mathbf n_g } \]

with bias dynamics such as

\[ \boxed{ \mathbf b_{g,k+1} = \mathbf b_{g,k} + \mathbf w_{bg,k}. } \]

An accelerometer model can similarly be written as

\[ \boxed{ \mathbf f_m = (\mathbf I+\mathbf S_a) \mathbf M_a \mathbf f_{true} + \mathbf b_a + \mathbf n_a. } \]

In Simulink, these effects can be implemented as separate blocks so they can be enabled, disabled, and varied independently.

This is particularly useful for Monte Carlo testing.

4.2.29.6 GNSS Measurement Block

A simplified GNSS subsystem might provide

\[ \boxed{ \mathbf z_{GNSS} = \begin{bmatrix} \mathbf r_{GNSS}\\ \mathbf v_{GNSS} \end{bmatrix} } \]

with

\[ \mathbf r_{GNSS} = \mathbf r_{true} + \mathbf n_r \]

and

\[ \mathbf v_{GNSS} = \mathbf v_{true} + \mathbf n_v. \]

A more realistic model may include update rate, latency, outages, geometry-dependent performance, invalid-fix conditions, clock-related effects, or raw measurements such as pseudorange and Doppler.

The required level of fidelity depends strongly on whether the estimator is loosely or tightly coupled.

4.2.29.7 Camera and Vision Measurement Blocks

A camera should normally not be simulated as directly providing the exact truth position unless that is genuinely the intended processed measurement interface.

Depending on the navigation architecture, the camera model may output quantities such as

\[ \boxed{ \text{Bearing}, \quad \text{Line of Sight}, \quad \text{Feature Coordinates}, \quad \text{Relative Pose} } \]

with appropriate uncertainty.

For a pinhole-camera representation,

\[ u=f_x\frac{X_C}{Z_C}+c_x, \]

\[ v=f_y\frac{Y_C}{Z_C}+c_y. \]

Thus the simulated measurement is generated from geometry rather than simply copying the true state.

The model can additionally represent field-of-view constraints:

\[ \boxed{ \text{Target Outside FOV} \Rightarrow \text{Measurement Invalid}. } \]

This becomes important when validating sensor fusion during dynamic manoeuvres.

4.2.29.8 LiDAR or Range-Sensor Blocks

A simple range measurement may be represented by

\[ \boxed{ \rho_m = \|\mathbf r_{rel}\| + n_\rho. } \]

More sophisticated LiDAR processing may provide range, bearing, point-cloud-derived geometry, or relative pose.

The Simulink sensor subsystem should also represent operational limitations such as minimum and maximum range, field of view, update frequency, measurement quality, and temporary loss of target.

For general RPO simulations, this enables the estimator to transition between different navigation sensors as geometry and range change, without assuming any mission-specific implementation.

4.2.29.9 Measurement Bus

Once individual sensors have been simulated, their outputs should normally be organized into a measurement interface.

For example:

MeasurementBus

may contain:

\[ \{ \text{gyro}, \text{accelerometer}, \text{GNSS}, \text{star tracker}, \text{Sun sensor}, \text{magnetometer}, \text{camera}, \text{LiDAR} \}. \]

But measurement values alone are not sufficient.

Each measurement should ideally travel with metadata such as

\[ \boxed{ \text{Value} + \text{Timestamp} + \text{Valid Flag} + \text{Status} + \text{Quality Information} } \]

because the estimator must know not merely what was measured, but also when it was measured and whether it should be trusted.

Implementation interpretation: a measurement bus is most useful when it behaves like a flight-software data contract. Every sensor packet should carry enough context for the estimator to decide whether the data are current, valid, correctly framed, and trustworthy. This prevents a numerical value from being treated as usable information simply because it exists on a signal line.

4.2.29.10 Measurement Preprocessing

Raw measurements may require preprocessing before estimator use.

A useful Simulink chain is

\[ \boxed{\text{Raw Sensor}} \rightarrow \boxed{\text{Calibration}} \rightarrow \boxed{\text{Frame Transformation}} \rightarrow \boxed{\text{Validity Check}} \rightarrow \boxed{\text{Estimator Measurement}}. \]

Possible preprocessing includes bias calibration, unit conversion, coordinate transformation, normalization, timestamp handling, outlier detection, saturation detection, and quality checks.

For example, a vector measurement may require

\[ \boxed{ \mathbf z^{B} = \mathbf C_{S}^{B}\mathbf z^{S} } \]

to transform it from the sensor frame S into the body frame B.

Frame mistakes are particularly dangerous because the resulting signals can remain numerically reasonable while being physically incorrect.

4.2.29.11 Estimator Subsystem

The estimator should normally be implemented as a clearly isolated subsystem.

Its inputs may include

\[ \mathbf u_k, \quad \mathbf z_k, \quad \text{validity flags}, \quad \Delta t, \quad \text{mode information}. \]

Its outputs may include

\[ \hat{\mathbf x}_k, \quad \mathbf P_k, \quad \mathbf r_k, \quad \mathbf S_k, \quad \text{health flags}. \]

The internal estimator architecture should reflect the fundamental recursive structure

\[ \boxed{ \text{Prediction} \rightarrow \text{Measurement Update} \rightarrow \text{Corrected State} } \]

rather than combining all calculations into one opaque block.

This makes the implementation easier to verify and debug.

4.2.29.12 Prediction Subsystem

For a discrete estimator,

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

where

\[ \hat{\mathbf x}_{k}^{-} \]

is the predicted state.

For a linear model,

\[ \boxed{ \hat{\mathbf x}_{k}^{-} = \mathbf F_{k-1} \hat{\mathbf x}_{k-1}^{+} + \mathbf G_{k-1}\mathbf u_{k-1}. } \]

For a Kalman-type estimator, covariance is propagated simultaneously:

\[ \boxed{ \mathbf P_k^{-} = \mathbf F_{k-1} \mathbf P_{k-1}^{+} \mathbf F_{k-1}^{T} + \mathbf Q_{k-1}. } \]

Prediction normally executes at the estimator's navigation propagation rate.

4.2.29.13 Measurement-Prediction Subsystem

The estimator must predict what each sensor should measure.

For a nonlinear sensor,

\[ \boxed{ \hat{\mathbf z}_k = h(\hat{\mathbf x}_k^{-}). } \]

The innovation is

\[ \boxed{ \mathbf r_k = \mathbf z_k - \hat{\mathbf z}_k. } \]

For a linear measurement model,

\[ \boxed{ \mathbf r_k = \mathbf z_k - \mathbf H_k\hat{\mathbf x}_k^{-}. } \]

The innovation is one of the most important diagnostic signals in the entire estimator.

It connects the predicted physical state to the actual sensor information.

4.2.29.14 Measurement Update Subsystem

For a linear Kalman filter,

\[ \mathbf S_k = \mathbf H_k \mathbf P_k^{-} \mathbf H_k^T + \mathbf R_k, \]

\[ \boxed{ \mathbf K_k = \mathbf P_k^{-} \mathbf H_k^T \mathbf S_k^{-1} } \]

and

\[ \boxed{ \hat{\mathbf x}_k^{+} = \hat{\mathbf x}_k^{-} + \mathbf K_k\mathbf r_k. } \]

The covariance is then corrected.

For example, the Joseph form is

\[ \boxed{ \mathbf P_k^{+} = (\mathbf I-\mathbf K_k\mathbf H_k) \mathbf P_k^{-} (\mathbf I-\mathbf K_k\mathbf H_k)^T + \mathbf K_k\mathbf R_k\mathbf K_k^T. } \]

This form is often attractive for implementation because it better preserves covariance symmetry and positive-semidefinite behavior under finite numerical precision.

4.2.29.15 Prediction and Update Scheduling

One of the most important practical differences between textbook equations and a Simulink implementation is scheduling.

Sensors rarely arrive simultaneously.

For example,

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

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

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

The estimator should therefore not execute every measurement correction at every simulation step.

Instead,

\[ \boxed{ \text{Every Estimator Step} \rightarrow \text{Predict} } \]

while

\[ \boxed{ \text{New Valid Sensor Measurement} \rightarrow \text{Update}. } \]

This separation is essential.

Why scheduling matters: prediction and correction are logically different tasks. Prediction advances the estimate using the process model, whereas measurement correction should occur only when new information is actually available. If Simulink executes correction blocks at the wrong rate, the filter may effectively reuse stale measurements and become unrealistically confident.

4.2.29.16 Multi-Rate Sensor Processing

Suppose the navigation propagation interval is

\[ T_{nav}=0.01\text{ s}. \]

The estimator predicts every 0.01 s.

A GNSS measurement may arrive every

\[ T_{GNSS}=0.1\text{ s}, \]

while a camera may update every

\[ T_{cam}=0.2\text{ s}. \]

The execution may therefore resemble

\[ \begin{array}{c|c|c|c} t & \text{Predict} & \text{GNSS Update} & \text{Camera Update}\\ \hline 0.00 & \checkmark & \checkmark & \checkmark\\ 0.01 & \checkmark & &\\ 0.02 & \checkmark & &\\ \vdots & \vdots & &\\ 0.10 & \checkmark & \checkmark &\\ 0.20 & \checkmark & \checkmark & \checkmark \end{array} \]

In Simulink, this can be represented using triggered subsystems, enabled subsystems, function-call subsystems, rate transitions, or carefully controlled execution logic.

The architecture should reproduce the intended flight-software timing rather than relying accidentally on Simulink block execution order.

4.2.29.17 Asynchronous Measurements

Different sensors may not be integer multiples of one common update period.

A measurement may arrive at time

\[ t_m \]

between estimator cycles.

The implementation must decide how to process that measurement.

Possible strategies include buffering it until the next navigation cycle, propagating to the measurement timestamp, performing an asynchronous update, or using delayed-measurement handling.

The essential principle is

\[ \boxed{ \text{Measurement Value Without Correct Time} \neq \text{Correct Measurement Information}. } \]

4.2.29.18 Sample Time Management

Sample times should be explicitly designed.

A typical model might contain

\[ T_{plant}, \quad T_{IMU}, \quad T_{nav}, \quad T_{GNSS}, \quad T_{vision}, \quad T_{control}. \]

These are not necessarily equal.

The model therefore needs a deliberate relationship between continuous dynamics and discrete algorithms.

For example,

\[ \boxed{ \text{Continuous/High-Rate Truth} \rightarrow \text{Sampled Sensors} \rightarrow \text{Discrete Estimator} \rightarrow \text{Discrete Controller}. } \]

A common implementation mistake is allowing inherited sample times to determine estimator behavior unintentionally.

4.2.29.19 Rate Transition Blocks

Signals crossing between rates may require explicit rate-transition handling.

For example,

\[ 100\text{ Hz IMU} \rightarrow 50\text{ Hz estimator} \]

or

\[ 10\text{ Hz estimator output} \rightarrow 100\text{ Hz controller} \]

creates a rate boundary.

Rate-transition logic helps control data transfer and deterministic execution.

This becomes increasingly important when moving from desktop simulation toward generated code, real-time execution, SIL, PIL, and HIL.

Practical effect: rate transitions are not only a Simulink bookkeeping issue. They define how data are held, transferred, and synchronized across tasks. When the model later moves toward generated code or real-time execution, those decisions directly affect determinism, latency, and whether downstream blocks receive coherent samples.

4.2.29.20 Initialization

Estimator initialization is a major part of implementation.

At simulation start,

\[ \hat{\mathbf x}_0 \]

must be assigned.

For covariance-based estimators,

\[ \boxed{ \mathbf P_0 = E[ (\mathbf x_0-\hat{\mathbf x}_0) (\mathbf x_0-\hat{\mathbf x}_0)^T ]. } \]

The initial covariance should represent realistic uncertainty.

An unrealistically small \mathbf P_0 tells the estimator that its initial estimate is highly reliable even when it is not.

Conversely, excessively large uncertainty may produce aggressive initial corrections.

Initialization tests should therefore include deliberate state-estimation errors:

\[ \hat{\mathbf x}_0 \neq \mathbf x_{true,0}. \]

The filter should demonstrate appropriate convergence.

4.2.29.21 Attitude Initialization

Attitude requires special treatment because quaternions must satisfy

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

The initial quaternion should therefore be normalized.

For error-state or multiplicative filters, the nominal quaternion and attitude-error state must also be initialized consistently.

An attitude initialization error should be introduced as a physical rotation, rather than by arbitrarily perturbing individual quaternion elements.

4.2.29.22 State Storage and Memory

Recursive filters require previous state information.

For example,

\[ \hat{\mathbf x}_{k-1}^{+} \rightarrow \hat{\mathbf x}_{k}^{-}. \]

Simulink can store these values using Unit Delay, Delay, Memory, Discrete State-Space blocks, MATLAB Function persistent states, or subsystem states.

Care is required to avoid algebraic loops.

The estimator is inherently recursive, so the model must explicitly preserve the distinction between

\[ k-1 \]

and

\[ k. \]

4.2.29.23 MATLAB Function Blocks

MATLAB Function blocks can be useful for implementing estimator mathematics that becomes cumbersome with elementary Simulink blocks.

For example, one block might implement

\[ [\hat{\mathbf x}^{-},\mathbf P^{-}] = \text{Predict} ( \hat{\mathbf x}^{+}, \mathbf P^{+}, \mathbf u, \Delta t ) \]

while another performs

\[ [\hat{\mathbf x}^{+},\mathbf P^{+}] = \text{Update} ( \hat{\mathbf x}^{-}, \mathbf P^{-}, \mathbf z ). \]

However, using one enormous MATLAB Function block for the entire navigation system can make debugging difficult.

A better engineering structure is usually modular:

\[ \boxed{\text{Propagation}} \]

\[ \boxed{\text{Sensor-Specific Measurement Model}} \]

\[ \boxed{\text{Innovation}} \]

\[ \boxed{\text{Validation}} \]

\[ \boxed{\text{Correction}}. \]

4.2.29.24 Sensor-Specific Update Subsystems

For a multi-sensor estimator, separate measurement-update subsystems can be useful.

For example:

\[ \boxed{\text{GNSS Update}} \]

\[ \boxed{\text{Star-Tracker Update}} \]

\[ \boxed{\text{Magnetometer Update}} \]

\[ \boxed{\text{Camera Update}} \]

\[ \boxed{\text{LiDAR Update}}. \]

Each subsystem can contain its own

\[ h_i(\mathbf x), \quad \mathbf H_i, \quad \mathbf R_i, \quad \mathbf r_i, \quad \text{validation logic}. \]

This architecture improves modularity and makes sensor dropout easier to test.

4.2.29.25 Measurement Validity Logic

A sensor measurement should not automatically trigger an estimator update simply because a numerical value exists.

A validity signal should accompany it.

Conceptually,

\[ \boxed{ \text{New Measurement} \land \text{Sensor Healthy} \land \text{Measurement Valid} } \Rightarrow \boxed{\text{Perform Update}}. \]

Otherwise,

\[ \boxed{\text{Prediction Only}}. \]

Validity checks may include sensor status, field of view, saturation, measurement age, geometry, signal quality, and residual consistency.

4.2.29.26 Innovation Gating

For a Kalman-type estimator, innovation covariance is

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

A normalized innovation statistic is

\[ \boxed{ \gamma_k = \mathbf r_k^T \mathbf S_k^{-1} \mathbf r_k. } \]

The measurement may be accepted if

\[ \gamma_k \le \gamma_{max} \]

and rejected otherwise.

Thus,

\[ \boxed{ \text{Measurement} \rightarrow \text{Residual} \rightarrow \text{Gate} \rightarrow \begin{cases} \text{Update}\\ \text{Reject} \end{cases} } \]

can be implemented directly in Simulink.

This provides an important bridge between sensor fusion and FDIR.

4.2.29.27 Handling Sensor Dropout

Sensor dropout should be explicitly represented.

Suppose the camera becomes unavailable.

Then

\[ \text{cameraValid}=0. \]

The estimator should skip the camera update while continuing propagation and any other available sensor updates.

Therefore,

\[ \boxed{ \text{Sensor Loss} \not\Rightarrow \text{Estimator Stops}. } \]

Instead,

\[ \boxed{ \text{Sensor Loss} \Rightarrow \text{Reduced Measurement Information}. } \]

The resulting growth in estimation uncertainty is itself an important quantity to monitor.

4.2.29.28 Measurement Latency

A sensor measurement generated at

\[ t_m \]

may reach the estimator at

\[ t_m+\tau. \]

A delay block can reproduce this effect in simulation, but the estimator must also process the timestamp correctly.

If a delayed measurement is compared with the current prediction,

\[ \mathbf z(t-\tau) - h[\hat{\mathbf x}(t)], \]

the resulting innovation contains timing mismatch.

Therefore, latency should not be treated merely as an arbitrary signal delay. It is part of the estimator's temporal architecture.

4.2.29.29 Out-of-Sequence Measurements

In complex systems, measurements may occasionally arrive in a different order from their acquisition times.

For example,

\[ t_{z_1}

but

\[ t_{arrival,z_1}>t_{arrival,z_2}. \]

The estimator must define what happens in this case.

Possible approaches include rejection, buffering and reordering, state-history correction, or repropagation.

The appropriate approach depends on estimator architecture and computational constraints.

4.2.29.30 Coordinate Frames

Every signal should have an explicitly defined coordinate frame.

Common frames include inertial, orbital/local, body, sensor, camera, navigation, and target-relative frames.

For example,

\[ \mathbf v^B = \mathbf C_I^B\mathbf v^I. \]

A measurement model and estimator must use consistent frame definitions.

A useful Simulink naming convention is therefore not merely

velocity

but something like

velocity_I

or

omega_B.

Frame information should be visible from the signal/interface definition rather than remembered informally.

4.2.29.31 Units

The same principle applies to units.

Signals should use a consistent convention such as SI units:

\[ m,\quad m/s,\quad rad,\quad rad/s,\quad N,\quad N\,m. \]

Mixing degrees and radians is a common source of estimator failure.

A robust implementation makes units explicit in signal names, bus definitions, documentation, or Simulink signal properties.

4.2.29.32 Quaternion Handling

Quaternion-based estimators require careful implementation.

After numerical propagation or correction,

\[ \mathbf q \]

may deviate slightly from unit norm.

Therefore,

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

may be required.

However, normalization should not be used to hide incorrect quaternion dynamics.

The implementation must also maintain one consistent convention for quaternion order, multiplication, direction of rotation, frame interpretation, and attitude-error definition.

4.2.29.33 Covariance Outputs

For Kalman-family estimators, the state estimate alone is not sufficient.

The estimator should expose

\[ \boxed{\mathbf P} \]

or at least important diagonal terms.

For state x_i,

\[ \boxed{ \sigma_i = \sqrt{P_{ii}}. } \]

This enables plots of

\[ e_i = x_{true,i}-\hat{x}_i \]

against uncertainty bounds such as

\[ \pm3\sigma_i. \]

These plots are extremely valuable for determining whether the estimator is internally consistent.

4.2.29.34 Covariance Symmetry

Numerical calculations can cause

\[ \mathbf P \neq \mathbf P^T \]

by a small amount.

A common numerical safeguard is

\[ \boxed{ \mathbf P \leftarrow \frac12(\mathbf P+\mathbf P^T). } \]

The covariance should also remain positive semidefinite.

Unexpected negative diagonal elements or strongly negative eigenvalues indicate a serious implementation or numerical problem.

4.2.29.35 Avoid Explicit Matrix Inversion

Expressions such as

\[ \mathbf K = \mathbf P^{-}\mathbf H^T \mathbf S^{-1} \]

are mathematically convenient, but explicit matrix inversion is generally undesirable in numerical implementation.

Instead of explicitly computing

\[ \mathbf S^{-1}, \]

the implementation should solve the corresponding linear system using numerically appropriate methods.

This improves numerical stability and computational behavior.

Numerical reason: forming an explicit inverse introduces unnecessary computation and can amplify roundoff error, especially when the innovation covariance is poorly conditioned. Solving the associated linear system preserves the same mathematics while using numerical methods that are generally more stable and better suited to implementation.

4.2.29.36 Navigation Output Bus

The estimator should expose a clean navigation interface to the rest of the GNC system.

For example:

NavBus

may contain

\[ \hat{\mathbf r}, \quad \hat{\mathbf v}, \quad \hat{\mathbf q}, \quad \hat{\boldsymbol{\omega}}, \quad \hat{\mathbf b}_g, \quad \hat{\mathbf b}_a, \]

together with

\[ \text{NavValid}, \quad \text{NavMode}, \quad \text{timestamp}, \quad \text{quality}. \]

This prevents guidance and control algorithms from depending on internal estimator implementation details.

Interface principle: the navigation output bus should expose what downstream GNC functions need without exposing how the estimator is internally implemented. That makes it possible to replace an EKF with another estimator architecture while keeping guidance and control interfaces unchanged, provided the output contract remains the same.

4.2.29.37 Truth Must Not Enter the Navigation Bus

This deserves explicit emphasis.

The navigation bus should contain

\[ \boxed{\hat{\mathbf x}} \]

rather than

\[ \boxed{\mathbf x_{true}}. \]

Truth may be used for

\[ \boxed{ \text{Verification} + \text{Plotting} + \text{Performance Evaluation} } \]

but not as a hidden input to operational GNC algorithms.

Otherwise the simulation no longer represents closed-loop navigation performance.

4.2.29.38 Closed-Loop GNC Integration

Once the estimator is connected to guidance and control, the architecture becomes

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

This is fundamentally different from testing an estimator offline.

Estimator errors now influence control commands.

Control commands influence the physical trajectory.

The changed trajectory then changes future sensor measurements.

Thus navigation and control become dynamically coupled.

4.2.29.39 Estimated State vs True State in the Controller

During early controller development, a controller may be tested using truth:

\[ \mathbf u = g(\mathbf x_{true}). \]

This is useful for isolating control-law performance.

But integrated GNC testing should eventually use

\[ \boxed{ \mathbf u = g(\hat{\mathbf x}). } \]

The difference between these two cases reveals how navigation error affects closed-loop control performance.

4.2.29.40 Actuator Commands as Estimator Inputs

Some estimators require knowledge of commanded or applied control.

For example,

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

But care is required in deciding whether \mathbf u_k represents commanded input or actual applied input.

For a real actuator,

\[ \mathbf u_{actual} \neq \mathbf u_{cmd} \]

because of saturation, delay, quantization, deadband, misalignment, or actuator uncertainty.

This distinction can become important in high-fidelity simulation.

Implementation consequence: if the estimator assumes the commanded actuation was perfectly achieved while the truth model applies actuator limits or delays, the estimator will accumulate model error. High-fidelity models should therefore define explicitly which actuation quantity is observable or reconstructable onboard and use that quantity consistently in the estimator dynamics.

4.2.29.41 Mode-Dependent Navigation

A spacecraft may not use the same sensors throughout every mission phase.

A general proximity-operations example could be

\[ \boxed{\text{Long Range}} \rightarrow \boxed{\text{Intermediate Range}} \rightarrow \boxed{\text{Close Approach}} \rightarrow \boxed{\text{Capture/Terminal Phase}}. \]

Sensor availability and quality can change with range and geometry.

The Simulink model should therefore permit sensor updates to be enabled or disabled according to navigation mode.

Importantly, mode switching should affect the available measurements, not artificially force the estimated state toward truth.

4.2.29.42 Estimator Mode Logic

A navigation system may include states such as

\[ \boxed{ \text{Initialize} \rightarrow \text{Acquire} \rightarrow \text{Navigate} \rightarrow \text{Degraded} \rightarrow \text{Reacquire} }. \]

Stateflow can be useful for implementing such logic.

For example:

This separates estimation mathematics from supervisory logic.

Architectural separation: estimator mode logic should decide which measurements and configurations are permitted, while the estimator mathematics should continue to perform propagation and correction consistently. Keeping supervisory logic separate from the numerical filter makes mode transitions easier to verify and prevents hidden changes in the estimator equations.

4.2.29.43 Logging Architecture

A sensor-fusion model should be designed for analysis from the beginning.

Important logged quantities include

\[ \mathbf x_{true}, \quad \hat{\mathbf x}, \quad \mathbf P, \quad \mathbf z, \quad \hat{\mathbf z}, \quad \mathbf r, \quad \mathbf S, \quad \mathbf K, \]

as well as validity flags, sensor timestamps, navigation mode, update events, controller commands, and actuator outputs.

Without these signals, diagnosing an estimator failure becomes unnecessarily difficult.

4.2.29.44 State-Error Logging

The basic estimation error is

\[ \boxed{ \mathbf e_k = \mathbf x_{true,k} - \hat{\mathbf x}_k. } \]

For position,

\[ \mathbf e_r = \mathbf r_{true} - \hat{\mathbf r}. \]

For velocity,

\[ \mathbf e_v = \mathbf v_{true} - \hat{\mathbf v}. \]

For attitude, direct quaternion subtraction is usually not the most meaningful metric.

Instead, form an attitude-error quaternion and obtain a physical angular error.

4.2.29.45 Innovation Logging

Innovation should be logged for each sensor:

\[ \boxed{ \mathbf r_i = \mathbf z_i - h_i(\hat{\mathbf x}^{-}). } \]

A well-performing estimator should generally produce innovations that are statistically compatible with the assumed measurement model.

Persistent non-zero innovation can indicate bias, model error, frame error, calibration error, or an incorrectly estimated state.

Large intermittent innovations may indicate outliers, timing errors, dropouts, or geometry problems.

4.2.29.46 Update-Event Logging

It is useful to log a binary signal for every measurement update:

\[ u_i(k) = \begin{cases} 1,&\text{sensor }i\text{ used}\\ 0,&\text{sensor }i\text{ not used}. \end{cases} \]

Similarly, a rejection flag can be logged.

This makes it possible to distinguish

\[ \boxed{\text{Sensor Produced Measurement}} \]

from

\[ \boxed{\text{Estimator Actually Used Measurement}}. \]

That distinction is crucial when diagnosing multi-sensor systems.

Diagnostic value: update-event logging makes filter behavior explainable. A large state change may be caused by a valid correction, a rejected measurement followed by propagation, or a sensor becoming available again. Without explicit update and rejection flags, these cases can look identical in the state history.

4.2.29.47 Data Inspector and Post-Processing

Simulink Data Inspector is useful for comparing runs and inspecting synchronized signals.

MATLAB post-processing can then calculate quantitative metrics such as

\[ RMSE = \sqrt{ \frac{1}{N} \sum_{k=1}^{N} e_k^2 }. \]

Other metrics may include maximum error, steady-state error, convergence time, innovation statistics, percentage of rejected measurements, navigation availability, covariance consistency, and closed-loop mission performance.

A good simulation therefore produces both plots and quantitative results.

4.2.29.48 Estimator Verification in Open Loop

Before connecting the estimator to the controller, it should be tested independently.

A useful architecture is

\[ \boxed{\text{Prescribed Truth Trajectory}} \rightarrow \boxed{\text{Sensor Simulation}} \rightarrow \boxed{\text{Estimator}}. \]

This allows estimator errors to be studied without controller behavior changing the trajectory.

Tests should include initialization errors, measurement noise, sensor bias, parameter errors, dropout, latency, outliers, and manoeuvres.

Only after satisfactory estimator behavior should the complete closed loop be emphasized.

4.2.29.49 Closed-Loop Verification

The next stage is

\[ \boxed{ \text{Plant} \rightarrow \text{Sensors} \rightarrow \text{Estimator} \rightarrow \text{Controller} \rightarrow \text{Plant}. } \]

This reveals effects that cannot be seen in estimator-only testing.

For example,

\[ \text{Navigation Noise} \rightarrow \text{Control Command Variation} \]

and

\[ \text{Navigation Bias} \rightarrow \text{Steady Control Error}. \]

Closed-loop verification should therefore examine both estimation performance and mission/control performance.

4.2.29.50 Solver Selection

Solver configuration matters because the model may contain continuous spacecraft dynamics together with discrete sensor and estimator algorithms.

The simulation may therefore contain

\[ \boxed{\text{Continuous Plant}} + \boxed{\text{Discrete Sensors}} + \boxed{\text{Discrete Estimator}}. \]

During algorithm development, a variable-step solver may be convenient for continuous truth dynamics.

For real-time or HIL-oriented development, a fixed-step architecture is generally more representative.

The important requirement is that solver behavior must not accidentally alter the intended sensor or estimator timing.

Key implementation point: solver selection should preserve the intended discrete timing of the estimator even when the physical plant is continuous. A numerically accurate plant solution is not enough if sensor sampling or estimator updates occur at unintended times.

4.2.29.51 Fixed-Step Implementation

A fixed-step model uses a base simulation interval

\[ T_s. \]

Other tasks may execute at integer multiples:

\[ T_{IMU}=N_1T_s, \]

\[ T_{nav}=N_2T_s, \]

\[ T_{GNSS}=N_3T_s. \]

This produces deterministic scheduling and is particularly important as the model moves toward real-time implementation.

The selected base step must be sufficiently small to represent the fastest required dynamics and task.

4.2.29.52 Numerical Precision

Desktop simulations commonly use double precision.

Flight processors may use different numerical representations.

Estimator implementation should therefore eventually consider numerical conditioning, single-precision behavior where relevant, matrix factorization, covariance symmetry, quaternion normalization, very small/large covariance values, and division by poorly conditioned quantities.

An algorithm that works mathematically may still fail numerically.

4.2.29.53 Algebraic Loops

Sensor-fusion models can accidentally create algebraic loops when the current estimator output is used immediately to construct an input that is required to calculate that same output.

Recursive estimation should maintain a clear temporal structure:

\[ \boxed{ \hat{\mathbf x}_{k-1}^{+} \rightarrow \hat{\mathbf x}_{k}^{-} \rightarrow \hat{\mathbf x}_{k}^{+}. } \]

Explicit delays or states should be used where required.

4.2.29.54 Parameter Management

Estimator parameters should not be scattered through individual blocks as unexplained numerical constants.

Useful parameter groups include

\[ \mathbf Q, \quad \mathbf R, \quad \mathbf P_0, \quad \mathbf x_0, \]

sensor update rates, sensor biases, noise parameters, frame transformations, gating thresholds, physical constants, and model parameters.

These can be organized through initialization scripts, structures, data dictionaries, or parameter objects.

A structured parameter architecture improves traceability and repeatability.

Engineering benefit: centralized parameter management ensures that tuning values, sensor characteristics, and model constants are traceable and reproducible. It also prevents two visually identical Simulink runs from using different hidden block parameters.

4.2.29.55 Configuration Separation

It is useful to separate

\[ \boxed{\text{Algorithm}} \]

from

\[ \boxed{\text{Configuration}}. \]

The estimator subsystem should define how estimation works.

Configuration should define which sensors are enabled, their noise levels, sample rates, initial conditions, model parameters, and test scenario.

This allows the same estimator implementation to be tested under many conditions without manually editing the model.

Why separation helps: keeping configuration outside the algorithm allows one verified estimator implementation to be reused across nominal, degraded, and verification cases without editing its internal equations. This reduces configuration drift and makes regression testing significantly more reliable.

4.2.29.56 Variant Subsystems

Variant subsystems can be useful when comparing estimator architectures.

For example,

\[ \boxed{ \text{Complementary Filter} \;|\; \text{Observer} \;|\; \text{EKF} \;|\; \text{MEKF} \;|\; \text{UKF} }. \]

All variants can share the same truth dynamics and sensor models.

This makes comparisons more meaningful because the estimators experience identical simulated conditions.

4.2.29.57 Reusable Sensor Interfaces

Sensor blocks should ideally expose standardized interfaces.

For example,

\[ \boxed{ \text{Sensor Output} = \{ \mathbf z, t, valid, status \}. } \]

This makes it easier to replace a simulated sensor with recorded data, a higher-fidelity sensor model, or eventually a hardware interface without redesigning the estimator.

This is one of the major advantages of a well-designed Simulink architecture.

Long-term advantage: a standardized sensor interface decouples sensor simulation from estimation logic. A simulated sensor, recorded dataset, or hardware-fed measurement can then use the same estimator input contract, which is especially valuable when progressing from desktop simulation toward SIL and HIL.

4.2.29.58 Recorded-Data Replay

An estimator should eventually be testable using recorded measurements rather than only simulated measurements.

Then

\[ \boxed{\text{Simulated Sensor Bus}} \]

can be replaced with

\[ \boxed{\text{Recorded Sensor Bus}} \]

while preserving the same estimator interface.

This supports regression testing and provides a natural bridge toward SIL/HIL and flight-data analysis.

4.2.29.59 Fault Injection

The simulation should permit deliberate fault injection.

Examples include

\[ \boxed{ \text{Bias Step}, \text{Noise Increase}, \text{Dropout}, \text{Stuck Measurement}, \text{Outlier}, \text{Latency}, \text{Misalignment} }. \]

A fault-injection layer should ideally sit within or immediately after the sensor model rather than modifying estimator internals.

This preserves architectural realism.

Verification interpretation: injecting faults at the sensor boundary tests the complete estimator response to realistic bad information. Altering internal estimator variables would test the algorithm differently and could bypass the same validation, gating, and health-monitoring logic that must protect the real system.

4.2.29.60 Verification Against Analytical Cases

Before testing complex nonlinear scenarios, individual estimator blocks should be verified using cases for which the expected behavior is known.

For a simple linear system,

\[ \mathbf x_{k+1} = \mathbf F\mathbf x_k \]

can be compared against direct MATLAB calculations.

Likewise, one prediction/update cycle can be calculated manually and compared with the Simulink output.

This helps identify implementation errors before complexity is added.

Why start simple: analytical or hand-computable cases isolate implementation correctness from model complexity. If one prediction or update step disagrees with an independent calculation, the discrepancy can be traced before nonlinear dynamics, asynchronous timing, and multiple sensors obscure the source.

4.2.29.61 MATLAB vs Simulink Cross-Verification

A particularly useful verification approach is to implement the same filter in both MATLAB and Simulink.

Using identical

\[ \mathbf x_0,\mathbf P_0,\mathbf Q,\mathbf R,\mathbf z_k \]

should produce equivalent results within expected numerical tolerance.

If they differ, possible causes include sample-time errors, block execution order, initialization, state-storage errors, indexing differences, or inconsistent equations.

Cross-verification value: MATLAB and Simulink provide two independent execution paths for the same mathematics. Agreement between them does not prove the estimator is physically correct, but disagreement is a strong indicator of implementation, timing, indexing, initialization, or state-management errors.

4.2.29.62 Testing Sensor Rates Individually

Multi-rate systems should be built incrementally.

First test one sensor.

Then add another.

For example,

\[ \boxed{\text{Propagation Only}} \]

then

\[ \boxed{\text{Propagation + GNSS}} \]

then

\[ \boxed{\text{Propagation + GNSS + IMU}} \]

then additional measurements.

This makes it much easier to identify which interface or update introduces instability.

Recommended interpretation: incremental sensor integration turns a difficult system-level debugging problem into a sequence of smaller verification problems. Each newly added sensor should demonstrate the expected innovation, correction direction, covariance effect, and update timing before another sensor is introduced.

4.2.29.63 Testing with Noise Disabled

Before adding stochastic effects, test deterministic behavior.

Set

\[ \mathbf Q=0, \qquad \mathbf R=0 \]

where appropriate for the particular verification case, and remove simulated sensor errors.

The estimator should then reproduce the expected deterministic behavior.

Noise should be introduced only after the fundamental implementation is verified.

What this test reveals: deterministic testing removes random variation so that sign errors, frame mistakes, incorrect state ordering, and scheduling problems are easier to see. Noise-free agreement should therefore be treated as a prerequisite, not as evidence of final performance.

4.2.29.64 Testing with Known Bias

After deterministic verification, introduce a known bias:

\[ b=b_{test}. \]

If bias is included in the estimator state, verify that

\[ \hat b \rightarrow b_{test} \]

under observable conditions.

This is a powerful way to test augmented-state estimators.

Observability matters: a bias state will converge only if the available measurements and motion provide information that distinguishes that bias from other state errors. Failure of \(\hat b\) to approach the injected bias is therefore not automatically a tuning problem; it may reveal weak observability or an incorrect measurement model.

4.2.29.65 Testing Dropout

Temporarily set

\[ valid_i=0. \]

Then examine whether the estimator continues prediction, uncertainty grows appropriately, other sensors continue updating, and reacquisition occurs without unacceptable transient behavior.

Dropout testing should be performed for each important measurement source.

What to inspect: during dropout, the state estimate may remain smooth while its uncertainty grows. That covariance growth is expected when information is lost. The important questions are whether prediction continues correctly, other sensors still contribute, navigation validity reflects the loss of information, and reacquisition occurs without an artificial reset.

4.2.29.66 Testing Timing Errors

Deliberately introduce sensor delay:

\[ \tau>0. \]

Compare correct timestamp handling with an implementation that ignores the delay.

The difference demonstrates why synchronization is part of navigation design rather than merely a software detail.

Why the comparison is useful: running the same scenario once with timestamp-correct processing and once with delay ignored separates timing architecture errors from ordinary sensor noise. This is an effective way to quantify how much estimation error comes from latency handling itself.

4.2.29.67 Testing Frame Errors

A deliberate small frame misalignment can be injected:

\[ \mathbf C_{S}^{B,true} \neq \mathbf C_{S}^{B,est}. \]

This can reveal whether residuals, bias states, or navigation errors respond as expected.

It also provides a useful test for sensor calibration sensitivity.

Interpretation: a small frame error can masquerade as sensor bias or state error because the measured vector remains numerically plausible. Injecting known misalignment is therefore a useful way to confirm that frame transformations, residuals, and calibration states are behaving physically.

4.2.29.68 Testing Initial Covariance

Several initial covariance cases should be evaluated:

\[ \mathbf P_0^{small}, \quad \mathbf P_0^{nominal}, \quad \mathbf P_0^{large}. \]

The purpose is to determine whether the estimator remains stable and whether its convergence behavior is consistent with its assumed initial confidence.

What the sweep demonstrates: changing \(\mathbf P_0\) should alter convergence behavior and early Kalman gains without changing the underlying truth. This makes the test useful for identifying overconfident initialization, excessive early correction, or covariance values that do not reflect the assumed initial-state uncertainty.

4.2.29.69 Testing Process Noise

Process noise represents uncertainty in the state evolution.

If

\[ \mathbf Q \]

is too small, the estimator may become overconfident in its model.

If it is too large, covariance may grow excessively and measurements may dominate.

Simulink allows systematic sweeps of these parameters while observing both estimation error and covariance consistency.

Interpretation: process noise controls how rapidly the estimator admits that its model may be wrong. The objective is not simply to obtain a small error trace, but to obtain an estimate whose actual error is statistically compatible with its reported covariance.

4.2.29.70 Testing Measurement Noise

Similarly, increasing

\[ \mathbf R \]

reduces the assumed confidence in the corresponding measurement.

For a Kalman filter,

\[ \mathbf R\uparrow \Rightarrow \mathbf K\downarrow \]

in the broad intuitive sense that noisier measurements generally receive less corrective influence.

This behavior can be demonstrated directly in simulation.

Interpretation: measurement noise controls how strongly the estimator trusts a measurement relative to its prediction. Sweeping \(\mathbf R\) should therefore change both the gain and the innovation statistics in a physically understandable direction.

4.2.29.71 Navigation Availability

The estimator should output not only a numerical state but also whether that state is operationally usable.

A navigation-valid signal may depend on sensor availability, convergence, covariance limits, estimator mode, residual behavior, and time since last valid update.

Conceptually,

\[ \boxed{ NavValid = f( \text{Estimator Health}, \mathbf P, \text{Sensor Status}, \text{Mode} ). } \]

Guidance and control can then respond appropriately to degraded navigation.

Operational meaning: a navigation solution can exist numerically while still being unsuitable for guidance or control. A separate validity signal allows downstream GNC logic to distinguish between an available estimate and an estimate that satisfies uncertainty, freshness, and health requirements.

4.2.29.72 Interface with Guidance

Guidance generally should not need to know whether the state came from an EKF, observer, or UKF.

It should receive a standardized navigation solution.

For example,

\[ \boxed{\text{Navigation}} \rightarrow \{ \hat{\mathbf r}, \hat{\mathbf v}, \hat{\mathbf q}, \hat{\boldsymbol\omega} \} \rightarrow \boxed{\text{Guidance}}. \]

This decouples navigation development from guidance development.

Design objective: guidance should consume a stable navigation interface rather than estimator-specific internals. This decoupling allows the navigation algorithm to evolve without forcing changes throughout the guidance layer.

4.2.29.73 Interface with Control

Similarly, the controller operates on estimated states and reference commands.

For attitude control,

\[ \boxed{ \mathbf e_q = \mathbf q_{ref} \otimes \hat{\mathbf q}^{-1} } \]

and

\[ \mathbf e_\omega = \boldsymbol{\omega}_{ref} - \hat{\boldsymbol{\omega}}. \]

For translation,

\[ \mathbf e_r = \mathbf r_{ref} - \hat{\mathbf r}, \]

\[ \mathbf e_v = \mathbf v_{ref} - \hat{\mathbf v}. \]

Navigation quality therefore directly affects control quality.

Closed-loop implication: because the controller acts on estimated rather than true states, navigation error directly becomes control error. This is why estimator verification must ultimately include closed-loop testing rather than stopping at estimator-only plots.

4.2.29.74 Sensor Fusion During General RPO

For a general rendezvous or proximity-operation scenario, the relative navigation state may be

\[ \boxed{ \mathbf x_{rel} = \begin{bmatrix} \mathbf r_{rel}\\ \mathbf v_{rel}\\ \mathbf q_{rel}\\ \boldsymbol{\omega}_{rel} \end{bmatrix}. } \]

Different measurements may become useful at different ranges.

The Simulink architecture should therefore permit measurements such as relative GNSS, bearing/vision information, range information, LiDAR-derived measurements, or relative pose to enter the same navigation framework according to availability and validity.

The key architectural idea is general:

\[ \boxed{ \text{Relative Dynamics} + \text{Available Relative Measurements} \rightarrow \text{Relative-State Estimate}. } \]

No particular mission-specific sensor sequence is required by this concept.

General RPO interpretation: relative-navigation sensors constrain different parts of the relative state with different accuracy and availability as geometry changes. A useful Simulink implementation therefore treats sensor availability and validity as dynamic information and fuses whichever measurements are appropriate without assuming a mission-specific sensor sequence.

4.2.29.75 Complete Simulink Execution Cycle

The entire estimator execution can now be summarized.

Step 1 — Propagate truth

\[ \mathbf x_{true,k} \rightarrow \mathbf x_{true,k+1}. \]

Step 2 — Generate sensor measurements

\[ \mathbf x_{true} \rightarrow \mathbf z_i. \]

Step 3 — Apply sensor effects

\[ \mathbf z_i \rightarrow \text{noise, bias, latency, dropout, quantization}. \]

Step 4 — Build measurement interfaces

\[ \boxed{ \mathbf z_i, t_i, valid_i, status_i }. \]

Step 5 — Predict estimator state

\[ \hat{\mathbf x}_{k}^{+} \rightarrow \hat{\mathbf x}_{k+1}^{-}. \]

Step 6 — Propagate uncertainty

\[ \mathbf P_k^{+} \rightarrow \mathbf P_{k+1}^{-}. \]

Step 7 — Check for new valid measurements

\[ new_i\land valid_i. \]

Step 8 — Predict corresponding measurement

\[ \hat{\mathbf z}_i = h_i(\hat{\mathbf x}^{-}). \]

Step 9 — Calculate innovation

\[ \mathbf r_i = \mathbf z_i-\hat{\mathbf z}_i. \]

Step 10 — Validate or gate innovation

\[ \mathbf r_i \rightarrow \text{Accept/Reject}. \]

Step 11 — Correct state and covariance

\[ \hat{\mathbf x}^{-},\mathbf P^{-} \rightarrow \hat{\mathbf x}^{+},\mathbf P^{+}. \]

Step 12 — Generate navigation output

\[ \boxed{ \hat{\mathbf x}^{+}, \mathbf P^{+}, NavValid }. \]

Step 13 — Send navigation solution to GNC

\[ \boxed{\text{Navigation}} \rightarrow \boxed{\text{Guidance}} \rightarrow \boxed{\text{Control}}. \]

Step 14 — Apply actuator response to truth dynamics

\[ \mathbf u_{cmd} \rightarrow \mathbf u_{actual} \rightarrow \text{Truth Dynamics}. \]

Step 15 — Log verification signals

\[ \mathbf x_{true}, \hat{\mathbf x}, \mathbf P, \mathbf r_i, \mathbf z_i, \mathbf u. \]

Then the cycle repeats.

4.2.29.76 Recommended Top-Level Simulink Organization

A clean top-level model can therefore contain the following major subsystems:

\[ \boxed{\text{1. Truth Dynamics}} \]

\[ \downarrow \]

\[ \boxed{\text{2. Sensor Models}} \]

\[ \downarrow \]

\[ \boxed{\text{3. Measurement Interface / Bus}} \]

\[ \downarrow \]

\[ \boxed{\text{4. Sensor Fusion / Navigation Estimator}} \]

\[ \downarrow \]

\[ \boxed{\text{5. Navigation Output Bus}} \]

\[ \downarrow \]

\[ \boxed{\text{6. Guidance}} \]

\[ \downarrow \]

\[ \boxed{\text{7. Control}} \]

\[ \downarrow \]

\[ \boxed{\text{8. Actuator Dynamics}} \]

with a parallel branch

\[ \boxed{\text{Logging and Verification}}. \]

Inside the estimator subsystem:

\[ \boxed{\text{Initialization}} \]

\[ \downarrow \]

\[ \boxed{\text{Prediction}} \]

\[ \downarrow \]

\[ \boxed{\text{Measurement Scheduling}} \]

\[ \downarrow \]

\[ \boxed{\text{Sensor-Specific Updates}} \]

\[ \downarrow \]

\[ \boxed{\text{State/Covariance Management}} \]

\[ \downarrow \]

\[ \boxed{\text{Navigation Health}}. \]

This organization keeps the estimator modular and allows each layer to be verified independently.

4.2.29.77 Common Simulink Implementation Mistakes

Several mistakes can produce a model that appears to work while not representing a valid sensor-fusion system.

The most important include:

A model should therefore be considered verified only after its information flow, timing, frames, units, estimator mathematics, and closed-loop behavior have all been checked.

4.2.29.78 Practical Development Sequence

A sensible development progression is:

\[ \boxed{\text{Verify Truth Dynamics}} \]

\[ \downarrow \]

\[ \boxed{\text{Verify Individual Sensor Models}} \]

\[ \downarrow \]

\[ \boxed{\text{Verify Estimator Prediction}} \]

\[ \downarrow \]

\[ \boxed{\text{Verify One Measurement Update}} \]

\[ \downarrow \]

\[ \boxed{\text{Add Multi-Rate Scheduling}} \]

\[ \downarrow \]

\[ \boxed{\text{Add Additional Sensors}} \]

\[ \downarrow \]

\[ \boxed{\text{Add Noise and Bias}} \]

\[ \downarrow \]

\[ \boxed{\text{Add Dropout, Delay, and Faults}} \]

\[ \downarrow \]

\[ \boxed{\text{Perform Closed-Loop GNC Testing}} \]

\[ \downarrow \]

\[ \boxed{\text{Perform Monte Carlo Verification}}. \]

This incremental approach is much safer than constructing a complete multi-sensor nonlinear closed-loop estimator at once and attempting to debug everything simultaneously.

4.2.29.79 Key Takeaways

A Simulink sensor-fusion model is not simply a graphical implementation of filter equations. It is an executable representation of the complete navigation information chain.

The most important architecture is

\[ \boxed{ \text{Truth Dynamics} \rightarrow \text{Sensor Models} \rightarrow \text{Measurements} \rightarrow \text{Estimator} \rightarrow \text{Navigation Solution} \rightarrow \text{GNC} }. \]

Truth and estimated states must remain strictly separated.

Sensor models should reproduce realistic measurement characteristics including noise, bias, update rate, latency, validity, dropout, frame transformations, and other relevant imperfections.

Prediction and measurement correction should be separated so that multi-rate and asynchronous sensors can be handled correctly.

Measurement buses should contain not only measurement values but also timestamps, validity and status information.

The estimator should expose both the estimated state and, where applicable, covariance and diagnostic quantities.

For covariance-based filters,

\[ \boxed{ \hat{\mathbf x} + \mathbf P + \text{Innovations} } \]

together provide much more information than the state estimate alone.

Logging should include truth, estimates, errors, measurements, predicted measurements, innovations, covariance, sensor-validity flags, update events, and GNC outputs.

Finally, the estimator should progress through increasingly realistic verification:

\[ \boxed{ \text{Deterministic} \rightarrow \text{Noisy} \rightarrow \text{Multi-Rate} \rightarrow \text{Faulted} \rightarrow \text{Closed Loop} \rightarrow \text{Monte Carlo} }. \]

This provides the natural transition to 4.2.30 Monte Carlo Verification of Sensor Fusion, where individual deterministic tests are expanded into statistical verification across sensor errors, initialization uncertainty, timing variations, model uncertainty, dropouts, estimator consistency, and mission-level performance.

Next: 4.2.30 Monte Carlo Verification of Sensor Fusion

The Simulink implementation has now established the complete executable navigation chain: truth dynamics, realistic sensor models, measurement interfaces, estimator prediction and correction, multi-rate scheduling, validity logic, logging, navigation outputs, closed-loop GNC interfaces, fault injection, and deterministic verification.

The next section expands these deterministic and case-by-case checks into statistical verification across many runs, where sensor errors, initialization uncertainty, timing variation, dynamic uncertainty, dropouts, and estimator consistency are assessed as distributions rather than as a single trajectory.

4.2.30 Monte Carlo Verification of Sensor Fusion

Statistical variation of sensor bias, noise, misalignment, initialization error, timing, dropouts, dynamic uncertainty, navigation errors, covariance consistency, success criteria, confidence intervals, NIS, NEES, and mission-level verification.

Continue to Sensor-Fusion Monte Carlo Verification →