clear?all;
?close?all;
clc;

load（‘E:\實驗室資料\學習資料\研一下學習資料\卡爾曼濾波與組合導航\kalman實驗報告__鄒瑩\實驗三\數據\IMU.dat‘）
load（‘E:\實驗室資料\學習資料\研一下學習資料\卡爾曼濾波與組合導航\kalman實驗報告__鄒瑩\實驗三\數據\GPS.dat‘）
%?Pi?=?3.1415926535897931;
%?WIEE?=?0.000072921151467;
%?Re?=?6378135.072;
%?g0?=?9.7803267714;
%?e?=?1.0?/?298.25;
%?T_IMU?=?0.01;??????%%導航數據時間間隔
%?T_GPS?=?0.05;??????%%GPS數據時間間隔
%?LOOP?=?360000;?????%%循環次數，數據長度
%?
%?
%?velocity_GPSINS（72003）?=?0;
%?position_GPSINS（72003）?=?0;
%?attitude（LOOP3）?=?0;
%?x_kf（LOOP/515）?=?0;
%?p_kf（LOOP/515）?=?0;
%?
%?%導航參數初始化賦值
%?Lat?=?GPS（13）*Pi/180;
%?Lon?=?GPS（14）*Pi/180;
%?Hei?=?GPS（15）;
%?Vx?=?GPS（16）;
%?Vy?=?GPS（17）;
%?Vz?=?GPS（18）;
%?
%?IMU（1:3600013:5）?=?IMU（1:3600013:5）*Pi/180/3600;
%?IMU（1:3600016:8）?=?IMU（1:3600016:8）*g0;
%?%姿態角
%?fai?=?305.34023*Pi/180;
%?theta?=?0.25097*Pi/180;
%?gama?=?1.78357*Pi/180;
%?
%?%初始方向余弦矩陣和四元數
%?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）];
%???
%?Kf_loop?=?0;
%?Q?=?diag（[（0.2*Pi/180/3600）^2（0.2*Pi/180/3600）^2（0.2*Pi/180/3600）^2（100E-6*g0）^2（100E-6*g0）^2（100E-6*g0）^2]）;
%?R?=?diag（[0.01^20.01^20.01^20.1^20.1^20.15^2]）;
%?p_filter?=?diag（[（0.5*Pi/180）^2（1*Pi/180/60）^2（1*Pi/180/60）^2?0.05^20.05^20.05^2?（2/Re）^2（2/Re）^22^2...?
%??????????????????（0.1*Pi/180/3600）^2（0.1*Pi/180/3600）^2（0.1*Pi/180/3600）^2（50E-6*g0）^2（50E-6*g0）^2（50E-6*g0）^2]）;
%?x_filter?=?zeros（151）;
%???
%?for?i=?1:360000
%?????Rmh?=?Re?*?（1?-?2?*?e?+?3?*?e?*?sin（Lat）?*?sin（Lat））?+?Hei;
%?????Rnh?=?Re?*?（1?+?e?*?sin（Lat）?*?sin（Lat））?+?Hei;
%?????g_s?=?g0?*?（1?+?0.00193185138639?*?sin（Lat）?*?sin（Lat））?/?sqrt（1?-?0.00669437999013?*?sin（Lat）?*?sin（Lat））?*?（1?-?2?*?Hei?/?Re）;
%?????Wien?=?[?0;?WIEE?*?cos（Lat）;?WIEE?*?sin（Lat）];			
%?????Wenn?=?[?-Vy?/?Rmh;?Vx?/?Rnh;?Vx?*?tan（Lat）?/?Rnh];
%?????Winn?=?Wien?+?Wenn;
%?????Winb?=?Cnb?*?Winn;
%?????Wibb?=?IMU（i3:5）‘;
%?????Wnbb?=?Wibb?-?Winb;
%?????angle?=?Wnbb?*?T_IMU;?????????%%每次姿態變化對應的小角度變化，用于姿態更新
%?????
%?????
%?%?edit?by?STX???????
%?%?fn?=?Cnb‘?*?IMU（i6:8）‘;??????%%比力在n系中的投影
%?%?V=[Vx;Vy;Vz];
%?%?FN=[fn（1）;fn（2）;fn（3）];
%?%?Wx_temp=2*Wien+Wenn;
%?%?g=[0;0;g_s];
%?%?difV=FN-cross（Wx_tempV）-g;
%?%?difVx=difV（1）;
%?%?difVy=difV（2）;
%?%?difVz=difV（3）;
%?%?Vx=V（1）;Vy=V（2）;Vz=V（3）;
%?
%?????
%??????fn?=?Cnb‘?*?IMU（i6:8）‘;??????%%比力在n系中的投影
%?????difVx?=?fn（1）?+?（2?*?WIEE

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件???????27612??2019-03-08?22:21??\INGPS.m
?????文件??????????36??2019-03-08?22:21??\no.txt