%%%%%%%%%%%%%%%%%捷聯慣導GPS組合導航Kalman濾波器%%%%%%%%%%%%%%%%
clc;
clear;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Wie=7.292115e-5;?????????????????%地球自轉角速
e=1/298.257;?????????????????????%地球扁律
Re=6378137;??????????????????????%地球半徑
STEP=0.1;????????????????????????%慣導解算周期
G0=9.7803;???????????????????????%重力加速度初值
arc=pi/180;??????????????????????%角度轉換
T=0.3;???????????????????????????%濾波周期，單位秒
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load?y_Ishuju;
load?y_Gshuju;


S_dat_I=y_I;
S_dat_G=y_G;
%%%%%%%%%%%%%%%%%%%%數據加載%%%%%%%%%%%%%%%%%%%%%%%%%%%

I=eye（13）;

A=zeros（13）;

X00=zeros（131）;

X00（11）=（10/60）*arc;
X00（21）=（10/60）*arc;
X00（31）=（10/60）*arc;
X00（41）=10;
X00（51）=10;
X00（61）=（4/60）*arc;
X00（71）=（4/60）*arc;
X00（81）=（0.5/3600）*arc;
X00（91）=（0.5/3600）*arc;
X00（101）=（0.5/3600）*arc;
X00（111）=（1e-4）*9.81;
X00（121）=（1e-4）*9.81;
X00（131）=（1e-4）*9.81;

P00=zeros（13）;?????????????????????????????????????????????%Klman濾波中估計誤差方差陣初值

P00（11）=（（10/60）*arc）*（（10/60）*arc）;
P00（22）=（（10/60）*arc）*（（10/60）*arc）;
P00（33）=（（10/60）*arc）*（（10/60）*arc）;
P00（44）=10*10;
P00（55）=10*10;
P00（66）=（（4/60）*arc）*（（4/60）*arc）;
P00（77）=（（4/60）*arc）*（（4/60）*arc）;
P00（88）=（（0.5/3600）*arc）*（（0.5/3600）*arc）;
P00（99）=（（0.5/3600）*arc）*（（0.5/3600）*arc）;
P00（1010）=（（0.5/3600）*arc）*（（0.5/3600）*arc）;
P00（1111）=（（1e-4）*9.81）*（（1e-4）*9.81）;
P00（1212）=（（1e-4）*9.81）*（（1e-4）*9.81）;
P00（1313）=（（1e-4）*9.81）*（（1e-4）*9.81）;

Q0=zeros（13）;??????????????????????????????????????????????%Klman濾波中過程噪聲方差陣初值

Q0（11）=（（1/3）*（（0.5/3600）*arc））*（（1/3）*（（0.5/3600）*arc））;
Q0（22）=（（1/3）*（（0.5/3600）*arc））*（（1/3）*（（0.5/3600）*arc））;
Q0（33）=（（1/3）*（（0.5/3600）*arc））*（（1/3）*（（0.5/3600）*arc））;
Q0（44）=（（1/3）*（1e-4）*9.81）*（（1/3）*（1e-4）*9.81）;
Q0（55）=（（1/3）*（1e-4）*9.81）*（（1/3）*（1e-4）*9.81）;

R1=zeros（4）;???????????????????????????????????????????????%Klman濾波中觀測噪聲方差陣初值

R1（11）=（0.3）^2;??????????????????????????????????????????
R1（22）=（0.3）^2;

R1（33）=（5/Re）^2;
R1（44）=（5/Re）^2;

H1=zeros（413）;????????????????????????????????????????????%Klman濾波中觀測陣初值

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%濾波開始%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mmm=round（T/STEP）;

for?j=0:1:length（S_dat_I）/mmm-2
????
????Sum=0;
????
????for?DRDER=（j*mmm+1）:1:（j+1）*mmm
????????
????????vx_G=S_dat_G（DRDER1）;
????????vy_G=S_dat_G（DRDER2）;
????????vz_G=S_dat_G（DRDER3）;
????????
????????jd_G=S_dat_G（DRDER4）;
????????wd_G=S_dat_G（DRDER5）;
????????
????????Vx_I=S_dat_I（DRDER1）;
????????Vy_I=S_dat_I（DRDER2）;
????????Vz_I=S_dat_I（DRDER3）;??????
????????
????????Q=S_dat_I（DRDER4）;
????????R=S_dat_I（DRDER5）;
????????W=S_dat_I（DRDER6）;
????
????????JD=S_dat_I（DRDER7）;
????????WD=S_dat_I（DRDER8）;
????????H=S_dat_I（DRDER9）;
????
????????fx_n=S_dat_I（DRDER10）;
????????fy_n=S_dat_I（DRDER11）;
????????fz_n=S_dat_I（DRDER12）;
????????
????????%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
????
????????q000=cos（Q/2）*cos（R/2）*cos（W/2）-sin（Q/2）

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2019-02-22?11:19??GPS-INS\
?????文件??????376832??2018-09-17?10:07??GPS-INS\INSGPS組合導航系統0510.ppt
?????文件????????7468??2008-04-27?16:47??GPS-INS\INSandGPS.m
?????文件??????737280??2008-04-27?16:18??GPS-INS\捷聯慣導0509.ppt
?????文件??????350208??2008-05-04?08:59??GPS-INS\組合導航若干基本問題.ppt