function [ j_theta j_gama j_fai ] = jing_alignmet2( theta0,gama0,fai0,theta,gama,fai,latitude,longitude )
%%捷聯慣導系統靜基座對準的卡爾曼濾波程序，觀測量為速度誤差,自己寫的啊激光陀螺尋北系統建模與參數辨識,捷聯式慣導系統初始對準方法研究
LL0 = latitude;%初始的緯度

LO0 =longitude;%初始的經度

g =  9.7803267714*(1+0.00193185138639*sin(LL0(1))^2)/sqrt(1-0.00669437999013*sin(LL0(1))^2);
Ra = 6378140;%地球參考橢球長軸半徑
e =  1/298.257;%0%地球橢球的橢圓度 要和g一起修改那
wie = 15.04088/3600/180*pi; %0;%%地球自轉角速度
wie_n=wie*cos(LL0);  %導航坐標系為當地地理坐標系
wie_u=wie*sin(LL0);
T_align = 3*60;%對準時間為5分鐘
T_samp = 0.01;%慣性器件的采樣時間
number_data  = T_align/T_samp;%數據個數為30000個
%% 設定初始捷聯矩陣
% fai = 0;theta = 0;gama=0;
%calculate of ideal strapdown_matrix from chenzhe
Cb_n = [sin(fai)*sin(theta)*sin(gama)+cos(fai)*cos(gama)   cos(fai)*sin(theta)*sin(gama)-sin(fai)*cos(gama)   -cos(theta)*sin(gama);
        sin(fai)*cos(theta)                                cos(fai)*cos(theta)              sin(theta);
        cos(fai)*sin(gama)-sin(fai)*sin(theta)*cos(gama)   -sin(fai)*sin(gama)-cos(fai)*sin(theta)*cos(gama)    cos(theta)*cos(gama) ];
Cn_b=Cb_n';
%% 初始值的設定
% 導航坐標系為東北天
% fiee = 1*60*60;%初始誤差角為1度，單位為角秒 1度=3600角秒
% fien = 1*60*60;
% fieu = 1*60*60;
%% 觀測量的生成

tra(T_samp,T_align,LL0,LO0,theta0,gama0,fai0);
acc_gy(1,T_samp,T_align);%陀螺儀加速度的輸出,帶噪聲
% fpin = fopen('guanceliang.dat','r');% 打開文件啊，觀測量速度誤差

jiesuan(theta,gama,fai);%生成解算后的數據
load imusolve_silent;
%% 東北天坐標系下系統的狀態矩陣（這里是連續系統）
A=zeros(10,10);                                                  
A(1,2)=2*wie_u;
A(1,4)=-g;
A(1,6)=C_nb(1,1);
A(1,7)=C_nb(1,2);
A(2,1)=-2*wie_u;
A(2,3)=g;
A(2,6)=C_nb(2,1);
A(2,7)=C_nb(2,2);
A(3,2)=-1/Ra;%自己添上的啊
A(3,4)=wie_u;
A(3,5)=-wie_n;
A(3,8)=C_nb(1,1);
A(3,9)=C_nb(1,2);
A(3,10)=C_nb(1,3);
A(4,1)=1/Ra;%自己添上的啊
A(4,3)=-wie_u;
A(4,8)=C_nb(2,1);
A(4,9)=C_nb(2,2);
A(4,10)=C_nb(2,3);
A(5,1)=tan(LL0)/Ra;%自己添上的啊
A(5,3)=wie_n;
A(5,8)=C_nb(3,1);
A(5,9)=C_nb(3,2);
A(5,10)=C_nb(3,3);
%觀測矩陣
H=zeros(2,10);                                                   
H(1,1)=1;
H(2,2)=1;
%初始狀態向量  初始狀態都設為0 初始狀態分別代表速度誤差2，姿態誤差角3，加速度測量誤差2，角速度測量誤差3
X=zeros(10,1);                                                   
X(3,1)=theta;
X(4,1)=gama;
X(5,1)=fai;
%初始觀測值   初始觀測值設為0
Z=zeros(2,1);
%初始方差陣的設置
%初始速度誤差取0.1米每妙 初始失準角均為1度 
%加速度計的初始偏值均取1e-4*g 陀螺的常值漂移取0.02度每小時
P=zeros(10,10);
P(1,1)=0.1^2;%水平速度的誤差
P(2,2)=0.1^2;
P(3,3)=(1*pi/180)^2;%姿態誤差角，1度，轉化為弧度
P(4,4)=(1*pi/180)^2;
P(5,5)=(1*pi/180)^2;
P(6,6)=(1e-4*g)^2;%加速度初始偏值
P(7,7)=(1e-4*g)^2;

P(8,8)=(2*pi/180/3600)^2;%陀螺常值偏移，轉化為弧度/秒
P(9,9)=(2*pi/180/3600)^2;
P(10,10)=(2*pi/180/3600)^2;
%＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝初始系統噪聲誤差陣＝＝＝＝＝＝＝＝＝＝＝＝＝＝%
%加速度計的隨機偏差為0.5e-4*g  陀螺的隨機漂移為1度每小時
Q=zeros(10,10);                                                 
Q(1,1)=(5e-5*g)^2;
Q(2,2)=(5e-5*g)^2;
Q(3,3)=(1*pi/180/3600)^2;
Q(4,4)=(1*pi/180/3600)^2;
Q(5,5)=(1*pi/180/3600)^2;
%初始觀測噪聲誤差陣
R=zeros(2,2);                                                   
R(1,1)=0.1^2;%0.1米/秒
R(2,2)=0.1^2;
%==================狀態矩陣和系統噪聲陣的離散化====================% 
T_discre=0.01;                                              %離散化時間
FI=eye(10);
Qdct=zeros(10);
        temp2=1;
        M{1}=Q;
        for j=1:10
            temp2=temp2*j;
            FI=FI+T_discre^j*(A)^j/temp2;
            if(j~=1)
                M{j}=A*M{j-1}+(A*M{j-1})';
            end
            Qdct =Qdct+T_discre^j*M{j}/temp2;
        end
        
for i=1:number_data;          %kalman濾波開始
         
%         fscanf(fpin, '%g', 1);
%         m=fscanf(fpin, '%g', 1);
%         n=fscanf(fpin, '%g', 1);
        m = Storedata_solve(1+i,4);%vx
        n = Storedata_solve(1+i,5);
        Z(1,1)=m;
        Z(2,1)=n;
        
        
        
%         a=fscanf(fpin, '%g', 1);                        %單位：角秒    
%         b=fscanf(fpin, '%g', 1);
%         c=fscanf(fpin, '%g', 1);   
a=Storedata_solve(1+i,10);% 單位：弧度
b=Storedata_solve(1+i,11);
c=Storedata_solve(1+i,12);
 
%============================== mine new2 ===============================%

%============================== new2 end ================================%


     P=FI*P*FI'+Qdct;
     K=P*H'*inv(H*P*H'+R);
     X=FI*X+K*(Z-H*FI*X);
     P=(eye(10)-K*H)*P*(eye(10)-K*H)'+K*R*K';
     
     %估計誤差的方差      
     ddeltav_e(i)=sqrt(P(1,1));                 %速度估計誤差 單位 m/s           
     ddeltav_n(i)=sqrt(P(2,2));              
     dfiee(i)=sqrt(P(3,3))*180*60/pi;           %姿態誤差角估計誤差 單位 角分
     dfien(i)=sqrt(P(4,4))*180*60/pi;
     dfieu(i)=sqrt(P(5,5))*180*60/pi;
     daccle_meter_e(i)=sqrt(P(6,6));%*1e+6/g;     %加速度計偏置 單位 ug 
     daccle_meter_n(i)=sqrt(P(7,7));%*1e+6/g;%標準單位啊
     dgyro_e(i)=sqrt(P(8,8))*180/pi*3600;%/60/60*3600;       %陀螺漂移  單位度每小時
     dgyro_n(i)=sqrt(P(9,9))*180/pi*3600;%/60/60*3600;
     dgyro_u(i)=sqrt(P(10,10))*180/pi*3600;%/60/60*3600;
     dfiee(i)=a-(X(3,1));%/60;  弧度                                  %保存三個姿態誤差角
     dfien(i)=b-(X(4,1));%/60;
     dfieu(i)=c-(X(5,1));%/60;

     
end
%----------------------------------- 畫圖----------------------------------
t=0.01:0.01:300/2;
figure(1)
plot(ddeltav_e);
title('aaa');
figure(2)
plot(ddeltav_n)
title('北向速度估計誤差');
figure(3)
subplot(3,1,1)
plot(dfiee)
subplot(3,1,2)
plot(dfien)
subplot(3,1,3)
plot(dfieu)
title('姿態角估計誤差');
figure(4)
plot(daccle_meter_e)
title('東向加速度估計誤差');
figure(5)
plot(daccle_meter_n)
title('北向速度估計誤差');
figure(6)
subplot(3,1,1)
plot(dgyro_e)
subplot(3,1,2)
plot(dgyro_n)
subplot(3,1,3)
plot(dgyro_u)
title('陀螺儀估計誤差');