% Sequential clustering algorithm for moving objects

% RvdM, February, 2008

clear MAXHYP MAXFIL HYP HYP2 KAL NHYP PCLUTTER;

global MAXHYP;      % maximum number of hypotheses
global MAXFIL;      % maximum number of filters

global HYP HYP2;    % array of active hypotheses
global KAL;         % array of Kalman filters

global NHYP;        % number of active hypotheses

global PCLUTTER;    % probablity of clutter measurement
global SIGMAX;      % standard deviation of measurement noise in x direction
global SIGMAY;      % standard deviation of measurement noise in y direction
global BOUND;       % circular state space boundary
global NKEEP;       % number of hypotheses to keep in gating
global ALPHA;       % Pnew = ALPHA * Pexisting
global PFACTOR;     % hypothesis with P<Pmax/PFACTOR are eliminated

% logging memory
global NHYPLOG;

MAXHYP=1000;
MAXFIL=2000;
MAXOBJ=10;

% *** KALMAN FILTER DESIGN ***

% compute discrete time model
[Ad, Bd, Cd, Dd, T]=discrete_object_model();

% covariance matrix of system noise
q1=0.01;
q2=0.01;
Q=[0 0 0 0; 0 q1 0 0; 0 0 0 0; 0 0 0 q2];

% covariance matrix of measurement noise
r1=1;
r2=1;
R=[r1 0; 0 r2];

% design Kalman filter
Kd=kalmangain(Ad, Bd, Cd, Dd, T, Q, R);

% *** SIMULATE MEASUREMENTS ***

% generate measurements
[tm, xm, ym, nmeas]=sim_measurements4();
%[tm, xm, ym, nmeas]=randomize_meas2(tm, xm, ym, nmeas, 25);
%[tm, xm, ym, nmeas]=clip_meas(tm, xm, ym, nmeas);
%[tm, xm, ym, nmeas]=read_measurements2();

% logging memory for result
RESULT=zeros(nmeas, 1+1+4*MAXOBJ); % store t, nobj, x, y of best hypothesis

sleep=1/10; % pause between plots

% *** SEQUENTIAL CLUSTERING ***

% now that measurements have been generated, we will process them in a
% sequential way

% at the first measurement, two hypotheses exist: the measurement is
% clutter or the measurement represents a newly detected object

% initialize MAXHYP hypotheses 
HYP=struct('hid', 1, 'association_id', 0, 'filter_id', [], 'nobj', 0, 'P', 1);
HYP(1:MAXHYP)=struct('hid', 1, 'association_id', 0, 'filter_id', [], 'nobj', 0, 'P', 1);

% initialize MAXFIL Kalman filters
KAL=struct('xh', zeros(4,1), 'u', [0; 0], 'Ad', Ad, 'Bd', Bd, 'Cd', Cd, 'Kd', Kd, 'active', 0);
KAL(1:MAXFIL)=struct('xh', zeros(4,1), 'u', [0; 0], 'Ad', Ad, 'Bd', Bd, 'Cd', Cd, 'Kd', Kd, 'active', 0);

HYP2=HYP;              % local copy of HYP

% tunable parameters of clustering algorithm
PCLUTTER=0.01;
ALPHA=0.02;
SIGMAX=2; % choose not too small
SIGMAY=0.5;
%BOUND=4;
NKEEP=70;
PFACTOR=40;

% clear log
NHYPLOG=[];

% at the start, no measurement is yet available and there is only one
% active hypothesis, which server as the initial posterior estimate
NHYP=1;

% initialize object visualization
hfig=visualize_init2();

% prepare sobs
sobs=struct('hfig', 0, 'h', 0, 'x', 0, 'y', 0, 'color', [0 0 0], 'radius', 0);
sobs.hfig=hfig;
sobs.color=[0 0 1];
sobs.radius=0.01;

% prepare sobj
sobj=struct('hfig', 0, 'h', 0, 'x', 0, 'y', 0, 'color', [0 0 0], 'radius', 0);
sobj(1:MAXOBJ)=struct('hfig', 0, 'h', 0, 'x', 0, 'y', 0, 'color', [0 0 0], 'radius', 0);
ha=zeros(3, MAXOBJ);
for k=1:MAXOBJ,
    sobj(k).hfig=hfig;
    sobj(k).color=[1 0 0];
    sobj(k).radius=0.3;
end
            
current_time=0;         % current time
previous_time=-1;

ifirst=1;

%for imeas=1:nmeas,
for imeas=1:91,

    disp(['*** measurement: ' num2str(imeas) ' ***']);
    z=[xm(imeas); ym(imeas)]; % imeas-th measurement
    disp(['Measurement z(' num2str(imeas) ') = ( ' num2str(z(1)) ', ' num2str(z(2)) ' )']);

    generate_offspring(z);
   
    disp(['New number of hypotheses = ' num2str(NHYP)]);
    
    %disp(['*** after offspring generation ***']);
    %print_hypotheses();
    
    current_time=kalman_update(current_time, z, tm(imeas));
    
%   apply likelihood update
    likelihood_update2(z);

%   normalize
    normalization2();

    %clip3(-1.5, 2.5, 0, 6); % argument is radius bound for position estimate
    clip3(-4, 4, 0, 8);
    
%   gating of discrete probability distribution
    nhyp_controller();

%   clear unused filters
    free_unused_filters();

%   Maximum A Posteriori (MAP) estimate
    p=zeros(NHYP, 1);
    for i=1:NHYP,
        p(i)=HYP(i).P;
    end
    [mx, i_best]=max(p);
    disp(['Most likely hypothesis so far = ' num2str(i_best) ', nobj =' num2str(HYP(i_best).nobj)]);
    % let's visualize
   
    %for k=1:HYP(i_best).nobj,
    %    idx=HYP(i_best).filter_id(k);
    %    disp([num2str(KAL(idx).xh(1)) ' ' num2str(KAL(idx).xh(2)) ' ' num2str(KAL(idx).xh(3)) ' ' num2str(KAL(idx).xh(4))]);
    %end

    RESULT(imeas, 1)=tm(imeas);
    RESULT(imeas, 2)=HYP(i_best).nobj;
    for k=1:HYP(i_best).nobj,
        idx=HYP(i_best).filter_id(k);
        RESULT(imeas, 2+k)=KAL(idx).xh(1);
        RESULT(imeas, 2+MAXOBJ+k)=KAL(idx).xh(3);
        RESULT(imeas, 2+2*MAXOBJ+k)=KAL(idx).xh(2);
        RESULT(imeas, 2+3*MAXOBJ+k)=KAL(idx).xh(4);
    end

    if 1 % put 0 for no plotting
        
    % prepare sobs
    sobs.x=xm(imeas);
    sobs.y=ym(imeas);
    sobs=visualize_add_object(sobs);

    if imeas>1 && tm(imeas)>tm(imeas-1)
        if ifirst
            ifirst=0;
        else
            for k=1:nobj_bak,
                visualize_remove_object(sobj(k));
                remove_arrow(ha(:,k));
            end
        end
    end
        
    if imeas>1 && tm(imeas)>tm(imeas-1)
        for k=1:HYP(i_best).nobj,
            idx=HYP(i_best).filter_id(k);
        
            % prepare sobj
            sobj(k).x=KAL(idx).xh(1);
            sobj(k).y=KAL(idx).xh(3);
            sobj(k)=visualize_add_object(sobj(k));
            
            x1=KAL(idx).xh(1);
            y1=KAL(idx).xh(3);
            x2=KAL(idx).xh(1)+KAL(idx).xh(2);
            y2=KAL(idx).xh(3)+KAL(idx).xh(4);
            ha(:,k)=draw_arrow(x1, y1, x2, y2);
            
            %[xttt, yttt]=circle(sobj(k).x, sobj(k).y, SIGMA);
            %line(xttt, yttt);
        end
    end
    
    pause(sleep);
    
    %sobs=visualize_fade_object(sobs);

    %if imeas>1 && tm(imeas)>tm(imeas-1)
    %    for k=1:HYP(i_best).nobj,
    %        visualize_remove_object(sobj(k));
    %    
    %        %% prepare sobj
    %        %sobj(k).color=[0 0 0];
    %        %sobj(k).radius=0.01;
    %    
    %        %sobj(k)=visualize_add_object(sobj(k));        
    %        %sobj(k)=visualize_fade_object(sobj(k));
    %    end
    %end
    
    title(['time = ' num2str(tm(imeas))]);

    if imeas>1 && tm(imeas)>tm(imeas-1)    
        nobj_bak=HYP(i_best).nobj;
    end
    
    end
end

visualize_exit();   % stop visualization

save -ascii result.dat RESULT