batch01.m

For Batch Estimation Method, the function and code supplyment.

%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');