Kalman Filter and Optical Flow (Lucas-Kanade, Farneback)

This document provides a comprehensive overview of the Kalman Filter and two prominent optical flow algorithms: Lucas-Kanade and Farneback. It covers their fundamental principles, mathematical formulations, use cases, and practical implementation examples.

Kalman Filter

The Kalman Filter is a recursive algorithm designed to estimate the state of a linear dynamic system from a series of incomplete or noisy measurements. It excels at predicting and updating the system's state, making it invaluable in fields like control systems, robotics, sensor fusion, and computer vision for tracking moving objects.

How the Kalman Filter Works

The Kalman Filter operates in two primary steps:

1. Prediction Step: This step forecasts the system's next state based on its current state and a predefined dynamic model.
2. Update Step: This step refines the predicted state by incorporating actual measurement data, correcting any deviations caused by noise or model inaccuracies.

Kalman Filter Equations

The Kalman Filter is mathematically defined by the following equations:

Prediction:

\begin{align*} \label{eq:1}\hat{x}{k|k-1} &= A \cdot \hat{x}{k-1|k-1} + B \cdot u_k \ P_{k|k-1} &= A \cdot P_{k-1|k-1} \cdot A^T + Q \end{align*}

Update:

\begin{align*} K_k &= P_{k|k-1} \cdot H^T \cdot (H \cdot P_{k|k-1} \cdot H^T + R)^{-1} \ \hat{x}{k|k} &= \hat{x}{k|k-1} + K_k \cdot (z_k - H \cdot \hat{x}{k|k-1}) \ P{k|k} &= (I - K_k \cdot H) \cdot P_{k|k-1} \end{align*}

Where:

• $\hat{x}$: State estimate
• $P$: Error covariance matrix (represents uncertainty in the state estimate)
• $A$: State transition matrix (describes how the state evolves over time without external input)
• $B$: Control matrix (relates control input to state change)
• $u_k$: Control input at time step $k$
• $Q$: Process noise covariance (accounts for uncertainty in the system model)
• $H$: Measurement matrix (relates the state to the measurement)
• $R$: Measurement noise covariance (accounts for uncertainty in the measurements)
• $z_k$: Actual measurement at time step $k$
• $K_k$: Kalman gain (determines how much to trust the measurement versus the prediction)
• $I$: Identity matrix

Kalman Filter Use Cases

• Tracking moving vehicles or individuals.
• Smoothing noisy GPS data for more accurate location.
• Enabling autonomous drone navigation.
• Predicting future object trajectories.

Example: 1D Kalman Filter for Position Estimation

This Python example demonstrates how a Kalman Filter can estimate the true position of an object from noisy sensor measurements.

import numpy as np
import matplotlib.pyplot as plt
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise

# Create Kalman Filter object
# dim_x = 2 (position, velocity), dim_z = 1 (position measurement)
kf = KalmanFilter(dim_x=2, dim_z=1)

# Initial state [position, velocity]
kf.x = np.array([[0.],    # initial position
                 [0.]])   # initial velocity

# State transition matrix (assuming constant velocity model)
# x_k = x_{k-1} + v_{k-1} * dt
# v_k = v_{k-1}
kf.F = np.array([[1., 1.],
                 [0., 1.]])

# Measurement function (we only observe the position)
kf.H = np.array([[1., 0.]])

# Covariance matrix - initial uncertainty
kf.P *= 1000.  # High initial uncertainty

# Measurement noise
kf.R = 5  # Variance of the measurement noise

# Process noise
# Generates a discrete white noise process for position and velocity
kf.Q = Q_discrete_white_noise(dim=2, dt=1, var=0.1)

# Simulate measurements (noisy observations of position)
np.random.seed(42)
true_position = [i for i in range(50)]
measurements = [x + np.random.normal(0, 5) for x in true_position]

# Store results
filtered_positions = []

# Process each measurement
for z in measurements:
    kf.predict()          # Predict the next state
    kf.update(z)          # Update the state with the new measurement
    filtered_positions.append(kf.x[0, 0]) # Store the estimated position

# Plotting the results
plt.figure(figsize=(10, 6))
plt.plot(measurements, label='Noisy Measurements')
plt.plot(true_position, label='True Position')
plt.plot(filtered_positions, label='Kalman Filter Estimate')
plt.legend()
plt.xlabel('Time')
plt.ylabel('Position')
plt.title('1D Kalman Filter Example for Position Estimation')
plt.grid(True)
plt.show()

Optical Flow

Optical flow refers to the pattern of apparent motion of objects, surfaces, or edges in a visual scene. It's caused by the relative motion between an observer and the scene. Optical flow algorithms estimate motion vectors for pixels across two consecutive video frames.

1. Lucas-Kanade Optical Flow

Description:

Lucas-Kanade is a sparse optical flow method. It focuses on tracking a limited set of distinctive feature points (corners) rather than every pixel. The core assumption is that the brightness of a point remains constant between consecutive frames and that the motion within a small neighborhood is uniform. It solves a system of linear equations to determine the displacement vector.

Lucas-Kanade Formula:

The fundamental assumption is the image brightness constancy: $I(x, y, t) = I(x + dx, y + dy, t + dt)$

This is approximated by: $I(x, y, t) \approx I(x + dx, y + dy, t + dt)$

Using Taylor series expansion, we get: $I(x, y, t) \approx I(x, y, t) + I_x \cdot dx + I_y \cdot dy + I_t \cdot dt$

Ignoring higher-order terms and rearranging, we get: $I_x \cdot dx + I_y \cdot dy + I_t \cdot dt = 0$

Or, $I_x \cdot v_x + I_y \cdot v_y + I_t = 0$, where $v_x = dx/dt$ and $v_y = dy/dt$.

For a small neighborhood around a feature point, this is typically formulated as: $A \cdot v = b$

Where:

• $A = \begin{bmatrix} \sum I_x^2 & \sum I_x I_y \ \sum I_x I_y & \sum I_y^2 \end{bmatrix}$ contains spatial gradients ($I_x$, $I_y$).
• $b = \begin{bmatrix} -\sum I_x I_t \ -\sum I_y I_t \end{bmatrix}$ contains temporal gradients ($I_t$).
• $v = \begin{bmatrix} v_x \ v_y \end{bmatrix}^T$ is the optical flow vector (pixel displacement).

Lucas-Kanade in OpenCV (Python):

# prev_gray: previous grayscale frame
# next_gray: current grayscale frame
# p0: points to track in prev_gray
p1, st, err = cv2.calcOpticalFlowPyrLK(prev_gray, next_gray, p0, None)

• p1: The new positions of the tracked points in next_gray.
• st: Status array; 1 for good points, 0 for points that were lost.
• err: Error for each good point.

Use Cases:

• Object tracking (e.g., tracking a ball or a person).
• Hand gesture recognition.
• Video stabilization.

2. Farneback Optical Flow

Description:

Farneback is a dense optical flow algorithm. Unlike sparse methods, it computes the motion vector for every pixel in the image. It achieves this by approximating the image neighborhood using a polynomial expansion and then calculating the displacement field based on these polynomial coefficients. This makes it suitable for estimating the motion of the entire image.

Farneback Key Concepts:

• Polynomial Approximation: Models each pixel neighborhood with a quadratic polynomial.
• Displacement Fields: Computes the optical flow vector for each pixel based on the differences in polynomial coefficients between frames.

Farneback in OpenCV (Python):

# prev_gray: previous grayscale frame
# next_gray: current grayscale frame
flow = cv2.calcOpticalFlowFarneback(prev_gray, next_gray,
                                    None,          # output flow
                                    pyr_scale=0.5, # pyramid scale factor
                                    levels=3,      # number of pyramid levels
                                    winsize=15,    # averaging window size
                                    iterations=3,  # number of iterations
                                    poly_n=5,      # size of pixel neighborhood
                                    poly_sigma=1.2, # standard deviation of Gaussian
                                    flags=0)       # operation flags

• The flow output is a 2-channel array where flow[y, x, 0] is the optical flow in the x-direction and flow[y, x, 1] is the optical flow in the y-direction for pixel (x, y).

Use Cases:

• Dense motion field estimation for analysis.
• Action recognition in videos.
• Scene flow estimation for autonomous driving.

Comparison Table

Optical Flow Example in Python (Lucas-Kanade)

This example demonstrates tracking feature points using the Lucas-Kanade method in OpenCV.

import cv2
import numpy as np

# Load video or capture from webcam
# Replace 'your_video.mp4' with the path to your video file, or use 0 for webcam
cap = cv2.VideoCapture('your_video.mp4')

# Parameters for Shi-Tomasi corner detection (to find good features to track)
feature_params = dict(maxCorners=100,
                      qualityLevel=0.3,
                      minDistance=7,
                      blockSize=7)

# Parameters for Lucas-Kanade optical flow
lk_params = dict(winSize=(15, 15),
                 maxLevel=2,
                 criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))

# Read the first frame and convert it to grayscale
ret, old_frame = cap.read()
old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)

# Find the good features to track in the first frame
p0 = cv2.goodFeaturesToTrack(old_gray, mask=None, **feature_params)

# Create a mask image for drawing the optical flow tracks
mask = np.zeros_like(old_frame)

while True:
    # Read the next frame
    ret, frame = cap.read()
    if not ret:
        break

    # Convert the current frame to grayscale
    frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # Calculate Optical Flow using Lucas-Kanade
    # p0: points from previous frame
    # frame_gray: current frame
    # old_gray: previous frame
    p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, frame_gray, p0, None, **lk_params)

    # Select good points (where tracking was successful)
    if p1 is not None:
        good_new = p1[st == 1]  # New positions of tracked points
        good_old = p0[st == 1]  # Old positions of tracked points

        # Draw the tracks
        for i, (new, old) in enumerate(zip(good_new, good_old)):
            a, b = new.ravel()  # New point coordinates
            c, d = old.ravel()  # Old point coordinates
            # Draw line from old point to new point
            mask = cv2.line(mask, (int(a), int(b)), (int(c), int(d)), (0, 255, 0), 2)
            # Draw circle at the new point
            frame = cv2.circle(frame, (int(a), int(b)), 5, (0, 0, 255), -1)

        # Combine the frame and the mask to show the tracks
        img = cv2.add(frame, mask)
        cv2.imshow('Optical Flow', img)

        # Update the previous frame and points for the next iteration
        old_gray = frame_gray.copy()
        p0 = good_new.reshape(-1, 1, 2) # Update points to track

    # Break the loop if 'q' is pressed
    if cv2.waitKey(30) & 0xFF == ord('q'):
        break

# Release the video capture object and close all windows
cap.release()
cv2.destroyAllWindows()

SEO Keywords

• Kalman filter explained
• Kalman filter equations and use cases
• Optical flow computer vision
• Lucas-Kanade optical flow method
• Farneback dense optical flow
• Kalman filter for object tracking
• Optical flow vs Kalman filter
• OpenCV optical flow tutorial
• Kalman filter applications robotics
• Motion estimation techniques in CV

Interview Questions

• What is a Kalman Filter and how does it work?
• Can you explain the prediction and update steps of the Kalman Filter?
• What are the key matrices involved in the Kalman Filter equations?
• How is the Kalman Gain calculated and what is its significance?
• What are the main differences between Lucas-Kanade and Farneback Optical Flow methods?
• How does the Lucas-Kanade method estimate optical flow?
• What applications are best suited for Farneback Optical Flow?
• How do Kalman Filters and Optical Flow complement each other in tracking systems?
• Can you describe an example use case of a Kalman Filter in autonomous navigation?
• How is Optical Flow implemented using OpenCV in Python?