A robot rarely relies on a single sensor. Reasons include: (i) noise and drift, (ii) occlusions or failures, (iii) partial observability, and (iv) complementary strengths across sensors.

We use a simple measurement model: a physical quantity \( x \) is observed by sensor \( i \) as \( z_i \), with error \( e_i \):

\[ z_i = h_i(x) + e_i. \]

Here \( h_i(\cdot) \) is the sensor’s measurement function (possibly linearized from Lesson 3's feature idea), and \( e_i \) summarizes all imperfections (noise, bias, quantization). We will treat \( e_i \) as unknown disturbances rather than random variables.

In the special case where all sensors measure the same scalar state directly, the model reduces to \( z_i = x + e_i \).

flowchart TD
  S1["Sensor 1: z1"] --> A["Preprocess / calibrate"]
  S2["Sensor 2: z2"] --> A
  S3["Sensor 3: z3"] --> A
  A --> B["Align units + timestamps"]
  B --> C["Fusion rule (average / LS / complementary)"]
  C --> D["Fused estimate x_hat"]
  D --> E["Perception output for robot"]
        

Multi-sensor fusion can be classified by the role sensors play:

• Redundant fusion: multiple sensors measure the same quantity (e.g., two range sensors for distance). Fusion reduces error or increases robustness.
• Complementary fusion: sensors measure different aspects of the same state (e.g., wheel encoder gives good low-frequency velocity, IMU gives good high-frequency acceleration).
• Competitive (confirmatory) fusion: sensors compete to reject outliers or faults, e.g., “majority vote” on a discrete event.

This lesson focuses on two deterministic fusion families: weighted least-squares for redundant/complementary measurements, and complementary filtering for frequency-domain combination.

Suppose \( m \) sensors measure the same scalar quantity \( x \):

\[ z_i = x + e_i,\quad i=1,\dots,m. \]

We want an estimate \( \hat{x} \) that best explains all measurements. A deterministic approach is to minimize a weighted squared error:

\[ J(x) = \sum_{i=1}^m w_i (z_i - x)^2, \quad w_i > 0. \]

The weights \( w_i \) represent confidence: larger weight means the sensor is trusted more (e.g., higher resolution or less drift).

Proposition (Weighted-average fusion): The minimizer of \( J(x) \) is

\[ \hat{x} = \frac{\sum_{i=1}^m w_i z_i}{\sum_{i=1}^m w_i}. \]

Proof: Differentiate \( J(x) \) with respect to \( x \):

\[ \frac{dJ}{dx} = \sum_{i=1}^m 2 w_i (x - z_i) = 2\left( x\sum_{i=1}^m w_i - \sum_{i=1}^m w_i z_i \right). \]

Setting \( \frac{dJ}{dx}=0 \) yields \( x\sum w_i = \sum w_i z_i \), hence \( \hat{x} = \frac{\sum w_i z_i}{\sum w_i} \). The second derivative is \( \frac{d^2J}{dx^2} = 2\sum w_i > 0 \), so the solution is a strict minimizer. □

If all sensors are equally reliable, take \( w_i = 1 \) and obtain the simple average \( \hat{x} = \frac{1}{m}\sum z_i \).

Real robots often estimate a vector state \( \mathbf{x} \in \mathbb{R}^n \), such as position, velocity, or feature parameters. Assume \( m \) sensors provide linear measurements:

\[ \mathbf{z}_i = \mathbf{H}_i \mathbf{x} + \mathbf{e}_i, \quad \mathbf{z}_i \in \mathbb{R}^{p_i}. \]

Define a quadratic cost:

\[ J(\mathbf{x}) = \sum_{i=1}^m (\mathbf{z}_i - \mathbf{H}_i\mathbf{x})^\top \mathbf{W}_i (\mathbf{z}_i - \mathbf{H}_i\mathbf{x}), \quad \mathbf{W}_i \succ 0. \]

This treats fusion as an optimization problem; no probabilistic interpretation is required.

Theorem (Weighted least-squares fusion): If \( \sum_{i=1}^m \mathbf{H}_i^\top \mathbf{W}_i \mathbf{H}_i \) is invertible, then the unique minimizer is

\[ \hat{\mathbf{x}} = \left(\sum_{i=1}^m \mathbf{H}_i^\top \mathbf{W}_i \mathbf{H}_i\right)^{-1} \left(\sum_{i=1}^m \mathbf{H}_i^\top \mathbf{W}_i \mathbf{z}_i\right). \]

Proof: Expand and differentiate:

\[ \nabla_{\mathbf{x}}J = -2\sum_{i=1}^m \mathbf{H}_i^\top \mathbf{W}_i(\mathbf{z}_i - \mathbf{H}_i\mathbf{x}). \]

Setting \( \nabla_{\mathbf{x}}J = \mathbf{0} \) gives the normal equations:

\[ \left(\sum_{i=1}^m \mathbf{H}_i^\top \mathbf{W}_i \mathbf{H}_i\right)\mathbf{x} = \sum_{i=1}^m \mathbf{H}_i^\top \mathbf{W}_i \mathbf{z}_i. \]

If the left matrix is invertible, the solution is unique and equals the stated expression. The Hessian is \( \nabla^2 J = 2\sum \mathbf{H}_i^\top\mathbf{W}_i\mathbf{H}_i \succ 0 \), ensuring a strict minimum. □

Interpretation: the fused estimate exists and is unique only if the combined sensors “see” all components of \( \mathbf{x} \). This mirrors observability ideas from linear control.

Complementary fusion is useful when sensors have different frequency strengths. Example: wheel encoder-based velocity is stable at low frequency but drifts at high frequency; an IMU captures high-frequency motion but has low-frequency bias.

Let \( y_1(t) \) and \( y_2(t) \) be two measurements of the same quantity. Choose a low-pass filter \( L(s) \) and a high-pass filter \( H(s) \) such that

\[ L(s) + H(s) = 1. \]

Then define the fused estimate in Laplace domain:

\[ \hat{X}(s) = L(s)Y_1(s) + H(s)Y_2(s). \]

A standard first-order design uses cutoff frequency \( \omega_c > 0 \):

\[ L(s)=\frac{1}{1+s/\omega_c},\qquad H(s)=\frac{s/\omega_c}{1+s/\omega_c}. \]

Note immediately that \( L(s)+H(s)=1 \); thus, if both sensors perfectly measured \( X(s) \), their fusion would recover it exactly.

In discrete time with sample period \( T \), define \( \alpha = \frac{\omega_c T}{1+\omega_c T} \). A simple complementary recursion is:

\[ \hat{x}[k] = (1-\alpha)\big(\hat{x}[k-1] + T\,u_2[k]\big) + \alpha\, y_1[k], \]

where \( y_1[k] \) is the low-frequency sensor (e.g., encoder velocity integrated), and \( u_2[k] \) is the high-frequency rate-like signal (e.g., IMU acceleration integrated once). This is a control-friendly fusion rule with tunable bandwidth.

We implement (i) weighted average fusion, (ii) vector WLS fusion, and (iii) a discrete complementary filter.

6.1 Python (NumPy)

import numpy as np

# -----------------------------
# (A) Scalar weighted fusion
# -----------------------------
z = np.array([1.02, 0.97, 1.10])   # three sensors
w = np.array([4.0, 2.0, 1.0])     # confidences
x_hat = (w @ z) / np.sum(w)
print("Scalar fused estimate:", x_hat)

# -----------------------------
# (B) Vector WLS fusion
# z_i = H_i x + e_i
# -----------------------------
# state x = [x_position, y_position]^T
H1 = np.eye(2)                    # GPS-like direct position
z1 = np.array([2.0, 1.0])

H2 = np.array([[1.0, 0.0]])       # range-only sensor to x-axis
z2 = np.array([1.8])

W1 = np.eye(2) * 5.0              # higher confidence
W2 = np.eye(1) * 2.0

A = H1.T @ W1 @ H1 + H2.T @ W2 @ H2
b = H1.T @ W1 @ z1 + H2.T @ W2 @ z2
x_hat_vec = np.linalg.solve(A, b)
print("Vector fused estimate:", x_hat_vec)

# -----------------------------
# (C) Discrete complementary filter
# -----------------------------
T = 0.01
omega_c = 2.0
alpha = (omega_c * T) / (1.0 + omega_c * T)

# y1: low-frequency measurement (e.g., encoder position)
# u2: high-frequency rate (e.g., IMU velocity increment)
y1 = np.sin(np.linspace(0, 2, 200)) + 0.05*np.random.randn(200)
u2 = np.gradient(y1, T) + 0.2*np.random.randn(200)

x_hat = np.zeros_like(y1)
for k in range(1, len(y1)):
    x_hat[k] = (1-alpha)*(x_hat[k-1] + T*u2[k]) + alpha*y1[k]

print("Complementary filter done.")
      

6.2 C++ (Eigen)

#include <iostream>
#include <Eigen/Dense>

int main() {
  // Scalar weighted fusion
  Eigen::Vector3d z; z << 1.02, 0.97, 1.10;
  Eigen::Vector3d w; w << 4.0, 2.0, 1.0;
  double x_hat = (w.dot(z)) / w.sum();
  std::cout << "Scalar fused estimate: " << x_hat << std::endl;

  // Vector WLS fusion
  Eigen::Matrix2d H1 = Eigen::Matrix2d::Identity();
  Eigen::Vector2d z1; z1 << 2.0, 1.0;

  Eigen::RowVector2d H2; H2 << 1.0, 0.0;
  Eigen::VectorXd z2(1); z2 << 1.8;

  Eigen::Matrix2d W1 = 5.0 * Eigen::Matrix2d::Identity();
  Eigen::MatrixXd W2(1,1); W2 << 2.0;

  Eigen::Matrix2d A = H1.transpose()*W1*H1 + H2.transpose()*W2*H2;
  Eigen::Vector2d b = H1.transpose()*W1*z1 + H2.transpose()*W2*z2;

  Eigen::Vector2d x_hat_vec = A.ldlt().solve(b);
  std::cout << "Vector fused estimate:\n" << x_hat_vec << std::endl;
  return 0;
}
      

6.3 Java (EJML)

import org.ejml.simple.SimpleMatrix;

public class FusionDemo {
  public static void main(String[] args) {
    // Scalar weighted fusion
    SimpleMatrix z = new SimpleMatrix(new double[][]{
      {1.02},
      {0.97},
      {1.10}
    });
    SimpleMatrix w = new SimpleMatrix(new double[][]{
      {4.0},
      {2.0},
      {1.0}
    });
    double xHat = w.transpose().mult(z).get(0) / w.elementSum();
    System.out.println("Scalar fused estimate: " + xHat);

    // Vector WLS fusion
    SimpleMatrix H1 = SimpleMatrix.identity(2);
    SimpleMatrix z1 = new SimpleMatrix(new double[][]{
      {2.0},
      {1.0}
    });

    SimpleMatrix H2 = new SimpleMatrix(new double[][]);
    SimpleMatrix z2 = new SimpleMatrix(new double[][]);

    SimpleMatrix W1 = SimpleMatrix.identity(2).scale(5.0);
    SimpleMatrix W2 = new SimpleMatrix(new double[][]);

    SimpleMatrix A = H1.transpose().mult(W1).mult(H1)
                   .plus(H2.transpose().mult(W2).mult(H2));
    SimpleMatrix b = H1.transpose().mult(W1).mult(z1)
                   .plus(H2.transpose().mult(W2).mult(z2));

    SimpleMatrix xHatVec = A.solve(b);
    System.out.println("Vector fused estimate:");
    xHatVec.print();
  }
}
      

6.4 MATLAB / Simulink

%% (A) Scalar weighted fusion
z = [1.02; 0.97; 1.10];
w = [4; 2; 1];
x_hat = (w'*z)/sum(w)

%% (B) Vector WLS fusion
H1 = eye(2);        z1 = [2.0; 1.0];
H2 = [1 0];         z2 = 1.8;
W1 = 5*eye(2);      W2 = 2;

A = H1'*W1*H1 + H2'*W2*H2;
b = H1'*W1*z1 + H2'*W2*z2;

x_hat_vec = A\b

%% (C) Discrete complementary filter
T = 0.01; omega_c = 2.0;
alpha = (omega_c*T)/(1+omega_c*T);

t = 0:T:2;
y1 = sin(t) + 0.05*randn(size(t));      % low-frequency sensor
u2 = gradient(y1,T) + 0.2*randn(size(t)); % high-frequency rate

xhat = zeros(size(t));
for k=2:length(t)
  xhat(k) = (1-alpha)*(xhat(k-1) + T*u2(k)) + alpha*y1(k);
end
plot(t,y1,t,xhat); legend('y1','fused');
      

Simulink hint: Implement the complementary filter using a Discrete-Time Integrator for \( \hat{x}[k-1] + T u_2[k] \), a Gain block for \( \alpha \), and a Sum block combining \( (1-\alpha) \) and \( \alpha \) pathways.

Problem 1 (Scalar fusion derivation): Given \( z_i = x + e_i \), show that minimizing \( J(x)=\sum_{i=1}^m w_i(z_i-x)^2 \) yields the weighted average estimator.

Solution: From Section 3, differentiate:

\[ \frac{dJ}{dx} = 2\left(x\sum_{i=1}^m w_i - \sum_{i=1}^m w_i z_i\right). \]

Setting this to zero gives \( \hat{x} = \frac{\sum w_i z_i}{\sum w_i} \). Since \( \frac{d^2J}{dx^2}=2\sum w_i > 0 \), it is the unique minimizer.

Problem 2 (Complementary unity-gain property): Assume \( L(s) + H(s) = 1 \). Show that if \( Y_1(s)=Y_2(s)=X(s) \) (perfect sensors), then \( \hat{X}(s)=X(s) \).

Solution: Substitute into fusion rule:

\[ \hat{X}(s)=L(s)X(s)+H(s)X(s)=(L(s)+H(s))X(s)=1\cdot X(s)=X(s). \]

Problem 3 (Numerical WLS): Two sensors measure a scalar distance: \( z_1=1.00 \), \( z_2=1.20 \). You trust sensor 1 twice as much as sensor 2. Compute \( \hat{x} \).

Solution: Choose weights \( w_1=2,\; w_2=1 \).

\[ \hat{x}=\frac{2(1.00)+1(1.20)}{2+1}=\frac{3.20}{3}=1.066\overline{6}. \]

Problem 4 (Vector fusion observability): Let \( \mathbf{x}\in\mathbb{R}^2 \) and two sensors: \( \mathbf{z}_1=\begin{bmatrix}1&0\\0&1\end{bmatrix}\mathbf{x} \), \( z_2=\begin{bmatrix}1&0\end{bmatrix}\mathbf{x} \). Show the WLS matrix is invertible for any positive weights.

Solution: With \( \mathbf{W}_1\succ 0 \), \( W_2>0 \),

\[ \mathbf{A} = \mathbf{H}_1^\top \mathbf{W}_1 \mathbf{H}_1 + \mathbf{H}_2^\top W_2 \mathbf{H}_2 = \mathbf{W}_1 + W_2 \begin{bmatrix}1&0\\0&0\end{bmatrix}. \]

Since \( \mathbf{W}_1\succ 0 \), adding a positive semidefinite term preserves positive definiteness. Hence \( \mathbf{A}\succ 0 \) and is invertible.

Multi-sensor fusion improves perception by using redundancy and complementarity. We derived deterministic fusion rules via weighted least squares for scalar and vector measurements, and introduced complementary filters as a frequency-domain fusion tool. These ideas form the intuitive foundation for later probabilistic fusion methods.