clear
clc
close?all
%%初始量定義
wie?=?0.000072921151467;?
Re	=?6378135.072;
g?=?9.7803267714;
e?=?1.0?/?298.25;
T?=?0.01;??????????????????????%IMU頻率100hz此程序中GPS頻率100hz
datanumber?=?72001;???????????%數(shù)據(jù)時(shí)間3600s??????????????????

a?=?load（‘C:\Users\wangbo\Desktop\2學(xué)習(xí)\程序\IMU.dat‘）;
w?=?a（:3:5）‘*pi/180/3600;????%陀螺儀輸出的角速率信息單位由°/h化為rad/s
f?=?a（:6:8）‘;????????????????%三軸比力輸出，單位g
a?=?load（‘C:\Users\wangbo\Desktop\2學(xué)習(xí)\程序\GPS.dat‘）;????
gps_pos?=?a（:3:5）;???????????????????????%GPS輸出的緯度、經(jīng)度、高度信息
gps_pos（:1:2）?=?gps_pos（:1:2）*pi/180;???%緯?經(jīng)單位化為弧度
gps_v?=?a（:6:8）;?????????????????????????%GPS輸出的東北天速度信息

%捷聯(lián)解算及卡爾曼相關(guān)
v=zeros（datanumber3）;????????????????????%組合后的速度信息ffffffff
atti?=?zeros（datanumber3）;???????????????%組合后的姿態(tài)信息
pos?=?zeros（datanumber3）;????????????????%組合后的位置信息

gyro=zeros（31）;
acc=zeros（31）;

x_kf?=?zeros（datanumber15）;?
p_kf?=?zeros（datanumber15）;

lat?=?40.0211142228246*pi/180;?????????????%組合導(dǎo)航的初始位置、姿態(tài)、速度
lon?=116.3703629769560*pi/180;
height?=43.0674;
fai?=?219.9744642380873*pi/180;
theta?=?-0.895865732956914*pi/180;
gama?=?0.640089448357591*pi/180;
Vx=gps_v（11）;
Vy=gps_v（12）;
Vz=gps_v（13）;

X_o=zeros（151）;????%X的初值選為0
X=zeros（151）;
?
%Q=diag（[（50e-6*g）^2（50e-6*g）^2（50e-6*g）^2（0.1*pi/180/3600）^2（0.1*pi/180/3600）^2（0.1*pi/180/3600）^2000000000]）;?????%隨機(jī)
Q=diag（[（0.008*pi/180/3600）^2（0.008*pi/180/3600）^2（0.008*pi/180/3600）^2（50e-6*g）^2（50e-6*g）^2（50e-6*g）^2000000000]）;
R=diag（[（0.001）^2（0.001）^2（0.001）^2（0.1）^2（0.1）^2（0.15）^2]）;???????
P=zeros（15）;
P_k=diag（[（0.00005*pi/180）^2（0.00005*pi/180）^2（0.00005*pi/180）^20.00005^20.00005^20.00005^22^22^22^2（0.001*pi/180/3600）^2（0.001*pi/180/3600）^2（0.001*pi/180/3600）^2（50e-6*g）^2（50e-6*g）^2（50e-6*g）^2]）;??????%
K=zeros（156）;
Z=zeros（61）;
I=eye（15）;

Cnb?=?[cos（gama）*cos（fai）-sin（gama）*sin（theta）*sin（fai）?cos（gama）*sin（fai）+sin（gama）*sin（theta）*cos（fai）?-sin（gama）*cos（theta）;	
?????????-cos（theta）*sin（fai）?cos（theta）*cos（fai）?sin（theta）;
?????????sin（gama）*cos（fai）+cos（gama）*sin（theta）*sin（fai）?sin（gama）*sin（fai）-cos（gama）*sin（theta）*cos（fai）?cos（gama）?*?cos（theta）];
q?=?[?cos（fai/2）*cos（theta/2）*cos（gama/2）?-?sin（fai/2）*sin（theta/2）*sin（gama/2）;
??????cos（fai/2）*sin（theta/2）*cos（gama/2）?-?sin（fai/2）*cos（theta/2）*sin（gama/2）;
??????cos（fai/2）*cos（theta/2）*sin（gama/2）?+?sin（fai/2）*sin（theta/2）*cos（gama/2）;
??????cos（fai/2）*sin（theta/2）*sin（gama/2）?+?sin（fai/2）*cos（theta/2）*cos（gama/2）];

Cnb_s=Cnb;
q_s=q;
??
??
for?i=1:1:datanumber
???????
????Rmh?=?Re?*?（1.0?-?2.0?*?e?+?3.0?*?e?*?sin（lat）?*?sin（lat））?+?height;
????Rnh?=?Re?*?（1.0?+?e?*?sin（lat）?*?sin（lat））?+?height;
????
????Wien?=?[?0;?wie?*?cos（lat）;?wie?*?sin（lat）];			
????Wenn?=?[?-Vy?/?Rmh;?Vx?/?Rnh;?Vx?*?tan（lat）?/?Rnh];
????Winn?=?Wien?+?Wenn;
????Winb?=?Cnb?*?Winn;
????
????for?j=1:3
????????gyro（j1）?=?w（ji）;
????????acc（j1）?=?f（ji）*g;?????%加速度信息，單位化為m/s^2
????end
????
????angle?=?（gyro?-?Winb）?