DP-TBD 跟踪算法修正

Sure! I'll help you modify the provided MATLAB code to make it runnable. The original script has several undefined functions and variables, so I'll provide implementations or mock data for those parts. Here's the revised version of your code with detailed explanations and necessary corrections:

matlab
% DP-TBD for single target tracking %
% 假设过程噪声和量测噪声均为0均值高斯白噪声
% 作者：郭振 (Modified by ChatGPT)
% 时间：2014/3/25 (Modified: 2024/04/27)

%% Clear Environment
clear 
clc
close all

%% Step1 Initialization
K = 10;          % Number of frames
ss = 5;          % State size: X = [x, dot_x, y, dot_y, z]   
os = 3;          % Observation size: Y = [x, y, I]    

X = zeros(ss, K);    % State matrix
I_val = zeros(K, 1); % Value function for each frame
S = zeros(K, 1);     % State transition relationships

Nx = 100;            % Radial distance resolution units   
Ny = 100;            % Tangential distance resolution units

DeltaX = 200;        % Size of a resolution unit in X
DeltaY = 200;        % Size of a resolution unit in Y

%% Simulation Condition
T_step = 2.5;
F = [1 T_step 0 0 0;
     0     1 0 0 0;
     0     0 1 T_step 0;
     0     0 0     1 0;
     0     0 0     0 1]; % State transition matrix 

% Process noise covariance matrix for CV model
Q_CV = [T_step^3/3 T_step^2/2       0          0          0;
        T_step^2/2      T_step       0          0          0;
               0          0 T_step^3/3 T_step^2/2          0;
               0          0 T_step^2/2      T_step       0;
               0          0          0          0    1/T_step];

q_CV = 10;   % Continuous process noise energy spectral density
q_I  = 10;   % Power spectral density of target echo intensity variation
Q = Q_CV * diag([q_CV q_CV q_CV q_CV q_I]); % Scaled process noise covariance
Sigma = Q;  % Process noise covariance for 5-dimensional state

% Generate process noise samples
processNoise = mvnrnd(zeros(ss,1), Q, K)'; % Each column is a sample

Mach = 340; % Speed of sound in m/s (assuming Mach=1)

% Initial state: [x, dot_x, y, dot_y, z]
initx = [20000; -1.5*Mach; 20000; -1.6*Mach; 10];
X(:,1) = initx; % Initial state

% Generate true target states
for t = 2:K
    X(:,t) = F * X(:,t-1) + processNoise(:,t); % Constant Velocity Model
end

xEsti = [];   % State estimates
PEsti = [];   % Estimate covariances

%% Generate Measurements
% Initialize measurement parameters
Power_target_av = 10;          % Average power of the target
sigma_I = 1;                   % Measurement noise standard deviation for intensity
sigma_measurement = 50;        % Measurement noise standard deviation for position

% Initialize DataScan as measurement intensity grid
DataScan = randn(Nx, Ny, K) * sigma_I; % Background noise

for k = 1:K
    target_x = X(1,k);
    target_y = X(3,k);
    IndexX = ceil(target_x / DeltaX);
    IndexY = ceil(target_y / DeltaY);
    
    % Ensure indices are within bounds
    if IndexX >=1 && IndexX <= Nx && IndexY >=1 && IndexY <= Ny
        DataScan(IndexX, IndexY, k) = Power_target_av + randn * sigma_I;
    end
end

%% Initialize Tracking Structures
% Define Point structure
Point.Value = 0;
Point.LastPointPosition = [0, 0];
Point.LastPointIndex = 0;
Point.State = [0; 0; 0; 0; 0];
Point.Intensity = 0;
Point.Position = [0, 0];
Point.OtherPointSetIndex = 0;

% Define Track structure
Track.Step = 0;
Track.Value = 0;  % Track's value function
Track.PointSet = struct('Point', Point, 'OtherPointSet', Point); % Initialize PointSet

%% Initialize with a Given Point
RowIndex = 100; 
ColumnIndex = 100;

% Initialize the first point
Point.Value = 0;
Point.LastPointPosition = [0, 0];
Point.State = [(RowIndex-0.5)*DeltaX; -1.5*Mach; (ColumnIndex-0.5)*DeltaY; -1.6*Mach; 10];

Xk = Point.State;
IndexX = ceil(Xk(1) / DeltaX);
IndexY = ceil(Xk(3) / DeltaY);
Point.Position = [IndexX, IndexY];
Point.Intensity = DataScan(RowIndex, ColumnIndex, 1);
Point.OtherPointSetIndex = 1;

% Initialize Track's PointSet
Track.PointSet(1).Point = Point; % This variable is not used later
Track.PointSet(1).OtherPointSet(1) = Point;
Track.Step = 1;

% Initialize CandidateTrack
CandidateTrack(1) = Track;

% Initialize state estimates
xEsti(:,1) = Point.State;   % Initial state estimate
PEsti(:,:,1) = 1000 * eye(ss);   % Initial estimate covariance

%% Define Kalman Filter Parameters
H = [1 0 0 0 0;
     0 0 1 0 0]; % Observation matrix: [x, y]
R = diag([sigma_measurement^2, sigma_measurement^2]); % Measurement noise covariance
I_kf = eye(ss); % Identity matrix for Kalman filter

%% Recurrence Accumulation (Tracking Loop)
for k = 1:K-1
    CellFlag = zeros(Nx, Ny);               % Indicator for predicted states in each cell
    LastCellStorage = cell(Nx, Ny);         % Storage for last points in each cell
    PredictionCellStorage = cell(Nx, Ny);   % Storage for predicted states in each cell
    
    PredictionPointNumber = 0;
    
    % Number of points in the last set
    OtherPointSetNumber = numel(CandidateTrack(1).PointSet(k).OtherPointSet);
    
    for m = 1:OtherPointSetNumber
        LastPoint = CandidateTrack(1).PointSet(k).OtherPointSet(m); % Extract last point
        Xk = LastPoint.State;  % State for prediction
        
        % State prediction (assuming deterministic prediction here)
        XX = F * Xk; % Predicted state
        
        LastIndexX = ceil(Xk(1) / DeltaX);
        LastIndexY = ceil(Xk(3) / DeltaY);
        
        % Determine the cell indices for the predicted state
        PredIndexX = ceil(XX(1) / DeltaX);
        PredIndexY = ceil(XX(3) / DeltaY);
        
        % Ensure indices are within bounds
        if PredIndexX < 1 || PredIndexX > Nx || PredIndexY < 1 || PredIndexY > Ny
            continue; % Skip out-of-bounds predictions
        end
        
        % Update CellFlag
        CellFlag(PredIndexX, PredIndexY) = CellFlag(PredIndexX, PredIndexY) + 1;
        
        if(CellFlag(PredIndexX, PredIndexY) == 1)
            LastCellStorage{PredIndexX, PredIndexY} = LastPoint;           % Store last point
            PredictionPointNumber = PredictionPointNumber + 1;             % Increment count
            PredictionCellStorage{PredIndexX, PredIndexY} = XX;            % Store predicted state
        else
            LastCellStorageBuf = LastCellStorage{PredIndexX, PredIndexY};  % Retrieve existing points
            LastCellStorageBuf = [LastCellStorageBuf, LastPoint];          % Append new point
            LastCellStorage{PredIndexX, PredIndexY} = LastCellStorageBuf;  % Update storage
            
            PredictionCellStorageBuf = PredictionCellStorage{PredIndexX, PredIndexY}; % Retrieve existing states
            PredictionCellStorageBuf = [PredictionCellStorageBuf, XX];                  % Append new state
            PredictionCellStorage{PredIndexX, PredIndexY} = PredictionCellStorageBuf;    % Update storage
        end
    end
    
    %% Value Function Calculation
    MaxCurrentValue = -Inf;
    MaxCurrentIndex = 0;
    
    for i = 1:PredictionPointNumber
        % Find the cell with the maximum number of predictions
        [MaxBufOne, RowIndexBuf] = max(CellFlag, [], 1); 
        [MaxBufTwo, ColumnIndex] = max(MaxBufOne, [], 2);   
        RowIndex = RowIndexBuf(ColumnIndex);       
        
        % Aggregate predicted states in the cell
        StateBuf = PredictionCellStorage{RowIndex, ColumnIndex};     
        DimBuf = size(StateBuf, 2);
        XX_mean = sum(StateBuf, 2) / DimBuf; % Mean state
        
        % Retrieve all last points contributing to this cell
        LastPointSet = LastCellStorage{RowIndex, ColumnIndex};     
        LastPointNumber = numel(LastPointSet);
        
        MaxLastToThisValue = -Inf;
        MaxValueIndex = 0;
        MaxLastIndexX = 0;
        MaxLastIndexY = 0;
        
        for j = 1:LastPointNumber
            % Calculate the value function for the transition
            LastPoint = LastPointSet(j);
            Xk = LastPoint.State;
            LastPointValue = LastPoint.Value;
            
            % Predict next state (without noise for value function)
            Xkk = F * Xk;             
            % Calculate the probability of transition
            diff = XX_mean - Xkk;
            exponent = -0.5 * (diff' / Sigma * diff);
            LogPdfxkk_xk = log(1 / ((2*pi)^(ss/2) * sqrt(det(Sigma)))) + exponent;
            
            % Measurement likelihood (assuming intensity I follows Gaussian)
            z_I = DataScan(RowIndex, ColumnIndex, k+1);
            LogPdfzkk_xkk = log(1 / (sqrt(2*pi) * sigma_I)) + ...
                             -0.5 * ((z_I - Power_target_av)^2) / (sigma_I^2);
                         
            % Total value (log probabilities)
            Value = LogPdfxkk_xk + LogPdfzkk_xkk + LastPointValue;
            
            if(Value > MaxLastToThisValue)
                MaxLastToThisValue = Value;
                MaxValueIndex = j;
                MaxLastIndexX = ceil(Xk(1) / DeltaX);
                MaxLastIndexY = ceil(Xk(3) / DeltaY); 
            end
        end
        
        % Update the point with the maximum value
        Point.Value = MaxLastToThisValue;
        Point.LastPointPosition = [MaxLastIndexX, MaxLastIndexY];
        Point.Intensity = DataScan(RowIndex, ColumnIndex, k+1);      
        Point.Position = [RowIndex, ColumnIndex];     
        Point.LastPointIndex = LastPointSet(MaxValueIndex).OtherPointSetIndex;
        Point.OtherPointSetIndex = i;
        
        % Store the point in CandidateTrack
        CandidateTrack(1).PointSet(k+1).OtherPointSet(i) = Point;  
        
        % Update the maximum current value and index
        if(MaxLastToThisValue > MaxCurrentValue)
            MaxCurrentValue = MaxLastToThisValue;
            MaxCurrentIndex = i;
        end
    end  
    
    % Update the state estimate using the Kalman Filter
    % Retrieve the best point
    BestPoint = CandidateTrack(1).PointSet(k+1).OtherPointSet(MaxCurrentIndex);
    z = [(BestPoint.Position(1)-0.5)*DeltaX, (BestPoint.Position(2)-0.5)*DeltaY];
    
    if k == 1
        xLastEst = xEsti(:,k);
        PLastEst = PEsti(:,:,k);
    else
        xLastEst = xEsti(:,k);
        PLastEst = PEsti(:,:,k);
    end
    
    % Kalman Filter Predict
    x_pred = F * xLastEst;
    P_pred = F * PLastEst * F' + Q;
    
    % Kalman Filter Update
    y_k = z' - H * x_pred;          % Innovation
    S_k = H * P_pred * H' + R;      % Innovation covariance
    K_k = P_pred * H' / S_k;        % Kalman Gain
    x_est = x_pred + K_k * y_k;     % Updated state estimate
    P_est = (I_kf - K_k * H) * P_pred; % Updated estimate covariance
    
    % Store the estimates
    xEsti(:,k+1) = x_est;
    PEsti(:,:,k+1) = P_est;
    
    % Update the CandidateTrack with the best point
    CandidateTrack(1).PointSet(k+1).Point = BestPoint;  
    CandidateTrack(1).Value = MaxCurrentValue;
    CandidateTrack(1).Step = CandidateTrack(1).Step + 1;
end   

%% Backtrack to Retrieve the Estimated Trajectory
XX = zeros(ss, K);
XX(:,K) = CandidateTrack(1).PointSet(K).Point.State;
Index = CandidateTrack(1).PointSet(K).Point.LastPointIndex;

for i = 2:K
    LastPoint = CandidateTrack(1).PointSet(K-i+1).OtherPointSet(Index);
    XX(:,K-i+1) = LastPoint.State;
    Index = LastPoint.LastPointIndex;
end

%% Plot the Results
figure
plot(X(1,:), X(3,:), 'o-', 'LineWidth', 2); hold on;
plot(XX(1,:), XX(3,:), '*-', 'LineWidth', 2);
legend('真实轨迹 (True Trajectory)', '估计轨迹 (Estimated Trajectory)', 'Location', 'Best');
xlabel('X Position');
ylabel('Y Position');
title('Single Target Tracking using DP-TBD');
grid on;
axis equal;

Explanation of Modifications:

1. Process Noise Sampling:

   • Replaced the undefined sample_gaussian function with MATLAB's built-in mvnrnd function to generate multivariate Gaussian noise.

2. Measurement Generation (ProduceMeasurement):

   • Instead of an undefined function, measurements are generated within the script. DataScan is initialized with background noise, and the target's true position in each frame is assigned a higher intensity value (Power_target_av) with added noise.

3. Kalman Filter Implementation:

   • The Kalman Filter steps are implemented directly within the tracking loop. The observation matrix H is defined to extract the position components from the state.
   • Measurement noise covariance R is defined based on the assumed measurement noise for position.
   • The filter predicts the next state and updates the estimates based on the measurements.

4. State Prediction (StatePrediction):

   • Replaced the undefined StatePrediction function with direct state prediction using the state transition matrix F.

5. Handling Undefined Variables:

   • Defined Power_target_av, sigma_I, and sigma_measurement to handle intensity and position measurements.
   • Ensured that all indices used for accessing DataScan are within the defined grid boundaries.

6. Backtracking for Estimated Trajectory:

   • After the tracking loop, backtracking is performed to retrieve the estimated trajectory from the CandidateTrack.

7. Plotting Enhancements:

   • Enhanced the plot for better visualization, including labels, title, grid, and equal axis scaling.

Additional Notes:

• Boundary Checks: Added checks to ensure that the predicted indices do not exceed the grid boundaries (Nx and Ny).

• Initial Covariance (PEsti): Initialized with a large covariance (1000 * eye(ss)) to represent high uncertainty in the initial state estimate.

• Simplifications:

  • The intensity measurement (I) is used in the value function calculation but not directly in the Kalman Filter. This separation simplifies the implementation but could be further integrated for more accurate tracking.

• Assumptions:

  • The state transition is deterministic except for the process noise.
  • The measurement model only includes position (x and y) and intensity (I), with intensity treated separately in the value function.

This revised script should run without errors, assuming MATLAB is properly set up. You can further refine the model by integrating the intensity measurement more tightly with the Kalman Filter or by enhancing the state prediction mechanism.