% VD algorithm
%
% 
clear;
clc;
T=2;
num=50;
% 真實軌跡
N1=400/T; N2=600/T; N3=610/T; N4=660/T; N5=900/T;

zx=zeros(N5,1);
zy=zeros(N5,1);
zz=zeros(N5,1);

x=zeros(N5,1);
y=zeros(N5,1);
z1=zeros(N5,1);
vx=zeros(N5,1);
vy=zeros(N5,1);
vz=zeros(N5,1);

x(1)=2000;
y(1)=10000;
z1(1)=10000;
vx(1)=0;
vy(1)=-150;
vz(1)=50;

ax=0;
ay=0;
az=0;

var=100; % 噪聲方差

for i=1:N5-1
    if(i>N1-1&i<N2-1)
        ax=0.075;
        ay=0.175;
        az=0.375;
        vx(i+1)=vx(i)+ax*T;
        vy(i+1)=vy(i)+ay*T;
        vz(i+1)=vz(i)+az*T;
    elseif(i>N2-1&i<N3-1)
        ax=0;
        ay=0;
        az=0;
        vx(i+1)=vx(i)+ax*T;
        vy(i+1)=vy(i)+ay*T;
        vz(i+1)=vz(i)+az*T;
    elseif(i>N3-1&i<N4-1)
        ax=-0.3;
        ay=-0.3;
        ay=-0.3;
        vx(i+1)=vx(i)+ax*T;
        vy(i+1)=vy(i)+ay*T;
        vz(i+1)=vz(i)+az*T;
    else
        ax=0;
        ay=0;
        az=0;
        vx(i+1)=vx(i)+ax*T;
        vy(i+1)=vy(i)+ay*T;
        vz(i+1)=vz(i)+az*T;
    end
    x(i+1)=x(i)+vx(i)*T+0.5*ax*T^2;
    y(i+1)=y(i)+vy(i)*T+0.5*ay*T^2;
    z1(i+1)=z1(i)+vz(i)*T+0.5*az*T^2;
end

%figure;
%plot3(x,y,z1,'r-');
%grid on;

rex=num:N5;
rey=num:N5;
rez=num:N5;

revx=num:N5;
revy=num:N5;
revz=num:N5;

% 濾波num次
for m=1:num
    % 加入噪聲
    nx=100*randn(N5,1);
    ny=100*randn(N5,1);
    nz=100*randn(N5,1);
    zx=x+nx;
    zy=y+ny;
    zz=z1+nz; % 注意：z1就是真實z，z標識被占用了
    
    % 濾波初始化
    rex(m,1)=2000;
    rey(m,1)=10000;
    rez(m,1)=4000;
    
    ki=0;
    low=1;high=0;
    u=0;
    ua=0;
    e=0.8;
    z=2:1;
    xks(1)=zx(1);
    yks(1)=zy(1);
    zks(1)=zz(1);
    xks(2)=zx(2);
    yks(2)=zy(2);
    zks(2)=zz(2);
    
    %o=4:4;g=4:2;h=2:4;q=2:2;xk=4:1;perr=4:4;
    o=6:6;g=6:3;h=2:4;q=3:3;xk=6:1;perr=6:6;
    o=[1,T,0,0,0,0;
        0,1,0,0,0,0;
        0,0,1,T,0,0;
        0,0,0,1,0,0
        0,0,0,0,1,T;
        0,0,0,0,0,1];
    h=[1,0,0,0,0,0;
        0,0,1,0,0,0
        0,0,0,0,1,0];
    g=[ T/2,0,0;
        T/2,0,0;
        0,T/2,0;
        0,T/2,0
        0,0,T/2;
        0,0,T/2];
    q=[10000,0,0;0,10000,0;0,0,10000];
    perr=[var^2,var^2/T,0,0,0,0;
          var^2/T,2*var^2/T^2,0,0,0,0;
          0,0,var^2,var^2/T,0,0;
          0,0,var^2/T,2*var^2/T^2,0,0
          0,0,0,0,var^2,var^2/T
          0,0,0,0,var^2/T,2*var^2/T^2];
    vx(1)=(zx(2)-zx(1))/2;
    vy(1)=(zy(2)-zy(1))/2;
    vz(1)=(zz(2)-zz(1))/2;
     
    xk=[zx(1);vx(1);zy(1);vy(1);zz(1);vz(1)];
    
    % kalman濾波開始
    for r=3:N5
        if(u<=40)
            if(low==0)
                [o,h,g,q,perr,xk]=lmode_initial(T,r,zx,zy,zz,vkxs,vkys,vkzs,perr);%xuan perr2
                z=3:1; % xuan 2*1
                high=0;
                low=1;
                ua=0;
            end
            z=[zx(r);zy(r);zz(r)];% xuan
            xk1=o*xk;
            perr1=o*perr*o';
            k=perr1*h'*inv(h*perr1*h'+q);
            xk=xk1+k*(z-h*xk1);
            perr=(eye(6)-k*h)*perr1;%xuan
            xks(r)=xk(1,1);
            yks(r)=xk(3,1);
            zks(r)=xk(5,1);
            vkxs(r)=xk(2,1);
            vkys(r)=xk(4,1);
            vkzs(r)=xk(6,1);
            xkls(r)=xk1(1,1);
            ykls(r)=xk1(3,1);
            zkls(r)=xk1(5,1);
            
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%????????????????????
            perr11(r)=perr(1,1);
            perr12(r)=perr(1,2);
            perr22(r)=perr(2,2);
            
            if(r>=20)
                v=z-h*xk1;
                w=h*perr*h'+q;
                p=v'*inv(w)*v;
                u=e*u+p;
                s(r-19)=u;
            end
        elseif(u>40)
            if(high==0)
                [o,g,h,q,perr,xk]=hmode_initial(T,r,e,zx,zy,zz,xkls,ykls,zkls,vkxs,vkys,vkzs,perr11,perr12,perr22);
                high=1;
                low=0;
                for i=r-5:r-1
                    z=[zx(i);zy(i);zz(i)];%xuan
                    xk1=o*xk;
                    perr1=o*perr*o';
                    k=perr1*h'*inv(h*perr1*h'+q);
                    xk=xk1+k*(z-h*xk1);
                    perr=(eye(9)-k*h)*perr1;
                    xks(r)=xk(1,1);
                    yks(r)=xk(3,1);
                    zks(r)=xk(5,1);
                    vkxs(r)=xk(2,1);
                    vkys(r)=xk(4,1);
                    vkzs(r)=xk(6,1);
                    xkls(r)=xk1(1,1);
                    ykls(r)=xk1(3,1);
                    zkls(r)=xk1(5,1);
                end
            end
            z=[zx(r);zy(r);zz(r)];
            xk1=o*xk;
            perr1=o*perr*o';
            k=perr1*h'*inv(h*perr1*h'+q);
            xk=xk1+k*(z-h*xk1);
            perr=(eye(9)-k*h)*perr1;
            xks(r)=xk(1,1);
            yks(r)=xk(3,1);
            zks(r)=xk(5,1);
            vkxs(r)=xk(2,1);
            vkys(r)=xk(4,1);
            vkzs(r)=xk(6,1);
            xkls(r)=xk1(1,1);
            ykls(r)=xk1(3,1);
            zkls(r)=xk1(5,1);
          
            
            %xuan 注意這里ag時加速度！
            %ag=[xk(5,1);xk(6,1)];
            ag=[xk(7,1);xk(8,1);xk(9,1)];
            perr2=perr;
            ki=ki+1;
            %pm=[perr(5,5),perr(5,6);perr(6,5),perr(6,6)];
            pm=[perr(7,7),perr(7,8),perr(7,9);perr(8,7),perr(8,8),perr(8,9);perr(9,7),perr(9,8),perr(9,9)];
            
            pa=ag'*inv(pm)*ag;
            sa(r)=pa;
            if(ki>5)
                u1=sa(r-4)+sa(r-3)+sa(r-2)+sa(r-1)+sa(r);
                sb(r)=u1;
                if(u1<20)
                    u=0;
                end
            end
        end
        rex(m,r)=xks(r);
        rey(m,r)=yks(r);
        rez(m,r)=zks(r);
        % xuan
        revx(m,r)=vkxs(r);
        revy(m,r)=vkys(r);
        revz(m,r)=vkzs(r);
    end %結束一次濾波
end

ex=0;ey=0;ez=0;eqx=0;eqy=0;eqz=0;
ex1=N5:1;ey1=N5:1;ez1=N5:1;qx=N5:1;qy=N5:1;qz=N5:1;

% 計算均值及均方差
for i=1:N5
    for j=1:num
        ex=ex+x(i)-rex(j,i);
        ey=ey+y(i)-rey(j,i);  
        ez=ez+z1(i)-rez(j,i); 
        eqx=eqx+(x(i)-rex(j,i))^2;
        eqy=eqy+(y(i)-rey(j,i))^2;
        eqz=eqz+(z1(i)-rez(j,i))^2;
    end
    ex1(i)=ex/num;
    ey1(i)=ey/num; 
    ez1(i)=ez/num;
    qx(i)=(eqx/num-(ex1(i)^2))^0.5;
    qy(i)=(eqy/num-(ey1(i)^2))^0.5;
    qz(i)=(eqz/num-(ez1(i)^2))^0.5;
    ex=0;eqx=0;ey=0;eqy=0;ez=0;eqz=0;
end

figure(1);
plot3(x,y,z1,'k-',zx,zy,zz,'g:',xks,yks,zks,'r-');
grid on;
%legend('真實軌跡','觀測樣本','估計軌跡');

figure(2);
subplot(2,2,1);
plot3(x,y,z1,'k-',zx,zy,zz,'g:',xks,yks,zks,'r-');
title('x方向位置濾波');
grid on;
subplot(2,2,2);
ih=1:N5;
plot(ih,x,'b',ih,zx,'g',ih,xks,'r');
title('x方向位置濾波');
grid on;
subplot(2,2,3);
ih=1:N5;
plot(ih,y,'b',ih,zy,'g',ih,yks,'r');
title('y方向位置濾波');
grid on;
subplot(2,2,4);
ih=1:N5;
plot(ih,z1,'b',ih,zz,'g',ih,zks,'r');
title('z方向位置濾波');
grid on;

figure(7)
ih=1:N5;
plot(ih,y,'b',ih,zy,'g',ih,yks,'r');
title('x方向位置濾波');
legend('真實軌跡','觀測樣本','估計軌跡');