Outdoor AMRs experience environment-induced sensing domain shift: the sensor model calibrated in clear daytime conditions becomes mismatched under fog, rain, dusk, night, backlighting, and mixed illumination. In Chapter 6 (Bayes filters), Chapter 7 (EKF/UKF), Chapter 8 (MCL), and Chapter 9 (mapping), we assumed a reasonably stable measurement likelihood. In the field, the likelihood itself becomes weather- and lighting-dependent.

Let the robot state be \( \mathbf{x}_k \) and a generic measurement be \( \mathbf{z}_k \). The standard model is

\[ \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k) + \mathbf{w}_k, \qquad \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k. \]

Under adverse weather/lighting, the noise is not stationary. A more realistic model is \( \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k(\eta_k)) \), where \( \eta_k \) is a latent environmental condition vector (fog density, rain rate, lux level, glare score, etc.). Thus perception robustness is fundamentally a problem of online sensor-model adaptation.

flowchart TD
  A["Weather and lighting state"] --> B["Sensor signal distortion"]
  B --> C["Feature degradation and spurious returns"]
  C --> D["Measurement likelihood mismatch"]
  D --> E["Localization and mapping uncertainty grows"]
  E --> F["Navigation risk and conservative behavior"]
  F --> G["Adaptive mitigation: preprocessing + confidence-aware fusion"]
        

We focus on camera and LiDAR because they dominate outdoor AMR perception. Radar is generally more robust to fog and low light, but has lower angular resolution; in practice, weather-aware fusion exploits this complementary failure behavior.

2.1 Camera Radiometric Model with Low-Light Noise

A simplified camera measurement at pixel \( \mathbf{u} \) can be written as

\[ I(\mathbf{u}) = \operatorname{clip}\!\left( g\big( e\,L(\mathbf{u}) + n_{shot}(\mathbf{u}) + n_{read}(\mathbf{u}) \big) \right), \]

where \( e \) is exposure gain, \( L \) is irradiance, and \( g(\cdot) \) is the camera response. A standard approximation is

\[ n_{shot} \sim \mathcal{N}(0,\sigma_{shot}^2), \quad \sigma_{shot}^2 \propto eL, \quad n_{read} \sim \mathcal{N}(0,\sigma_{read}^2). \]

Therefore, the signal-to-noise ratio (SNR) in low light approximately behaves as

\[ \operatorname{SNR} \approx \frac{eL}{\sqrt{\kappa eL + \sigma_{read}^2} }. \]

2.2 Fog / Haze Atmospheric Scattering Model

For visible cameras, fog/haze is often modeled by the atmospheric scattering equation:

\[ I(\mathbf{u}) = J(\mathbf{u})\,t(\mathbf{u}) + A\big(1-t(\mathbf{u})\big), \qquad t(\mathbf{u}) = \exp\big(-\beta d(\mathbf{u})\big). \]

A contrast law follows: \( C(d)=C_0\exp(-\beta d) \). For threshold \( C_{min} \), the maximum visible distance is

\[ d_{max} = \frac{1}{\beta}\ln\!\left(\frac{C_0}{C_{min} }\right). \]

2.3 Rain/Snow and LiDAR Attenuation Models

A simplified camera degradation model under precipitation and blur is

\[ I_{obs} = (I_{clear} * h_{psf}) + I_{streak} + n. \]

For LiDAR, a simplified received power model at range \( r \) is

\[ P_r(r) \propto \frac{\rho}{r^2}\exp(-2\alpha r), \]

with weather-dependent attenuation coefficient \( \alpha \). Fog, rain, and snow also create backscatter clutter and unstable returns, so range covariance is condition-dependent.

3.1 Confidence-Conditioned Measurement Covariance

Let \( q_k \in (0,1] \) denote a sensor confidence score computed from signal quality metrics (contrast, blur, LiDAR clutter ratio, etc.). A practical covariance scheduler is

\[ \mathbf{R}_k = \frac{1}{q_k}\mathbf{R}_0, \qquad 0 < q_k \le 1. \]

3.2 EKF Update with Weather-Aware Covariance

\[ \mathbf{S}_k = \mathbf{H}_k \mathbf{P}^-_k \mathbf{H}_k^\top + \mathbf{R}_k, \qquad \mathbf{K}_k = \mathbf{P}^-_k \mathbf{H}_k^\top \mathbf{S}_k^{-1}, \]

\[ \hat{\mathbf{x} }^+_k = \hat{\mathbf{x} }^-_k + \mathbf{K}_k \big( \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x} }^-_k \big), \]

\[ \mathbf{P}^+_k = (\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)\mathbf{P}^-_k(\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)^\top + \mathbf{K}_k\mathbf{R}_k\mathbf{K}_k^\top. \]

3.3 Proposition and Proof: Covariance Inflation is Conservative

Proposition: If the predicted information matrix and observation matrix are fixed, and \( \mathbf{R}(q)=\mathbf{R}_0/q \) with \( 0 < q_1 \le q_2 \le 1 \), then \( \mathbf{P}^+(q_1) \succeq \mathbf{P}^+(q_2) \).

Proof: In information form,

\[ \mathbf{Y}^+(q)=\mathbf{Y}^- + \mathbf{H}^\top \mathbf{R}(q)^{-1}\mathbf{H} = \mathbf{Y}^- + q\,\mathbf{H}^\top \mathbf{R}_0^{-1}\mathbf{H}. \]

Since \( q_1 \le q_2 \) and \( \mathbf{H}^\top \mathbf{R}_0^{-1}\mathbf{H} \succeq 0 \), we have \( \mathbf{Y}^+(q_1) \preceq \mathbf{Y}^+(q_2) \). Inversion reverses the order on positive-definite matrices, so

\[ \mathbf{P}^+(q_1)=\big(\mathbf{Y}^+(q_1)\big)^{-1} \succeq \big(\mathbf{Y}^+(q_2)\big)^{-1} = \mathbf{P}^+(q_2). \]

Hence confidence reduction (bad weather / bad lighting) yields a more conservative posterior covariance.

3.4 NIS Gating

\[ \epsilon_k = \boldsymbol{\nu}_k^\top \mathbf{S}_k^{-1}\boldsymbol{\nu}_k, \qquad \boldsymbol{\nu}_k = \mathbf{z}_k - h(\hat{\mathbf{x} }^-_k). \]

If \( \epsilon_k > \gamma \) (chi-square threshold), reject or further down-weight the measurement. This is especially effective for burst outliers caused by raindrops, snowflakes, or glare.

A robust outdoor AMR stack should explicitly convert signal quality into estimator uncertainty. The architecture below is compatible with EKF, UKF, and particle-filter pipelines already introduced in earlier chapters.

flowchart TD
  A["Raw sensors: RGB, LiDAR, IMU, GPS"] --> B["Signal conditioning"]
  B --> C["Quality estimators per sensor"]
  C --> D["Confidence scores q_cam, q_lidar, q_gps"]
  D --> E["Measurement extraction"]
  E --> F["Covariance scheduler R_k = R0 / q_k"]
  F --> G["Filter update + NIS gating"]
  G --> H["Pose and map with uncertainty"]
  H --> I["Navigation speed and safety margins adapted"]
        

Typical quality indicators include image gradient energy, blur metrics, saturation fraction, LiDAR near-range clutter ratio, return intensity dispersion, and GNSS quality flags. The key requirement is calibration: each metric must be mapped to a covariance multiplier using controlled experiments.

The Python script demonstrates a synthetic fog + low-light camera model and a confidence-aware EKF for camera/LiDAR fusion.

Code file: Chapter18_Lesson4.py

# Chapter18_Lesson4.py
# Weather/Lighting Effects on Perception (Outdoor and Field AMR)
# This script demonstrates:
# 1) A simple fog + low-light camera degradation model
# 2) Quality metrics (contrast / gradient energy proxy)
# 3) Confidence-aware EKF measurement covariance inflation for camera/LiDAR updates
#
# Related robotics stacks for deployment:
# - ROS 2 (sensor_msgs, image_transport, nav_msgs)
# - OpenCV (camera preprocessing), PCL (point-cloud filtering)
# - robot_localization / custom EKF nodes

import numpy as np
import matplotlib.pyplot as plt

rng = np.random.default_rng(7)

# ------------------------------------------------------------
# Part A: Synthetic radiometric degradation (1-D signal proxy)
# ------------------------------------------------------------
n = 400
x = np.linspace(0.0, 1.0, n)

# "Clear" reflectance-like profile with edges and texture
J = 0.25 + 0.35 * (x > 0.2) + 0.15 * np.sin(14 * np.pi * x)
J += 0.20 * (x > 0.58)
J = np.clip(J, 0.0, 1.0)

# Depth profile (farther on the right side)
d = 5.0 + 30.0 * x  # meters
beta = 0.07         # fog extinction coefficient [1/m]
A = 0.85            # atmospheric light

t = np.exp(-beta * d)
I_fog = J * t + A * (1.0 - t)

# Low-light + shot/read noise proxy
exposure_scale = 0.28
read_sigma = 0.015
shot_sigma = np.sqrt(np.maximum(I_fog * exposure_scale, 1e-6)) * 0.05
I_low = exposure_scale * I_fog + rng.normal(0.0, shot_sigma + read_sigma, size=n)
I_low = np.clip(I_low, 0.0, 1.0)

# Simple "illumination compensation" and contrast normalization
gamma = 0.6
I_enh = np.power(np.clip(I_low, 0.0, 1.0), gamma)
I_enh = (I_enh - I_enh.min()) / (I_enh.max() - I_enh.min() + 1e-12)

def michelson_contrast(sig):
    smax = float(np.max(sig))
    smin = float(np.min(sig))
    return (smax - smin) / (smax + smin + 1e-12)

def gradient_energy(sig):
    g = np.diff(sig)
    return float(np.mean(g * g))

print("=== Signal Quality Metrics ===")
print(f"Michelson contrast (clear): {michelson_contrast(J):.4f}")
print(f"Michelson contrast (fog+low-light): {michelson_contrast(I_low):.4f}")
print(f"Michelson contrast (enhanced): {michelson_contrast(I_enh):.4f}")
print(f"Gradient energy (clear): {gradient_energy(J):.6f}")
print(f"Gradient energy (fog+low-light): {gradient_energy(I_low):.6f}")
print(f"Gradient energy (enhanced): {gradient_energy(I_enh):.6f}")

# ------------------------------------------------------------
# Part B: Confidence-aware EKF (2-state constant velocity model)
# ------------------------------------------------------------
dt = 0.1
T = 250
F = np.array([[1.0, dt],
              [0.0, 1.0]])
Q = np.array([[1e-4, 0.0],
              [0.0, 3e-3]])

H_cam = np.array([[1.0, 0.0]])
H_lidar = np.array([[1.0, 0.0]])

R_cam0 = np.array([[0.6**2]])
R_lidar0 = np.array([[0.25**2]])

def weather_profile(k):
    fog = 0.10 + 0.35 * np.exp(-0.5 * ((k - 120) / 22.0)**2)
    lux = 1.0 if k < 140 else 0.25
    rain = 0.05 + 0.25 * np.exp(-0.5 * ((k - 180) / 14.0)**2)
    return fog, lux, rain

def camera_confidence(fog, lux, rain):
    q = (lux**0.7) * np.exp(-2.2 * fog) * np.exp(-1.4 * rain)
    return float(np.clip(q, 0.05, 1.0))

def lidar_confidence(fog, rain):
    q = np.exp(-1.3 * fog) * np.exp(-0.6 * rain)
    return float(np.clip(q, 0.10, 1.0))

def ekf_predict(xhat, P):
    xhat = F @ xhat
    P = F @ P @ F.T + Q
    return xhat, P

def ekf_update(xhat, P, z, H, R):
    y = np.array([[z]]) - H @ xhat
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    xhat = xhat + K @ y
    I2 = np.eye(2)
    P = (I2 - K @ H) @ P @ (I2 - K @ H).T + K @ R @ K.T
    nis = float(y.T @ np.linalg.inv(S) @ y)
    return xhat, P, nis

x_true = np.zeros((2, T))
x_true[:, 0] = np.array([0.0, 0.35])
for k in range(1, T):
    w = rng.multivariate_normal(np.zeros(2), Q)
    x_true[:, k] = F @ x_true[:, k - 1] + w

x_conf = np.array([[0.0], [0.0]])
P_conf = np.diag([1.0, 0.5])

x_fixed = np.array([[0.0], [0.0]])
P_fixed = np.diag([1.0, 0.5])

est_conf = []
est_fixed = []
nis_cam_hist = []
nis_lidar_hist = []

for k in range(T):
    fog, lux, rain = weather_profile(k)
    qc = camera_confidence(fog, lux, rain)
    ql = lidar_confidence(fog, rain)

    cam_sigma = float(np.sqrt(R_cam0[0, 0] / qc))
    lidar_sigma = float(np.sqrt(R_lidar0[0, 0] / ql))
    z_cam = x_true[0, k] + rng.normal(0.0, cam_sigma)
    z_lidar = x_true[0, k] + rng.normal(0.0, lidar_sigma)

    x_conf, P_conf = ekf_predict(x_conf, P_conf)
    x_fixed, P_fixed = ekf_predict(x_fixed, P_fixed)

    x_conf, P_conf, nis_cam = ekf_update(x_conf, P_conf, z_cam, H_cam, R_cam0 / qc)
    x_conf, P_conf, nis_lidar = ekf_update(x_conf, P_conf, z_lidar, H_lidar, R_lidar0 / ql)

    x_fixed, P_fixed, _ = ekf_update(x_fixed, P_fixed, z_cam, H_cam, R_cam0)
    x_fixed, P_fixed, _ = ekf_update(x_fixed, P_fixed, z_lidar, H_lidar, R_lidar0)

    est_conf.append(x_conf[:, 0].copy())
    est_fixed.append(x_fixed[:, 0].copy())
    nis_cam_hist.append(nis_cam)
    nis_lidar_hist.append(nis_lidar)

est_conf = np.array(est_conf)
est_fixed = np.array(est_fixed)

pos_rmse_conf = float(np.sqrt(np.mean((est_conf[:, 0] - x_true[0, :])**2)))
pos_rmse_fixed = float(np.sqrt(np.mean((est_fixed[:, 0] - x_true[0, :])**2)))

print("\n=== EKF Localization Error ===")
print(f"Position RMSE (confidence-aware): {pos_rmse_conf:.4f}")
print(f"Position RMSE (fixed covariance):  {pos_rmse_fixed:.4f}")
print(f"Mean camera NIS: {np.mean(nis_cam_hist):.3f}")
print(f"Mean LiDAR NIS:  {np.mean(nis_lidar_hist):.3f}")

k_axis = np.arange(T)

plt.figure(figsize=(10, 4))
plt.plot(x, J, label="Clear signal")
plt.plot(x, I_low, label="Fog + low-light + noise")
plt.plot(x, I_enh, label="Enhanced")
plt.xlabel("Normalized pixel coordinate")
plt.ylabel("Intensity (normalized)")
plt.title("Synthetic camera degradation and enhancement proxy")
plt.legend()
plt.tight_layout()
plt.show()

plt.figure(figsize=(10, 4))
plt.plot(k_axis, x_true[0, :], label="True position")
plt.plot(k_axis, est_conf[:, 0], label="EKF (confidence-aware)")
plt.plot(k_axis, est_fixed[:, 0], label="EKF (fixed R)")
plt.xlabel("Time step")
plt.ylabel("Position")
plt.title("Weather-aware covariance adaptation improves robustness")
plt.legend()
plt.tight_layout()
plt.show()
      

The C++ code provides a compact scalar fusion example. In a full AMR implementation, pair this with ROS2, OpenCV, and PCL modules.

Code file: Chapter18_Lesson4.cpp

// Chapter18_Lesson4.cpp
// Weather/Lighting Effects on Perception (Outdoor and Field AMR)
// A compact confidence-aware fusion example (scalar position) in C++.

#include <iostream>
#include <vector>
#include <cmath>
#include <iomanip>
#include <algorithm>
#include <random>

struct ScalarKalman {
    double x{0.0};
    double P{1.0};
    double Q{0.01};

    void predict() { P += Q; }

    double update(double z, double R) {
        const double S = P + R;
        const double K = P / S;
        const double innov = z - x;
        x = x + K * innov;
        P = (1.0 - K) * P;
        return (innov * innov) / S;
    }
};

double cameraConfidence(double fog, double lux, double rain) {
    double q = std::pow(lux, 0.7) * std::exp(-2.2 * fog) * std::exp(-1.4 * rain);
    return std::max(0.05, std::min(1.0, q));
}

double lidarConfidence(double fog, double rain) {
    double q = std::exp(-1.3 * fog) * std::exp(-0.6 * rain);
    return std::max(0.10, std::min(1.0, q));
}

int main() {
    std::mt19937 gen(42);
    std::normal_distribution<double> wproc(0.0, 0.06);

    const int T = 220;
    const double Rcam0 = 0.60 * 0.60;
    const double Rlidar0 = 0.25 * 0.25;

    std::vector<double> xTrue(T, 0.0);
    for (int k = 1; k < T; ++k) {
        xTrue[k] = xTrue[k - 1] + 0.10 + wproc(gen);
    }

    ScalarKalman kfAdaptive;
    ScalarKalman kfFixed;
    kfAdaptive.Q = 0.02;
    kfFixed.Q = 0.02;

    double sqErrAdaptive = 0.0;
    double sqErrFixed = 0.0;
    double nisCamMean = 0.0;
    double nisLidarMean = 0.0;

    for (int k = 0; k < T; ++k) {
        const double fog = 0.08 + 0.35 * std::exp(-0.5 * std::pow((k - 100) / 20.0, 2.0));
        const double rain = 0.03 + 0.20 * std::exp(-0.5 * std::pow((k - 160) / 16.0, 2.0));
        const double lux = (k < 130) ? 1.0 : 0.28;

        const double qc = cameraConfidence(fog, lux, rain);
        const double ql = lidarConfidence(fog, rain);

        const double camStd = std::sqrt(Rcam0 / qc);
        const double lidarStd = std::sqrt(Rlidar0 / ql);

        std::normal_distribution<double> nCam(0.0, camStd);
        std::normal_distribution<double> nLidar(0.0, lidarStd);

        const double zCam = xTrue[k] + nCam(gen);
        const double zLidar = xTrue[k] + nLidar(gen);

        kfAdaptive.predict();
        kfFixed.predict();

        const double nisCam = kfAdaptive.update(zCam, Rcam0 / qc);
        const double nisLidar = kfAdaptive.update(zLidar, Rlidar0 / ql);

        kfFixed.update(zCam, Rcam0);
        kfFixed.update(zLidar, Rlidar0);

        nisCamMean += nisCam;
        nisLidarMean += nisLidar;
        sqErrAdaptive += std::pow(kfAdaptive.x - xTrue[k], 2.0);
        sqErrFixed += std::pow(kfFixed.x - xTrue[k], 2.0);
    }

    nisCamMean /= static_cast<double>(T);
    nisLidarMean /= static_cast<double>(T);

    const double rmseAdaptive = std::sqrt(sqErrAdaptive / static_cast<double>(T));
    const double rmseFixed = std::sqrt(sqErrFixed / static_cast<double>(T));

    std::cout << "Adaptive fusion RMSE : " << rmseAdaptive << "\n";
    std::cout << "Fixed-R fusion RMSE  : " << rmseFixed << "\n";
    std::cout << "Mean camera NIS      : " << nisCamMean << "\n";
    std::cout << "Mean LiDAR NIS       : " << nisLidarMean << "\n";
    return 0;
}
      

The Java version mirrors the same logic for middleware or edge-side AMR components that ingest quality scores and perform confidence-aware fusion.

Code file: Chapter18_Lesson4.java

// Chapter18_Lesson4.java
// Weather/Lighting Effects on Perception (Outdoor and Field AMR)

import java.util.Random;

public class Chapter18_Lesson4 {

    static class ScalarKalman {
        double x = 0.0;
        double P = 1.0;
        double Q = 0.02;

        void predict() { P += Q; }

        double update(double z, double R) {
            double S = P + R;
            double K = P / S;
            double innov = z - x;
            x = x + K * innov;
            P = (1.0 - K) * P;
            return (innov * innov) / S;
        }
    }

    static double clamp(double v, double lo, double hi) {
        return Math.max(lo, Math.min(hi, v));
    }

    static double cameraConfidence(double fog, double lux, double rain) {
        double q = Math.pow(lux, 0.7) * Math.exp(-2.2 * fog) * Math.exp(-1.4 * rain);
        return clamp(q, 0.05, 1.0);
    }

    static double lidarConfidence(double fog, double rain) {
        double q = Math.exp(-1.3 * fog) * Math.exp(-0.6 * rain);
        return clamp(q, 0.10, 1.0);
    }

    static double[] gammaNormalize(double[] sig, double gamma) {
        double[] y = new double[sig.length];
        double mn = Double.POSITIVE_INFINITY;
        double mx = Double.NEGATIVE_INFINITY;
        for (int i = 0; i < sig.length; i++) {
            y[i] = Math.pow(clamp(sig[i], 0.0, 1.0), gamma);
            mn = Math.min(mn, y[i]);
            mx = Math.max(mx, y[i]);
        }
        double d = (mx - mn) + 1e-12;
        for (int i = 0; i < y.length; i++) y[i] = (y[i] - mn) / d;
        return y;
    }

    static double contrast(double[] x) {
        double mn = Double.POSITIVE_INFINITY;
        double mx = Double.NEGATIVE_INFINITY;
        for (double v : x) { mn = Math.min(mn, v); mx = Math.max(mx, v); }
        return (mx - mn) / (mx + mn + 1e-12);
    }

    public static void main(String[] args) {
        Random rng = new Random(11);

        int n = 300;
        double[] clear = new double[n];
        double[] deg = new double[n];
        double beta = 0.08, A = 0.85, exposure = 0.30;

        for (int i = 0; i < n; i++) {
            double x = i / (double)(n - 1);
            double J = 0.20 + (x > 0.18 ? 0.35 : 0.0) + 0.12 * Math.sin(12.0 * Math.PI * x) + (x > 0.60 ? 0.18 : 0.0);
            J = clamp(J, 0.0, 1.0);
            clear[i] = J;
            double depth = 4.0 + 28.0 * x;
            double t = Math.exp(-beta * depth);
            double Ifog = J * t + A * (1.0 - t);
            double sigma = Math.sqrt(Math.max(exposure * Ifog, 1e-6)) * 0.05 + 0.015;
            deg[i] = clamp(exposure * Ifog + sigma * rng.nextGaussian(), 0.0, 1.0);
        }
        double[] enh = gammaNormalize(deg, 0.6);
        System.out.printf("Contrast clear=%.4f degraded=%.4f enhanced=%.4f%n", contrast(clear), contrast(deg), contrast(enh));

        final int T = 220;
        final double Rcam0 = 0.60 * 0.60;
        final double Rlidar0 = 0.25 * 0.25;
        double[] truth = new double[T];
        for (int k = 1; k < T; k++) truth[k] = truth[k-1] + 0.10 + 0.05 * rng.nextGaussian();

        ScalarKalman adapt = new ScalarKalman();
        ScalarKalman fixed = new ScalarKalman();

        double seA = 0.0, seF = 0.0;
        for (int k = 0; k < T; k++) {
            double fog = 0.10 + 0.30 * Math.exp(-0.5 * Math.pow((k - 110) / 22.0, 2.0));
            double rain = 0.03 + 0.22 * Math.exp(-0.5 * Math.pow((k - 170) / 14.0, 2.0));
            double lux = (k < 135) ? 1.0 : 0.25;
            double qc = cameraConfidence(fog, lux, rain);
            double ql = lidarConfidence(fog, rain);

            double zCam = truth[k] + Math.sqrt(Rcam0 / qc) * rng.nextGaussian();
            double zLidar = truth[k] + Math.sqrt(Rlidar0 / ql) * rng.nextGaussian();

            adapt.predict(); fixed.predict();
            adapt.update(zCam, Rcam0 / qc);
            adapt.update(zLidar, Rlidar0 / ql);
            fixed.update(zCam, Rcam0);
            fixed.update(zLidar, Rlidar0);

            seA += Math.pow(adapt.x - truth[k], 2.0);
            seF += Math.pow(fixed.x - truth[k], 2.0);
        }

        System.out.printf("Adaptive RMSE=%.4f%n", Math.sqrt(seA / T));
        System.out.printf("Fixed-R RMSE=%.4f%n", Math.sqrt(seF / T));
    }
}
      

The MATLAB script is suitable for teaching and can be ported into Simulink as a sequence of blocks: quality estimation → covariance scheduling → EKF update.

Code file: Chapter18_Lesson4.m

% Chapter18_Lesson4.m
% Weather/Lighting Effects on Perception (Outdoor and Field AMR)

rng(5);

%% Part A: Synthetic camera degradation
n = 400;
x = linspace(0,1,n);

J = 0.25 + 0.35*(x >= 0.2) + 0.15*sin(14*pi*x) + 0.20*(x >= 0.58);
J = min(max(J,0),1);

d = 5 + 30*x;
beta = 0.07;
A = 0.85;

t = exp(-beta .* d);
I_fog = J .* t + A .* (1 - t);

exposure = 0.28;
readSigma = 0.015;
shotSigma = sqrt(max(I_fog*exposure,1e-6)) * 0.05;
I_low = exposure .* I_fog + randn(1,n) .* (shotSigma + readSigma);
I_low = min(max(I_low,0),1);

gamma = 0.6;
I_enh = I_low .^ gamma;
I_enh = (I_enh - min(I_enh)) / (max(I_enh) - min(I_enh) + 1e-12);

michelson = @(s) (max(s)-min(s)) / (max(s)+min(s)+1e-12);
fprintf('Contrast clear=%.4f, degraded=%.4f, enhanced=%.4f\n', michelson(J), michelson(I_low), michelson(I_enh));

%% Part B: Confidence-aware EKF
dt = 0.1; T = 250;
F = [1 dt; 0 1];
Q = [1e-4 0; 0 3e-3];
H = [1 0];
Rcam0 = 0.60^2;
Rlidar0 = 0.25^2;

xTrue = zeros(2,T); xTrue(:,1) = [0; 0.35];
for k = 2:T
    xTrue(:,k) = F*xTrue(:,k-1) + mvnrnd([0;0], Q)';
end

xA = [0;0]; PA = diag([1 0.5]);
xF = [0;0]; PF = diag([1 0.5]);
estA = zeros(2,T); estF = zeros(2,T);

cameraConf = @(fog,lux,rain) min(max((lux^0.7)*exp(-2.2*fog)*exp(-1.4*rain), 0.05), 1.0);
lidarConf  = @(fog,rain)     min(max(exp(-1.3*fog)*exp(-0.6*rain), 0.10), 1.0);

for k = 1:T
    fog = 0.10 + 0.35*exp(-0.5*((k-120)/22)^2);
    rain = 0.05 + 0.25*exp(-0.5*((k-180)/14)^2);
    lux = 1.0; if k >= 140, lux = 0.25; end

    qc = cameraConf(fog,lux,rain);
    ql = lidarConf(fog,rain);

    zCam = xTrue(1,k) + sqrt(Rcam0/qc)*randn;
    zLidar = xTrue(1,k) + sqrt(Rlidar0/ql)*randn;

    xA = F*xA; PA = F*PA*F' + Q;
    xF = F*xF; PF = F*PF*F' + Q;

    [xA, PA] = ekf1DUpdate(xA, PA, zCam, H, Rcam0/qc);
    [xA, PA] = ekf1DUpdate(xA, PA, zLidar, H, Rlidar0/ql);

    [xF, PF] = ekf1DUpdate(xF, PF, zCam, H, Rcam0);
    [xF, PF] = ekf1DUpdate(xF, PF, zLidar, H, Rlidar0);

    estA(:,k) = xA; estF(:,k) = xF;
end

rmseA = sqrt(mean((estA(1,:) - xTrue(1,:)).^2));
rmseF = sqrt(mean((estF(1,:) - xTrue(1,:)).^2));
fprintf('Adaptive RMSE=%.4f, Fixed-R RMSE=%.4f\n', rmseA, rmseF);

figure; plot(1:T, xTrue(1,:), 1:T, estA(1,:), 1:T, estF(1,:));
legend('Truth','Adaptive EKF','Fixed-R EKF'); grid on;
xlabel('Step'); ylabel('Position'); title('Weather-aware covariance adaptation');

function [x, P] = ekf1DUpdate(x, P, z, H, R)
    y = z - H*x;
    S = H*P*H' + R;
    K = (P*H') / S;
    x = x + K*y;
    I = eye(size(P));
    P = (I - K*H)*P*(I - K*H)' + K*R*K';
end
      

The Mathematica script supports symbolic and numeric analysis of the attenuation model, visibility threshold, and confidence-aware information updates.

Code file: Chapter18_Lesson4.nb

(* Chapter18_Lesson4.nb *)
(* Weather/Lighting Effects on Perception (Outdoor and Field AMR) *)

ClearAll["Global`*"];

beta = 0.07;
Aatm = 0.85;
J[d_] := Clip[0.25 + 0.35 UnitStep[d - 12] + 0.15 Sin[0.8 d], {0, 1}];
t[d_] := Exp[-beta d];
Ifog[d_] := J[d] t[d] + Aatm (1 - t[d]);

C0 = 0.9;
Cmin = 0.05;
C[d_] := C0 Exp[-beta d];
dMax = (1/beta) Log[C0/Cmin];

Print["dmax = ", N[dMax, 4]];
Print["dC/dd = ", FullSimplify[D[C[d], d]]];

Yminus = { {4.0, 0.0}, {0.0, 1.5} };
H = { {1.0, 0.0} };
R0 = { {0.25^2} };

Yplus[c_] := Yminus + c Transpose[H].Inverse[R0].H;
Pplus[c_] := Inverse[Yplus[c]];

Do[
  Print["c=", c, "  Pplus=", MatrixForm[Pplus[c]]],
  {c, {1.0, 0.7, 0.3, 0.1} }
];

Plot[{J[d], Ifog[d]}, {d, 0, 35},
  PlotLegends -> {"Clear", "Foggy"},
  PlotLabel -> "Fog attenuation model"];

Plot[{C[d], Cmin}, {d, 0, 70},
  PlotLegends -> {"Contrast", "Threshold"},
  PlotLabel -> "Visibility threshold"];
      

Problem 1 (Visibility distance under fog): Assume object contrast decays as \( C(d)=C_0\exp(-\beta d) \) and the detector requires \( C(d) \ge C_{min} \). Derive the maximum detectable distance and prove that it decreases monotonically with \( \beta \).

Solution: Solve \( C_0\exp(-\beta d_{max}) = C_{min} \), giving

\[ d_{max} = \frac{1}{\beta}\ln\!\left(\frac{C_0}{C_{min} }\right). \]

Since \( C_0 > C_{min} > 0 \), the logarithm is positive. Differentiating yields

\[ \frac{\partial d_{max} }{\partial \beta} = -\frac{1}{\beta^2}\ln\!\left(\frac{C_0}{C_{min} }\right) < 0, \]

so visibility-limited range strictly decreases as fog density increases.

Problem 2 (Confidence scaling and Kalman gain): In a scalar measurement model with predicted variance \( P^- \) and \( R(q)=R_0/q \), prove that \( K(q) \) increases with \( q \).

Solution: The scalar gain is

\[ K(q) = \frac{P^-}{P^- + R_0/q} = \frac{qP^-}{qP^- + R_0}. \]

Differentiating gives

\[ \frac{dK}{dq} = \frac{P^- R_0}{(qP^- + R_0)^2} > 0. \]

Therefore higher confidence increases the Kalman gain.

Problem 3 (Low-light SNR asymptotics): For \( \operatorname{SNR}(s)=s/\sqrt{s+\sigma_{read}^2} \), derive asymptotic behavior for \( s \ll \sigma_{read}^2 \) and \( s \gg \sigma_{read}^2 \).

Solution:

• If \( s \ll \sigma_{read}^2 \), then \( \operatorname{SNR}(s) \approx s/\sigma_{read} \).
• If \( s \gg \sigma_{read}^2 \), then \( \operatorname{SNR}(s) \approx \sqrt{s} \).

Thus low-light performance is read-noise dominated and highly sensitive to exposure, while bright scenes become shot-noise limited.

Problem 4 (NIS gating order): Explain why weather-induced covariance inflation should be applied before NIS gating, and state the gate condition.

Solution: The NIS statistic is

\[ \epsilon_k = \boldsymbol{\nu}_k^\top \mathbf{S}_k^{-1}\boldsymbol{\nu}_k. \]

If the filter uses a clear-weather covariance during bad weather, \( \mathbf{S}_k \) is underestimated and \( \epsilon_k \) becomes artificially large, causing false rejections. Therefore inflate \( \mathbf{R}_k \) first using confidence, then apply the gate \( \epsilon_k > \gamma \) to reject impulsive outliers.

Problem 5 (Preprocessing vs uncertainty): Why is image enhancement alone insufficient for probabilistic consistency in outdoor AMR perception?

Solution: Enhancement may improve visual contrast but does not guarantee restoration of the original measurement statistics. Residual blur, amplified noise, and depth-dependent errors remain. Therefore the estimator must adapt covariances using post-processing quality indicators rather than assuming unchanged clear-weather noise.

Weather and lighting effects in outdoor AMR perception are best treated as changes in the sensor likelihood model, not only as image-processing problems. Physically grounded degradation models, confidence-aware covariance scheduling, and innovation gating together provide a robust bridge from raw sensors to statistically consistent localization and mapping.