%DGPS自校正濾波/平滑算法與DNS誤差參數(shù)辨識(shí)程序
sd1=duoplwz1(:,1);
sg1=gps1(:,2);
WD1=gps1(:,3);
WF1=gps1(:,4);
WM1=gps1(:,5);
JD1=gps1(:,6);
JF1=gps1(:,7);
JM1=gps1(:,8);
H1=duoplwz1(:,2);
hg1=duoplwz1(:,6);
hy1=duoplwz1(:,7);
vx01=duoplwz1(:,4)./1000;
vy01=duoplwz1(:,3)./1000;
vz01=duoplwz1(:,5)./1000;

H0=90-H1;
sdh=floor(sd1./10000);
sdf=floor((sd1-sdh*10000)./100);
sdm=sd1-sdh*10000-sdf*100;
Td1=sdh*3600+sdf*60+sdm;
    
sgh=floor(sg1./10000);
sgf=floor((sg1-sgh*10000)./100);
sgm=sg1-sgh*10000-sgf*100;
Tg1=sgh*3600+sgf*60+sgm;
 
for i=1:1:10416
w1(i)=(WD1(i)+WF1(i)./60+WM1(i)./3600);
j1(i)=(JD1(i)+JF1(i)./60+JM1(i)./3600);
end
w1(1016)=w1(1017);
j1(1016)=j1(1017);
w1(3137)=w1(3135);
j1(3137)=j1(3135);
w1(3136)=w1(3135);
j1(3136)=j1(3135);
 
H=[1,0];
a=6378.137*10.^3;
e=6.69437999014*10.^(-3);
W=mean(w1);
T=1;
R1=((1-(e*sin(W*pi./180)).^2)^(1.5))./(a*(1-e.^2));
R2=((1-(e*sin(W*pi./180)).^2)^(0.5))./(a*cos(W*pi./180));
R1=R1*180./pi;
R2=R2*180./pi;
A1=[1,T*R1;0,1];
A2=[1,T*R2;0,1];
D1=[0.5*(T.^2).*R1,T].';
D2=[0.5*(T.^2).*R2,T].';

I=eye(2);
a1=-2;
a2=1;
y1=w1';
m1=armax(y1,[2,2],'SearchDirection','gn');
[C1,P1,DE1]=th2par(m1);
d11=C1(3,1);
d21=C1(4,1);
M1=[0.5*R1*T.^4,6;0.25*R1*T.^4,-4;0,1];
s1=[(1+d11.^2+d21.^2)*DE1;(d11+d11*d21)*DE1;d21*DE1];
S1=inv(M1'*M1)*M1'*s1;
dv1=S1(2,1);
de1=S1(1,1);
 
y2=j1';
m2=armax(y2,[2,2],'SearchDirection','gn');
[C2,P2,DE2]=th2par(m2);
d12=C2(3,1);
d22=C2(4,1);
M2=[0.5*R2*T.^4,6;0.25*R2*T.^4,-4;0,1];
s2=[(1+d12.^2+d22.^2)*DE2;(d12+d12*d22)*DE2;d22*DE2];
S2=inv(M2'*M2)*M2'*s2;
dv2=S2(2,1);
de2=S2(1,1);
 
dv11=dv1.^(0.5)./R1;
dv12=dv2.^(0.5)./R2;
de11=de1.^(0.5)./R1;
de12＝de2.^(0.5)./R2;

I=eye(2);
f0=I;
f1=A1*f0+a1*I;
f=[H;H*f1];
f=inv(f'*f)*f'
M02=1-dv1/DE1;
M12=d11-a1*dv1/DE1;
M2=[M02,M12].'
K1=f*M2; 
F11=A1;
F1=[I-K1*H]*F11;
p1=dv1*K1(1,1);
p2=dv1*K1(2,1);
p3=dv1*K1(2,1);
q1=dv1*K1(1,1)/(1-K1(1,1));
q2=(dv1+q1)*K1(2,1);
p4=(q2-p2-D1(1,1)*D1(2,1)*de1)./A1(1,2);
P1=[p1,p2;p3,p4];
q4=p4+K1(2,1)*q2;
Q1=[q1,q2;q2,q4];
 
I=eye(2);
f0=I;
f1=A2*f0+a1*I;
f=[H;H*f1];
f=inv(f'*f)*f'
M02=1-dv2/DE2;
M12=d12-a1*dv2/DE2;
M2=[M02,M12].'
K2=f*M2; 
F12=A2;
F2=[I-K2*H]*F12;
p1=dv2*K2(1,1);
p2=dv2*K2(2,1);
p3=dv2*K2(2,1);
q1=dv2*K2(1,1)/(1-K2(1,1));
q2=(dv2+q1)*K2(2,1);
p4=(q2-p2-D2(1,1)*D2(2,1)*de2)./A2(1,2);
P2=[p1,p2;p3,p4];
q4=p4+K2(2,1)*q2;
Q2=[q1,q2;q2,q4];
 
Y1=[w1(1),0]';
Y2=[j1(1),0]';
for t=2:10416
Y11=A1*Y1;
Y22=A2*Y2;
y11(t)=Y11(1,1);
y21(t)=Y11(2,1);
y12(t)=Y22(1,1);
y22(t)=Y22(2,1);
Y1=F1*Y1+K1*w1(t);
Y2=F2*Y2+K2*j1(t);
x11(t)=Y1(1,1);
x21(t)=Y1(2,1);
x12(t)=Y2(1,1);
x22(t)=Y2(2,1);
end
 
Q1=inv(Q1);
A10=P1*A1'*Q1;
Q2=inv(Q2);
A20=P2*A2'*Q2;
X01=Y1;
X02=Y2;  
 xw01(10416)=X01(1,1);
 xj01(10416)=X02(1,1);
  t=10416-1;
    
while t>0
Y11=[y11(t+1),y21(t+1)]';
    Y22=[y12(t+1),y22(t+1)]';
    Y1=[x11(t),x21(t)]';
    Y2=[x12(t),x22(t)]';
    X01=Y1+A10*(X01-Y11);
    X02=Y2+A20*(X02-Y22);
    xw01(t)=X01(1,1);
    xj01(t)=X02(1,1);
t=t-1
end
xw01(1)=w1(1);
xj01(1)=j1(1);
sw=(xw01-w1)./R1;
sj=(xj01-j1)./R2;

j=1;
for i=1:2027
  while (Tg1(j)~=Td1(i));
      j=j+1;
  end 
      W1(i)=xw01(j);
      J1(i)=xj01(j);  
end
 
a=6378.137*10.^3;
e=6.69437999014*10.^(-3);
N1(1)=W1(1);
E1(1)=J1(1);  
X1=[0,0,0,0]';
I=eye(4);
P=10.^2*I;
WW=eye(2);  

for t=2:2027
    s=Td1(t)-Td1(t-1);
R1=((1-(e*sin(W1(t-1)*pi./180)).^2)^(1.5))./(a*(1-e.^2));
R2=((1-(e*sin(W1(t-1)*pi./180)).^2)^(0.5))./(a*cos(W1(t-1)*pi./180));
R1=R1*180./pi;
R2=R2*180./pi;
s1=s*vx01(t)*cos(H0(t-1)*pi./180)*cos(hg1(t-1)*pi./180);
s2=-s*vy01(t)*sin(H0(t-1)*pi./180)*cos(fy1(t-1)*pi./180);
s3=s*vx01(t)*sin(H0(t-1)*pi./180)*cos(hg1(t-1)*pi./180);
s4=s*vy01(t)*cos(H0(t-1)*pi./180)*cos(fy1(t-1)*pi./180);
F=[s1,s2,s3,s4;s3,s4,-s1,-s2];
SE=(J1(t)-J1(t-1))./R2;
SN=(W1(t)-W1(t-1))./R1;
Z=[SE,SN]';
P=P-P*F'*inv(WW+F*P*F')*F*P;
X1=X1+P*F'*(Z-F*X1);
x1(t)=X1(1,1);
x2(t)=X1(2,1);
x3(t)=X1(3,1);
x4(t)=X1(4,1);
end

se(1)=J1(1);
sn(1)=W1(1);
se0(1)=J1(1);
sn0(1)=W1(1);
for t=2:2027
    s=Td1(t)-Td1(t-1);
R1=((1-(e*sin(sn(t-1)*pi./180)).^2)^(1.5))./(a*(1-e.^2));
R2=((1-(e*sin(sn(t-1)*pi./180)).^2)^(0.5))./(a*cos(sn(t-1)*pi./180));
R1=R1*180./pi;
R2=R2*180./pi;
s1=s*vx01(t)*cos(H0(t-1)*pi./180)*cos(hg1(t-1)*pi./180);
s2=-s*vy01(t)*sin(H0(t-1)*pi./180)*cos(fy1(t-1)*pi./180);
s3=s*vx01(t)*sin(H0(t-1)*pi./180)*cos(hg1(t-1)*pi./180);
s4=s*vy01(t)*cos(H0(t-1)*pi./180)*cos(fy1(t-1)*pi./180);
F=[s1,s2,s3,s4;s3,s4,-s1,-s2];
VG=F*X1;
c11=cos(hg1(t-1)*pi./180)*cos(H0(t-1)*pi./180)-sin(hg1(t-1)*pi./180)*sin(fy1(t-1)*pi./180)*sin(H0(t-1)*pi./180);
c12=-cos(fy1(t-1)*pi./180)*sin(H0(t-1)*pi./180);
c13=sin(hg1(t-1)*pi./180)*cos(H0(t-1)*pi./180)+cos(hg1(t-1)*pi./180)*sin(fy1(t-1)*pi./180)*sin(H0(t-1)*pi./180);
c21=cos(hg1(t-1)*pi./180)*sin(H0(t-1)*pi./180)+sin(hg1(t-1)*pi./180)*sin(fy1(t-1)*pi./180)*cos(H0(t-1)*pi./180);
c22=cos(fy1(t-1)*pi./180)*cos(H0(t-1)*pi./180);
c23=sin(hg1(t-1)*pi./180)*sin(H0(t-1)*pi./180)-cos(hg1(t-1)*pi./180)*sin(fy1(t-1)*pi./180)*cos(H0(t-1)*pi./180);
c31=-sin(hg1(t-1)*pi./180)*cos(fy1(t-1)*pi./180);
c32=sin(fy1(t-1)*pi./180);
c33=cos(hg1(t-1)*pi./180)*cos(fy1(t-1)*pi./180);
C=[c11,c12,c13;c21,c22,c23;c31,c32,c33];
vg=s*C*[vx01(t),vy01(t),vz01(t)]';
se0(t)=se0(t-1)+vg(1,1)*R2;
sn0(t)=sn0(t-1)+vg(2,1)*R1;
sj0(t)=se0(t)-J1(t);
sw0(t)=sn0(t)-W1(t);
ssj0(t)=sj0(t)./R2;
ssw0(t)=sw0(t)./R1;
se(t)=se(t-1)+VG(1,1)*R2;
sn(t)=sn(t-1)+VG(2,1)*R1;
sj1(t)=se(t)-J1(t);
sw1(t)=sn(t)-W1(t);
ssj1(t)=sj1(t)./R2;
ssw1(t)=sw1(t)./R1;
end