%new strapdown program  08_9_26
 %%%% simulate the quite state, change the error source to observe the
 %%%% influence;
 clear all
 close all
 clc
 %%%%%%%%%%%%%%%%%%%%%%%%%%%% Setting the simulate condition
 Simu_Time =1*3600;               %仿真时间，单位秒。
 SampleFreq =1;                    %采样频率，单位Hz >1 ，
 Coordinate = 1;      %%%%%%%%%%%%%%% choose coordinate 1 is NED; 2 is ENU
 Simu_Mode = 1;       %%%%%%%%%%%% choose Simulate mode: 1 is quite;2 is  generate Xwa frome a,w  or motion state;
 %%%%%%%%%%%%%%%%%% 3 is generate Xwa from carrier's track;
 Plot_Mode = 1;              %%%%%%%%%%%%% plot about 1 sec; 2 min; 3 h.
 isKalmanFilter = 1;           %%%%%%%%%%5 choose to use kalmanfilter.
 Kalman_Comp = 1;               %%%%%%%%%%% compensate: 0 none; 1 output check; 2 feedback check; 3 integrate compensatePara = Para_Setting;
 Plot_Style = 1;            %%%%%%%%%%%%%%%% set plot style: 1 integ; 2 compare;
 %%%%%%%%%%%%%%%%%%%%%%%%%%%55 choose the inertial system state: damp net.
 Para.A_E = [0 1 0 0 0 0]';                    %%%%%%%%%%%%%%%%% huangdemng P139_140 inter horizontal damp net A_E(1) = 1 yes, 0 no.
 Para.K1 = 0;                                  %%%%%%%%%%%5 1 out damp, 0 inner.
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%
 Para.Comp_Step = 100;               %%%%%%%%%%%%%5 output check and feedback check interval
 if Simu_Mode == 3
     SampleFreq = 1;
     Plot_Mode = 1;
 end;
 if isKalmanFilter==1
     kalman_filter_mode = 1;         %%%%%%%%%%%%%%% 1 velocity integrate; 2 position integrate; 3 EC pose integ
     %%%%%%%%%%%%%velocity and position integrate;
     nTGPS = [1;1];   %%%%%%%%%%%% the time that GPS or EC cycle is INS cycle;
     %%%%%%%%%%%%%%% note here, STEP = 2/SampleFreq;
     kalman_Para = initial_Kalman(Para,kalman_filter_mode);
 end;
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% adaptive filter fumengyin P98
 kalman_Para.Adaptive_F = 0;       %%%%%%%%%5555 0 none, 1 use;
 kalman_Para.Alpha = 1;  %%%%%%%%%% suaijianyinz alpha, beihang lunwen.
 kalman_Para.d = 0.000001;
 %%%%%%%%%%%%%%%%
 %%%%%%%%%%%%%%%%%%% 新型联邦最小二乘滤波算法及应用 改造。
 kalman_Para.LSF = 0;  %%%%%%%%%%%least square filter
 kalman_Para.LSF_lamad = 0.999; %%%%% the value is near 1
 %%%%%%初始设置
 AA = [];                        % 保存计算姿态值
 VV = [];                        % 保存计算速度值
 POS = [];                       % 保存计算位置值
 %%%%%%%%%%
 AA_Comp = [];                        % 保存滤波后姿态值
 VV_Comp  = [];                        % 保存滤波后速度值
 POS_Comp  = [];                       % 保存滤波后位置值[Xw Xa AA0] = Data_Genetate(Simu_Mode,Simu_Time,SampleFreq,Coordinate,Para);
STEP = 2/SampleFreq;                     % 龙格库塔法步长 2*t  t为采样周期 t=1/SampleFreq;
 M = length(Xw);
 CalcCnt = 1;h = waitbar(0,'navigation is running...');
nTime = [0 0];
 while CalcCnt <= (M-2)
     waitbar(CalcCnt/M,h);
     Vr = zeros(3,1);
     [Para kalman_Para Xw]  = AVP_Calc(Para.bFirstRun_AVP,CalcCnt,Xw,Xa,Para,kalman_Para,STEP,Kalman_Comp,Vr);
     AA = [AA Para.A];
     VV = [VV Para.V];
     POS = [POS Para.Pos];    %% calculate the Error
     %%%%%%
     AA_Comp = [AA_Comp Para.A_Comp];
     VV_Comp = [VV_Comp Para.V_Comp];
     POS_Comp = [POS_Comp Para.Pos_Comp];    if and(isKalmanFilter == 1,nTime(1) >= nTGPS(1)-1)
         nTime(1) = 0;
         kalman_Para = measurement_update(Para,kalman_Para,STEP*nTGPS(1),nTime(2),kalman_filter_mode);
         if nTime(2) >= nTGPS(2)-1
             nTime(2) = 0;
             kalman_Para = Kalman_Filter(Para,kalman_Para,CalcCnt,STEP*nTGPS(2),nTGPS(2),Kalman_Comp);
             Para.x = kalman_Para.x;   %%%%%%%% easy to compensate
         else
             nTime(2) =  nTime(2) + 1;
         end;
     else
         nTime(1) = nTime(1) +1;
     end;
     CalcCnt = CalcCnt+2;
 end
 %% the end
 close(h);
 fprintf('navigation stop!\n');
 %%%%%%%%%%%55 display somthing
 dispAVP(AA_Comp,VV_Comp,POS_Comp,AA0,AA,Para,length(kalman_Para.X),kalman_filter_mode);
 if Plot_Style == 2
     Plot2(Plot_Mode,STEP,nTGPS,Para,AA,VV,POS,AA_Comp,VV_Comp,POS_Comp,Xw,Xa,AA0,isKalmanFilter,kalman_Para);
 else
     Plot3(Plot_Mode,STEP,nTGPS,Para,AA,VV,POS,AA_Comp,VV_Comp,POS_Comp,Xw,Xa,AA0,isKalmanFilter,kalman_Para);
 end;