Vision and LiDAR Fusion

4.2.24 Vision and LiDAR Fusion

Vision and LiDAR fusion combines the complementary strengths of passive optical sensing and active range sensing to estimate the relative position, orientation, geometry, and motion of a target.

A camera is very effective at measuring:

• angular direction,
• image features,
• edges and contours,
• target identity or geometry,
• relative orientation from visual structure.

LiDAR is particularly effective at measuring:

• direct range,
• depth,
• three-dimensional surface geometry,
• point clouds,
• target shape,
• range-dependent pose information.

The central idea is therefore

\[ \boxed{ \text{Vision Information} + \text{LiDAR Geometry} \rightarrow \text{Improved Relative Pose Estimation} } \]

Vision and LiDAR do not simply provide two estimates that are averaged together. Each sensor measures a different aspect of the physical geometry, and a fusion algorithm combines these measurements using the appropriate sensor models and uncertainty.

For general spacecraft relative-navigation applications, the fused state may contain

\[ \boxed{ \mathbf x = \begin{bmatrix} \mathbf r_{rel}\\ \mathbf v_{rel}\\ \mathbf q_{rel}\\ \boldsymbol\omega_{rel} \end{bmatrix} } \]

where

• \(\mathbf r_{rel}\) is relative position,
• \(\mathbf v_{rel}\) is relative velocity,
• \(\mathbf q_{rel}\) is relative attitude,
• \(\boldsymbol\omega_{rel}\) is relative angular velocity.

Additional states may represent:

• camera alignment,
• LiDAR alignment,
• camera intrinsics,
• range bias,
• timing offsets,
• target angular motion,
• calibration uncertainty.

4.2.24.1 Why Fuse Vision and LiDAR?

The motivation comes from their complementary weaknesses.

A camera can estimate target direction very accurately but may have weak instantaneous depth information.

LiDAR directly measures depth but may provide less appearance information than an image.

Therefore:

\[ \boxed{ \text{Camera} \rightarrow \text{Strong Angular / Visual Information} } \]

while

\[ \boxed{ \text{LiDAR} \rightarrow \text{Strong Range / Depth Information}. } \]

Together:

\[ \boxed{ \text{Angular Information} + \text{Depth Information} \rightarrow \text{Better 3-D Geometry}. } \]

This complementary information is one of the strongest reasons to combine the two sensors.

4.2.24.2 Measurement Complementarity

Suppose a camera provides a line-of-sight unit vector

\[ \mathbf u_{LOS} \]

and LiDAR provides range

\[ \rho. \]

Then a simple relative-position reconstruction is

\[ \boxed{ \mathbf r_{rel} = \rho\mathbf u_{LOS}. } \]

This equation illustrates the physical complementarity directly.

The camera provides direction.

The LiDAR provides scale.

Together, they define a three-dimensional relative vector.

4.2.24.3 Vision Measurement Model

A camera forms a perspective projection of a three-dimensional target point.

For a target point expressed in the camera frame as

\[ \mathbf p^C = \begin{bmatrix} X_C\\ Y_C\\ Z_C \end{bmatrix}, \]

the pinhole-camera model gives

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

and

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

Thus the image coordinates depend nonlinearly on the three-dimensional target geometry.

This nonlinearity is fundamental to vision-based relative navigation.

4.2.24.4 Camera Intrinsic Matrix

Camera intrinsic parameters may be represented by

\[ \boxed{ \mathbf K = \begin{bmatrix} f_x & 0 & c_x\\ 0 & f_y & c_y\\ 0 & 0 & 1 \end{bmatrix}. } \]

Here

• \(f_x,f_y\) represent focal scaling,
• \(c_x,c_y\) represent the principal point.

The projection relationship can be written conceptually as

\[ \boxed{ \mathbf p_{image} \sim \mathbf K \mathbf p^C. } \]

Errors in camera intrinsics can appear directly as navigation bias.

4.2.24.5 Lens Distortion

Real cameras do not follow the ideal pinhole model perfectly.

Distortion can include:

• radial distortion,
• tangential distortion,
• higher-order lens effects.

An undistorted normalized point

\[ (x,y) \]

may be related to distorted image coordinates through a nonlinear calibration model.

Therefore image measurements should generally be corrected using calibrated intrinsic parameters before being used for high-accuracy navigation.

4.2.24.6 Bearing Measurements

A camera can naturally provide bearing information.

Given a calibrated pixel coordinate, a camera-frame direction can be formed:

\[ \boxed{ \tilde{\mathbf u}^C = \mathbf K^{-1} \begin{bmatrix} u\\ v\\ 1 \end{bmatrix}. } \]

Normalize:

\[ \boxed{ \mathbf u^C = \frac{\tilde{\mathbf u}^C} {\|\tilde{\mathbf u}^C\|}. } \]

This produces the observed line-of-sight direction.

4.2.24.7 Bearing Does Not Directly Give Range

From a single monocular image, a bearing measurement constrains the target to a ray:

\[ \boxed{ \mathbf r_{rel} = \rho\mathbf u_{LOS}, \qquad \rho>0. } \]

The direction is measured.

The scale \(\rho\) remains unknown unless additional information is available.

This additional information may come from:

• LiDAR,
• known target size,
• stereo vision,
• multiple observations over time,
• dynamics.

4.2.24.8 LiDAR Range Measurement

A LiDAR determines distance using emitted light and measured return time.

For a simple time-of-flight system,

\[ \boxed{ \rho = \frac{c\Delta t}{2} } \]

where

• c is the speed of light,
• \(\Delta t\) is round-trip travel time.

The factor of two accounts for the outgoing and returning signal path.

4.2.24.9 LiDAR Measurement Model

A scalar LiDAR range can be represented as

\[ \boxed{ z_\rho = \rho + b_\rho + v_\rho } \]

where

• \(b_\rho\) is range bias,
• \(v_\rho\) is random measurement noise.

For a relative position vector,

\[ \rho = \|\mathbf r_{rel}\|. \]

Therefore:

\[ \boxed{ z_\rho = \|\mathbf r_{rel}\| + b_\rho + v_\rho. } \]

4.2.24.10 Why LiDAR Adds Scale Information

Camera bearing provides

\[ \mathbf u_{LOS}. \]

LiDAR provides

\[ \rho. \]

Thus:

\[ \boxed{ \mathbf r_{rel} = \rho\mathbf u_{LOS}. } \]

Without LiDAR, a monocular camera may know the target direction very accurately while still remaining uncertain about distance.

LiDAR directly reduces that ambiguity.

4.2.24.11 LiDAR Point Clouds

Many LiDAR systems provide more than one range measurement.

They generate a set of three-dimensional points:

\[ \boxed{ \mathcal P = \{ \mathbf p_1^L, \mathbf p_2^L, \ldots, \mathbf p_N^L \}. } \]

Each point describes a surface return in the LiDAR frame.

A point cloud therefore contains target shape information in addition to range.

4.2.24.12 Point-Cloud Geometry

A point cloud can provide:

• target dimensions,
• surface normals,
• corners,
• edges,
• planes,
• object orientation.

If a target geometry model is known, point-cloud alignment can estimate relative pose.

Conceptually,

\[ \boxed{ \text{Measured Point Cloud} + \text{Target Model} \rightarrow \text{Relative Pose}. } \]

4.2.24.13 Rigid Point-Cloud Registration

Suppose model points are

\[ \mathbf p_i^{M} \]

and measured LiDAR points are

\[ \mathbf p_i^{L}. \]

The registration problem seeks

\[ \mathbf R,\mathbf t \]

such that

\[ \boxed{ \mathbf p_i^{L} \approx \mathbf R \mathbf p_i^{M} + \mathbf t. } \]

Here

• \(\mathbf R\) estimates orientation,
• \(\mathbf t\) estimates translation.

4.2.24.14 Iterative Closest Point

A common registration approach is Iterative Closest Point (ICP).

Conceptually:

\[ \boxed{ \text{Match Points} \rightarrow \text{Estimate Transform} \rightarrow \text{Update Alignment} \rightarrow \text{Repeat}. } \]

ICP attempts to minimize a cost such as

\[ \boxed{ J = \sum_i \| \mathbf p_i^{meas} - ( \mathbf R\mathbf p_i^{model} + \mathbf t ) \|^2. } \]

The method works best when the initial pose estimate is already reasonably close.

4.2.24.15 ICP Limitations

ICP may perform poorly when:

• initialization is poor,
• only a small target region is visible,
• geometry is symmetric,
• point density is low,
• many outliers exist,
• the target contains repetitive surfaces.

Therefore a low ICP residual does not automatically imply a unique or accurate pose estimate.

4.2.24.16 Point-to-Plane Registration

Instead of minimizing point-to-point distance, registration can minimize distance to local surfaces.

A point-to-plane residual may be

\[ \boxed{ r_i = \mathbf n_i^T \left[ \mathbf p_i^{meas} - ( \mathbf R\mathbf p_i^{model} + \mathbf t ) \right]. } \]

where

\[ \mathbf n_i \]

is a surface normal.

This can improve convergence for smooth surfaces.

4.2.24.17 Camera Image Features

Camera-based navigation can use:

• corners,
• edges,
• line segments,
• blobs,
• fiducial markers,
• natural image features,
• silhouette contours.

Each feature provides geometric information about the target.

The most useful feature type depends on target appearance and range.

4.2.24.18 Feature Detection

The first step is detecting candidate image features:

\[ \boxed{ \text{Image} \rightarrow \text{Feature Detector} \rightarrow \{\mathbf p_i\}. } \]

Detection alone does not determine what physical target point each image feature represents.

That requires feature association.

4.2.24.19 Feature Association

Suppose an image feature

\[ \mathbf p_i \]

is assumed to correspond to target point

\[ \mathbf P_j. \]

An incorrect correspondence can generate a large pose-estimation error.

Therefore:

\[ \boxed{ \text{Feature Detection} \neq \text{Feature Association}. } \]

Robust association is essential for optical navigation.

4.2.24.20 Reprojection Error

Once a pose hypothesis exists, a known target feature can be projected into the image.

Let the measured pixel be

\[ \mathbf z_i \]

and the predicted pixel be

\[ \hat{\mathbf z}_i. \]

Then the reprojection residual is

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

A small reprojection error indicates consistency with the hypothesized pose.

4.2.24.21 Perspective-n-Point Pose Estimation

If several known three-dimensional target points are associated with their two-dimensional image positions, the relative pose can be estimated through a Perspective-n-Point problem.

Conceptually:

\[ \boxed{ \{ \mathbf P_i^{3D} \} + \{ \mathbf p_i^{2D} \} \rightarrow \mathbf R,\mathbf t. } \]

This produces both relative orientation and translation.

4.2.24.22 Why PnP Still Depends on Geometry

PnP performance depends strongly on:

• number of features,
• feature distribution,
• depth variation,
• image noise,
• target symmetry,
• camera calibration.

Features distributed widely over the target generally provide stronger pose constraints than features clustered in a small region.

4.2.24.23 Target Detection

Before pose estimation, the target must be detected.

A general processing chain is

\[ \boxed{ \text{Image} \rightarrow \text{Target Detection} \rightarrow \text{Feature Extraction} \rightarrow \text{Pose Estimation}. } \]

Target detection identifies where the object is in the image.

Pose estimation then determines its relative three-dimensional geometry.

4.2.24.24 Detection Versus Tracking

Detection asks:

Where is the target in the current image?

Tracking asks:

Given previous state information, where should the target appear now?

These are different problems.

Recursive navigation can make tracking easier because it predicts the target location.

4.2.24.25 Navigation-Aided Image Processing

Suppose the navigation filter predicts the target image position

\[ \hat{\mathbf p} \]

with covariance

\[ \mathbf P_p. \]

Image processing can search only within an uncertainty region around that prediction.

Therefore:

\[ \boxed{ \text{Navigation Prediction} \rightarrow \text{Reduced Image Search Region}. } \]

This reduces computational cost and false detections.

4.2.24.26 Camera Search Window

If the predicted image uncertainty is approximately Gaussian, a search ellipse may be defined using

\[ \boxed{ ( \mathbf p-\hat{\mathbf p} )^T \mathbf P_p^{-1} ( \mathbf p-\hat{\mathbf p} ) < \gamma. } \]

The threshold \(\gamma\) determines the search confidence region.

4.2.24.27 Optical Pose Estimation

Optical pose estimation seeks

\[ \boxed{ \hat{\mathbf r}_{rel}, \qquad \hat{\mathbf q}_{rel}. } \]

A high-level fusion architecture may treat this optical pose as one measurement.

Alternatively, individual image features can be fused directly.

4.2.24.28 High-Level Vision Fusion

A high-level architecture is

\[ \boxed{ \text{Image} \rightarrow \text{Pose Estimator} \rightarrow (\hat{\mathbf r}_{vision}, \hat{\mathbf q}_{vision}) \rightarrow \text{Fusion Filter}. } \]

Advantages include:

• simpler filter interfaces,
• modular vision software,
• lower estimator dimension.

But some raw image information is lost.

4.2.24.29 Low-Level Vision Fusion

In a lower-level approach,

\[ \boxed{ \text{Image Features} \rightarrow \text{Navigation Filter}. } \]

The filter directly predicts pixel coordinates and forms image residuals.

Advantages include:

• direct use of raw measurement geometry,
• potentially better covariance consistency,
• no need to compress all information into one pose estimate.

The cost is greater complexity.

4.2.24.30 High-Level LiDAR Fusion

LiDAR processing may independently produce:

\[ \hat{\mathbf r}_{LiDAR}, \qquad \hat{\mathbf q}_{LiDAR}. \]

These may then be used as pose measurements:

\[ \boxed{ \mathbf z_L = \begin{bmatrix} \hat{\mathbf r}_{LiDAR}\\ \hat{\boldsymbol\theta}_{LiDAR} \end{bmatrix}. } \]

This resembles high-level vision fusion.

4.2.24.31 Low-Level LiDAR Fusion

A lower-level estimator may instead use:

• raw ranges,
• point coordinates,
• plane constraints,
• surface normals.

This preserves more information but requires a more complicated measurement model.

4.2.24.32 Vision–LiDAR Fusion Levels

Three broad architectures are possible:

\[ \boxed{ \text{Raw Measurement Fusion} } \]

\[ \boxed{ \text{Feature-Level Fusion} } \]

\[ \boxed{ \text{Pose-Level Fusion}. } \]

The appropriate level depends on:

• computational budget,
• sensor interface,
• required accuracy,
• calibration quality,
• desired robustness.

4.2.24.33 Raw Measurement Fusion

Raw fusion combines measurements such as:

• camera pixels,
• LiDAR ranges,
• IMU measurements.

The filter directly models each measurement.

This provides maximum control over uncertainty modeling but can significantly increase computational load.

4.2.24.34 Feature-Level Fusion

Feature-level fusion combines intermediate geometric information.

Examples include:

• image corners,
• LiDAR edges,
• surface planes,
• object contours.

This can preserve meaningful structure while reducing raw data volume.

4.2.24.35 Pose-Level Fusion

Pose-level fusion combines independently generated poses:

\[ \mathbf T_{vision} \]

and

\[ \mathbf T_{LiDAR}. \]

This is simpler but requires reliable covariance for each pose estimate.

Poor pose covariance can produce incorrect fusion weighting.

4.2.24.36 Relative Pose Representation

A rigid-body pose can be represented by

\[ \boxed{ \mathbf T = \begin{bmatrix} \mathbf R & \mathbf t\\ \mathbf 0^T & 1 \end{bmatrix}. } \]

where

• \(\mathbf R\in SO(3)\) is orientation,
• \(\mathbf t\in\mathbb R^3\) is translation.

This homogeneous transformation is widely used in camera–LiDAR geometry.

4.2.24.37 Pose Composition

Suppose

\[ \mathbf T_{BC} \]

describes the camera with respect to the body frame and

\[ \mathbf T_{CL} \]

describes LiDAR relative to the camera.

Then transformations can be composed:

\[ \boxed{ \mathbf T_{BL} = \mathbf T_{BC} \mathbf T_{CL}. } \]

Transformation order matters.

4.2.24.38 Camera–LiDAR Extrinsic Calibration

Vision and LiDAR measurements cannot be fused correctly unless the rigid transformation between their frames is known.

Let

\[ \mathbf T_{CL} = \begin{bmatrix} \mathbf R_{CL} & \mathbf t_{CL}\\ 0 & 1 \end{bmatrix}. \]

This defines the LiDAR-to-camera transformation.

Calibration errors appear directly as systematic fusion errors.

4.2.24.39 Rotational Calibration Error

Suppose the true LiDAR-to-camera rotation is

\[ \mathbf R_{CL} \]

but the estimator uses

\[ \hat{\mathbf R}_{CL}. \]

A small calibration error can be represented by

\[ \delta\boldsymbol\alpha. \]

Then measured LiDAR directions are projected incorrectly into the camera frame.

This can create structured residuals across many measurements.

4.2.24.40 Translational Calibration Error

A translational offset error

\[ \delta\mathbf t_{CL} \]

causes range-dependent projection errors.

This becomes especially important at close range because the sensor baseline is no longer negligible compared with target distance.

4.2.24.41 Why Calibration Becomes More Important at Close Range

At long range:

\[ \rho \gg \|\mathbf t_{CL}\|. \]

The camera–LiDAR baseline may have relatively little effect.

At close range, however,

\[ \rho \sim \|\mathbf t_{CL}\| \]

can make lever-arm effects significant.

Thus:

\[ \boxed{ \text{Smaller Separation} \rightarrow \text{Greater Sensitivity to Extrinsic Calibration}. } \]

4.2.24.42 Temporal Calibration

Spatial calibration alone is not sufficient.

The camera and LiDAR measurements must also refer to compatible epochs.

Let camera timestamp be

\[ t_C \]

and LiDAR timestamp be

\[ t_L. \]

Then

\[ \Delta t = t_C-t_L. \]

If the target moves significantly during \(\Delta t\), spatial fusion becomes inconsistent.

4.2.24.43 Timing Error During Relative Motion

A first-order positional discrepancy caused by timing offset is approximately

\[ \boxed{ \delta\mathbf r \approx \mathbf v_{rel}\Delta t. } \]

For relative attitude,

\[ \boxed{ \delta\boldsymbol\theta \approx \boldsymbol\omega_{rel}\Delta t. } \]

Thus timing accuracy becomes increasingly important as relative translational or angular rates increase.

4.2.24.44 Rolling-Shutter Cameras

Some cameras do not expose the whole image simultaneously.

A rolling-shutter camera reads different image rows at slightly different times.

Then:

\[ \boxed{ \text{One Image} \neq \text{One Exact Instant}. } \]

During rapid relative motion or rotation, this can distort target geometry.

4.2.24.45 Global-Shutter Cameras

A global-shutter camera exposes the image approximately simultaneously.

This simplifies geometric interpretation because the whole image corresponds more closely to one epoch.

For high-dynamic relative navigation, this can significantly reduce motion-induced image distortion.

4.2.24.46 LiDAR Scan Timing

A scanning LiDAR also does not necessarily observe the whole target at one instant.

Different points can correspond to different timestamps.

Therefore the resulting point cloud may contain motion distortion.

4.2.24.47 Point-Cloud Motion Distortion

If the relative motion during a scan is significant,

\[ \boxed{ \mathbf p_i = \mathbf p_i(t_i) } \]

rather than all points being measured at one common time.

Ignoring this can distort the apparent target shape.

Motion compensation may therefore be necessary.

4.2.24.48 Motion Compensation

If relative velocity and angular rate are estimated, each measurement can be approximately transformed to a common reference time.

Conceptually:

\[ \boxed{ \text{Raw Time-Distributed Measurements} \rightarrow \text{Motion Compensation} \rightarrow \text{Common-Epoch Geometry}. } \]

This can improve camera–LiDAR registration.

4.2.24.49 Vision Measurement Covariance

Image-feature uncertainty can be represented by

\[ \boxed{ \mathbf R_{pixel} = \begin{bmatrix} \sigma_u^2 & \sigma_{uv}\\ \sigma_{uv} & \sigma_v^2 \end{bmatrix}. } \]

Pixel uncertainty depends on:

• image quality,
• feature detector,
• target size,
• blur,
• illumination.

It should not always be assumed constant.

4.2.24.50 LiDAR Measurement Covariance

LiDAR uncertainty may depend on:

• range,
• reflectivity,
• incidence angle,
• return signal strength,
• surface geometry.

Therefore:

\[ \boxed{ \mathbf R_L = \mathbf R_L( \rho, \text{incidence}, \text{reflectivity}, \text{quality} ). } \]

Dynamic covariance can better represent actual measurement quality.

4.2.24.51 Pose Covariance

A pose estimate should ideally include covariance.

A simple local pose-error vector may be

\[ \boxed{ \delta\mathbf x_{pose} = \begin{bmatrix} \delta\mathbf r\\ \delta\boldsymbol\theta \end{bmatrix}. } \]

Then pose covariance is

\[ \boxed{ \mathbf P_{pose} = E[ \delta\mathbf x_{pose} \delta\mathbf x_{pose}^T ]. } \]

4.2.24.52 Pose Covariance Is Usually Anisotropic

Pose accuracy is rarely identical in every direction.

For example:

• camera may strongly constrain cross-line-of-sight position,
• LiDAR may strongly constrain radial distance,
• planar geometry may weakly constrain one rotation.

Thus:

\[ \boxed{ \sigma_x \neq \sigma_y \neq \sigma_z. } \]

Full covariance should be preserved when possible.

4.2.24.53 Target Geometry and Observability

Sensor information depends on target shape.

A target with:

• corners,
• depth variation,
• asymmetric geometry,

provides stronger pose constraints than a featureless symmetric target.

Therefore:

\[ \boxed{ \text{Target Geometry} \rightarrow \text{Pose Observability}. } \]

4.2.24.54 Planar Targets

A nearly planar visible surface may provide weaker information about some pose directions.

For example, certain translations or rotations can produce similar measurements.

Therefore low point-cloud residual does not necessarily mean full 6-DOF pose is equally accurate.

4.2.24.55 Symmetric Targets

A symmetric target may produce multiple geometrically similar solutions.

For example, rotating the target by a symmetry angle may generate nearly identical measurements.

This can produce a multi-modal pose posterior.

4.2.24.56 Visual Ambiguity

A camera can experience ambiguity due to:

• repeated textures,
• repeated structural elements,
• symmetry,
• shadows,
• low resolution.

LiDAR geometry can help distinguish solutions that look visually similar.

This is another reason sensor fusion improves robustness.

4.2.24.57 LiDAR Ambiguity

LiDAR can also be ambiguous.

A flat or repetitive surface may provide insufficient structure to determine all pose components.

Vision may provide texture information that resolves this ambiguity.

Therefore:

\[ \boxed{ \text{Vision Ambiguity} \neq \text{LiDAR Ambiguity} } \]

and this difference is valuable.

4.2.24.58 Joint Geometric Constraint

If both camera and LiDAR observe the same target geometry, their residuals can be combined:

\[ \boxed{ J = J_{vision} + J_{LiDAR}. } \]

For example:

\[ J_{vision} = \sum_i \| \mathbf z_i-\hat{\mathbf z}_i \|^2 \]

and

\[ J_{LiDAR} = \sum_j \| \mathbf p_j-\hat{\mathbf p}_j \|^2. \]

A weighted form may incorporate covariance.

4.2.24.59 Weighted Joint Cost

A statistically motivated cost is

\[ \boxed{ J = \mathbf r_C^T \mathbf R_C^{-1} \mathbf r_C + \mathbf r_L^T \mathbf R_L^{-1} \mathbf r_L. } \]

This prevents one sensor from dominating merely because it produces more measurements.

4.2.24.60 Why Number of Measurements Is Not the Same as Information

A camera may produce hundreds of features.

A LiDAR may produce thousands of points.

But:

\[ \boxed{ \text{More Measurements} \neq \text{Proportionally More Independent Information}. } \]

Many measurements may be highly correlated or constrain the same state direction.

Geometry and covariance determine actual information content.

4.2.24.61 Information Matrix

Measurement information can be approximated by

\[ \boxed{ \mathbf \Lambda = \mathbf H^T \mathbf R^{-1} \mathbf H. } \]

This shows that information depends on both:

• measurement precision,
• measurement sensitivity.

A low-noise measurement that is insensitive to one state direction adds little information about that direction.

4.2.24.62 Vision–LiDAR EKF

A fused EKF may use

\[ \mathbf x = [ \mathbf r_{rel}, \mathbf v_{rel}, \delta\boldsymbol\theta_{rel}, \ldots ]^T. \]

Propagation:

\[ \boxed{ \hat{\mathbf x}_k^- = f( \hat{\mathbf x}_{k-1}^+ ) } \]

and

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

Vision and LiDAR measurements are then applied asynchronously.

4.2.24.63 Vision Measurement Update

For visual measurement

\[ \mathbf z_C, \]

predict

\[ \hat{\mathbf z}_C = h_C(\hat{\mathbf x}^-). \]

Innovation:

\[ \boxed{ \mathbf r_C = \mathbf z_C - \hat{\mathbf z}_C. } \]

Innovation covariance:

\[ \boxed{ \mathbf S_C = \mathbf H_C \mathbf P^- \mathbf H_C^T + \mathbf R_C. } \]

4.2.24.64 LiDAR Measurement Update

Similarly,

\[ \boxed{ \mathbf r_L = \mathbf z_L - h_L(\hat{\mathbf x}^-) } \]

and

\[ \boxed{ \mathbf S_L = \mathbf H_L \mathbf P^- \mathbf H_L^T + \mathbf R_L. } \]

Camera and LiDAR updates can be processed sequentially if they arrive at different times.

4.2.24.65 Sequential Fusion

Suppose a camera measurement arrives at

\[ t_C \]

and a LiDAR measurement arrives at

\[ t_L. \]

Then a natural sequence is

\[ \boxed{ t_C: \text{Vision Update} } \]

followed by

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

There is no requirement that the measurements be artificially synchronized into one update if their true timestamps differ.

4.2.24.66 Simultaneous Fusion

If camera and LiDAR measurements truly correspond to the same epoch, they may be stacked:

\[ \boxed{ \mathbf z = \begin{bmatrix} \mathbf z_C\\ \mathbf z_L \end{bmatrix}. } \]

Then

\[ \mathbf R = \begin{bmatrix} \mathbf R_C & \mathbf R_{CL}\\ \mathbf R_{LC} & \mathbf R_L \end{bmatrix}. \]

Cross-covariance terms should be included if sensor errors are correlated.

4.2.24.67 Camera–LiDAR Correlation

Vision and LiDAR may share error sources such as:

• body-frame alignment,
• timing errors,
• target geometry errors,
• common navigation-state uncertainty.

Treating such errors as independent can cause overconfidence.

4.2.24.68 Innovation Gating

For a measurement innovation

\[ \mathbf r \]

with covariance

\[ \mathbf S, \]

Normalized Innovation Squared is

\[ \boxed{ \mathrm{NIS} = \mathbf r^T \mathbf S^{-1} \mathbf r. } \]

This can be used to reject inconsistent visual or LiDAR measurements.

4.2.24.69 Vision-Specific Validation

Visual measurements may be validated using:

• reprojection error,
• feature count,
• feature distribution,
• detection confidence,
• optical-flow consistency,
• target-in-frame checks.

These sensor-specific checks should occur before or alongside estimator-level gating.

4.2.24.70 LiDAR-Specific Validation

LiDAR measurements may be validated using:

• return strength,
• point count,
• registration residual,
• geometric consistency,
• range limits,
• target-shape agreement.

A low-quality LiDAR point cloud should not be given the same covariance as a high-quality point cloud.

4.2.24.71 RANSAC for Vision

Feature association can contain outliers.

RANSAC-type methods repeatedly estimate geometry from subsets and identify measurements consistent with one model.

Conceptually:

\[ \boxed{ \text{Candidate Features} \rightarrow \text{Hypotheses} \rightarrow \text{Consensus Set}. } \]

Only the consistent subset should normally enter the pose solution.

4.2.24.72 Robust LiDAR Registration

Point clouds may also contain outliers.

Robust registration can use:

• trimmed ICP,
• robust loss functions,
• correspondence rejection,
• surface-normal consistency,
• range gating.

This reduces sensitivity to spurious points.

4.2.24.73 Robust Loss Functions

Instead of minimizing pure squared residuals,

\[ r^2, \]

a robust estimator may use a loss

\[ \rho(r) \]

that grows more slowly for large residuals.

Conceptually:

\[ \boxed{ \text{Large Residual} \rightarrow \text{Reduced Influence}. } \]

This can prevent a few bad measurements from dominating the pose solution.

4.2.24.74 Huber-Type Weighting

A Huber-style weighting behaves quadratically for small residuals and approximately linearly for large residuals.

This preserves sensitivity near the solution while reducing the influence of outliers.

Robust weighting may be applied at:

• feature level,
• point-cloud level,
• filter measurement level.

4.2.24.75 Occlusion

The target may be only partially visible.

Occlusion can arise from:

• spacecraft structure,
• target rotation,
• appendages,
• line-of-sight obstruction.

Partial visibility changes both image features and point-cloud geometry.

The estimator should not assume the entire target is always observed.

4.2.24.76 Partial Point Clouds

LiDAR may observe only one side of the target.

Then the visible geometry may become poorly conditioned for some rotations or translations.

Pose covariance should reflect this loss of information.

4.2.24.77 Partial Visual Features

Similarly, the camera may see only a small set of target features.

The pose estimator may still produce a solution, but its uncertainty can increase substantially.

Feature count alone is not sufficient; feature geometry also matters.

4.2.24.78 Field of View

A camera requires the target to remain within its optical field of view.

If

\[ |\alpha|>\alpha_{FOV} \]

or

\[ |\beta|>\beta_{FOV}, \]

the target may disappear from the image.

Thus:

\[ \boxed{ \text{Pointing} \rightarrow \text{Vision Availability}. } \]

4.2.24.79 LiDAR Field of Regard

LiDAR has similar geometric constraints.

Useful measurements may require

\[ \boxed{ \rho_{\min} < \rho < \rho_{\max} } \]

and target direction within scan limits.

A healthy LiDAR can therefore be unavailable simply because of geometry.

4.2.24.80 Sensor Availability Is State Dependent

Vision and LiDAR availability can depend on the current relative state.

Examples:

• target outside field of view,
• target too close,
• target too far,
• unfavorable orientation.

Thus sensor availability is not always an independent random event.

4.2.24.81 Illumination Effects on Vision

Passive cameras depend strongly on illumination.

Performance can degrade due to:

• eclipse,
• shadow,
• glare,
• Sun intrusion,
• Earth background,
• low contrast.

Image measurement covariance may therefore need to vary with image quality.

4.2.24.82 LiDAR and Illumination

LiDAR is active and therefore less dependent on natural illumination than a passive camera.

However, strong background light can still affect some optical ranging systems.

This makes LiDAR useful when visual contrast becomes poor.

4.2.24.83 Target Reflectivity

LiDAR return strength depends on target surface reflectivity.

Low-reflectivity surfaces may produce:

• fewer valid points,
• larger range noise,
• reduced maximum usable range.

Reflectivity can therefore influence measurement covariance.

4.2.24.84 Incidence Angle

LiDAR performance can degrade when the laser strikes a surface at a shallow angle.

A large incidence angle may reduce returned energy and increase ranging uncertainty.

Therefore:

\[ \boxed{ R_{LiDAR} = f( \rho, \text{reflectivity}, \text{incidence angle} ). } \]

4.2.24.85 Vision Resolution Versus Range

Suppose the target physical dimension is

\[ L. \]

Its approximate image size decreases with range:

\[ \boxed{ p \propto \frac{fL}{\rho}. } \]

At long range, the target occupies fewer pixels.

This reduces the ability to detect detailed features and estimate pose.

4.2.24.86 Angular Error to Position Error

Suppose camera angular uncertainty is

\[ \sigma_\theta. \]

Equivalent lateral uncertainty is approximately

\[ \boxed{ \sigma_{cross} \approx \rho\sigma_\theta. } \]

Thus the same angular accuracy produces larger translational uncertainty at longer range.

4.2.24.87 LiDAR Range Accuracy Versus Range

LiDAR accuracy may also vary with range.

Some systems maintain relatively good range precision across a useful range band, while others degrade strongly with signal strength.

Therefore sensor datasheet range accuracy should be translated into the actual estimator measurement covariance.

4.2.24.88 Sensor Dominance Changes With Range

At longer range:

\[ \boxed{ \text{Vision} \rightarrow \text{Strong Angular Information}. } \]

At shorter range:

\[ \boxed{ \text{LiDAR} \rightarrow \text{Strong Detailed Geometry}. } \]

The fused estimator can therefore change weighting smoothly with range instead of hard-switching between sensors.

4.2.24.89 Long-Range Vision Use

At long range, the target may appear as:

• a point source,
• a small blob,
• a low-detail silhouette.

The camera may provide mainly:

• bearing,
• target detection,
• coarse line of sight.

Detailed pose may not yet be observable.

4.2.24.90 Intermediate-Range Vision Use

At intermediate range, the target may reveal:

• contours,
• structural features,
• identifiable corners.

This can enable increasingly accurate position and orientation estimation.

4.2.24.91 Close-Range Vision Use

At close range, detailed image features can support:

• precise pose,
• target orientation,
• local structure identification.

However, close range also increases sensitivity to:

• camera lever arm,
• calibration,
• field-of-view limits,
• partial visibility.

4.2.24.92 Long-Range LiDAR Use

LiDAR may have limited point density or signal strength at long distance.

It may initially provide:

• sparse range,
• coarse target detection.

Useful pose information often increases as the target approaches.

4.2.24.93 Close-Range LiDAR Use

At shorter distances, LiDAR can produce denser and more accurate geometry.

This supports:

• point-cloud registration,
• surface reconstruction,
• accurate range,
• relative attitude estimation.

4.2.24.94 Close-Range Navigation

Vision–LiDAR fusion is especially valuable for general close-range relative navigation because the estimator may require accurate knowledge of:

\[ \boxed{ \mathbf r_{rel}, \qquad \mathbf v_{rel}, \qquad \mathbf q_{rel}, \qquad \boldsymbol\omega_{rel}. } \]

At these ranges, pose geometry often matters more than simple center-to-center range.

4.2.24.95 Relative Pose State

A relative pose state can be written as

\[ \boxed{ \mathbf x_{pose} = \begin{bmatrix} \mathbf r_{rel}\\ \mathbf q_{rel} \end{bmatrix}. } \]

For dynamic estimation:

\[ \boxed{ \mathbf x = \begin{bmatrix} \mathbf r_{rel}\\ \mathbf v_{rel}\\ \mathbf q_{rel}\\ \boldsymbol\omega_{rel} \end{bmatrix}. } \]

4.2.24.96 Pose Error State

A local error state may use

\[ \boxed{ \delta\mathbf x = \begin{bmatrix} \delta\mathbf r\\ \delta\mathbf v\\ \delta\boldsymbol\theta\\ \delta\boldsymbol\omega \end{bmatrix}. } \]

The attitude error is represented by a three-dimensional small-angle vector rather than direct quaternion subtraction.

4.2.24.97 Relative Quaternion Error

Define

\[ \boxed{ \delta\mathbf q = \mathbf q_{true} \otimes \hat{\mathbf q}^{-1}. } \]

For small errors:

\[ \boxed{ \delta\mathbf q \approx \begin{bmatrix} 1\\ \frac12\delta\boldsymbol\theta \end{bmatrix}. } \]

This is suitable for EKF-style local attitude updates.

4.2.24.98 Pose Innovation

Suppose a vision or LiDAR subsystem outputs relative pose.

Translation innovation is

\[ \boxed{ \mathbf r_p = \mathbf r_{meas} - \hat{\mathbf r}. } \]

Orientation innovation should be formed multiplicatively rather than through direct quaternion subtraction.

4.2.24.99 Orientation Innovation

For measured quaternion

\[ \mathbf q_m \]

and predicted quaternion

\[ \hat{\mathbf q}, \]

define

\[ \boxed{ \delta\mathbf q = \mathbf q_m \otimes \hat{\mathbf q}^{-1}. } \]

The vector part gives an approximate small-angle innovation:

\[ \boxed{ \mathbf r_\theta \approx 2\delta\mathbf q_v. } \]

4.2.24.100 Quaternion Sign Ambiguity

Quaternions satisfy

\[ \boxed{ \mathbf q \equiv -\mathbf q. } \]

A vision pose estimator may therefore return either sign.

Before forming quaternion residuals, the sign should be made consistent, for example by enforcing

\[ \boxed{ \mathbf q_m^T \hat{\mathbf q} \ge0. } \]

Otherwise a mathematically identical attitude could appear as a large error.

4.2.24.101 EKF Pose Update

For pose measurement

\[ \mathbf z \]

with predicted measurement

\[ h(\hat{\mathbf x}^-), \]

the innovation is

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

for Euclidean components, combined with appropriate multiplicative attitude residuals.

The update then follows the standard Kalman structure.

4.2.24.102 UKF for Vision–LiDAR Fusion

Vision projection is strongly nonlinear.

LiDAR registration may also produce nonlinear measurement relationships.

A UKF can therefore be useful when:

• pose uncertainty is large,
• camera projection nonlinearity is significant,
• relative geometry changes rapidly.

The UKF propagates sigma points through the nonlinear models instead of relying only on first-order Jacobians.

4.2.24.103 Particle Filters for Ambiguous Pose

If several pose hypotheses are plausible,

\[ p(\mathbf x|\mathbf z) \]

may be multi-modal.

Examples include:

• symmetric targets,
• uncertain feature correspondence,
• weakly observed depth.

A particle filter can represent multiple hypotheses simultaneously.

4.2.24.104 Pose Initialization

Vision–LiDAR tracking requires an initial pose estimate.

Possible sources include:

• coarse camera detection,
• LiDAR range,
• known orbital geometry,
• previous navigation state,
• GNSS-based relative position.

The initialization covariance should reflect actual uncertainty.

4.2.24.105 Initialization With Camera Bearing and LiDAR Range

If the camera provides

\[ \mathbf u_{LOS} \]

and LiDAR provides

\[ \rho, \]

initial relative position can be formed:

\[ \boxed{ \hat{\mathbf r}_0 = \rho \mathbf u_{LOS}. } \]

Its covariance should reflect both angular and range uncertainty.

4.2.24.106 Initial Covariance Geometry

If LiDAR range is accurate but camera angle is uncertain, uncertainty will be elongated mainly transverse to the line of sight.

If camera bearing is accurate but LiDAR range is uncertain, uncertainty will be elongated mainly radially.

Thus initial covariance should reproduce physical sensor geometry.

4.2.24.107 Observability of Relative Position

With only camera bearing, range may be poorly observable.

With only LiDAR range, direction may be poorly observable.

Together:

\[ \boxed{ \text{Bearing} + \text{Range} \rightarrow \text{Strong Instantaneous Position Observability}. } \]

This is the core observability advantage of vision–LiDAR fusion.

4.2.24.108 Observability of Relative Attitude

Relative attitude becomes observable through:

• visual feature geometry,
• LiDAR surface geometry,
• known target model.

The quality depends strongly on how asymmetric and geometrically rich the visible target region is.

4.2.24.109 Weak Attitude Geometry

Suppose LiDAR sees one flat surface.

Rotation about the surface normal may be weakly constrained.

Similarly, a visually symmetric target may provide poor yaw observability.

Thus:

\[ \boxed{ \text{Visible Geometry} \rightarrow \text{Relative-Attitude Observability}. } \]

4.2.24.110 Joint Observability

Vision and LiDAR may constrain different pose directions.

If camera information matrix is

\[ \mathbf\Lambda_C \]

and LiDAR information matrix is

\[ \mathbf\Lambda_L, \]

then approximately

\[ \boxed{ \mathbf\Lambda_{fused} = \mathbf\Lambda_C + \mathbf\Lambda_L. } \]

The fused geometry can therefore reduce weak directions present in either sensor individually.

4.2.24.111 Measurement Dropout

Either sensor may temporarily become unavailable.

Camera dropout can result from:

• poor illumination,
• target outside field of view,
• image processing failure.

LiDAR dropout can result from:

• range limits,
• poor reflectivity,
• scan geometry,
• hardware or processing failure.

The estimator should continue with remaining sensors where possible.

4.2.24.112 Camera Dropout

If camera measurements disappear while LiDAR remains valid, the estimator can continue relative-state estimation.

However, uncertainty may increase particularly in directions that were strongly constrained by visual information.

4.2.24.113 LiDAR Dropout

If LiDAR disappears while the camera remains valid, angular information may remain excellent.

But direct range or depth uncertainty may grow.

This illustrates why degraded covariance should depend on which sensor was lost.

4.2.24.114 Covariance During Dropout

A correct estimator should show

\[ \boxed{ \text{Loss of Measurement Information} \rightarrow \text{Covariance Growth}. } \]

The covariance may grow anisotropically according to the information lost.

4.2.24.115 Sensor Reacquisition

When a sensor returns, the estimator should not immediately trust it without validation.

Check:

• sensor health,
• target identity,
• geometry,
• innovation,
• covariance,
• timing.

A large residual after a long outage may indicate either genuine accumulated drift or a bad reacquired measurement.

4.2.24.116 Vision Reacquisition

After camera tracking loss, the system may use predicted relative state to estimate where the target should reappear.

The search region can be expanded according to propagated uncertainty.

This is more efficient than searching the full image blindly.

4.2.24.117 LiDAR Reacquisition

LiDAR reacquisition can similarly use predicted:

• range,
• bearing,
• target pose,

to focus processing on likely returns.

Estimator prediction can therefore aid LiDAR data association.

4.2.24.118 Cross-Sensor Validation

Camera and LiDAR can validate one another.

For example, a camera predicts target direction:

\[ \mathbf u_C. \]

LiDAR points transformed into the camera frame should lie near the corresponding image region.

Disagreement can indicate:

• calibration error,
• timing error,
• incorrect target association,
• sensor fault.

4.2.24.119 Projecting LiDAR Into the Image

A LiDAR point

\[ \mathbf p^L \]

can be transformed into the camera frame:

\[ \boxed{ \mathbf p^C = \mathbf R_{CL}\mathbf p^L + \mathbf t_{CL}. } \]

Then project:

\[ \boxed{ \begin{bmatrix} u\\ v\\ 1 \end{bmatrix} \sim \mathbf K \mathbf p^C. } \]

This allows direct image–point-cloud comparison.

4.2.24.120 Image–LiDAR Registration Error

After projection, one may compare:

• LiDAR projected edges,
• image edges,
• target contours.

A systematic offset between them can indicate poor extrinsic calibration.

Thus camera–LiDAR fusion itself can be used to monitor calibration quality.

4.2.24.121 Online Extrinsic Calibration

An estimator may augment calibration parameters:

\[ \boxed{ \delta\mathbf x = [ \delta\mathbf x_{nav}, \delta\boldsymbol\alpha_{CL}, \delta\mathbf t_{CL} ]^T. } \]

This allows online estimation of camera–LiDAR alignment.

However, these states are useful only if sufficiently observable.

4.2.24.122 Calibration Observability

If the target is viewed from nearly the same geometry continuously, calibration parameters may be poorly observable.

Changing viewing geometry can separate:

• target pose error,
• camera–LiDAR alignment error.

Thus manoeuvres may help calibration.

4.2.24.123 Too Many Calibration States

Adding:

• intrinsics,
• extrinsics,
• range biases,
• timing offsets,
• target-model errors

all at once can create a poorly conditioned estimator.

Therefore:

\[ \boxed{ \text{More Calibration States} \neq \text{Automatically Better Accuracy}. } \]

Only observable quantities should normally be estimated.

4.2.24.124 Adaptive Measurement Covariance

Sensor covariance may be adapted according to current quality.

For camera:

\[ \boxed{ \mathbf R_C = f( N_{features}, \text{reprojection error}, \text{blur}, \text{illumination} ). } \]

For LiDAR:

\[ \boxed{ \mathbf R_L = f( N_{points}, \text{registration error}, \rho, \text{geometry} ). } \]

This can make fusion more responsive to actual measurement quality.

4.2.24.125 Danger of Arbitrary Adaptive Covariance

Changing covariance without physical or statistical justification can make the estimator inconsistent.

Therefore adaptive covariance mappings should be calibrated from:

• simulation,
• test data,
• Monte Carlo results,
• empirical sensor characterization.

4.2.24.126 Vision Failure Modes

Important visual failure modes include:

• missed detection,
• false detection,
• feature mismatch,
• glare,
• blur,
• low contrast,
• shadow transition,
• target symmetry,
• incorrect camera calibration.

Each can produce a different residual pattern.

4.2.24.127 LiDAR Failure Modes

LiDAR failure modes include:

• missing returns,
• spurious returns,
• low reflectivity,
• saturation,
• poor incidence angle,
• background returns,
• scan distortion,
• alignment errors.

Robust fusion should distinguish transient bad data from persistent sensor faults.

4.2.24.128 Sensor-Level Health Flags

Useful outputs may include

\[ \boxed{ \texttt{CAM\_VALID} } \]

and

\[ \boxed{ \texttt{LIDAR\_VALID}. } \]

These should reflect more than whether hardware is powered.

They may depend on:

• measurement quality,
• geometry,
• consistency,
• recent innovation statistics.

4.2.24.129 Vision–LiDAR Fusion Validity

The fused solution may publish

\[ \boxed{ \texttt{POSE\_VALID} \in \{0,1\}. } \]

Validity may depend on:

• pose covariance,
• number of valid sensors,
• observability,
• innovation statistics,
• numerical health.

4.2.24.130 Degraded Pose Mode

If only one sensor remains available, the fused solution may continue in a degraded mode.

For example:

\[ \boxed{ \text{LiDAR Lost} \rightarrow \text{Vision-Only Pose With Larger Depth Uncertainty}. } \]

or

\[ \boxed{ \text{Camera Lost} \rightarrow \text{LiDAR-Only Pose With Geometry-Dependent Uncertainty}. } \]

4.2.24.131 Pose Accuracy Metrics

Translation error is

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

Scalar translation error:

\[ \boxed{ e_r = \|\delta\mathbf r\|. } \]

Relative orientation error can be computed using quaternion error.

4.2.24.132 Attitude Error Metric

Let

\[ \delta\mathbf q = \mathbf q_{true} \otimes \hat{\mathbf q}^{-1}. \]

Then

\[ \boxed{ e_\theta = 2\cos^{-1} (|\delta q_0|). } \]

Pose performance should generally report translation and attitude separately.

4.2.24.133 Reprojection Error Metric

For N image features,

\[ \boxed{ e_{reproj} = \sqrt{ \frac{1}{N} \sum_{i=1}^{N} \| \mathbf z_i-\hat{\mathbf z}_i \|^2 }. } \]

This measures image-space consistency.

It does not alone guarantee accurate three-dimensional pose.

4.2.24.134 Point-Cloud Registration Error

A representative registration RMSE is

\[ \boxed{ e_{pc} = \sqrt{ \frac{1}{N} \sum_{i=1}^{N} \| \mathbf p_i^{meas} - \hat{\mathbf p}_i \|^2 }. } \]

Again, low error does not automatically imply good observability in every pose direction.

4.2.24.135 Innovation Statistics

For camera innovation

\[ \mathbf r_C, \]

\[ \boxed{ \mathrm{NIS}_C = \mathbf r_C^T \mathbf S_C^{-1} \mathbf r_C. } \]

For LiDAR innovation:

\[ \boxed{ \mathrm{NIS}_L = \mathbf r_L^T \mathbf S_L^{-1} \mathbf r_L. } \]

These allow sensor-specific consistency monitoring.

4.2.24.136 Pose NEES

For pose truth error

\[ \delta\mathbf x_{pose} \]

and covariance

\[ \mathbf P_{pose}, \]

\[ \boxed{ \mathrm{NEES} = \delta\mathbf x_{pose}^T \mathbf P_{pose}^{-1} \delta\mathbf x_{pose}. } \]

This tests whether the estimator's uncertainty matches actual pose error.

4.2.24.137 Innovation Whiteness

A well-modeled estimator should not show strongly predictable innovation structure.

Persistent residual trends may indicate:

• calibration errors,
• timing errors,
• target-model errors,
• incorrect covariance,
• unmodeled motion.

Innovation autocorrelation is therefore an important diagnostic.

4.2.24.138 Monte Carlo Vision–LiDAR Testing

Monte Carlo testing should vary:

• initial relative pose,
• camera noise,
• LiDAR noise,
• camera alignment,
• LiDAR alignment,
• timing offsets,
• illumination,
• feature detection,
• point-cloud density,
• target geometry,
• target motion,
• sensor dropouts.

This establishes statistical robustness.

4.2.24.139 Monte Carlo Performance Measures

For run j,

\[ \delta\mathbf r^{(j)}(t) \]

and

\[ \delta\boldsymbol\theta^{(j)}(t) \]

can be recorded.

Then evaluate:

\[ \boxed{ \mathrm{RMSE}, \quad P_{95}, \quad P_{99}, \quad e_{\max}, \quad T_{\mathrm{recover}}. } \]

NIS and NEES statistics should also be examined.

4.2.24.140 Sensor-Dropout Monte Carlo Tests

Important cases include:

• camera loss,
• LiDAR loss,
• both sensors temporarily unavailable,
• sensor reacquisition.

Metrics should include:

• covariance growth,
• maximum pose error,
• continuity,
• recovery time.

4.2.24.141 Calibration Monte Carlo Tests

Randomize:

\[ \delta\boldsymbol\alpha_{CL} \]

and

\[ \delta\mathbf t_{CL}. \]

Evaluate how calibration uncertainty affects:

• relative position,
• relative attitude,
• innovation statistics,
• pose covariance.

This establishes alignment requirements.

4.2.24.142 Timing Monte Carlo Tests

Randomize camera and LiDAR timestamp offsets:

\[ \Delta t_C, \qquad \Delta t_L. \]

Then study pose error as relative rate changes.

This reveals the timing accuracy required for a given navigation performance.

4.2.24.143 Range-Dependent Testing

Fusion should be evaluated across multiple ranges.

For example:

\[ \boxed{ \rho_1 > \rho_2 > \rho_3. } \]

At each range assess:

• feature count,
• LiDAR point density,
• pose error,
• covariance,
• observability.

4.2.24.144 Geometry-Dependent Testing

Different target orientations should also be tested.

Some orientations may expose rich geometry.

Others may expose nearly planar or symmetric surfaces.

Thus:

\[ \boxed{ \text{Same Range} \neq \text{Same Pose Accuracy}. } \]

4.2.24.145 Illumination Testing

Vision testing should include:

• bright illumination,
• dim illumination,
• backlighting,
• shadow,
• glare.

LiDAR can provide important geometric continuity when passive vision degrades.

4.2.24.146 Motion Testing

Test:

• low angular rate,
• high angular rate,
• low relative velocity,
• high relative velocity.

High dynamics stress:

• timing,
• blur,
• LiDAR scan distortion,
• filter propagation.

4.2.24.147 Image Blur Testing

Introduce realistic exposure time

\[ T_{exp} \]

and target image rate

\[ \dot p. \]

Approximate blur magnitude:

\[ \boxed{ \Delta p \propto \dot pT_{exp}. } \]

Evaluate feature detection and pose degradation.

4.2.24.148 LiDAR Scan-Distortion Testing

Simulate target motion during the LiDAR scan.

Then compare:

• uncompensated point cloud,
• motion-compensated point cloud.

This determines whether scan timing must be modeled explicitly.

4.2.24.149 Target-Model Error Testing

Perturb:

• dimensions,
• feature locations,
• surface geometry,
• marker positions.

Then evaluate resulting pose bias.

Target-model uncertainty can become an important error source for model-based vision and LiDAR.

4.2.24.150 Model Error Versus Sensor Error

A systematic disagreement does not always mean a sensor is faulty.

It may originate from an incorrect target model.

Therefore:

\[ \boxed{ \text{Measurement Residual} = \text{Sensor Error} + \text{Model Error} + \text{State Error}. } \]

Diagnostics should consider all three.

4.2.24.151 Feature Distribution Metric

Feature quality can depend on both number and spatial distribution.

Many features clustered together may provide weaker pose information than fewer widely distributed features.

Therefore measurement-quality logic should consider geometry, not only feature count.

4.2.24.152 Point-Cloud Geometry Metric

Similarly, LiDAR point-cloud quality depends on:

• spatial extent,
• surface-normal diversity,
• depth variation.

A large number of points from one flat plane may still provide poor rotational observability.

4.2.24.153 Sensor Information Weighting

Fusion weighting should follow uncertainty.

Conceptually:

\[ \boxed{ \text{Higher Measurement Uncertainty} \rightarrow \text{Lower Information Weight}. } \]

The filter should not trust vision simply because a camera is normally precise, nor LiDAR simply because it gives range.

Current measurement quality matters.

4.2.24.154 Smooth Weight Transition

As separation changes, sensor confidence can vary continuously.

For example:

\[ \boxed{ \mathbf R_C=\mathbf R_C(\rho) } \]

and

\[ \boxed{ \mathbf R_L=\mathbf R_L(\rho). } \]

This naturally changes Kalman gain without a hard sensor switch.

4.2.24.155 Avoiding Double Counting

Suppose a visual pose estimate and the image features used to generate that pose are both fused.

They are not independent measurements.

Fusing both without accounting for correlation double-counts information.

Therefore:

\[ \boxed{ \text{Pose Derived From Features} + \text{Same Features} \neq \text{Independent Measurements}. } \]

4.2.24.156 Vision–LiDAR Sensor Handover

Vision–LiDAR fusion often exists inside a larger sensor suite.

At one range the camera may be dominant.

At another range LiDAR may become dominant.

The estimator should transition based on:

• validity,
• geometry,
• covariance,
• range,
• measurement quality.

This leads naturally to sensor-handover logic.

4.2.24.157 Overlap Region

An overlap region where both sensors perform well is valuable.

It allows:

\[ \boxed{ \text{Camera} + \text{LiDAR} \rightarrow \text{Cross-Validation}. } \]

This supports calibration, consistency checking, and smooth handover.

4.2.24.158 Handover Without State Discontinuity

Sensor handover should not create an artificial state jump.

The fused state should remain continuous while the information contribution from sensors changes.

This is better achieved through covariance-based weighting than blind replacement of one pose estimate with another.

4.2.24.159 Vision–LiDAR and IMU Integration

Vision and LiDAR can also be fused with inertial propagation.

A general architecture is

\[ \boxed{ \text{IMU} \rightarrow \text{High-Rate Propagation} } \]

with

\[ \boxed{ \text{Camera / LiDAR} \rightarrow \text{Relative-State Correction}. } \]

The IMU bridges periods between external measurements.

4.2.24.160 Why the IMU Helps During Optical Processing Delay

Image and point-cloud processing may require significant computation.

IMU propagation can maintain a high-rate estimate while slower vision and LiDAR measurements are being processed.

Delayed measurements should still be applied at their correct timestamps.

4.2.24.161 Vision–LiDAR–IMU State

A general state may contain

\[ \boxed{ \mathbf x = \begin{bmatrix} \mathbf r_{rel}\\ \mathbf v_{rel}\\ \mathbf q_{rel}\\ \boldsymbol\omega_{rel}\\ \mathbf b_a\\ \mathbf b_g \end{bmatrix}. } \]

Additional sensor-calibration states may be appended where necessary.

4.2.24.162 Multi-Rate Architecture

Typical processing rates may satisfy

\[ \boxed{ f_{IMU} > f_{camera} > f_{LiDAR} } \]

although actual systems vary.

This naturally produces an asynchronous estimator.

Each measurement should be processed at its actual timestamp.

4.2.24.163 Event-Driven Fusion

A suitable architecture is:

\[ \boxed{ \begin{array}{c} \text{Propagate State}\\ \downarrow\\ \text{Camera Measurement Arrives?}\\ \downarrow\\ \text{Vision Update}\\ \downarrow\\ \text{LiDAR Measurement Arrives?}\\ \downarrow\\ \text{LiDAR Update} \end{array} } \]

rather than forcing all sensors into one uniform measurement packet.

4.2.24.164 Vision–LiDAR Fusion and Observability

The combination is particularly powerful because camera and LiDAR sensitivity directions differ.

Camera:

\[ \boxed{ \text{Strong Angular Sensitivity}. } \]

LiDAR:

\[ \boxed{ \text{Strong Radial / Surface Geometry Sensitivity}. } \]

Their joint information can reduce poorly observed pose directions.

4.2.24.165 Navigation Integrity

A fused pose solution should communicate whether it is trustworthy.

Accuracy asks:

How close is the estimated pose to truth?

Integrity asks:

Can the system detect when that pose should not be trusted?

Thus:

\[ \boxed{ \text{Good Pose Navigation} = \text{Accuracy} + \text{Consistency} + \text{Fault Awareness}. } \]

4.2.24.166 Vision–LiDAR Failure Detection

Evidence of sensor problems can come from:

• repeated NIS failures,
• disagreement between sensors,
• sudden feature loss,
• abnormal point-cloud residual,
• impossible geometry.

Persistent inconsistencies may trigger sensor isolation.

4.2.24.167 Cross-Check Residual

If vision estimates position

\[ \hat{\mathbf r}_C \]

and LiDAR estimates

\[ \hat{\mathbf r}_L, \]

a simple cross-check is

\[ \boxed{ \mathbf d = \hat{\mathbf r}_C - \hat{\mathbf r}_L. } \]

But \(\mathbf d\) must be interpreted with the combined covariance.

4.2.24.168 Statistical Cross-Check

If

\[ \mathbf P_C \]

and

\[ \mathbf P_L \]

are independent pose-position covariances, then

\[ \boxed{ \mathbf S_d = \mathbf P_C+\mathbf P_L. } \]

A normalized discrepancy can be formed:

\[ \boxed{ D^2 = \mathbf d^T \mathbf S_d^{-1} \mathbf d. } \]

This is more meaningful than a raw Euclidean difference alone.

4.2.24.169 Sensor Isolation

If one sensor repeatedly disagrees with:

• the predicted state,
• another independent sensor,
• its own expected uncertainty,

it may be isolated.

A general logic is

\[ \boxed{ \text{Residual Failure} \rightarrow \text{Persistent Confirmation} \rightarrow \text{Sensor Isolation}. } \]

4.2.24.170 Graceful Degradation

After isolating one sensor, the estimator should continue with the remaining information when possible.

For example:

\[ \boxed{ \text{Camera Failure} \rightarrow \text{LiDAR-Dominant Navigation} } \]

or

\[ \boxed{ \text{LiDAR Failure} \rightarrow \text{Vision-Dominant Navigation}. } \]

The covariance should reflect reduced observability.

4.2.24.171 Numerical Health

The estimator should monitor:

\[ \boxed{ \mathbf P=\mathbf P^T } \]

and

\[ \boxed{ \lambda_i(\mathbf P)\ge0. } \]

For attitude:

\[ \boxed{ \|\mathbf q\|\approx1. } \]

Also monitor:

• NaN,
• Inf,
• failed matrix inversion,
• invalid pose transformations.

4.2.24.172 Computational Load

Vision and LiDAR can be computationally intensive.

Camera processing may include:

• feature extraction,
• descriptor matching,
• pose solving.

LiDAR processing may include:

• point filtering,
• correspondence search,
• registration.

The navigation architecture must account for processing latency as well as algorithm accuracy.

4.2.24.173 Processing Latency Versus Accuracy

A more complex algorithm may provide a more accurate pose but require longer processing time.

The useful question is therefore not only

\[ \boxed{ \text{How Accurate Is the Measurement?} } \]

but also

\[ \boxed{ \text{How Old Is the Measurement When It Becomes Available?} } \]

Latency can negate some benefits of high measurement accuracy if not handled correctly.

4.2.24.174 Hardware Acceleration

Vision and point-cloud processing may use:

• GPU,
• FPGA,
• dedicated vision processors,
• parallel CPU processing.

The estimator itself should remain aware of the measurement timestamp even if processing finishes later.

4.2.24.175 Software Interface

A sensor-processing module should ideally provide:

• measurement value,
• measurement covariance,
• measurement timestamp,
• validity,
• quality metrics.

For example:

\[ \boxed{ \{ \mathbf z, \mathbf R, t, \texttt{VALID}, \texttt{QUALITY} \}. } \]

This creates a clean interface between perception and estimation.

4.2.24.176 Flight-Software Processing Sequence

A representative sequence is:

1. propagate relative state,
2. propagate covariance,
3. receive camera frame,
4. detect target,
5. extract and associate features,
6. validate visual measurement,
7. receive LiDAR scan,
8. filter point cloud,
9. register target geometry,
10. validate LiDAR measurement,
11. transform all measurements into consistent frames,
12. compensate lever arms and timing,
13. form innovations,
14. apply statistical gating,
15. update fused state,
16. update covariance,
17. update sensor health,
18. publish relative pose and validity.

4.2.24.177 Published Fusion Output

The estimator may publish

\[ \boxed{ \hat{\mathbf r}_{rel}, \quad \hat{\mathbf v}_{rel}, \quad \hat{\mathbf q}_{rel}, \quad \hat{\boldsymbol\omega}_{rel}, \quad \mathbf P. } \]

Sensor-specific outputs may include

\[ \texttt{CAM\_VALID}, \qquad \texttt{LIDAR\_VALID}, \qquad \texttt{POSE\_VALID}. \]

4.2.24.178 Practical Design Philosophy

A robust Vision–LiDAR fusion system should:

1. calibrate camera intrinsics accurately,
2. calibrate camera–LiDAR extrinsics,
3. maintain frame consistency,
4. account for sensor lever arms,
5. use true measurement timestamps,
6. compensate scan or exposure timing where required,
7. validate camera features,
8. reject visual outliers,
9. validate point-cloud geometry,
10. reject LiDAR outliers,
11. represent sensor covariance realistically,
12. allow covariance to depend on geometry and quality,
13. account for target symmetry,
14. monitor observability,
15. support sensor dropout,
16. support reacquisition,
17. avoid double counting derived information,
18. maintain pose continuity,
19. monitor filter consistency,
20. verify performance statistically.

4.2.24.179 Key Takeaways

1. Vision and LiDAR measure complementary aspects of geometry.

\[ \boxed{ \text{Vision} \rightarrow \text{Bearing / Appearance / Features} } \]

\[ \boxed{ \text{LiDAR} \rightarrow \text{Range / Depth / Surface Geometry}. } \]

2. Their combination improves three-dimensional relative-state observability.

\[ \boxed{ \text{Bearing} + \text{Range} \rightarrow \text{Relative Position}. } \]

3. A monocular camera does not directly provide range from one unconstrained observation.

4. LiDAR directly contributes scale and depth.

5. Point clouds can support full relative-pose estimation.

6. Optical pose estimation depends strongly on feature geometry.

7. LiDAR pose estimation depends strongly on surface geometry.

8. Target symmetry can create ambiguous pose solutions.

9. Camera–LiDAR extrinsic calibration is fundamental.

\[ \boxed{ \mathbf T_{CL} = \begin{bmatrix} \mathbf R_{CL} & \mathbf t_{CL}\\ 0 & 1 \end{bmatrix}. } \]

10. Spatial calibration alone is insufficient; timing must also be calibrated.

\[ \boxed{ \delta\mathbf r \approx \mathbf v_{rel}\Delta t. } \]

11. Rolling-shutter cameras and scanning LiDAR can contain intra-frame timing effects.

12. Vision and LiDAR measurements should be fused using their measurement models, not simply averaged.

13. Raw, feature-level, and pose-level fusion are all possible.

14. Pose-level fusion is simpler but depends heavily on credible pose covariance.

15. Sensor uncertainty is generally anisotropic.

16. Measurement quality changes with range and geometry.

\[ \boxed{ \mathbf R = \mathbf R( \rho, \text{geometry}, \text{quality} ). } \]

17. Vision quality depends strongly on illumination, resolution, blur, and target appearance.

18. LiDAR quality depends on range, reflectivity, incidence angle, and visible surface geometry.

19. Sensor weighting should change with actual uncertainty rather than fixed preference.

20. Vision–LiDAR overlap provides useful cross-validation.

21. Losing one sensor does not necessarily eliminate all relative-navigation capability.

22. Covariance should grow in directions that become weakly observed during sensor dropout.

23. Vision and LiDAR reacquisition should be validated statistically.

24. Camera and LiDAR outputs may share common errors and therefore may not be independent.

25. NIS can monitor measurement consistency.

\[ \boxed{ \mathrm{NIS} = \mathbf r^T \mathbf S^{-1} \mathbf r. } \]

26. NEES can monitor state-covariance consistency when truth is available.

\[ \boxed{ \mathrm{NEES} = \delta\mathbf x^T \mathbf P^{-1} \delta\mathbf x. } \]

27. Monte Carlo validation should include alignment, timing, noise, geometry, illumination, target motion, dropouts, and calibration uncertainty.

28. Processing latency is part of sensor performance.

29. A clean measurement interface should provide measurement, covariance, timestamp, validity, and quality.

30. The central engineering principle is

\[ \boxed{ \begin{array}{c} \text{Vision constrains how the target appears and in which direction it lies,}\\[4pt] \text{while LiDAR constrains distance and three-dimensional surface geometry.}\\[4pt] \text{Accurate calibration, timing, uncertainty modelling, and measurement validation}\\[4pt] \text{allow these complementary measurements to form a robust relative-pose estimate.} \end{array} } \]

The next section is naturally 4.2.25 Sensor Handover and Mode-Dependent Fusion, where the focus shifts from fusing vision and LiDAR simultaneously to deciding which sensors should dominate in far-range, mid-range, and close-range navigation, how overlapping sensors should be validated, and how the estimator should transition between measurement sources without producing artificial state or covariance discontinuities.

Next: 4.2.25 Sensor Handover and Mode-Dependent Fusion

After developing Vision and LiDAR fusion, including camera bearing measurements, optical pose estimation, LiDAR ranging and point-cloud geometry, calibration, timing, uncertainty, observability, validation, dropout handling, and multi-rate fusion, the next section focuses on how a navigation system transitions between sensor combinations as operating conditions and measurement quality change.