% this script is to simulate the navigation algorithm 


clear;
load craft;

%[b a]=butter(5,pi/20,'low');omega_nb_b=filter(b,a,omega_ib_b);
q(1,:)=euler2quat([gama(1), theta(1),cesi(1)]);%對于函數euler2quat，輸入變量為一組歐拉角，其定義為分別繞X,Y,Z軸旋轉得到的角度[angle_x angle_y angle_z]
T=0.06;

V_en_n_c(1,:)=[0 0 0];
Lon_c(1)=120*pi/180;
Lat_c(1)=25*pi/180;
h_c(1)=0;
h_a=h;
h_i(1)=0;
S_c(1,:)=[0 0 0];

for i=1:length(t)

 if i<length(t)
       delta_t=t(i+1)-t(i);
   else delta_t=t(i)-t(i-1);
 end
 
R(i)=R0+h_c(i);

if i<length(t)
    h_c(i+1)=h_a(i+1);
else h_c(i+1)=h_a(i);
end;

%     if i<length(t)
%            h_i(i+1)=h_i(i)-V_en_n_c(i,3)*delta_t;
%            h_c(i+1)=h_a(i+1).^0.8+h_i(i+1).^0.2;%  注意0^0=1
%       else h_c(i+1)=h_a(i).^0.8+h_i(i).^0.2;
%     end
   
    Lat_c(i+1)=Lat_c(i)+V_en_n_c(i,1)*delta_t/R(i);
    Lon_c(i+1)=Lon_c(i)+V_en_n_c(i,2)*sec(Lat_c(i))*delta_t/R(i);
    
    

    omega_en_n_c(i,1)=V_en_n_c(i,2)/R(i);
    omega_en_n_c(i,2)=-V_en_n_c(i,1)/R(i);
    omega_en_n_c(i,3)=-V_en_n_c(i,2)*tan(Lat_c(i))/R(i);
    
     P_e_c(i,:)=[R(i).*cos(Lat_c(i)).*cos(Lon_c(i))  R(i).*cos(Lat_c(i)).*sin(Lon_c(i))  R(i).*sin(Lat_c(i))];
     C_ie_c=  [cos(omega_ie*t(i)) -sin(omega_ie*t(i)) 0
               sin(omega_ie*t(i)) cos(omega_ie*t(i))  0
                       0                   0          1];
     P_i_c(i,:)=P_e_c(i,:)*C_ie_c';
     P_n0_c(i,:)=P_i_c(i,:)*InitialCen'+[0 0 R0];%n0系為彈體發射點所處的LGV系
     S_c(i,:)=P_n0_c(i,:);
    
    C_en_c{i}=[-sin(Lat_c(i))*cos(Lon_c(i))       -sin(Lat_c(i))*sin(Lon_c(i))     cos(Lat_c(i))
                  -sin(Lon_c(i))                      cos(Lon_c(i))                  0
               -cos(Lat_c(i))*cos(Lon_c(i))    -cos(Lat_c(i))*sin(Lon_c(i))     -sin(Lat_c(i))]; 
           P_v_c(i,:)=P_e_c(i,:)*C_en_c{i}';
          
      
           
     omega_ie_n_c(i,:)=omega_ie_e*C_en_c{i}';
     omega_in_n_c(i,:)=omega_en_n_c(i,:)+omega_ie_n_c(i,:);
     C_nb_c{i}=quat2dcm(q(i,:));
     omega_nb_n_c(i,:)=omega_meas(1,:,i)*C_nb_c{i}-omega_in_n_c(i,:);
     omega_in_b_c(i,:)=omega_in_n_c(i,:)*C_nb_c{i}';
     
%      omega_ib_b_c(i,:)=omega_meas(i,:);%+epsino*C_nb{i}';%epsino為陀螺儀漂移在導航系n上的投影
     omega_nb_b_c(i,:)=omega_meas(1,:,i)-omega_in_b_c(i,:).*timecount(t(i),t(i));
     

 %%計算平臺誤差角     
%      C_nnc{i}=C_nb_c{i}'*C_nb{i};delta_q(i,:)=dcm2quat(C_nnc{i});%q所對應的旋轉角度是動系b相對于定系n的角度，而C_nb=quat2dcm(q)
%      delta_fi(i,:)=quat2euler(delta_q(i,:)).*180./pi;%計算平臺誤差角,單位為度
     delta_C_bn=(C_nb_c{i}-C_nb{i})';
     delta_fi_mat=-(delta_C_bn*C_nb_c{i});%delta_fi_mat為delta_fi的斜對稱形式
     delta_fi(i,:)=[delta_fi_mat(3,2) delta_fi_mat(1,3) delta_fi_mat(2,1)].*(180/pi);
      
     
  %%四元數更新
       if i==1 
         q(i+1,:)=euler2quat([gama(i+1), theta(i+1),cesi(i+1)]);
         elseif i<length(t)
                oomega_ib_b_c=interp1([t(i) ;t(i+1)],[omega_meas(1,:,i); omega_meas(1,:,i+1)],'linear','pp');                        
                a=omega_nb_b_c(i,:); 
                b=oomega_ib_b_c.coefs(:,1)';%這里a,b為對角速度omega_ib_b_c的捏線性擬和系數，均設為行向量                    
                dh=delta_t;                  %pp中，域coefs的系數是按自變量的將冪排列的，也就是說：coefs(:,1)為自變量最高次冪的系數
               delta_theta(i,:)=a.*dh+b.*dh^2./2+cross(a,b).*dh^3./12;  %此處是對omega_nb_b_c做一次線性擬合求得的旋轉矢量delta_theta;    
            
%                omega_nb_b_sim=[tt(:,i) omega_meas(i:i+nmb,:)-repmat(omega_in_b(i,:),nmb+1,1)];
%                sim('rotvecmdl');

%                delta_theta_span=(omega_meas(:,:,i)-repmat(omega_in_b_c(i,:),nmb,1)).*(delta_t/nmb);                 
%                delta_theta(i,:)=delta_theta_span(1,:)+delta_theta_span(2,:)+delta_theta_span(3,:)+delta_theta_span(4,:)+(cross(delta_theta_span(1,:),delta_theta_span(2,:))+cross(delta_theta_span(3,:),delta_theta_span(4,:))).*214./315 ...
%                 +(cross(delta_theta_span(1,:),delta_theta_span(3,:))+cross(delta_theta_span(2,:),delta_theta_span(4,:))).*46./105+cross(delta_theta_span(1,:),delta_theta_span(4,:)).*54./105+cross(delta_theta_span(2,:),delta_theta_span(3,:)).*214./315;

             delta_theta0=norm(delta_theta(end,:));
             u=normr(delta_theta(end,:));
             q_h=[cos(delta_theta0/2) sin(delta_theta0/2)*u(1) sin(delta_theta0/2)*u(2) sin(delta_theta0/2)*u(3)];
             q(i+1,:)=quatmultiply(q(i,:),q_h);
             q(i+1,:)=quatnormalize(q(i+1,:));
       else      q(i+1,:)=q(i,:);
    end

g_c(i,1)=g0*(1-2*h_c(i)/R0);
f_n_c(i,:)=Ameas(1,:,i)*C_nb_c{i};%+a_bias;  %a_bias為加速計常值零偏在n系上的投影，而在imu模型里，Abias是零偏在機體系b上的投影
     if i<length(t)
     V_en_n_c(i+1,:)=V_en_n_c(i,:)+(f_n_c(i,:)+[0 0 g_c(i)]-cross(omega_en_n_c(i,:)+2.*omega_ie_n_c(i,:),V_en_n_c(i,:))-cross(cross(omega_ie_n_c(i,:),omega_ie_n_c(i,:),2),P_v_c(i,:),2)).*delta_t;%梯形積分求速度
     end
%          if i==1%四階子樣優化補償劃船誤差
%              V_en_n_c(i+1,:)=V_en_n_c(i,:)+(Ameas(1,:,i)*C_nb_c{i}+[0 0 g_c(i)]-cross(omega_en_n_c(i,:)+2.*omega_ie_n_c(i,:),V_en_n_c(i,:))-cross(cross(omega_ie_n_c(i,:),omega_ie_n_c(i,:),2),P_v_c(i,:),2)).*delta_t;
%          elseif i<length(t)
%                 delta_V_span=Ameas(:,:,i).*(delta_t/nmb);
%                 delta_V=delta_V_span(1,:)+delta_V_span(2,:)+delta_V_span(3,:)+delta_V_span(4,:);
%                 delta_theta_gyro=delta_theta_span(1,:)+delta_theta_span(2,:)+delta_theta_span(3,:)+delta_theta_span(4,:);% delta_theta_gyro為陀螺儀在[t(i) t(i+1)]之間的輸出
%                delta_V_gcorm=([0 0 g_c(i)]-cross(omega_en_n_c(i,:)+2.*omega_ie_n_c(i,:),V_en_n_c(i,:))).*delta_t;
%                delta_V_rotm=cross(delta_theta_gyro,delta_V)./2;
%                k1=763/945;k2=334/945;k3=526/945;k4=654/945;
%                delta_V_sculm=k1.*(cross(delta_theta_span(1,:),delta_V_span(2,:))+cross(delta_theta_span(3,:),delta_V_span(4,:))+cross(delta_V_span(1,:),delta_theta_span(2,:))+cross(delta_V_span(3,:),delta_theta_span(4,:))) ...
%                  +k2.*(cross(delta_theta_span(1,:),delta_V_span(3,:))+cross(delta_theta_span(2,:),delta_V_span(4,:))+cross(delta_V_span(1,:),delta_theta_span(3,:))+cross(delta_V_span(2,:),delta_theta_span(4,:)))...
%                  +k3.*(cross(delta_theta_span(1,:),delta_V_span(4,:))+cross(delta_V_span(1,:),delta_theta_span(4,:)))...
%                  +k4.*(cross(delta_theta_span(2,:),delta_V_span(3,:))+cross(delta_V_span(2,:),delta_theta_span(3,:)));
%                 V_en_n_c(i+1,:)=V_en_n_c(i,:)+delta_V_gcorm+(delta_V+delta_V_rotm+delta_V_sculm)*C_nb_c{i};     
%          end
    
%%計算地速V_en_n_c的準確程度與兩個量有關：1是采樣時間delta_t,2是姿態矩陣C_nb_c，其中，delta_t在小到10e-2以
%%后速度誤差與它的關系就不大了，因為與其相關的只是計算速度的截斷誤差，對于一階線性算法，其截斷誤差為delta_t^2
%%當delta_t=10e-2時，截斷誤差應為10e-4,所以一味的增加采樣頻率并不能使誤差不斷減小，當采樣率增加到一定程度時，
%%C_nb_c的準確性就成為速度誤差的決定性因素。
  
    
    
%     if i<2
%              V_en_n_c(i+1,:)=V_en_n_c(i,:)+(f_n_c(i,:)-cross(omega_en_n_c(i,:)+2.*omega_ie_n_c(i,:),V_en_n_c(i,:))+[0 0 g_c(i)]).*delta_t;
%     else
%     delta_Vrot=delta_t.*cross(omega_nb_b(i,:),f_b(i,:))./2;%補償速度更新的旋轉效應
%     ff_n_c=interp1(t(i-1:i),f_n_c(i-1:i,:),'linear','pp');
%     pp_delta_V_en_n_c=fnint(ff_n_c);
%     delta_V_en_n_c=ppval(pp_delta_V_en_n_c,t(i)+delta_t)';
%     V_en_n_c(i+1,:)=V_en_n_c(i,:)+delta_V_en_n_c-(cross(omega_en_n_c(i,:)+2.*omega_ie_n_c(i,:),V_en_n_c(i,:))+[0 0 g_c(i)]).*delta_t;%地速解算
%     end
    

%       if i<length(t)
%               tspan=t(i:i+1);
%       else
%           tspan=[t(i)  t(i)+delta_t]';
%       end
%       [tspan VV_en_n]=ode45(@(tpoint,VV_en_n) difV_en(tpoint,VV_en_n,f_n_c(i,:),omega_en_n_c(i,:),omega_ie_n_c,g_c),tspan,V_en_n_c(i,:));
%       V_en_n_c(i+1,:)=VV_en_n(end,:);


%          if i<5
%             V_en_n_c(i+1,:)=V_en_n_c(i,:)+(f_n_c(i,:)-cross(omega_en_n_c(i,:)+2.*omega_ie_n_c(i,:),V_en_n_c(i,:))+[0 0 g_c(i)]).*delta_t;
%          else
%          
%          ff_n_c=spline(t(i-4:i)',f_n_c(i-4:i,:)');
%          oomega_ie_n_c=spline(t(i-4:i)',omega_ie_n_c(i-4:i,:)');
%          oomega_en_n_c=spline(t(i-4:i)',omega_en_n_c(i-4:i,:)');
%          gg_c=spline(t(i-4:i)',g_c(i-4:i)',1);
         
%         f1=ppval(ff_n_c,t(i))'-cross(ppval(oomega_en_n_c,t(i))'+2.*ppval(oomega_ie_n_c,t(i))',V_en_n_c(i,:))+[0 0 ppval(gg_c,t(i))];
%         f2=ppval(ff_n_c,t(i)+delta_t/2)'-cross(ppval(oomega_en_n_c,t(i)+delta_t/2)'+2.*ppval(oomega_ie_n_c,t(i)+delta_t/2)',V_en_n_c(i,:)+f1./2)+[0 0 ppval(gg_c,t(i)+delta_t/2)];
%         f3=ppval(ff_n_c,t(i)+delta_t/2)'-cross(ppval(oomega_en_n_c,t(i)+delta_t/2)'+2.*ppval(oomega_ie_n_c,t(i)+delta_t/2)',V_en_n_c(i,:)+f2./2)+[0 0 ppval(gg_c,t(i)+delta_t/2)];
%         f4=ppval(ff_n_c,t(i)+delta_t)'-cross(ppval(oomega_en_n_c,t(i)+delta_t)'+2.*ppval(oomega_ie_n_c,t(i)+delta_t)',V_en_n_c(i,:)+f3)+[0 0 ppval(gg_c,t(i)+delta_t)];
%         
%         V_en_n_c(i+1,:)=V_en_n_c(i,:)+(f1+2.*f2+2.*f3+f4).*delta_t./6;