weight_imm.m

加权与交互式模型相结合

function weight_imm()  %final's correct
clear all;
close all;
%===================load data=======================================
load zxdata_gain;     %position parameter
load weight_memory;   %采样时刻中所有的参数
%=======================parameter setup==============================
r=10;
ts=1;
tt=3*ts;             % sample period
%F=[1 ts 0 
%   0 1  0 
%  0 0  1];         %fliter ratio(fraction)
%H=[1 0 0];           %measure matrix
%B=[0.5*ts*ts  
%   ts         
%   1];              %in filter equation EQUALS"G"
%q=10;
%dis=1000; %位移
%vel=450;  %速度
%acc=100;  %加速度
%xx01=[dis vel 0]';            %状态的初始值
dis1=1000; %位移
dis2=500; %位移
dis3=2000; %位移
vel1=-450;  %速度
vel2=300;  %速度
vel3=-400;  %速度
acc1=-10;  %加速度
acc2=20;  %加速度
acc3=15;  %加速度
xx1=[dis1 vel1 0]';            %状态的初始值
xx2=[dis2 vel2 0]';            %状态的初始值
xx3=[dis3 vel3 0]';            %状态的初始值
XE1=[dis1 vel1 0]'; 
XE2=[dis2 vel2 0]'; 
XE3=[dis3 vel3 0]'; 
x1_1=XE1;
x1_2=XE1;
x2_1=XE2;
x2_2=XE2;
x3_1=XE3;
x3_2=XE3;
PE=[6000    0      0
   0      50    0
   0       0    0];
PE1=PE; %各个目标总的协方差
PE2=PE;
PE3=PE;
p1_1=[100   0    0;
    0     10   0;
    0       0  0];%各个目标每个模型的协方差
p1_2=[100   0    0;
    0     10   0;
    0       0  1];
p2_1=[100   0    0;
    0     10   0;
    0       0  0];%各个目标每个模型的协方差
p2_2=[100   0    0;
    0     10   0;
    0       0  1];
p3_1=[100   0    0;
    0     10   0;
    0       0  0];%各个目标每个模型的协方差
p3_2=[100   0    0;
    0     10   0;
    0       0  1];
u_10=0.5;          %目标1在模型i在k-1时刻的概率
u_20=0.5;          %目标1在模型i在k-1时刻的概率
u1=[u_10 u_20];
u2=[u_10 u_20];
u3=[u_10 u_20];
rx=r^2;              %measurement  deviation
traceproperty=zeros(3,3); %tracking character 
traceproperty_memory=zeros(3*time,3); %in order to memory tracking character
l1=[1 1 0 0;
    0 0 1/2  0;
    0 0 1/2  0;
    0 0 0 1];
l2=[1 0 0 0;
    0 1 0 1;
    0 0 1 0];             %change  array
 dection_memory=zeros(30,3); 
 track_memory=zeros(30,3);
 class_memory=zeros(30,3);   %在采样时刻不同目标的不同任务的权重
%pd=0.8;               %detection's limit
%  pfa=1e10-6;
%===================================================================
%                            whole circle
%===================================================================
num1=1;
num2=1;
num3=1;
for i=1:time
   scanwhich=1;
   entropy(i,1)=PE1(1,1)+PE1(1,2)+PE1(2,1)+PE1(2,2);
   entropy(i,2)=PE2(1,1)+PE2(1,2)+PE2(2,1)+PE2(2,2);
   entropy(i,3)=PE3(1,1)+PE3(1,2)+PE3(2,1)+PE3(2,2);                 %predict
   %PP1=F*PE1*F'+B*q*B';
   %PP2=F*PE2*F'+B*q*B';
   %PP3=F*PE3*F'+B*q*B';
   %entropy(i,1)=PP1(1,1)+PP1(1,2)+PP1(2,1)+PP1(2,2)-entropy(i,1)
   %entropy(i,2)=PP2(1,1)+PP2(1,2)+PP2(2,1)+PP2(2,2)-entropy(i,2)
   %entropy(i,3)=PP3(1,1)+PP3(1,2)+PP3(2,1)+PP3(2,2)-entropy(i,3)   %entropy
   %=================estimate which target's message gain is most=================
  % if ((entropy(1))<(entropy(2)))
  %          scanwhich=2;
  %       if((entropy(2))<(entropy(3)))
  %          scanwhich=3;
  %       end
  % else
  %       if ((entropy(1))<(entropy(3)))
  %          scanwhich=3;
  %       end
  % end;
   %===================get all targets' job weight =================================
   k=1;
 for j=3*i-2:3*i
   v=v_memory(j,:)';
   job_weight(:,k)=l2*l1*v;
   k=k+1;
 end;  
dection_memory(i,:)=job_weight(1,:);
track_memory(i,:)=job_weight(2,:);
class_memory(i,:)=job_weight(3,:);    %memory all target's job weight
%==================when message gain is less than 80,by target's job weight to obtain target========
%if (entropy(i,scanwhich)<80)
%    scanwhich=0;
%end;
%if (scanwhich==0)
    %Entropy=abs(entropy(1))+abs(entropy(2))+abs(entropy(3));
    for j=1:3
       % entropy_weight(j)=abs(entropy(j))/Entropy;
        for m=1:3
        sum_weight(m,j)=entropy(j)*job_weight(m,j);
        end;
        Sum_weight(3*i-2:3*i,j)=sum_weight(:,j);
    end;
   
    %====================find target's detection,tracking or class weight is most=====================
    
  for j=1:3 
       scanwhich_job=1;  
   if sum_weight(j,1)<sum_weight(j,2)
       scanwhich_job=2;
         if sum_weight(j,2)<sum_weight(j,3)
           scanwhich_job=3;
         end;
   else
       if sum_weight(j,1)<sum_weight(j,3)
           scanwhich_job=3;
       end;
   end;
     traceproperty(j,scanwhich_job)=1;
 end;
  %=======================only find target's tracking weight is most===================================
scanwhich_track=1;
    if sum_weight(2,1)<sum_weight(2,2)
        scanwhich_track=2;
        if sum_weight(2,2)<sum_weight(2,3)
            scanwhich_track=3;
        end;
    else
        if sum_weight(2,1)<sum_weight(2,3)
            scanwhich_track=3;
        end;
    end;
    scanwhich=scanwhich_track;
    %end;
%===================================if end .viz find target tracking which weight is most===============
%===================================target which has most weight and deal with it=======================
 if(scanwhich==1)                %target 1
      ZS1=Z1(i);
      [XE1,PE1,p1_1,p1_2,x1_1,x1_2,xx1,u1,num1]=immkf(p1_1,p1_2,x1_1,x1_2,xx1,u1,num1);
       traceproperty_track_grid(i,1)=1;
       %num1=num1+1;
       if (num1>24)&(num1<=50)
          xx1(3)=acc1;
      end;
     % X1(i,1)=xx1(1,1);
 else
     % PE1=F*PE1*F'+B*q*B';
     % XE1=F*XE1;
     traceproperty_track_grid(i,1)=0;
 end;
 XX1(i,1)=XE1(1,1);
 X1(i,1)=xx1(1,1);
 PX1(i)=PE1(1,1)+PE1(2,2)+PE1(3,3);
 num1=num1+1;

if(scanwhich==2)               %target 2
      ZS2=Z2(i);
      [XE2,PE2,p2_1,p2_2,x2_1,x2_2,xx2,u2,num2]=immkf(p2_1,p2_2,x2_1,x2_2,xx2,u2,num2);
      traceproperty_track_grid(i,2)=1;
     %num2=num2+1;
       if (num2>24)&(num2<=50)
          xx2(3)=acc2;
      end;
     % X2(i,1)=xx2(1,1);
else
     % PE2=F*PE2*F'+B*q*B';
     % XE2=F*XE2;
      traceproperty_track_grid(i,2)=0;
end;
XX2(i,1)=XE2(1,1);
 X2(i,1)=xx2(1,1);
PX2(i)=PE2(1,1)+PE2(2,2)+PE2(3,3);
num2=num2+1;
    
if(scanwhich==3)              %target 3
      ZS3=Z3(i);
     [XE3,PE3,p3_1,p3_2,x3_1,x3_2,xx3,u3,num3]=immkf(p3_1,p3_2,x3_1,x3_2,xx3,u3,num3);
      traceproperty_track_grid(i,3)=1;
     % num3=num3+1;
       if (num3>24)&(num3<=50)
          xx3(3)=acc3;
      end;
     % X3(i,1)=xx3(1,1);
else
   %  PE3=F*PE3*F'+B*q*B';
   %  XE3=F*XE3;
     traceproperty_track_grid(i,3)=0;
end;
XX3(i,1)=XE3(1,1);
X3(i,1)=xx3(1,1);
PX3(i)=PE3(1,1)+PE3(2,2)+PE3(3,3);
num3=num3+1;
weight_value=job_weight;
end;
%================for end=============================
traceproperty_memory=traceproperty_track_grid;  %memory tracking character

%====================judge capability and draw picture=====================================
m_grid1=0;
m_grid2=0;
m_grid3=0;
for i=1:time
   m_grid1=m_grid1+(XX1(i,1)-X1(i,1))^2;
   m_grid2=m_grid2+(XX2(i,1)-X2(i,1))^2;
   m_grid3=m_grid3+(XX3(i,1)-X3(i,1))^2; 
end  
m_grid1=sqrt((m_grid1)/(time));
m_grid2=sqrt((m_grid2)/(time));
m_grid3=sqrt((m_grid3)/(time));


figure(1);
plot(XX1(:,1),'-k');
hold on;
plot(X1(:,1),'-r')
legend('信息增量和POSET相结合的估计','状态');
xlabel('目标1');
ylabel('位移');


figure(2);
plot(XX2(:,1),'-k');
hold on;
plot(X2(:,1),'-r')
legend('信息增量和POSET相结合的估计','状态');
xlabel('目标2');
ylabel('位移');


figure(3);
plot(XX3(:,1),'-k');
hold on;
plot(X3(:,1),'-r');
legend('信息增量和POSET相结合的估计','状态');
xlabel('目标3');
ylabel('位移');


figure(4);
subplot(3,1,1);
plot(PX1,'-.r');
legend('信息增量','信息增量和POSET相结合');
xlabel('目标1');
ylabel('协方差');
hold on;
%axis([0 30 0 2000]);

subplot(3,1,2);
plot(PX2,'-.r');
legend('信息增量','信息增量和POSET相结合');
xlabel('目标2');
ylabel('协方差');
hold on;
%axis([0 30 0 2000]);

subplot(3,1,3);
plot(PX3,'-.r');
legend('信息增量','信息增量和POSET相结合');
xlabel('目标3');
ylabel('协方差');
hold on;
%axis([0 30 0 2000]);


traceproperty_memory;
m_grid_grid=(m_grid1+m_grid2+m_grid3)/3
traceproperty;
 
figure(5);
plot(traceproperty_memory(:,1),'*k');
xlabel('信息增量和POSET相结合');
ylabel('目标1的跟踪情况');
axis([0,time,-1 3]);

figure(6);
plot(traceproperty_memory(:,2),'*k');
xlabel('信息增量和POSET相结合');
ylabel('目标2的跟踪情况');
axis([0,time,-1 3]);

figure(7);
plot(traceproperty_memory(:,3),'*k');
xlabel('信息增量和POSET相结合');
ylabel('目标3的跟踪情况');
axis([0,time,-1 3]);

ee=sum((entropy),2)/3;
figure(8)
plot(ee,'r');
hold on;
%dection_memory
%track_memory
%class_memory   
%Sum_weight