plot_3d_kf1.m

一个目标跟踪系统的MATLAB 源程序包

                if max(max(cost)) < 0
                    [track(1).X, track(1).Cov] = prekalman(track(1).X, track(1).Cov,vm,Qk,F,G);
                    [track(2).X, track(2).Cov] = prekalman(track(2).X, track(2).Cov,vm,Qk,F,G);
                else
                    [q,omiga,assign] = auction_2D(cost);
                    % update tracks
                    ind1 = find(~(assign-1));
                    if isempty(ind1) | cost(1, ind1) < 0
                        [track(1).X, track(1).Cov] = prekalman(track(1).X, track(1).Cov,vm,Qk,F,G);
                    else
                        [track(1).X, track(1).Cov] = stdkalman(track(1).X, track(1).Cov,...
                            z(:,ind1), Qk, R(:,:,ind1), vm, wm, F, G, H, Ik);
                    end
                    ind2 = find(~(assign-2));
                    if isempty(ind2) | cost(2, ind2) < 0
                        [track(2).X, track(2).Cov] = prekalman(track(2).X, track(2).Cov,vm,Qk,F,G);
                    else
                        [track(2).X, track(2).Cov] = stdkalman(track(2).X, track(2).Cov,...
                            z(:,ind2), Qk, R(:,:,ind2), vm, wm, F, G, H, Ik);
                    end
                end
                [track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).X, track(1).Cov,vm,Qk,F2,G2);
                [track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).X, track(2).Cov,vm,Qk,F2,G2);
                
            else   % ind01~=0
                % form the cost for 3-D assignment
                k1 = 1;
                for j=1:ind00
                    if measurement(i).flag(j) == 0
                        [z1(1,k1), z1(2,k1), R1(:,:,k1)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), ...
                        measurement(i).pos(1), measurement(i).pos(2), C_r, C_b);
                        plot(z1(1,k1), z1(2,k1), 'k*');                    
                        k1 = k1 + 1;
                        [z1(1,k1), z1(2,k1), R1(:,:,k1)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), ...
                        measurement(i).pos(1), measurement(i).pos(2), C_r, C_b);
                        k1 = k1 + 1;
                    else
                        [z1(1,k1), z1(2,k1), R1(:,:,k1)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), ...
                        measurement(i).pos(1), measurement(i).pos(2), C_r/sqrt(12), C_b/sqrt(12));
                        plot(z1(1,k1), z1(2,k1), 'c*');                    
                        k1 = k1 + 1;
                    end                        
                end
                k2 = 1;
                for j=1:ind01
                    if measurement(i+1).flag(j) == 0
                        [z2(1,k2), z2(2,k2), R2(:,:,k2)] = CalcMeasCov2(measurement(i+1).range(j), measurement(i+1).bearing(j), ...
                        measurement(i+1).pos(1), measurement(i+1).pos(2), C_r, C_b);
                        plot(z2(1,k2), z2(2,k2), 'k*');                    
                        k2 = k2 + 1;
                        [z2(1,k2), z2(2,k2), R2(:,:,k2)] = CalcMeasCov2(measurement(i+1).range(j), measurement(i+1).bearing(j), ...
                        measurement(i+1).pos(1), measurement(i+1).pos(2), C_r, C_b);
                        k2 = k2 + 1;
                    else
                        [z2(1,k2), z2(2,k2), R2(:,:,k2)] = CalcMeasCov2(measurement(i+1).range(j), measurement(i+1).bearing(j), ...
                        measurement(i+1).pos(1), measurement(i+1).pos(2), C_r/sqrt(12), C_b/sqrt(12));
                        plot(z2(1,k2), z2(2,k2), 'c*');                    
                        k2 = k2 + 1;
                    end      
                end
                for j=1:k1-1
                    for k=1:k2-1
                        cost(1, j, k) = lr_3d_kalman(track(1).X, track(1).Cov, z1(:,j), R1(:,:,j), z2(:,k), R2(:,:,k), Qk, vm, wm, F, G, F2, G2, H, Ik, Pd, lambda);
                        cost(2, j, k) = lr_3d_kalman(track(2).X, track(2).Cov, z1(:,j), R1(:,:,j), z2(:,k), R2(:,:,k), Qk, vm, wm, F, G, F2, G2, H, Ik, Pd, lambda);                        
                    end
                end
                if max(max(max(cost))) < 0
                    [track(1).X, track(1).Cov] = prekalman(track(1).X, track(1).Cov,vm,Qk,F,G);
                    [track(2).X, track(2).Cov] = prekalman(track(2).X, track(2).Cov,vm,Qk,F,G);
                    [track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).X, track(1).Cov,vm,Qk,F2,G2);
                    [track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).X, track(2).Cov,vm,Qk,F2,G2);
                else
                    assign = mySD(-cost);
                    ind1 = assign(1,2);
                    if ind1 == 0 
                        [track(1).X, track(1).Cov] = prekalman(track(1).X, track(1).Cov,vm,Qk,F,G);
                        if ndims(assign) < 3
                            [track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).X, track(1).Cov,vm,Qk,F2,G2);
                        else
                            ind3 = assign(1,3);
                            if cost(1, ind1, ind3) < 0
                                [track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).X, track(1).Cov,vm,Qk,F2,G2);
                            else
                                [track(1).est(:,i+1), track(1).P(:,:,i+1)] = stdkalman(track(1).X, track(1).Cov, ...
                                z2(:,ind3), Qk, R2(:,:,ind3), vm, wm, F2, G2, H, Ik);
                            end
                        end
                    else
                        [track(1).X, track(1).Cov] = stdkalman(track(1).X, track(1).Cov, ...
                        z1(:,ind1), Qk, R1(:,:,ind1), vm, wm, F, G, H, Ik);
                        if ndims(assign) < 3
                            [track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).X, track(1).Cov,vm,Qk,F2,G2);
                        else
                            ind3 = assign(1,3);
                            if cost(1, ind1, ind3) < 0
                                [track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).X, track(1).Cov,vm,Qk,F2,G2);
                            else
                                [track(1).est(:,i+1), track(1).P(:,:,i+1)] = stdkalman(track(1).X, track(1).Cov, ...
                                z2(:,ind3), Qk, R2(:,:,ind3), vm, wm, F2, G2, H, Ik);
                            end                
                        end
                    end
                    ind2 = assign(2,2);
                    if ind2 == 0 
                        [track(2).X, track(2).Cov] = prekalman(track(2).X, track(2).Cov,vm,Qk,F,G);
                        if ndims(assign) < 3
                            [track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).X, track(2).Cov,vm,Qk,F2,G2);
                        else
                            ind4 = assign(2,3);
                            if cost(2, ind2, ind4) < 0
                                [track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).X, track(2).Cov,vm,Qk,F2,G2);
                            else
                                [track(2).est(:,i+1), track(2).P(:,:,i+1)] = stdkalman(track(2).X, track(2).Cov,...
                                z2(:,ind4), Qk, R2(:,:,ind4), vm, wm, F2, G2, H, Ik);
                            end                    
                        end
                    else
                        [track(2).X, track(2).Cov] = stdkalman(track(2).X, track(2).Cov, ...
                        z1(:,ind2), Qk, R1(:,:,ind2), vm, wm, F, G, H, Ik);
                        if ndims(assign) < 3
                            [track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).X, track(2).Cov,vm,Qk,F2,G2);
                        else
                            ind4 = assign(2,3);
                            if cost(2, ind2, ind4) < 0
                                [track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).X, track(2).Cov,vm,Qk,F2,G2);
                            else
                                [track(2).est(:,i+1), track(1).P(:,:,i+1)] = stdkalman(track(2).X, track(2).Cov, ...
                                z2(:,ind4), Qk, R2(:,:,ind4), vm, wm, F2, G2, H, Ik);
                            end                
                        end
                    end                
                end % 3-D assignment
            end % if ind00 == 0 ...
        end % for

        plot(track(1).est(1, i+1), track(1).est(3, i+1), 'g.');
        plot(track(2).est(1, i+1), track(2).est(3, i+1), 'g.');
        center1 = [track(1).est(1,i+1), track(1).est(3, i+1)];
        covariance1 = [track(1).P(1,1,i+1), track(1).P(1,3,i+1); track(1).P(3,1,i+1), track(1).P(3,3,i+1)];
        [X1, Y1] = get_ellipsoid_gate(center1, covariance1, 1.96);
        plot(X1, Y1, 'm');
        center2 = [track(2).est(1,i+1), track(2).est(3, i+1)];
        covariance2 = [track(2).P(1,1,i+1), track(2).P(1,3,i+1); track(2).P(3,1,i+1), track(2).P(3,3,i+1)];
        [X2, Y2] = get_ellipsoid_gate(center2, covariance2, 1.96);
        plot(X2, Y2, 'm');
        axis([100e3,130e3,147e3,151e3]);
        pause(.5);
        frozen_time = measurement(i).time;
        track(1).time(i+1) = measurement(i+1).time;
        track(2).time(i+1) = measurement(i+1).time;
    end
    
xlabel('x position (m)');
ylabel('y position (m)');
title('3-D hard assignment, sensor 1&2');
hold off;