In advanced manipulation, the robot often observes cluttered 3D scenes with multiple moving objects (hands, tools, parts, etc.). To reason about grasping, placing, or avoiding collisions, the robot must infer:

• A dense 3D motion field over space, i.e. scene flow \( \mathbf{v}(\mathbf{X}) \) giving velocity of each visible point \( \mathbf{X} \in \mathbb{R}^3 \).
• Object-level trajectories and identities, i.e. per-object states \( \mathbf{x}_t^{(k)} \) tracked over time from noisy detections.

Scene flow refines the robot's geometric understanding, while tracking aggregates these local motions into temporally consistent object hypotheses. The conceptual pipeline is:

flowchart TD
  A["RGB-D or stereo frames at times t, t+1"] --> B["3D point cloud construction"]
  B --> C["Scene flow estimation (3D motion field)"]
  C --> D["Motion-based clustering into objects"]
  D --> E["Initialize or update object states"]
  E --> F["Predictive tracking over time for manipulation"]
        

In the rest of the lesson we formalize scene flow as a variational or graph-based optimization problem, then derive probabilistic state-space models and filtering algorithms that consume these motion cues for robust object tracking.

Consider a calibrated pinhole camera observing a point \( \mathbf{X}(t) = (X(t), Y(t), Z(t))^{\top} \) in camera coordinates at time \( t \). The image projection is

\[ \mathbf{x}(t) = \begin{bmatrix} x(t) \\[2pt] y(t) \end{bmatrix} = \pi(\mathbf{X}(t)) = \begin{bmatrix} f_x \dfrac{X(t)}{Z(t)} + c_x \\ f_y \dfrac{Y(t)}{Z(t)} + c_y \end{bmatrix}, \]

where \( f_x, f_y \) are focal lengths and \( c_x, c_y \) is the principal point.

The scene flow is the 3D velocity field \( \mathbf{v}(\mathbf{X}, t) = \dfrac{d\mathbf{X}}{dt} \). For a small time increment \( \Delta t \) we can approximate

\[ \mathbf{X}(t+\Delta t) \approx \mathbf{X}(t) + \mathbf{v}(\mathbf{X}(t), t)\,\Delta t. \]

The corresponding 2D optical flow \( \mathbf{u}(t) = (u_x, u_y)^{\top} \) satisfies

\[ \mathbf{x}(t+\Delta t) \approx \mathbf{x}(t) + \mathbf{u}(t), \quad \mathbf{u}(t) = \dfrac{\partial \pi}{\partial \mathbf{X}} \bigg\rvert_{\mathbf{X}(t)} \mathbf{v}(\mathbf{X}(t), t)\,\Delta t, \]

where the Jacobian of the projection is

\[ \dfrac{\partial \pi}{\partial \mathbf{X}} = \begin{bmatrix} \dfrac{f_x}{Z} & 0 & -\dfrac{f_x X}{Z^2} \\ 0 & \dfrac{f_y}{Z} & -\dfrac{f_y Y}{Z^2} \end{bmatrix}. \]

Thus, given depth \( Z \) and a 2D optical flow estimate, we can backproject to a 3D motion estimate; conversely, given 3D scene flow we can predict the induced optical flow.

A standard assumption in variational formulations is brightness constancy:

\[ I(\mathbf{x}(t), t) \approx I(\mathbf{x}(t)+\mathbf{u}(t), t+\Delta t), \]

and in RGB-D settings a depth constancy constraint:

\[ D(\mathbf{x}(t), t) \approx D(\mathbf{x}(t)+\mathbf{u}(t), t+\Delta t), \]

where \( I \) is intensity (or color) and \( D \) is depth. These constraints couple image measurements to the unknown motion field.

Variational methods estimate scene flow by minimizing an energy functional that trades off data fidelity and spatial regularity. Let \( \Omega \subset \mathbb{R}^2 \) denote the image domain and \( \mathbf{v} : \Omega \rightarrow \mathbb{R}^3 \) the unknown 3D velocity field parameterized over pixels. One quadratic model is

\[ E(\mathbf{v}) = \int_{\Omega} \left\| \mathbf{X}_t(\mathbf{p}) + \mathbf{v}(\mathbf{p})\,\Delta t - \mathbf{X}_{t+\Delta t}(\mathbf{p}) \right\|^2 + \lambda \left\|\nabla \mathbf{v}(\mathbf{p})\right\|^2 \, d\mathbf{p}, \]

where \( \mathbf{X}_t(\mathbf{p}) \) and \( \mathbf{X}_{t+\Delta t}(\mathbf{p}) \) are 3D points reconstructed from depth (or stereo) at times \( t \) and \( t+\Delta t \), \( \lambda > 0 \) is a smoothness weight, and \( \nabla \mathbf{v} \) is the spatial gradient of the vector field.

For simplicity, consider a single component \( v_k \) of the vector field \( \mathbf{v} = (v_1, v_2, v_3)^{\top} \). The energy decouples into three similar terms, each of the form

\[ E_k(v_k) = \int_{\Omega} \left( a_k(\mathbf{p}) + v_k(\mathbf{p})\,\Delta t - b_k(\mathbf{p}) \right)^2 + \lambda \left\|\nabla v_k(\mathbf{p})\right\|^2 \, d\mathbf{p}, \]

where the scalar fields \( a_k \), \( b_k \) are derived from \( \mathbf{X}_t \) and \( \mathbf{X}_{t+\Delta t} \). Taking the first variation and setting it to zero yields the Euler–Lagrange equation

\[ 2\Delta t\left(a_k + v_k\,\Delta t - b_k\right) - 2\lambda \Delta v_k = 0, \quad \text{on } \Omega, \]

i.e.

\[ \left(\Delta t^2\right)v_k - \lambda \Delta v_k = \Delta t \left(b_k - a_k\right), \]

where \( \Delta \) is the Laplace operator. In practice we discretize this PDE on a grid and solve the resulting sparse linear system (e.g. using Gauss–Seidel or conjugate gradient iterations). The 3D vector field \( \mathbf{v} \) is estimated by solving three such coupled systems (with possible anisotropic or discontinuity-preserving regularizers).

Thus, variational scene flow is essentially a regularized inverse problem with a quadratic (or robust) objective over a very high dimensional parameter space.

When the robot works directly with 3D point clouds (e.g., from a depth camera in a manipulator workspace), it is natural to represent scene flow on a graph. Let there be \( N \) points at time \( t \) with positions \( \mathbf{X}_i^t \in \mathbb{R}^3 \), and positions \( \mathbf{X}_i^{t+\Delta t} \) at the next time step (e.g., found via nearest-neighbor matching).

The unknown 3D velocities are \( \mathbf{v}_i \in \mathbb{R}^3, i = 1,\dots,N \). A standard quadratic energy is

\[ E(\{\mathbf{v}_i\}) = \sum_{i=1}^N \left\| \mathbf{X}_i^t + \mathbf{v}_i\,\Delta t - \mathbf{X}_i^{t+\Delta t} \right\|^2 + \lambda \sum_{(i,j)\in \mathcal{E}} w_{ij} \left\|\mathbf{v}_i - \mathbf{v}_j\right\|^2, \]

where \( \mathcal{E} \) is an edge set (e.g. k-nearest neighbors) and \( w_{ij} \) are affinity weights encoding spatial proximity or surface similarity.

Stacking all velocities into a vector \( \mathbf{v} \in \mathbb{R}^{3N} \) and defining a suitable data matrix \( \mathbf{A} \) and right-hand side \( \mathbf{b} \), the energy becomes

\[ E(\mathbf{v}) = \left\|\mathbf{A}\mathbf{v} - \mathbf{b}\right\|^2 + \lambda\,\mathbf{v}^{\top} \left(\mathbf{L}\otimes \mathbf{I}_3\right)\mathbf{v}, \]

where \( \mathbf{L} \) is the graph Laplacian and \( \mathbf{I}_3 \) the 3 × 3 identity. Differentiating and setting the gradient to zero yields the normal equations

\[ \left( \mathbf{A}^{\top}\mathbf{A} + \lambda\,\mathbf{L}\otimes \mathbf{I}_3 \right)\mathbf{v} = \mathbf{A}^{\top}\mathbf{b}, \]

a sparse symmetric positive definite linear system. Its solution can be computed with iterative solvers; the graph structure naturally preserves local rigidity and encourages piecewise-smooth motion, which is well suited to multiple moving rigid objects in manipulation scenes.

Scene flow delivers dense or sparse 3D motion estimates; to reason at the object level, we model each object as a state-space process. For a single object, let \( \mathbf{x}_t \in \mathbb{R}^n \) denote its state at time \( t \) (e.g. 3D position and velocity) and \( \mathbf{z}_t \) the measurement (e.g. a detection or centroid from segmentation).

A generic stochastic motion model is

\[ \mathbf{x}_t = f(\mathbf{x}_{t-1}, \mathbf{u}_t, \mathbf{w}_t), \quad \mathbf{w}_t \sim p_{\mathbf{w}}, \]

and a measurement model is

\[ \mathbf{z}_t = h(\mathbf{x}_t, \mathbf{v}_t), \quad \mathbf{v}_t \sim p_{\mathbf{v}}. \]

Under the standard Markov assumptions, the posterior density obeys the Bayes filter recursion.

Prediction step:

\[ p(\mathbf{x}_t \mid \mathbf{z}_{1:t-1}) = \int p(\mathbf{x}_t \mid \mathbf{x}_{t-1}) \, p(\mathbf{x}_{t-1} \mid \mathbf{z}_{1:t-1}) \, d\mathbf{x}_{t-1}. \]

Update step:

\[ p(\mathbf{x}_t \mid \mathbf{z}_{1:t}) = \dfrac{ p(\mathbf{z}_t \mid \mathbf{x}_t)\, p(\mathbf{x}_t \mid \mathbf{z}_{1:t-1}) }{ \int p(\mathbf{z}_t \mid \mathbf{x}_t)\, p(\mathbf{x}_t \mid \mathbf{z}_{1:t-1})\, d\mathbf{x}_t }. \]

For linear-Gaussian models this recursion is solved analytically by the Kalman filter; for more general models we use particle filters or other approximations. Scene flow can enter these models by:

• defining the process model in terms of rigid body motions inferred from local flow;
• providing additional motion-consistency likelihoods in the measurement model.

A widely used model for tracking a rigid object in the image plane (or on a planar workspace) is the constant-velocity model. Take the state

\[ \mathbf{x}_t = \begin{bmatrix} x_t & y_t & v_{x,t} & v_{y,t} \end{bmatrix}^{\top}, \]

where \( (x_t, y_t) \) is the 2D position of the object center and \( (v_{x,t}, v_{y,t}) \) its velocity. For time step \( \Delta t \) the linear dynamics are

\[ \mathbf{x}_t = \mathbf{F} \mathbf{x}_{t-1} + \mathbf{w}_t, \quad \mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}), \]

with

\[ \mathbf{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}. \]

Observations are often the measured position from detections or segmentation:

\[ \mathbf{z}_t = \begin{bmatrix} z_{x,t} \\ z_{y,t} \end{bmatrix} = \mathbf{H} \mathbf{x}_t + \mathbf{v}_t, \quad \mathbf{v}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}), \]

with

\[ \mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix}. \]

Under these assumptions the posterior distribution \( p(\mathbf{x}_t \mid \mathbf{z}_{1:t}) \) remains Gaussian with mean \( \hat{\mathbf{x}}_{t\mid t} \) and covariance \( \mathbf{P}_{t\mid t} \). The Kalman filter recursion is:

Prediction:

\[ \hat{\mathbf{x}}_{t\mid t-1} = \mathbf{F}\,\hat{\mathbf{x}}_{t-1\mid t-1}, \quad \mathbf{P}_{t\mid t-1} = \mathbf{F}\,\mathbf{P}_{t-1\mid t-1}\,\mathbf{F}^{\top} + \mathbf{Q}. \]

Update:

\[ \mathbf{K}_t = \mathbf{P}_{t\mid t-1} \mathbf{H}^{\top} \left( \mathbf{H}\mathbf{P}_{t\mid t-1}\mathbf{H}^{\top} + \mathbf{R} \right)^{-1}, \]

\[ \hat{\mathbf{x}}_{t\mid t} = \hat{\mathbf{x}}_{t\mid t-1} + \mathbf{K}_t\left( \mathbf{z}_t - \mathbf{H}\hat{\mathbf{x}}_{t\mid t-1} \right), \]

\[ \mathbf{P}_{t\mid t} = \left( \mathbf{I} - \mathbf{K}_t \mathbf{H} \right)\mathbf{P}_{t\mid t-1}. \]

The process noise covariance \( \mathbf{Q} \) is often derived from a continuous-time white-acceleration model. In one dimension, for acceleration variance parameter \( q \),

\[ \mathbf{Q}_{1\mathrm{D}} = q \begin{bmatrix} \dfrac{\Delta t^4}{4} & \dfrac{\Delta t^3}{2} \\ \dfrac{\Delta t^3}{2} & \Delta t^2 \end{bmatrix}, \]

and for two dimensions we use \( \mathbf{Q} = \mathbf{Q}_{1\mathrm{D}} \otimes \mathbf{I}_2 \).

In manipulation scenes there are typically multiple moving objects. A common strategy is to maintain one filter per hypothesized object and solve a data association problem that assigns detections to tracks at each time step.

Let there be \( M \) existing tracks with predicted states \( \hat{\mathbf{x}}_{t\mid t-1}^{(i)} \) and \( N \) detections \( \mathbf{z}_t^{(j)} \). For each pair we compute a cost, e.g. squared Mahalanobis distance

\[ C_{ij} = \left( \mathbf{z}_t^{(j)} - \mathbf{H}\hat{\mathbf{x}}_{t\mid t-1}^{(i)} \right)^{\top} \mathbf{S}_t^{(i)-1} \left( \mathbf{z}_t^{(j)} - \mathbf{H}\hat{\mathbf{x}}_{t\mid t-1}^{(i)} \right), \]

where \( \mathbf{S}_t^{(i)} = \mathbf{H}\mathbf{P}_{t\mid t-1}^{(i)}\mathbf{H}^{\top} + \mathbf{R} \). We then solve an assignment problem:

\[ \min_{\{y_{ij}\}} \sum_{i=1}^M \sum_{j=1}^N C_{ij} y_{ij} \]

\[ \text{subject to } \sum_{j=1}^N y_{ij} \leq 1,\quad \sum_{i=1}^M y_{ij} \leq 1,\quad y_{ij} \in \{0,1\}. \]

This is the classical linear assignment problem, efficiently solved by the Hungarian algorithm. Unassigned tracks may be propagated as occluded; unassigned detections can spawn new tracks.

Scene flow can be used here to:

• cluster moving points into candidate objects, providing initial tracks;
• refine the predicted velocity components of each track, yielding better motion extrapolation and gating.

flowchart TD
  S["Previous object states and covariances"] --> PRED["Predict with motion model"]
  PRED --> DET["Input detections and motion clusters"]
  DET --> ASSOC["Build cost matrix and solve assignment"]
  ASSOC --> UPD["Update assigned tracks via filters"]
  UPD --> MAN["Manage births, occlusions, deletions"]
  MAN --> S
        

In Python, common libraries for 3D perception and tracking in robotics include open3d (point clouds, registration), opencv-python (2D vision), and numpy for linear algebra. Below is a minimal example of a constant-velocity Kalman filter in 2D, followed by a sketch of integrating Open3D-based scene flow.

import numpy as np

class KalmanFilterCV2D:
    """
    Constant-velocity Kalman filter in 2D.
    State x = [px, py, vx, vy]^T
    Measurement z = [px, py]^T
    """
    def __init__(self, dt=0.1, process_var=1.0, meas_var=4.0):
        self.dt = dt

        # State transition and measurement matrices
        self.F = np.array([
            [1.0, 0.0, dt,  0.0],
            [0.0, 1.0, 0.0, dt ],
            [0.0, 0.0, 1.0, 0.0],
            [0.0, 0.0, 0.0, 1.0]
        ], dtype=float)

        self.H = np.array([
            [1.0, 0.0, 0.0, 0.0],
            [0.0, 1.0, 0.0, 0.0]
        ], dtype=float)

        # Process and measurement noise
        q = process_var
        dt2 = dt * dt
        dt3 = dt2 * dt
        dt4 = dt2 * dt2

        Q1d = np.array([
            [0.25 * dt4, 0.5 * dt3],
            [0.5 * dt3,  dt2]
        ])
        self.Q = q * np.block([
            [Q1d,          np.zeros((2, 2))],
            [np.zeros((2, 2)), Q1d]
        ])

        self.R = meas_var * np.eye(2)

        # Initialize state and covariance
        self.x = np.zeros((4, 1))
        self.P = np.eye(4) * 1e3

    def predict(self):
        self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + self.Q

    def update(self, z):
        """z is a length-2 iterable [zx, zy]."""
        z = np.asarray(z, dtype=float).reshape(2, 1)
        y = z - (self.H @ self.x)
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        self.x = self.x + (K @ y)
        I = np.eye(self.P.shape[0])
        self.P = (I - K @ self.H) @ self.P

    def position(self):
        return self.x[0, 0], self.x[1, 0]


# Example usage with hypothetical detections (e.g., from segmentation)
kf = KalmanFilterCV2D(dt=0.1)
detections = [(0.0, 0.0), (0.9, 0.1), (2.1, 0.2), (3.0, 0.1)]

for z in detections:
    kf.predict()
    kf.update(z)
    print("Estimated position:", kf.position())
      

A simple way to approximate scene flow in Python using Open3D is to run local rigid registrations between successive point clouds:

import open3d as o3d

# Assume pcd_t and pcd_tp1 are open3d.geometry.PointCloud objects
# obtained from two RGB-D frames via open3d.geometry.RGBDImage.create_from_...

threshold = 0.02  # correspondence distance
trans_init = np.eye(4)

reg = o3d.pipelines.registration.registration_icp(
    pcd_t, pcd_tp1, threshold, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPoint()
)

T = reg.transformation  # 4x4 rigid transform from time t to t+1

# Scene flow approximation: v(x) ≈ (T * x - x) / dt for each point x
points = np.asarray(pcd_t.points)
ones = np.ones((points.shape[0], 1))
hom = np.hstack((points, ones))
moved = (T @ hom.T).T[:, :3]
flow = (moved - points) / kf.dt  # approximate 3D velocity per point
      

More sophisticated methods use non-rigid registration or explicit variational formulations over the point cloud graph, but the rigid approximation is a useful building block for manipulation scenarios where many objects move rigidly.

In C++, scene flow and tracking in robotics are often implemented using PCL (Point Cloud Library), Eigen for linear algebra, and OpenCV for 2D perception. Below is a minimal constant-velocity Kalman filter implemented with Eigen.

#include <Eigen/Dense>

struct KalmanFilterCV2D {
    double dt;
    Eigen::Matrix4d F;
    Eigen::Matrix<double, 2, 4> H;
    Eigen::Matrix4d Q;
    Eigen::Matrix2d R;
    Eigen::Vector4d x;
    Eigen::Matrix4d P;

    KalmanFilterCV2D(double dt_, double process_var, double meas_var)
        : dt(dt_)
    {
        F.setIdentity();
        F(0, 2) = dt;
        F(1, 3) = dt;

        H.setZero();
        H(0, 0) = 1.0;
        H(1, 1) = 1.0;

        double q = process_var;
        double dt2 = dt * dt;
        double dt3 = dt2 * dt;
        double dt4 = dt2 * dt2;

        Eigen::Matrix2d Q1d;
        Q1d << 0.25 * dt4, 0.5 * dt3,
                0.5 * dt3,  dt2;

        Q.setZero();
        Q.block<2, 2>(0, 0) = Q1d;
        Q.block<2, 2>(2, 2) = Q1d;
        Q *= q;

        R = meas_var * Eigen::Matrix2d::Identity();

        x.setZero();
        P = Eigen::Matrix4d::Identity() * 1e3;
    }

    void predict() {
        x = F * x;
        P = F * P * F.transpose() + Q;
    }

    void update(const Eigen::Vector2d& z) {
        Eigen::Vector2d y = z - H * x;
        Eigen::Matrix2d S = H * P * H.transpose() + R;
        Eigen::Matrix<double, 4, 2> K = P * H.transpose() * S.inverse();
        x = x + K * y;
        Eigen::Matrix4d I = Eigen::Matrix4d::Identity();
        P = (I - K * H) * P;
    }

    Eigen::Vector2d position() const {
        return x.segment<2>(0);
    }
};
      

Scene flow in C++ can be implemented by registering PCL point clouds (e.g., using pcl::IterativeClosestPoint) and computing per-point velocities as in the Python sketch.

In Java, OpenCV provides bindings for optical flow and feature tracking. Combining these with a small matrix library or manual linear algebra is sufficient for implementing Kalman filters and 2D tracking suitable for manipulation tasks.

public class KalmanFilterCV2D {
    private final double dt;
    private final double[][] F = new double[4][4];
    private final double[][] H = new double[2][4];
    private final double[][] Q = new double[4][4];
    private final double[][] R = new double[2][2];
    private final double[] x = new double[4];
    private final double[][] P = new double[4][4];

    public KalmanFilterCV2D(double dt, double processVar, double measVar) {
        this.dt = dt;

        // Initialize F as identity with dt in velocity terms
        for (int i = 0; i < 4; ++i) {
            F[i][i] = 1.0;
        }
        F[0][2] = dt;
        F[1][3] = dt;

        // Measurement matrix
        H[0][0] = 1.0;
        H[1][1] = 1.0;

        double dt2 = dt * dt;
        double dt3 = dt2 * dt;
        double dt4 = dt2 * dt2;
        double q = processVar;

        double[][] Q1d = {
            {0.25 * dt4, 0.5 * dt3},
            {0.5 * dt3,  dt2}
        };

        zeroMatrix(Q);
        addBlock(Q, Q1d, 0, 0, q);
        addBlock(Q, Q1d, 2, 2, q);

        R[0][0] = measVar;
        R[1][1] = measVar;

        identityMatrix(P, 1e3);
    }

    // Helper methods zeroMatrix, identityMatrix, addBlock, matMul, matAdd, matSub,
    // matInv2x2 would be implemented with straightforward loops.

    public void predict() {
        double[] xNew = matMul(F, x);
        double[][] PNew = matAdd(matMul(F, matMul(P, transpose(F))), Q);
        System.arraycopy(xNew, 0, x, 0, 4);
        copyMatrix(PNew, P);
    }

    public void update(double[] z) {
        double[] zPred = matMul(H, x);
        double[] y = matSub(z, zPred);
        double[][] S = matAdd(matMul(H, matMul(P, transpose(H))), R);
        double[][] SInv = matInv2x2(S);
        double[][] K = matMul(matMul(P, transpose(H)), SInv);

        double[] Ky = matMul(K, y);
        for (int i = 0; i < 4; ++i) {
            x[i] += Ky[i];
        }

        double[][] I = identityMatrix(4, 1.0);
        double[][] KH = matMul(K, H);
        double[][] PNew = matMul(matSub(I, KH), P);
        copyMatrix(PNew, P);
    }

    public double[] position() {
        return new double[] {x[0], x[1]};
    }
}
      

OpenCV's Java API (e.g., Video.calcOpticalFlowPyrLK) can provide 2D motion cues analogous to scene flow for planar tasks.

11.1 MATLAB / Simulink

MATLAB provides vision.PointTracker and the Computer Vision Toolbox for optical flow, as well as built-in support for Kalman filtering. A simple script for constant-velocity tracking is:

dt = 0.1;
F = [1 0 dt 0;
     0 1 0  dt;
     0 0 1  0;
     0 0 0  1];

H = [1 0 0 0;
     0 1 0 0];

q = 1.0;
dt2 = dt^2;
dt3 = dt^3;
dt4 = dt^4;
Q1d = [0.25 * dt4, 0.5 * dt3;
       0.5 * dt3,  dt2];
Q = q * blkdiag(Q1d, Q1d);

R = 4.0 * eye(2);

x = zeros(4, 1);
P = 1e3 * eye(4);

measurements = [0 0;
                0.9 0.1;
                2.1 0.2;
                3.0 0.1];

for k = 1:size(measurements, 1)
    % Prediction
    x = F * x;
    P = F * P * F.' + Q;

    % Update
    z = measurements(k, :).';
    y = z - H * x;
    S = H * P * H.' + R;
    K = P * H.' / S;
    x = x + K * y;
    P = (eye(4) - K * H) * P;

    fprintf("Estimated position: %.3f, %.3f\n", x(1), x(2));
end
      

In Simulink, the same model can be implemented by connecting blocks: matrix gains for \( \mathbf{F} \) and \( \mathbf{H} \), a Kalman Filter block (or equivalent custom subsystem), and input signals from a perception pipeline block that delivers detections.

11.2 Wolfram Mathematica

Mathematica can express the Kalman recursion in a functional style, with symbolic or numeric matrices:

dt = 0.1;
F = { {1, 0, dt, 0},
     {0, 1, 0, dt},
     {0, 0, 1, 0},
     {0, 0, 0, 1} };
H = { {1, 0, 0, 0},
     {0, 1, 0, 0} };

q = 1.0;
dt2 = dt^2; dt3 = dt^3; dt4 = dt^4;
Q1d = { {0.25 dt4, 0.5 dt3},
       {0.5 dt3,  dt2} };
Q = q KroneckerProduct[IdentityMatrix[2], Q1d];
R = 4.0 IdentityMatrix[2];

x0 = {0., 0., 0., 0.};
P0 = 10.^3 IdentityMatrix[4];

measurements = { {0., 0.},
                {0.9, 0.1},
                {2.1, 0.2},
                {3.0, 0.1} };

kalmanStep[{x_, P_}, z_] := Module[
  {xPred, PPred, y, S, K, xNew, PNew},
  xPred = F.x;
  PPred = F.P.Transpose[F] + Q;
  y = z - H.xPred;
  S = H.PPred.Transpose[H] + R;
  K = PPred.Transpose[H].Inverse[S];
  xNew = xPred + K.y;
  PNew = (IdentityMatrix[4] - K.H).PPred;
  {xNew, PNew}
];

result = Rest@FoldList[kalmanStep, {x0, P0}, measurements];
positions = result[[All, 1, {1, 2}]];
positions
      

Mathematica's symbolic capabilities can also be used to derive and verify properties of the Kalman filter, such as the steady-state error covariance for a given choice of \( \mathbf{Q} \) and \( \mathbf{R} \).

Problem 1 (Projection Jacobian and Optical Flow Constraint): Starting from the pinhole projection \( x = f_x X/Z + c_x \), \( y = f_y Y/Z + c_y \), derive the Jacobian \( \dfrac{\partial \pi}{\partial \mathbf{X}} \) and show that the optical flow \( \mathbf{u} \) is given (to first order) by

\[ \mathbf{u} = \begin{bmatrix} u_x \\ u_y \end{bmatrix} = \dfrac{\partial \pi}{\partial \mathbf{X}} \mathbf{v} \,\Delta t, \]

where \( \mathbf{v} = (v_X, v_Y, v_Z)^{\top} \) is the 3D velocity.

Solution: By direct differentiation, \( x = f_x X/Z + c_x \) gives \( \partial x/\partial X = f_x/Z \), \( \partial x/\partial Y = 0 \), \( \partial x/\partial Z = -f_x X/Z^2 \). Similarly, \( y = f_y Y/Z + c_y \) yields \( \partial y/\partial Y = f_y/Z \) and \( \partial y/\partial Z = -f_y Y/Z^2 \). Thus

\[ \dfrac{\partial \pi}{\partial \mathbf{X}} = \begin{bmatrix} \dfrac{f_x}{Z} & 0 & -\dfrac{f_x X}{Z^2} \\ 0 & \dfrac{f_y}{Z} & -\dfrac{f_y Y}{Z^2} \end{bmatrix}. \]

Under small motion, \( \mathbf{X}(t+\Delta t) \approx \mathbf{X}(t) + \mathbf{v}\Delta t \), and linearizing \( \pi \) gives \( \mathbf{x}(t+\Delta t) \approx \mathbf{x}(t) + (\partial \pi/\partial \mathbf{X})\mathbf{v}\Delta t \), so \( \mathbf{u} = (\partial \pi/\partial \mathbf{X})\mathbf{v}\Delta t \).

Problem 2 (Euler–Lagrange Equation for Scalar Velocity): Consider the scalar energy

\[ E(v) = \int_{\Omega} \left( a(\mathbf{p}) + v(\mathbf{p})\,\Delta t - b(\mathbf{p}) \right)^2 + \lambda \left\|\nabla v(\mathbf{p})\right\|^2 \, d\mathbf{p}. \]

Derive the Euler–Lagrange equation satisfied by the minimizer \( v \).

Solution: Using calculus of variations, the first variation in direction \( \delta v \) is

\[ \delta E = \int_{\Omega} 2\left(a + v\,\Delta t - b\right)\Delta t\,\delta v + 2\lambda \nabla v \cdot \nabla (\delta v) \, d\mathbf{p}. \]

Integrating the second term by parts and assuming natural boundary conditions gives

\[ \delta E = \int_{\Omega} \left[ 2\Delta t\left(a + v\,\Delta t - b\right) - 2\lambda \Delta v \right]\delta v \, d\mathbf{p}. \]

Setting \( \delta E = 0 \) for all \( \delta v \) yields

\[ 2\Delta t\left(a + v\,\Delta t - b\right) - 2\lambda \Delta v = 0, \]

i.e. \( (\Delta t^2)v - \lambda \Delta v = \Delta t(b - a) \), which matches the equation used in the variational derivation.

Problem 3 (Gaussianity of the Kalman Posterior): Show that in the linear-Gaussian model of Section 6, if \( p(\mathbf{x}_{t-1} \mid \mathbf{z}_{1:t-1}) \) is Gaussian, then both \( p(\mathbf{x}_t \mid \mathbf{z}_{1:t-1}) \) and \( p(\mathbf{x}_t \mid \mathbf{z}_{1:t}) \) are also Gaussian.

Solution: If \( \mathbf{x}_{t-1} \mid \mathbf{z}_{1:t-1} \sim \mathcal{N}(\hat{\mathbf{x}}_{t-1\mid t-1}, \mathbf{P}_{t-1\mid t-1}) \), then the linear transformation \( \mathbf{x}_t = \mathbf{F}\mathbf{x}_{t-1} + \mathbf{w}_t \), with independent Gaussian noise \( \mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}) \), implies \( \mathbf{x}_t \mid \mathbf{z}_{1:t-1} \) is Gaussian with mean \( \mathbf{F}\hat{\mathbf{x}}_{t-1\mid t-1} \) and covariance \( \mathbf{F}\mathbf{P}_{t-1\mid t-1}\mathbf{F}^{\top} + \mathbf{Q} \). The likelihood \( p(\mathbf{z}_t \mid \mathbf{x}_t) \) is Gaussian with linear mean \( \mathbf{H}\mathbf{x}_t \). The product of a Gaussian prior and Gaussian likelihood is proportional to a Gaussian posterior (by completing the square), so \( p(\mathbf{x}_t \mid \mathbf{z}_{1:t}) \) is also Gaussian.

Problem 4 (Assignment Problem Formulation): Consider two tracks and three detections. Explain why the data association problem can be written as a linear assignment problem over a bipartite graph, and write down the constraints on the binary assignment variables.

Solution: We view tracks and detections as nodes in the two partitions of a bipartite graph; potential assignments are edges with costs \( C_{ij} \). The goal is to select a subset of edges minimizing total cost such that no track or detection participates in more than one edge. Binary variables \( y_{ij} \) indicate whether track \( i \) is matched to detection \( j \). The constraints are: \( \sum_j y_{ij} \leq 1 \) for each track and \( \sum_i y_{ij} \leq 1 \) for each detection, with \( y_{ij} \in \{0,1\} \). This is exactly the form of the linear assignment problem.

Problem 5 (Using Scene Flow for Track Initialization): Suppose you have a dense scene flow estimate on a point cloud. Propose a principled method for initializing object tracks from this flow without any pre-trained detector, and explain what assumptions your method relies on.

Solution: One method is:

1. Build a graph whose nodes are 3D points and edges connect spatial neighbors.
2. Define an affinity based on motion similarity, e.g. \( \exp(-\left\|\mathbf{v}_i - \mathbf{v}_j\right\|^2/\sigma^2) \).
3. Run clustering (e.g. spectral clustering) over this graph to group points with approximately rigid, coherent motion.
4. For each cluster, compute its centroid and average velocity to define an initial state \( \mathbf{x}_0^{(k)} \), then start a Kalman filter for that cluster.

This method assumes that individual objects move approximately rigidly and that two different objects have sufficiently distinct motion so that the clustering is stable. It also assumes that the scene flow is accurate enough to separate motions at object boundaries.

We introduced scene flow as a dense 3D motion field derived from RGB-D or stereo observations, and derived both variational and graph-based formulations that yield large sparse linear systems. We then connected these motion estimates to probabilistic object tracking via Bayesian filtering, focusing on the linear-Gaussian case where the Kalman filter arises. Finally, we formulated multi-object tracking as a data-association problem and illustrated implementations in Python, C++, Java, MATLAB, Simulink, and Mathematica, preparing the ground for more advanced perception modules in later lessons.