?? begin1.asv
字號:
% read_frame(int no) 量測數據暫存
% frameleave(:,:,t) 量測數據暫存 (3*5 t)
function [track,frameR,frametheta, frameq]=begin1(spt,frameleavings,tracknum,track, frameR,frametheta,frameq)
t=spt;
% if t >= 4;
% m=t-3;
% for i=1:4; % 時間 每四秒存一次數據
% data(:,2,m)
frameR(:,4) = frameleavings(:,2,t); % r 暫存的數據
frameq(:,4) = frameleavings(:,4,t); %
frametheta(:,4)= frameleavings(:,3,t); %
frameused(:,:) = zeros(3,4);
% m=m+1;
% end
% end
frametotal =3;
START_GATE=3;
PREDI_GATE =6;
% 對frame數組進行起始處理
for i=1: frametotal % (第 1 列）
trackpoints(i)=0;
trackconnect(i)=0;
for j=1: frametotal % (第 2 列）
distants(i,j) = abs(frameR(j,2)-frameR( i,1));
if distants(i,j)<=START_GATE % (???) 第一步判斷
trackpoints(i)=trackpoints(i)+1;
trackconnect(i)=trackconnect(i)+1; % 待起始航跡相關次數
frameused(i,1)=1;
frameused(j,2)=1;
i;
% 調整航跡
if i==j
else
ahR=frameR(i,2);
ahtheta=frametheta(i,2);
ahq=frameq(i,2);
frameR(i,2)=frameR(j,2);
frametheta(i,2)=frametheta(j,2);
frameq(i,2)=frameq(j,2);
frameR(j,2)=ahR;
frametheta(j,2)=ahtheta;
frameq(j,2)=ahq;
% frameused[i,2]=1;
% frameused[j,2]=0;
end
end
end
end
% 第三楨
% (第 3 列）
for i=1: frametotal % (第 2 列）
if frameused(i,1)==1
for k=1:frametotal % (第 3 列）
point3.x= 2*frameR(i,2)- frameR(i,1); % 預測第三個點中心做判斷
point3.y= 2*frametheta(i,1)- frametheta(i,1);
point3.z= 2*frameq(i,1)- frameq(i,1);
distants2(k)=sqrt((point3.x-frameR(k,3))^2+(point3.y-frametheta(k,3))^2+ (point3.z-frameq(k,3))^2);
if distants2(k)<= PREDI_GATE % (???)
trackpoints(i)=trackpoints(i)+1;
trackconnect(i)=trackconnect(i)+1; % 待起始航跡相關次數
frameused(k,3)=1;
% else
% trackconnect(i)=trackconnect(i); % 待起始航跡相關次數
% end
% 調整航跡
if k==i
else
% ah=frame(i,3);
% frame(i,3)=frame(k,3);
% frame(k,2)=ah;
ahR=frameR(i,3);
ahtheta=frametheta(i,3);
ahq=frameq(i,3);
frameR(i,3)=frameR(j,3);
frametheta(i,3)=frametheta(j,3);
frameq(i,3)=frameq(j,3);
frameR(j,3)=ahR;
frametheta(j,3)=ahtheta;
frameq(j,3)=ahq;
end
end
end
% else % 取最小的distants(k) K
% frameR(i,3)=point3.x;
% frametheta(i,3)=point3.y;
% frameq(i,3)=point3.z;
end % ( 45 )
end % ( 44 )
for i=1: frametotal % (第 3 列）
if frameused(i,1)==1
for k=1: frametotal % (第4 列） frameR[:,i] % 調用3通道 濾波判斷
% 輸入調整
T=1;
Sigma0=0.2;
Rmeasure=[65^2,0,0;
0,0.001^2,0;
0,0,0.001^2];
Rk1=Rmeasure(1,1);
Rk2=Rmeasure(2,2);
Rk3=Rmeasure(3,3);
Fkcv=[1,T,0;
0,1,0;
0,0,0];
Qkcv=[T*T*T*T/4,T*T*T/2,0;
T*T*T/2,T*T,0;
0,0,0]*Sigma0*Sigma0;
H=[1,0,0];
C0kcv=[1,0,0];
Pk1=[1,0,0;
0,1,0;
0,0,1]*10000;
Zr(k)=[frameR(k,4)]';
Ztheta(k)=[frametheta(k,4)]';
Zq(k)=[frameq(k,4)]';
Xkr(:,3)=[frameR(i,3);(frameR(i,3)-frameR(i,2))/T;eps]; % ????
Xktheta(:,3)=[frametheta(i,3);(frametheta(i,3)-frametheta(i,2))/T;eps];
Xkq(:,3)=[frameq(i,3);(frameq(i,3)-frameq(i,2))/T;eps];
[Xkr,Pkr]=fkalmancv(Xkr(:,3),Pk1,Fkcv,Qkcv,Rk1,H,Zr(k));
[Xktheta,Pktheta]=fkalmancv(Xktheta(:,3),Pk1,Fkcv,Qkcv,Rk2,H,Ztheta(k));
[Xkq,Pkq]=fkalmancv(Xkq(:,3),Pk1,Fkcv,Qkcv,Rk3,H,Zq(k));
Xkk1(:,k)=[Xkr(1,:);Xktheta(1,:);Xkq(1,:)];
sigma_r_radar=65;
sigma_theta_radar=0.001;
sigma_q_radar=0.001;
kr(k)=sqrt(sigma_r_radar+Pkr(1,1)^2);
ktheta(k)=sqrt(sigma_theta_radar+Pktheta(1,1)^2);
kq(k)=sqrt(sigma_q_radar+Pkq(1,1)^2);
S(:,:,k)=diag([kr(k),ktheta(k),kq(k)]);
d(:,k)=Xkk1(:,k)-[frameR(k,4),frametheta(k,4),frameq(k,4)]'; % 待改
a11(:,k)=d(:,k)'*inv(S(:,:,k))*d(:,k);
a11(:,k);
end
% 取其中最小的值的下標
% a11=min[a11(:,1),a11(:,2),a11(:,3)];
k = find (a11==min(a11));
a11(:,k)
% a11= 5.4752e-005
gama=5;
if a11<=gama
trackpoints(i)=trackpoints(i)+1;
trackconnect(i)=trackconnect(i)+1; % 待起始航跡相關次數
frameused(k,4)=1;
ahR=frameR(i,4);
ahtheta=frametheta(i,4);
ahq=frameq(i,4);
frameR(i,4)=frameR(k,4);
frametheta(i,4)=frametheta(k,4);
frameq(i,4)=frameq(k,4);
frameR(k,4)=ahR;
frametheta(k,4)=ahtheta;
frameq(k,4)=ahq;
% ah=frame(i,4);
% frame(i,4)=frame(k,4);
% frame(k,4)=ah;
else
trackconnect(i)=trackconnect(i); % 待起始航跡相關次數
end
end % (30)
end % (17)
% tracknum 原來航跡數
if trackpoints(i)==3 & trackconnect(i)>=2 % 起始新航跡
tracknum=tracknum+1; % 航跡數加1
% trackR(tracknum,:) = frameR(i,:); % 把值賦給。。。
% tracktheta(tracknum,:) = frametheta(i,:);
% trackq(tracknum,:) = frameq(i,:);
% 更新航跡
track(tracknum,1,spt)=tracknum;
track(tracknum,2,spt)=frameR(i,:);
track(tracknum,3,spt)=frametheta(i,:);
track(tracknum,4,spt)=frameq(i,:);
track(tracknum,5,spt)= Xkk1(1,k);
track(tracknum,6,spt)= Xkk1(2,k);
track(tracknum,7,spt)= Xkk1(3,k);
frameR(i,:)=0; % 重新賦 0
frametheta(i,:)=0;
frameq(i,:)=0;
end
% 前三列數組調整
% for t=t+1
for k=2:4
frameR(:,k-1,spt) = frameR(:,k);
frametheta(:,k-1,spt) = frametheta(:,k);
frameq(:,k-1,spt) = frameq(:,k);
end
frameR(:,4) = 0;
frametheta(:,4) = 0;
frameq(:,4) = 0;
% end
% frameR
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -