%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ù)
%v??????ins輸出速度，作為濾波器系統(tǒng)參數(shù)
%fn?????ins輸出導(dǎo)航系下比力，作為濾波器參數(shù)
%quat???ins輸出四元數(shù)，作為濾波器參數(shù)
%Q??????系統(tǒng)噪聲方差
%R??????測量噪聲方差
%Ta?????加表誤差漂移相關(guān)時間
%Tg?????陀螺儀誤差漂移相關(guān)時間
%tao????迭代步長
%%%%%%%輸入向量均為行向量%%%%%%%%%%%%%

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

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

%???東北天速度
Ve0?	=?v（:1）;
Vn0???=?v（:2）;
Vu0???=?v（:3）;
%???導(dǎo)航位置
L0????=?p（:1）;
h0????=?p（:3）;

%卡爾曼濾波參數(shù)初始化
PP（1:181:18）?=?diag（[1/（36*57）?1/（36*57）?1/57?0.0001?0.0001?0.0001?0?0?1?0.1/（57*3600）?0.1/（57*3600）?0.1/（57*3600）?0.04/（57*3600）?0.04/（57*3600）?0.04/（57*3600）?1e-4?1e-4?1e-4].^2）;???%初始誤差協(xié)方差陣
PP0?					=?PP;
X?						=?zeros（181）;??%初始狀態(tài)
E_attitude?		=?zeros（13）;
E_position?		=?zeros（13）;
E_velocity?		=?zeros（13）;

n?=?size（Dp1）;
for?i=1:n-1
????%參數(shù)賦值
????Ve?		=?Ve0（i）;
????Vn?		=?Vn0（i）;
????Vu?		=?Vu0（i）;
????L?		=?L0（i）;
????h?		=?h0（i）;
????fe?		=?Fn（i1）;
????fn?		=?Fn（i2）;
????fu?		=?Fn（i3）;
????Rm?		=?Re*（1-2*e+3*e*sin（L）^2）;
????Rn?		=?Re*（1-e*sin（L）^2）;
????%由四元數(shù)計算姿態(tài)陣
????q?		=?quat（i:）;
????Cnb?	=?[1-2*（q（3）^2+q（4）^2）?????2*（q（2）*q（3）-q（1）*q（4））?2*（q（2）*q（4）+q（1）*q（3））;
???????????	?2*（q（2）*q（3）+q（1）*q（4））?1-2*（q（2）^2+q（4）^2）?????2*（q（3）*q（4）-q（1）*q（2））;
?????????????2*（q（2）*q（4）-q（1）*q（3））?2*（q（3）*q（4）+q（1）*q（2））?1-2*（q（2）^2+q（3）^2）];

????%連續(xù)系統(tǒng)狀態(tài)轉(zhuǎn)換陣?F?的時間更新
????F????????????=?zeros（1818）;
????F（12）???????=?wie*sin（L）+Ve*tan（L）/（Rn+h）;
????F（13）???????=?-（wie*cos（L）+Ve/（Rn+h））;
????F（15）???????=?-1/（Rm+h）;
????F（19）???????=?Vn/（Rm+h）^2;
????F（21）???????=?-（wie*sin（L）+Ve*tan（L）/（Rn+h））;
????F（23）???????=?-Vn/（Rm+h）;
????F（24）???????=?1/（Rn+h）;
????F（27）???????=?-wie*sin（L）;
????F（29）???????=?-Ve/（Rn+h）^2;
????F（31）???????=?wie*cos（L）+Ve/（Rn+h）;
????F（32）???????=?Vn/（Rm+h）;
????F（34）???????=?tan（L）/（Rn+h）;
????F（37）???????=?wie*cos（L）+Ve*（sec（L）^2）/（Rn+h）;
????F（39）???????=?-Ve*tan（L）/（Rn+h）^2;
????F（42）???????=?-fu;
????F（43）???????=?fn;
????F（44）???????=?Vn*tan（L）/（Rm+h）-Vu/（Rm+h）;
????F（45）???????=?2*wie*sin（L）+Ve*tan（L）/（Rn+h）;
????F（46）???????=?-（2*wie*cos（L）+Ve/（Rn+h））;
????F（47）???????=?2*wie*cos（L）*Vn+Ve*Vn*sec（L）^2/（Rn+h）+2*wie*sin（L）*Vu;
????F（49）???????=?（Ve*Vu-Ve*Vn*tan（L））/（Rn+h）^2;
????F（51）???????=?fu;
????F（53）???????=?-fe;
????F（54）???????=?-2*（wie*sin（L）+Ve*tan（L）/（Rn+h））;
????F（55）???????=?-Vu/（Rm+h）;
????F（56）???????=?-Vn/（Rm+h）;
????F（57）???????=?-（2*wie*cos（L）+Ve*（sec（L）^2）/（Rn+h））*Ve;
????F（59）???????=?（Ve^2*tan（L）+Vn*Vu）/（Rn+h）^2;
????F（61）?

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件???????78336??2008-05-20?16:48??GPS-SINS組合導(dǎo)航matlab\GPS_INS位置組合結(jié)果.doc
?????文件????????4650??2008-05-22?17:20??GPS-SINS組合導(dǎo)航matlab\kalman_GPS_INS_position_sp_NFb.m
?????文件??????631454??2008-05-20?15:14??GPS-SINS組合導(dǎo)航matlab\ode500.mat
?????文件????????4172??2008-05-22?17:22??GPS-SINS組合導(dǎo)航matlab\s_GPS_INS_position_sp_demo.m
?????文件?????????428??2008-05-22?17:25??GPS-SINS組合導(dǎo)航matlab\程序說明.txt
?????目錄???????????0??2018-07-04?21:26??GPS-SINS組合導(dǎo)航matlab\