%============本程序為捷聯慣導系統靜基座對準卡爾曼濾波程序的修正程序=====
%==========================觀測量速度誤差===========================
%==========================================================================
clear all
close all
clc
%------------------------------常量定義區----------------------------------
L=45*pi/180;     %初始緯度
g0=9.78049;
g=g0*(1+0.0052884*sin(L)*sin(L)-0.0000059*sin(2*L)*sin(2*L));    %實驗當地重力加速度計算
wie=0.00007272205216643039903848711535368;	        %地球自轉角速度  單位：弧度每秒
wie_e=0;
wie_n=wie*cos(L);  %導航坐標系為當地地理坐標系
wie_u=wie*sin(L);

Re=6378137.0;	       %地球半徑:1984年WGS-84全球
T_align=5*60;          %對準時間為5分鐘
T_samp=0.01;           %慣性器件的采樣時間
number_data=T_align/T_samp;        %數據個數 30000個
H_JM=60*60*180/pi;     %角秒到弧度
JM_H=pi/180/60/60;     %弧度到角秒
%--------------------------設定初始捷聯矩陣-----------------------------------
%姿態角設定
yaw_angle=pi*45/180;          %偏航角
pitching_angle=pi*30/180;     %俯仰角
turn_angle=pi*5/180;          %滾動角
%calculate of ideal strapdown_matrix from chenzhe
Cbt_idea11=cos(turn_angle)*cos(yaw_angle)-sin(turn_angle)*sin(pitching_angle)*sin(yaw_angle);
Cbt_idea12=cos(turn_angle)*sin(yaw_angle)+sin(turn_angle)*sin(pitching_angle)*cos(yaw_angle);
Cbt_idea13=-sin(turn_angle)*cos(pitching_angle);
Cbt_idea21=-cos(pitching_angle)*sin(yaw_angle);
Cbt_idea22=cos(pitching_angle)*cos(yaw_angle);
Cbt_idea23=sin(pitching_angle);
Cbt_idea31=sin(turn_angle)*cos(yaw_angle)+cos(turn_angle)*sin(pitching_angle)*sin(yaw_angle);
Cbt_idea32=sin(turn_angle)*sin(yaw_angle)-cos(turn_angle)*sin(pitching_angle)*cos(yaw_angle);
Cbt_idea33=cos(turn_angle)*cos(pitching_angle);
Cbt_idea=[Cbt_idea11 Cbt_idea12 Cbt_idea13
          Cbt_idea21 Cbt_idea22 Cbt_idea23
          Cbt_idea31 Cbt_idea32 Cbt_idea33];
Ctb_idea=Cbt_idea';
%------------------------------初始值的設定--------------------------------
%導航坐標系選東北天
fiee=1*60*60;        %初始誤差角均為1度  單位角秒
fien=1*60*60;
fieu=1*60*60;
fpin=fopen('guanceliang3.dat', 'r');          %打開文件，觀測量數據（人造）
%東北天坐標系下系統的狀態矩陣(連續系統）
A=zeros(10,10);                                                  
A(1,2)=2*wie_u;
A(1,4)=-g*JM_H;
A(1,6)=Ctb_idea(1,1);
A(1,7)=Ctb_idea(1,2);
A(2,1)=-2*wie_u;
A(2,3)=g*JM_H;
A(2,6)=Ctb_idea(2,1);
A(2,7)=Ctb_idea(2,2);
A(3,4)=wie_u;
A(3,5)=-wie_n;
A(3,8)=Ctb_idea(1,1);
A(3,9)=Ctb_idea(1,2);
A(3,10)=Ctb_idea(1,3);
A(4,3)=-wie_u;
A(4,8)=Ctb_idea(2,1);
A(4,9)=Ctb_idea(2,2);
A(4,10)=Ctb_idea(2,3);
A(5,3)=wie_n;
A(5,8)=Ctb_idea(3,1);
A(5,9)=Ctb_idea(3,2);
A(5,10)=Ctb_idea(3,3);
%觀測矩陣
H=zeros(2,10);                                                   
H(1,1)=1;
H(2,2)=1;
%初始狀態向量  初始狀態都設為0
X=zeros(10,1);                                                   
X(3,1)=0*60*60;
X(4,1)=0*60*60;
X(5,1)=0*60*60;
%初始觀測值   初始觀測值設為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*60*60)^2;
P(4,4)=(1*60*60)^2;
P(5,5)=(1*60*60)^2;
P(6,6)=(1e-4*g)^2;
P(7,7)=(1e-4*g)^2;
P(8,8)=(0.02*60*60/3600)^2;
P(9,9)=(0.02*60*60/3600)^2;
P(10,10)=(0.02*60*60/3600)^2;

%＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝初始系統噪聲誤差陣＝＝＝＝＝＝＝＝＝＝＝＝＝＝%
%加速度計的隨機偏差為0.5e-4*g  陀螺的隨機漂移為0.01度每小時
Q=zeros(10,10);                                                 
Q(1,1)=(5e-5*g)^2;
Q(2,2)=(5e-5*g)^2;
Q(3,3)=(0.01*60*60/3600)^2;
Q(4,4)=(0.01*60*60/3600)^2;
Q(5,5)=(0.01*60*60/3600)^2;
%初始觀測噪聲誤差陣
R=zeros(2,2);                                                   
R(1,1)=0.1^2;
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
        
        
%============================== mine new1===============================


%============================ new1 end===================================
        

        
for i=1:number_data/2;          %kalman濾波開始
         
        fscanf(fpin, '%g', 1);
        m=fscanf(fpin, '%g', 1);
        n=fscanf(fpin, '%g', 1);
      
        Z(1,1)=m;
        Z(2,1)=n;
        
        
        
        a=fscanf(fpin, '%g', 1);                        %單位：角秒    
        b=fscanf(fpin, '%g', 1);
        c=fscanf(fpin, '%g', 1);   
 
%============================== 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))/60/60*3600;       %陀螺漂移  單位度每小時
     dgyro_n(i)=sqrt(P(9,9))/60/60*3600;
     dgyro_u(i)=sqrt(P(10,10))/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(t,ddeltav_e)
%figure(2)
%plot(t,ddeltav_n)
figure(3)
subplot(3,1,1)
plot(t,dfiee)
subplot(3,1,2)
plot(t,dfien)
subplot(3,1,3)
plot(t,dfieu)
%figure(4)
%plot(t,daccle_meter_e)
%figure(5)
%plot(t,daccle_meter_n)
%figure(6)
%subplot(3,1,1)
%plot(t,dgyro_e)
%subplot(3,1,2)
%plot(t,dgyro_n)
%subplot(3,1,3)
%plot(t,dgyro_u)