?? batch01.m
字號:
%BATCH01.m
%------------------ Batch Processing ----------------------------- %
% 2006/12/10 made by vin --- 2007/11/1 change by Wilson
clear all;
close all
load obs.dat
load xy.dat
fp1=fopen('batch01.dat','w');
% ---------------- IC ------------------------------------------- %
% Estimate time 11~30
% Estimate [Xm,Ym,Vx,Vy,acx,acy,bd,Cd]
a=10;b=30;
time(:,1)=xy(a+1:b+1,1);
obs=obs(a+1:b+1,:);
xy=xy(a+1:b+1,:);
Xref0=xy(1,2:9);
Jprev=1; % Assume Jprev is bigger than J in order to run first loop
J=0;
inttimes=0;
while ((abs(sqrt(J)-sqrt(Jprev)))> 0.5)
inttimes=inttimes+1;
%--------------------------------------------------------
N=zeros(8,1); M=zeros(8,8); Fio=eye(8);
W=[1 0 0; 0 1 0; 0 0 1]; % Three Obs - 3*3 matrix
Xref=Xref0;
%--------X* Reference Path Integration -------------------%
for T=1:1:size(obs,1)-1; % number of data input
% ----------------------------------------------------------------- %
[t,xref] = ode45('x_dot',[T T+1],Xref); % State Int
k = size(xref,1);
Xref = xref(k,1:8) ;
for i=1:k-1 % STM Integration
A = [0,0,1,0,0,0,0,0;
0,0,0,1,0,0,0,0;
0,0,-xref(i,7)-2*xref(i,3)*xref(i,8),0,1,0,-xref(i,3),-xref(i,3)*xref(i,3);
0,0,0,-xref(i,7)-2*xref(i,4)*xref(i,8),0,1,-xref(i,4),-xref(i,4)*xref(i,4);
0,0,0,0,0,0,0,0 ;
0,0,0,0,0,0,0,0 ;
0,0,0,0,0,0,0,0 ;
0,0,0,0,0,0,0,0 ];
dt=t(i+1)-t(i) ;
Fio = expm(A*dt)*Fio ; % Fi=Fi_to ti %% Error state transition
end
% ----------------------------------------------------------------- %
% Observation Fixed Station
Xf1=10; Yf1=120;
Xf2=120; Yf2=10;
Xf3=60 ; Yf3=150;
%-----------Moving body start
Xm=Xref(1); % Reference Path Integration value
Ym=Xref(2);
Lu1=sqrt((Xm-Xf1)^2+(Ym-Yf1)^2);
Lu2=sqrt((Xm-Xf2)^2+(Ym-Yf2)^2);
Lu3=sqrt((Xm-Xf3)^2+(Ym-Yf3)^2);
G=[ sqrt((Xm-Xf1)^2+(Ym-Yf1)^2);
sqrt((Xm-Xf2)^2+(Ym-Yf2)^2);
sqrt((Xm-Xf3)^2+(Ym-Yf3)^2) ];
Hh=[ (Xm-Xf1)/Lu1 (Ym-Yf1)/Lu1 0 0 0 0 0 0;
(Xm-Xf2)/Lu2 (Ym-Yf2)/Lu2 0 0 0 0 0 0;
(Xm-Xf3)/Lu3 (Ym-Yf3)/Lu3 0 0 0 0 0 0];
H = Hh*Fio;
z= obs(T+1,2:4)'-G;
zz(:,T+1)=z(:,1);
Jprev = J;
J = J + z'*W*z;
if inttimes>=10;
Jprev = J;
end
N = N + H'*W*z;
M = M + H'*W*H;
end
% ----------------------------------------------------------------- %
xh =inv(M)*N;
Xref0=Xref0+xh' ; % at t0 The Initialization
end
%---------- The lasted Integration-------------------------
Xe=Xref0;
out=[time(1,1) Xe];
fprintf(fp1,'%6.0f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n',out);
for T=1:1:size(obs,1)-1; % number of data input
[t,xe] = ode45('x_dot',[T T+1],Xe);
k = size(xe,1);
Xe= xe(k,1:8); % Last estimate
out=[time(T+1,1) Xe];
fprintf(fp1,'%6.0f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n',out);
end
fclose(fp1);
%----------------------- Plot ---------------------------------------- %
load batch01.dat
figure(1)
plot(batch01(:,2),batch01(:,3),'r:o',xy(:,2),xy(:,3),'b:*');
title(' X v.s Y ');
legend('Estimate','True');
xlabel(' X ');
ylabel(' Y ');
figure(2)
subplot(2,1,1);
plot(batch01(:,1),batch01(:,2),'r:o',batch01(:,1),xy(:,2),'b:*');
title(' X - Estimate VS. True ');
xlabel(' time = 11 to 30 ');
ylabel(' X - distance ');
legend('Estimate','True');
subplot(2,1,2);
plot(batch01(:,1),batch01(:,2)-xy(:,2),'r:o');
title(' X - Difference ');
xlabel(' time = 11 to 30 ');
ylabel(' X - error ');
legend('error');
figure(3)
subplot(2,1,1);
plot(batch01(:,1),batch01(:,3),'r:o',batch01(:,1),xy(:,3),'b:*');
title(' Y - Estimate VS. True ');
xlabel(' time = 11 to 30 ');
ylabel(' Y (Estimate VS. True) ');
legend('Estimate','True');
subplot(2,1,2);
plot(batch01(:,1),batch01(:,3)-xy(:,3),'r:o');
title(' Y - Difference ');
xlabel(' time = 11 to 30 ');
ylabel(' Y - erroe ');
legend('error');
figure(4)
subplot(2,1,1);
plot(batch01(:,1),batch01(:,4),'r:o',batch01(:,1),xy(:,4),'b:*');
title(' Vx - Estimate VS. True ');
xlabel(' time = 11 to 30 ');
ylabel(' Vx (Estimate VS. True) ');
legend('Estimate','True');
subplot(2,1,2);
plot(batch01(:,1),batch01(:,4)-xy(:,4),'r:o');
title(' Vx - Difference ');
xlabel(' time = 11 to 30 ');
ylabel(' Vx - error ');
legend('error');
figure(5)
subplot(2,1,1);
plot(batch01(:,1),batch01(:,5),'r:o',batch01(:,1),xy(:,5),'b:*');
title(' Vy - Estimate VS. True ');
xlabel(' time = 11 to 30 ');
ylabel(' Vy (Estimate VS. True) ');
legend('Estimate','True');
subplot(2,1,2);
plot(batch01(:,1),batch01(:,5)-xy(:,5),'r:o');
title(' Vy - Difference ');
xlabel(' time = 11 to 30 ');
ylabel(' Vy - error ');
legend('error');
figure(6)
subplot(2,1,1);
plot(batch01(:,1),batch01(:,6),'r:o',batch01(:,1),xy(:,6),'b:*');
title(' acx - Estimate VS. True ');
xlabel(' time = 11 to 30 ');
ylabel(' acx (Estimate VS. True) ');
legend('Estimate','True');
subplot(2,1,2);
plot(batch01(:,1),batch01(:,6)-xy(:,6),'r:o');
title(' acx - Difference ');
xlabel(' time = 11 to 30 ');
ylabel(' acx - error ');
legend('error');
figure(7)
subplot(2,1,1);
plot(batch01(:,1),batch01(:,7),'r:o',batch01(:,1),xy(:,7),'b:*');
title(' acy - Estimate VS. True ');
xlabel(' time = 11 to 30 ');
ylabel(' acy (Estimate VS. True) ');
legend('Estimate','True');
subplot(2,1,2);
plot(batch01(:,1),batch01(:,7)-xy(:,7),'r:o');
title(' acy - Difference ');
xlabel(' time = 11 to 30 ');
ylabel(' acy - error ');
legend('error');
figure(8)
subplot(2,1,1);
plot(batch01(:,1),batch01(:,8),'r:o',batch01(:,1),xy(:,8),'b:*');
title(' bd - Estimate VS. True ');
xlabel(' time = 11 to 30 ');
ylabel(' bd (Estimate VS. True) ');
legend('Estimate','True');
subplot(2,1,2);
plot(batch01(:,1),batch01(:,8)-xy(:,8),'r:o');
title(' bd - Difference ');
xlabel(' time = 11 to 30 ');
ylabel(' bd - error ');
legend('error');
figure(9)
subplot(2,1,1);
plot(batch01(:,1),batch01(:,9),'r:o',batch01(:,1),xy(:,9),'b:*');
title(' Cd - Estimate VS. True ');
xlabel(' time = 11 to 30 ');
ylabel(' Cd (Estimate VS. True) ');
legend('Estimate','True');
subplot(2,1,2);
plot(batch01(:,1),batch01(:,9)-xy(:,9),'r:o');
title(' Cd - Difference ');
xlabel(' time = 11 to 30 ');
ylabel(' Cd - error ');
legend('error');
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -