%利用參數誤差進行地形重建的程序080311
%[3 -3 -0.01 -0.0035 -2]
x_canshu=[ -0.32987458214671  -0.46722071107143  -0.00964584278773   0.00061281118510  -1.61329184925155]
 

eps0=1e-8;
pi=3.14159265;
H=8000;
H_original=H;
f=9.6e9;
c=3e8;
lamda=c/f;
thetab=pi/6;
thetab_original=thetab;
b=0.6;
b_original=b;
bn_original=b;
bv=0;  %%%%r2變化，theta1變，z公式變
bn=b;
M=100;

dtz=peaks(M)*100;
L_Known=9;
x_distance=0; 
y_distance=1; 
r1MModH=dtz;

for i=1:1:M
    for k=1:1:M
              r1(i,k)=sqrt(i^2+(k+8000-50)^2+(H-dtz(i,k)).^2);
              r2(i,k)=sqrt((H-dtz(i,k)+b*sin(thetab)).^2+(k+8000-50-b*cos(thetab)).^2+(i-bv).^2);
        thetap(i,k)=asin(i/r1(i,k));  %查了幾天才發現除這里要加點，不然結果大不相同啊(程序改動，現在不需要了）
    end
end

%--------------------------------------------------------沿距離向取L_Known個已知點
for k=1:L_Known
    x_Known=1;
   H_Known(k)=r1MModH(1+(k-1)*x_distance,x_Known+(k-1)*y_distance);
end
if i==0
     for x=1:1:M
       for y=1:1:M  
           dx1=dtz(x,y);  
          r1(x,y)=sqrt(x^2+(y)^2+(H_original-dx1)^2);   
          r2(x,y)=sqrt((H_original-dx1+b_original*sin(thetab_original)).^2+(y-b_original*cos(thetab_original))^2+(x-bv)^2);       
       end
     end
     beta=asin(x./r1);
end
    fai =2*pi*(r2-r1)/lamda;
     
        
      CanshuDelta=[3 -3 -0.01 -0.0035 -2]-x_canshu;      
 % CanshuDelta=[0 0 0 0 0 ]-x_canshu;      
    H_tempH=H_Known;
    H=H-CanshuDelta(1);
r1=r1-CanshuDelta(2);
b=b-CanshuDelta(3);
thetab=thetab-CanshuDelta(4);       %delta_thetab;
	
  fai=fai-CanshuDelta(5);
	

theta1=asin((b.^2+bv.^2)./r1/2/b-lamda*fai/2/pi/b-lamda^2*fai.^2/8/pi/pi/b./r1);

for i=1:1:M
    for k=1:1:M
        z_improve(i,k)=H-r1(i,k).*(-(sin(theta1(i,k))-bv/b*sin(thetap(i,k)))*sin(thetab)+sqrt(cos(thetap(i,k)).^2-(sin(theta1(i,k))-bv/b*sin(thetap(i,k))).^2).*cos(thetab));
      end
end     %%%%%%%%%%%%%%加入誤差后的高度：
%theta=acos(cos(0)*cos(thetab+0+theta1));
 %1.定標點,點的高度差

errorH=z_improve-dtz;
figure(1)
  mesh(dtz)
  figure(2)
  mesh(z_improve)
  figure(3)
mesh( errorH)