导航解算 卡尔曼滤波

clc;
clear;
format long
%% 定义变量
%数据时间（s）
Total_Time_s=600;    
%kalman 滤波周期
Kalman_Time_s=1;
%惯导解算周期（s）
dt=0.01;
%是否反馈（1为反馈）
Fankui_Guandao=1;  
%选用数据段（1为读数据；0为造数据）
Shuju_Laiyuan=1;
%数据个数
n=Total_Time_s/dt;
%kalman运算次数
n1=Total_Time_s/Kalman_Time_s;
%% 惯导存储信息
V1=zeros（31）;V=zeros（n3）;Pos=zeros（n3）;Gest=zeros（n3）;wibb=zeros（n3）;
fb=zeros（n3）;I=eye（4）;C1=zeros（33）;fp=zeros（31）;wepp=zeros（13）;
wipp=zeros（31）;wipb=zeros（31）;wpbb=zeros（3n）;dxt=zeros（44）; data=zeros（n6）;pp=zeros（n2）;
wiep=zeros（31）;q=zeros（41）;Vep=[0 0 0];
%% 定义初值
%姿态角（单位弧度）
% pitch=0.00574143129887982*pi/180;
% roll=-0.00570276481225802*pi/180;
% yaw=0.0939588201389148*pi/180;
pitch=0.5*pi/180;
roll=-0.5*pi/180;
yaw=1*pi/180;
%经纬度（单位弧度）
Longitude=116.34340341*pi/180;
Lat=39.9778*pi/180;
%地球半径
re=6378137;
%椭球率
e=1.0/298.257223563;
%游动方位角
a=0;
%地球自转角速度（弧度每秒）
wie=7.292115147e-5;
%惯导解算高度
h=40;
%高度计高度
hr=40;
% X=0;
k=1;
%惯导器件反馈初值
w1=0;w2=0;w3=0;f1=0;f2=0;f3=0;

%求C 初始位置矩阵
C=[-sin（a）*sin（Lat）*cos（Longitude）-cos（a）*sin（Longitude） -sin（a）*sin（Lat）*sin（Longitude）+cos（a）*cos（Longitude） sin（a）*cos（Lat）;
   -cos（a）*sin（Lat）*cos（Longitude）+sin（a）*sin（Longitude） -cos（a）*sin（Lat）*sin（Longitude）-sin（a）*cos（Longitude） cos（a）*cos（Lat）;
   cos（Lat）*cos（Longitude） cos（Lat）*sin（Longitude） sin（Lat）];
% g的计算
g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;
g=g0*（1+gk1*C（33）^2）*（1-2*h/re）/sqrt（1-gk2*C（33）^2）;
%初始T 姿态矩阵
T=[cos（roll）*cos（yaw）-sin（roll）*sin（pitch）*sin（yaw） -cos（pitch）*sin（yaw） sin（roll）*cos（yaw）+cos（roll）*sin（pitch）*sin（yaw）;
   cos（roll）*sin（yaw）+sin（roll）*sin（pitch）*cos（yaw） cos（pitch）*cos（yaw） sin（roll）*sin（yaw）-cos（roll）*sin（pitch）*cos（yaw）;
   -sin（roll）*cos（pitch） sin（pitch） cos（roll）*cos（pitch）];
   %初始四元数计算
  q（2）=0.5*sign（T（32）-T（23））*sqrt（1+T（11）-T（22）-T（33））;
  q（3）=0.5*sign（T（13）-T（31））*sqrt（1-T（11）+T（22）-T（33））;
  q（4）=0.5*sign（T（21）-T（12））*sqrt（1-T（11）-T（22）+T（33））;
  q（1）=sqrt（1-q（2）^2-q（3）^2-q（4）^2）;
%阻尼系数
K1=0.03;K2=4.0*10^（-4）+2*g/（re+h）;K3=2*10^（-6）;
ad=0;vd=0;a0=0;  
  Rm=re*（1-2*e+3*e*C（33）^2）;           Rn=re*（1+e*C（33）^2）; 
  %% 读文件
if Shuju_Laiyuan==1 %导入数据
% load（‘IMU_data_noise.mat‘）;
load（‘E:/静态1.txt‘）;
end
if Shuju_Laiyuan==0  %仿造数据
   ug=1e-6*g;                   rd=pi/180/3600;            
   for i=1:n
       data（i1）=     0          +0.02*rd   ;     %        +random（‘normal‘0（0.01*rd）1） ;  
       data（i2）=wie*cos（Lat）    +0.02*rd    ;     %         +random（‘normal‘0（0.01*rd）1） ;  
       data（i3）=wie*sin（Lat）    +0.02*rd     ;     %       +random（‘normal‘0（0.01*rd）1） ;     
       data（i4）=     0         +100*ug       ;     %      +random（‘normal‘0（50*ug）1）   ;  
       data（i5）=     0         +100*ug       ;     %        +random（‘normal‘0（50*ug）1）  ;  
       data（i6）=     g          +100*ug      ;     %         +random（‘normal‘0（50*ug）1） ; 
   end
end
%%
 %%卡曼滤波初始化%%
%估计值初值
x=zeros（151）;
fai=zeros（3