Algorithm Architecture

The Interactive Multiple Model (IMM) algorithm enhances tracking robustness by dynamically switching among multiple mosion models—such as Constant Velocity (CV), Constant Turn (CT), Constant Acceleration (CA), and potentially others—to accommodate target maneuvers. Each model is processed using a Kalman-type filter (e.g., EKF or CKF). The IMM framework consists of four key steps:

1. Model Mixing: Combine prior state estimates using Markovian transition probabilities.
2. Model-Specific Filtering: Perform prediction and update independently for each model.
3. Likelihood-Based Probability Update: Compute model likelihoods from innovation residuals to update model probabilities.
4. State Fusion: Generate a global estimate as a weighted sum of individual model outputs.

MATLAB Implementation

System Configuraton

dt = 1;                    % Sampling interval (seconds)
numSteps = 200;            % Total time steps
timeVec = 0:dt:numSteps;

% Markov transition matrix for 4 models: CV, CT, CA, CS
transProb = [0.85, 0.05, 0.05, 0.05;
             0.05, 0.80, 0.05, 0.10;
             0.05, 0.05, 0.85, 0.05;
             0.05, 0.10, 0.05, 0.80];

% Initial model probabilities
modelProbs = [0.90; 0.01; 0.08; 0.01];

% Process and measurement noise
Q_cv = 0.1 * diag([0.1, 0.1, 0.1, 0.1]);
Q_ca = 0.1 * diag([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]);
R_meas = 0.001 * eye(2);

Ground Truth Trajectory Generation

trueTraj = zeros(numSteps+1, 2);
initVel = [2; 1];
turnRate = deg2rad(3);
accelVec = [0.1; 0.05];

for k = 1:length(timeVec)
    t = timeVec(k);
    if t <= 30
        trueTraj(k,:) = (initVel' * t);
    elseif t <= 60
        theta = turnRate * (t - 30);
        trueTraj(k,:) = [50*sin(theta), 50*(1 - cos(theta))];
    elseif t <= 90
        basePos = trueTraj(61,:);
        trueTraj(k,:) = basePos + initVel'*(t-60) + 0.5*accelVec'*(t-60).^2;
    else
        theta = -turnRate * (t - 90);
        trueTraj(k,:) = [30*sin(theta), 30*(1 - cos(theta))];
    end
end

measurements = trueTraj + sqrt(R_meas) * randn(size(trueTraj));

Filter Initialization

% State vectors
state_cv = [0; 0; 0; 0];          % [x, y, vx, vy]
state_ca = [0; 0; 0; 0; 0; 0];    % [x, y, vx, vy, ax, ay]
state_ct = [0; 0; 0; 0; 0.1];     % [x, y, vx, vy, ω]

% Error covariance matrices
P_cv = diag([10, 10, 1, 1]);
P_ca = diag([10, 10, 1, 1, 0.1, 0.1]);
P_ct = diag([10, 10, 1, 1, 0.01]);

% Store in cell arrays for iteration
states = {state_cv, state_ct, state_ca, state_cv};  % Example: reuse CV as fourth model
covs = {P_cv, P_ct, P_ca, P_cv};
processNoise = {Q_cv, Q_cv, Q_ca, Q_cv};  % Simplified assumption for CT/CS

IMM Main Loop

estTraj = zeros(4, numSteps);
estCovHistory = zeros(4, 4, numSteps);

% Predefine measurement matrix
H = [1 0 0 0; 0 1 0 0];  % For CV/CT (4D); CA uses first 4 rows similarly

for k = 1:numSteps
    % Step 1: Model mixing
    mu_bar = transProb * modelProbs;
    
    % Normalize to avoid numerical drift
    mu_bar = mu_bar / sum(mu_bar);
    
    mixedStates = zeros(4, 4);
    mixedCovs = zeros(4, 4, 4);
    
    for i = 1:4
        denom = sum(transProb(i,:) .* modelProbs);
        if denom == 0, continue; end
        weights = (transProb(i,:) .* modelProbs) / denom;
        
        % Mix states (assuming all states projected to 4D for fusion)
        x_mix = zeros(4,1);
        P_mix = zeros(4,4);
        for j = 1:4
            xj = states{j}(1:4);
            x_mix = x_mix + weights(j) * xj;
            diff = xj - x_mix;
            P_mix = P_mix + weights(j) * (covs{j}(1:4,1:4) + diff*diff');
        end
        mixedStates(:,i) = x_mix;
        mixedCovs(:,:,i) = P_mix;
    end
    
    % Step 2: Model-conditioned filtering
    updatedStates = cell(1,4);
    updatedCovs = cell(1,4);
    likelihoods = zeros(4,1);
    
    for m = 1:4
        % Prediction
        Fm = getTransitionMatrix(m, dt);  % User-defined function
        x_pred = Fm * mixedStates(:,m);
        P_pred = Fm * mixedCovs(:,:,m) * Fm' + processNoise{m}(1:4,1:4);
        
        % Update
        z = measurements(k,:)';
        innov = z - H * x_pred;
        S = H * P_pred * H' + R_meas;
        K = P_pred * H' / S;
        x_upd = x_pred + K * innov;
        P_upd = (eye(4) - K*H) * P_pred;
        
        updatedStates{m} = x_upd;
        updatedCovs{m} = P_upd;
        
        % Likelihood
        likelihoods(m) = exp(-0.5 * innov' / S * innov);
    end
    
    % Step 3: Update model probabilities
    modelProbs = mu_bar .* likelihoods;
    modelProbs = modelProbs / sum(modelProbs);
    
    % Step 4: Global estimate
    x_global = zeros(4,1);
    P_global = zeros(4,4);
    for m = 1:4
        x_global = x_global + modelProbs(m) * updatedStates{m};
    end
    for m = 1:4
        dx = updatedStates{m} - x_global;
        P_global = P_global + modelProbs(m) * (updatedCovs{m} + dx*dx');
    end
    
    estTraj(:,k) = x_global;
    estCovHistory(:,:,k) = P_global;
    
    % Update states for next iteration
    states = updatedStates;
    covs = updatedCovs;
end

Helper Functions

function F = getTransitionMatrix(modelIdx, dt)
    switch modelIdx
        case 1  % CV
            F = [1 0 dt 0;
                 0 1 0 dt;
                 0 0 1 0;
                 0 0 0 1];
        case 2  % CT (simplified linearized version)
            omega = 0.1;  % nominal turn rate
            F = [1 0 sin(omega*dt)/omega 0;
                 0 1 0 (1-cos(omega*dt))/omega;
                 0 0 cos(omega*dt) 0;
                 0 0 -sin(omega*dt) 1];
        case 3  % CA
            F = [1 0 dt 0 0.5*dt^2 0;
                 0 1 0 dt 0 0.5*dt^2;
                 0 0 1 0 dt 0;
                 0 0 0 1 0 dt;
                 0 0 0 0 1 0;
                 0 0 0 0 0 1];
            F = F(1:4, :);  % Project to 4D for consistency
        otherwise
            F = eye(4);
    end
end

Visualization

figure;
plot(trueTraj(:,1), trueTraj(:,2), 'r--', 'LineWidth', 2); hold on;
plot(measurements(:,1), measurements(:,2), 'b.', 'MarkerSize', 8);
plot(estTraj(1,:), estTraj(2,:), 'g-', 'LineWidth', 1.5);
legend('True Trajectory', 'Measurements', 'IMM Estimate');
xlabel('X (m)'); ylabel('Y (m)');
title('IMM Tracking Performance');

figure;
plot(1:numSteps, squeeze(estCovHistory(1,1,:)), 'k-', ...
     1:numSteps, squeeze(estCovHistory(2,2,:)), 'c--');
legend('Position X Variance', 'Position Y Variance');
xlabel('Time Step'); ylabel('Variance (m^2)');
title('Estimation Uncertainty Over Time');

Optimization Approaches

• Adaptive Model Set: Dynamically include or exclude models based on maneuver detection (e.g., add deceleration model during braking events).
• Advanced Filtering: Replace EKF with Cubature Kalman Filter (CKF) or use square-root formulations for improved numerical stability in nonlinear scenarios.
• Computational Efficiency: Parallelize per-model filtering using parfor, or exploit sparsity in covariance matrices to reduce matrix inversion costs.

Application Domains

• Aerial surveillance of evasive drones
• Tracking vehicles during emergency lane changes in autonomous driving
• Multi-object tracking in crowded urban environments with occlusions