%          end


%        V_en_n_c(i+1,1)=V_en_n_c(i,1)+(f_n_c(i,1)-(V_en_n_c(i,2)*sec(Lat_c(i))/R(i)+2*omega_ie_e(3))*sin(Lat_c(i))*V_en_n_c(i,2)+V_en_n_c(i,1)/R(i)*V_en_n_c(i,3))*delta_t;
%        V_en_n_c(i+1,2)=V_en_n_c(i,2)+(f_n_c(i,2)+(V_en_n_c(i,2)*sec(Lat_c(i))/R(i)+2*omega_ie_e(3))*(sin(Lat_c(i))*V_en_n_c(i,1)+cos(Lat_c(i))*V_en_n_c(i,3)))*delta_t;
%        V_en_n_c(i+1,3)=V_en_n_c(i,3)+(f_n_c(i,3)-V_en_n_c(i,1)/R(i)*V_en_n_c(i,1)-(V_en_n_c(i,2)*sec(Lat_c(i))/R(i)+2*omega_ie_e(3))*cos(Lat_c(i))*V_en_n_c(i,2)+g_c)*delta_t;
         
%       if i<length(t)
%               tspan=t(i:i+1);
%                if i<4
%                    q(i+1,:)=euler2quat([gama(i+1), theta(i+1),cesi(i+1)]);
%                
%                else
%                    options=odeset('RelTol',1e-7);
%                    [tpoint qq]=ode45(@(tpoint,q) difq(tpoint,q,omega_nb_b_c(i,:),t(1:i)),tspan,q(i,:),options);
%                    q(i+1,:)=qq(end,:);
%                end
%               
%           else tspan=[t(i)  t(i)+delta_t]';
%                options=odeset('RelTol',1e-7);
%               [tpoint qq]=ode45(@(tpoint,q) difq(tpoint,q,omega_nb_b_c(i,:),t(1:i)),tspan,q(i,:),options);%對于常微分方程形式：y'=f(t,y),f可以有其他參數，但頭兩個輸入變量必須為時間t和在該時間的狀態y
%               q(i+1,:)=qq(end,:);%  對于四元數微分方程來說，q指的是由參考系轉向動系這個轉動所代表的四元數，方程中的角速度代表的也是動系相對于參考系的角速度
%           end

%   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的捏線性擬和系數，均設為行向量                    
%                      h=delta_t;               %pp中，域coefs的系數是按自變量的將冪排列的，也就是說：coefs(:,1)為自變量最高次冪的系數
%                      delta_theta(i,:)=a.*h+b.*h^2./2+cross(a,b).*h^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

%     if i==1 
%          q(i+1,:)=euler2quat([gama(i+1), theta(i+1),cesi(i+1)]);
%     else        
%            alfa=delta_t.*omega_nb_b_c(i,:);
%            omega_nb_b_crct=cross(alfa,omega_nb_b_c(i,:))./2+omega_nb_b_c(i,:);
%            delta_theta0=norm(omega_nb_b_crct)*delta_t;
%            M_delta_theta0=delta_t.*[           0            -omega_nb_b_crct(1)   -omega_nb_b_crct(2)       -omega_nb_b_crct(3)
%                                     omega_nb_b_crct(1)                0            omega_nb_b_crct(3)       -omega_nb_b_crct(2)   
%                                     omega_nb_b_crct(2)      -omega_nb_b_crct(3)           0                  omega_nb_b_crct(1)       
%                                     omega_nb_b_crct(3)       omega_nb_b_crct(2)   -omega_nb_b_crct(1)              0          ];
          
%            delta_theta0=norm(omega_nb_b_c(i,:))*delta_t;
%            M_delta_theta0=delta_t.*[           0             -omega_nb_b_c(i,1)    -omega_nb_b_c(i,2)      -omega_nb_b_c(i,3)
%                                       omega_nb_b_c(i,1)         0                   omega_nb_b_c(i,3)      -omega_nb_b_c(i,2)
%                                       omega_nb_b_c(i,2)      -omega_nb_b_c(i,3)             0               omega_nb_b_c(i,1)  
%                                       omega_nb_b_c(i,3)       omega_nb_b_c(i,2)    -omega_nb_b_c(i,1)              0   ];
%           q(i+1,:)=q(i,:)*(cos(delta_theta0/2).*eye(4)+(sin(delta_theta0/2)/delta_theta0).*M_delta_theta0)';
%           q(i+1,:)=quatnormalize(q(i+1,:));       
%      end


     C_nb_c{i+1}=quat2dcm(q(i+1,:));
     [cesi_c(i+1) theta_c(i+1) gama_c(i+1)]=dcm2angle(C_nb_c{i+1});%計算姿態角

     R_sim(i,:)=[t(i) R(i)];%R_sim為errormodel中的輸入參數，需要加上時間信息
    
     delta_Ven_n_c(i,:)=V_en_n_c(i,:)-Ven_n(i,:);
end

%計算平臺偏差角fi
% for j=1:length(t)
%     q_ture(j,:)=dcm2quat(C_nb{j});
%     delta_q(j,:)=quatmultiply(q_ture(j,:),quatconj(q(j,:)));
%     fi=2.*delta_q(j,2:4);
%     delta_fi(j,:)=fi;
% end

%計算狀態誤差
%給誤差狀態方程傳遞參數
for j=1:length(t)
delta_omega_ib_n(j,:)=[t(j) (omega_meas(1,:,j)-omega_ib_b(j,:))*C_nb_c{j}+omega_meas(1,:,j)*(C_nb_c{j}-C_nb{j})];%(omega_meas(1,:,j)-omega_ib_b(j,:))*C_nb_c{j}+omega_meas(1,:,j)*(C_nb_c{j}-C_nb{j})
delta_omega_in_n(j,:)=[t(j) omega_in_n_c(j,:)-omega_in_n(j,:)];
delta_f_n(j,:)=[t(j) (Ameas(1,:,j)-f_b(j,:))*C_nb_c{j}];
delta_omega_ie_n(j,:)=[t(j) omega_ie_n_c(j,:)-omega_ie_n(j,:)];
omega_nb_n_sim(j,:)=[t(j),omega_nb_n_c(j,:)];
end;
omega_in_n_sim=[t omega_in_n_c];
omega_ie_n_sim=[t omega_ie_n_c];
Ven_n_sim=[t V_en_n_c];
f_n_sim=[t f_n_c];
Lat_sim=[t Lat_c(1:length(t))'./(2*pi)];

sim('errormodel',t)
save navsim

%設地速初值為0，則位置速率初值也為0



figure,
subplot(3,1,1),plot([delta_fi(:,1) fi_sim(:,1)]);xlabel('time/s');ylabel('北向平臺誤差角 deg');grid on;
subplot(3,1,2),plot([delta_fi(:,2) fi_sim(:,2)]);xlabel('time/s');ylabel('東向平臺誤差角 deg');grid on;
subplot(3,1,3),plot([delta_fi(:,3) fi_sim(:,3)]);xlabel('time/s');ylabel('地向平臺誤差角 deg');grid on;

figure,
plot([(V_en_n_c(1:length(t),1)-Ven_n(:,1)) delta_Ven_n(:,1)]);xlabel('time/s');ylabel('北向速度偏差 m/s');grid on;
figure,
plot([(V_en_n_c(1:length(t),2)-Ven_n(:,2)) delta_Ven_n(:,2)]);xlabel('time/s');ylabel('東向速度偏差 m/s');grid on;
figure,
plot([(V_en_n_c(1:length(t),3)-Ven_n(:,3)) delta_Ven_n(:,3)]);xlabel('time/s');ylabel('地向速度偏差 m/s');grid on;
% figure,
% plot((omega_en_n_c(:,1)-omega_en_n(:,1)));xlabel('time/s');ylabel('北向位置角速率偏差 rad/s');grid on;
% figure,
% plot((omega_en_n_c(:,2)-omega_en_n(:,2)));xlabel('time/s');ylabel('東向位置角速率偏差 rad/s');grid on;
% figure,
% plot((omega_en_n_c(:,3)-omega_en_n(:,3)));xlabel('time/s');ylabel('地向位置角速率偏差 rad/s');grid on;

% figure,
% subplot(1,2,1),plot(Lat.*180./pi);xlabel('time/s');ylabel('真實緯度');subplot(1,2,2),plot(Lat_c.*180./pi);xlabel('time/s');ylabel('解算緯度');
% figure,
% subplot(1,2,1),plot(Lon.*180./pi);xlabel('time/s');ylabel('真實經度');subplot(1,2,2),plot(Lon_c.*180./pi);xlabel('time/s');ylabel('解算經度');
% figure,
% subplot(1,2,1),plot(h_a);xlabel('time/s');ylabel('高度');subplot(1,2,2),plot(h_c);xlabel('time/s');ylabel('解算高度');

figure,
subplot(1,2,1),plot([(Lat_c(1:length(t))'-Lat).*180./pi delta_Lat]);xlabel('time/s');ylabel('緯度偏差 degree');grid on;
subplot(1,2,2),plot([(Lon_c(1:length(t))'-Lon).*180./pi delta_Lon]);xlabel('time/s');ylabel('經度偏差 degree');grid on;

% figure,
%  subplot(1,2,1),plot(cesi.*180./pi), xlabel('time/s');ylabel('航向角cesi');subplot(1,2,2),plot(cesi_c.*180./pi),xlabel('time/s');ylabel('解算航向角cesi');
%  figure,
%  subplot(1,2,1),plot(theta.*180./pi), xlabel('time/s');ylabel('俯仰角theta');subplot(1,2,2),plot(theta_c.*180./pi),xlabel('time/s');ylabel('解算俯仰角theta');
%  figure,
%  subplot(1,2,1),plot(gama.*180./pi),xlabel('time/s');ylabel('滾動角gama'); subplot(1,2,2),plot(gama_c.*180./pi),xlabel('time/s');ylabel('解算滾動角gama');

figure,
subplot(3,1,1),plot(cesi_c(1:length(t))'-cesi);xlabel('time/s');ylabel('航向角偏差');grid on;
subplot(3,1,2),plot(theta_c(1:length(t))'-theta);xlabel('time/s');ylabel('俯仰角偏差');grid on;
subplot(3,1,3),plot(gama_c(1:length(t))'-gama);xlabel('time/s');ylabel('滾動角偏差');grid on;

 figure,
 subplot(3,1,1),plot((S_c(1:length(t),1)-S(:,1)));xlabel('time/s');ylabel('航向偏差');grid on;
 subplot(3,1,2),plot((S_c(1:length(t),2)-S(:,2)));xlabel('time/s');ylabel('射程偏差');grid on;
 subplot(3,1,3),plot((S_c(1:length(t),3)-S(:,3)));xlabel('time/s');ylabel('高度偏差');grid on;

% figure,
% subplot(3,1,1),plot((P_i_c(1:length(t),1)-P_i(:,1)));xlabel('time/s');ylabel('航向偏差');grid on;
% subplot(3,1,2),plot((P_i_c(1:length(t),2)-P_i(:,2)));xlabel('time/s');ylabel('射程偏差');grid on;
% subplot(3,1,3),plot((P_i_c(1:length(t),3)-P_i(:,3)));xlabel('time/s');ylabel('高度偏差');grid on;

out{1}=fi_sim;out{2}=delta_Ven_n; out{3}=S_c-S;
save delta 'delta_Ven_n_c' 'delta_Ven_n' 'fi_sim' 'C_nb_c';

for i=1:length(t)
    delta_norm_f_n(i)=norm(f_n_c(i,:)-f_n(i,:));
end
figure,
plot(delta_norm_f_n);xlabel('time/s');ylabel('比力計算偏差');