% LBL卡爾曼濾波/平滑算法與DNS誤差參數辨識程序
zx=lbl(:,1);               
zy=lbl(:,2);             
tl=lbl(:,3);    
td=dns(:,7);                
hx=dns(:,6);                
vx=dns(:,1); 
vy=dns(:,2); 
fy=dns(:,4);
hg=dns(:,5);
 
zx0=zx(236:447);
zy0=zy(236:447);
m=armax(zx0,[2,2],'SearchDirection','gn');
[C,P0,DE]=th2par(m);
a1=-2;
a2=1;
d1=C(3,1);
d2=C(4,1);
b0=0;
b1=0.5;
b2=0.5;
M=[b1.^2+b2.^2,1+a1.^2+a2.^2;b1*b2,a1+a1*a2;0,a2];
s=[(1+d1.^2+d2.^2)*DE;(d1+d1*d2)*DE;d2*DE];
S2=inv(M'*M)*M'*s;
Qx=S2(1,1);
Rx=S2(2,1);
 
m=armax(zy0,[2,2],'SearchDirection','gn');
[C,P0,DE]=th2par(m);
a1=-2;
a2=1;
d1=C(3,1);
d2=C(4,1);
b0=0;
b1=0.5;
b2=0.5;
M=[b1.^2+b2.^2,1+a1.^2+a2.^2;b1*b2,a1+a1*a2;0,a2];
s=[(1+d1.^2+d2.^2)*DE;(d1+d1*d2)*DE;d2*DE];
S2=inv(M'*M)*M'*s;
Qy=S2(1,1);
Ry=S2(2,1);
 
I=eye(2);
Px=10.^2*I;
Py=10.^2*I;
X=[zx(1),8]';
Y=[zy(1),-0.5]';;
x01(1)=zx(1);
y01(1)=zy(1);
 
for t=2:450
  T=tl(t)-tl(t-1);
A=[1,T;0,1];
B=[0.5*T.^2,T]';
D=[0.5*T.^2,T]';
H=[1,0];
X=A*X;
x01(t)=X(1,1);
x02(t)=X(2,1);
P1x=A*Px*A.'+B*Qx*B.';
Kx=P1x*H.'*((H*P1x*H.'+Rx).^(-1));
Px=(I-Kx*H)*P1x;
X=X+Kx*(zx(t)-H*X);
x1(t)=X(1,1);
x2(t)=X(2,1);
p1x11(t)=P1x(1,1);
p1x12(t)=P1x(1,2);
p1x21(t)=P1x(2,1);
p1x22(t)=P1x(2,2);
px11(t)=Px(1,1);
px12(t)=Px(1,2);
px21(t)=Px(2,1);
px22(t)=Px(2,2);
Y=A*Y;
y01(t)=Y(1,1);
y02(t)=Y(2,1);
P1y=A*Py*A.'+B*Qy*B.';
Ky=P1y*H.'*((H*P1y*H.'+Ry).^(-1));
Py=(I-Ky*H)*P1y;
Y=Y+Ky*(zy(t)-H*Y);
y1(t)=Y(1,1);
y2(t)=Y(2,1);
p1y11(t)=P1y(1,1);
p1y12(t)=P1y(1,2);
p1y21(t)=P1y(2,1);
p1y22(t)=P1y(2,2);
py11(t)=Py(1,1);
py12(t)=Py(1,2);
py21(t)=Py(2,1);
py22(t)=Py(2,2);
end
 
X1=X;
Y1=Y;
t=450-1;
x(450)=X(1,1);
y(450)=Y(1,1);
while t>0   
T=tl(t+1)-tl(t);
A=[1,T;0,1];
B=[0.5*T.^2,T]';
D=[0.5*T.^2,T]';
H=[1,0];
P1x=[p1x11(t+1),p1x12(t+1);p1x21(t+1),p1x22(t+1)];
Px=[px11(t),px12(t);px21(t),px22(t)];
Ax=Px*A'*inv(P1x);
X=[x1(t),x2(t)]';
X0=[x01(t+1),x02(t+1)]';
X1=X+Ax*(X1-X0);   
x(t)=X1(1,1);
P1y=[p1y11(t),p1y12(t);p1y21(t),p1y22(t)];
Py=[py11(t),py12(t);py21(t),py22(t)];   
Ay=Py*A'*inv(P1y);
Y=[y1(t),y2(t)]';
Y0=[y01(t+1),y02(t+1)]';
Y1=Y+Ax*(Y1-Y0);  
y(t)=Y1(1,1);     
t=t-1
end
 
x(1)=zx(1);
y(1)=zy(1);
zx1=x(1:450);
zy1=y(1:450);
vx1=vx;
vy1=vy;
hx1=(0+hx).*pi./180;
fy1=fy.*pi./180;
hg1=hg.*pi./180; 
X=[1,1,0,0]'; 
I=eye(4);
P=10.^2*I;
WW=eye(2); 
S1=0;
S2=0;
S3=0;
S4=0;
i=2
for t=2:4975  
s0=td(t)-td(t-1);  
s1=s0*vx1(t)*cos(hx1(t))*cos(hg1(t));
s2=-s0*vy1(t)*sin(hx1(t))*cos(fy1(t));
s3=s0*vx1(t)*sin(hx1(t))*cos(hg1(t));
s4=s0*vy1(t)*cos(hx1(t))*cos(fy1(t));
S1=S1+s1;
S2=S2+s2;
S3=S3+s3;
S4=S4+s4;
    if (i<=450&floor(td(t))==tl(i))
    ZX=zx1(i)-zx1(i-1);  
    ZY=zy1(i)-zy1(i-1);
    Z=[ZX,ZY]';
    F=[S1,S2,S3,S4;S3,S4,-S1,-S2];
    P=P-P*F'*inv(WW+F*P*F')*F*P; 
    X=X+P*F'*(Z-F*X);
    X0000=[X(1,1),1,X(3,1),0]';
    x01(i)=X(1,1);
    x02(i)=X(2,1);
    x03(i)=X(3,1);
    x04(i)=X(4,1);
S1=0;
S2=0;
S3=0;
S4=0;
    i=i+1;
    end
end
sx1(1)=x(1);
sy1(1)=y(1);
sx2(1)=x(1);
sy2(1)=y(1);
for t=2:4975
s0=td(t)-td(t-1);  
s1=s0*vx1(t)*cos(hx1(t))*cos(hg1(t));
s2=-s0*vy1(t)*sin(hx1(t))*cos(fy1(t));
s3=s0*vx1(t)*sin(hx1(t))*cos(hg1(t));
s4=s0*vy1(t)*cos(hx1(t))*cos(fy1(t)); 
F=[s1,s2,s3,s4;s3,s4,-s1,-s2];
V1=F*[1,1,0,0]';
V2=F*X;
sx1(t)=sx1(t-1)+V1(1,1);
sy1(t)=sy1(t-1)+V1(2,1);
sx2(t)=sx2(t-1)+V2(1,1);
sy2(t)=sy2(t-1)+V2(2,1);
end 
i=2
SX1(1)=sx1(1);
SY1(1)=sy1(1);
SX2(1)=sx2(1);
SY2(1)=sy2(1);
for t=2:4975
 if (i<=450&floor(td(t))==tl(i))
SX1(i)=sx1(t);
SY1(i)=sy1(t);
SX2(i)=sx2(t);
SY2(i)=sy2(t);
i=i+1;
 end
end
sx01=x-SX1; 
sy01=y-SY1;
sx02=x-SX2;
sy02=y-SY2;