%GPS/INS無反饋位置組合?卡爾曼濾波器

%%%%%%%%%%%%%%%%%%
%edit?by?horsejun
%%%%%%%%%%%%%%%%%%

%每秒更新一次速度位置誤差
%連續(xù)狀態(tài)系統(tǒng)方程
%dx?=?F*x?+?G*w
%z?=?H*x?+?v
%離散狀態(tài)系統(tǒng)方程
%x（k+1）?=?A*x（k）?+?B*w（k）
%z（k+1）?=?C*x（k+1）?+?v（k+1）

function?[E_attitude?E_velocity?E_position?PP]?=?kalman_GPS_INS_position_sp_NFb（Dp?v?p?quat?Fn?Q?R?Tg?Ta?tao）

%輸入
%Dp?????量測位置誤差，?作為濾波器輸入
%Dv?????量測速度誤差，?作為濾波器輸入，
%p??????ins輸出位置，作為濾波器系統(tǒng)參數(shù)決定狀態(tài)轉(zhuǎn)移矩陣
%v??????ins輸出速度，作為濾波器系統(tǒng)參數(shù)，決定狀態(tài)轉(zhuǎn)移矩陣
%fn?????ins輸出導(dǎo)航系下比力，作為濾波器參數(shù)
%quat???ins輸出四元數(shù)，作為濾波器參數(shù)
%Q??????系統(tǒng)噪聲方差
%R??????測量噪聲方差
%Ta?????加表誤差漂移相關(guān)時(shí)間
%Tg?????陀螺儀誤差漂移相關(guān)時(shí)間
%tao????迭代步長
%%%%%%%輸入向量均為行向量%%%%%%%%%%%%%

%輸出
%E_position?????位置預(yù)測值
%E-velocity?????速度預(yù)測值

%各參數(shù)初始化
Re?		=?6378245;???%地球長半徑
e?		=?1/298.257;??%地球扁率
wie?	=?7.292e-5;??%地球自轉(zhuǎn)角速度

%???東北天速度
Ve0

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????文件???????78336??2008-05-20?16:48??GPS_INS位置組合Matlab仿真源碼\GPS_INS位置組合結(jié)果.doc
?????文件????????4751??2014-09-23?11:21??GPS_INS位置組合Matlab仿真源碼\kalman_GPS_INS_position_sp_NFb.asv
?????文件????????4750??2014-09-04?10:58??GPS_INS位置組合Matlab仿真源碼\kalman_GPS_INS_position_sp_NFb.m
?????文件??????631454??2008-05-20?15:14??GPS_INS位置組合Matlab仿真源碼\ode500.mat
?????文件????????4101??2014-06-09?13:32??GPS_INS位置組合Matlab仿真源碼\s_GPS_INS_position_sp_demo.asv
?????文件????????4101??2014-09-04?10:14??GPS_INS位置組合Matlab仿真源碼\s_GPS_INS_position_sp_demo.m
?????文件?????????428??2008-05-22?17:25??GPS_INS位置組合Matlab仿真源碼\程序說明.txt