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


%每秒更新一次速度位置誤差
%連續(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）???????=?-fn;
????F（62）???????=?fe;
????F（64）???????=?2

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件??????78336??2008-05-20?16:48??GPS_INS位置組合程序——好\GPS_INS位置組合結(jié)果.doc

?????文件???????4593??2008-09-19?21:41??GPS_INS位置組合程序——好\kalman_GPS_INS_position_sp_NFb.asv

?????文件???????4592??2008-11-05?20:28??GPS_INS位置組合程序——好\kalman_GPS_INS_position_sp_NFb.m

?????文件???????7256??2006-12-20?15:43??GPS_INS位置組合程序——好\KF_SINS.m

?????文件?????631454??2008-05-20?15:14??GPS_INS位置組合程序——好\ode500.mat

?????文件??????21292??2008-11-26?20:27??GPS_INS位置組合程序——好\shixiong.m

?????文件???????4104??2008-08-28?14:28??GPS_INS位置組合程序——好\s_GPS_INS_position_sp_demo.asv

?????文件???????4017??2009-03-02?11:03??GPS_INS位置組合程序——好\s_GPS_INS_position_sp_demo.m

?????文件????????162??2008-09-13?10:00??GPS_INS位置組合程序——好\~$S_INS位置組合結(jié)果.doc

?????文件????????351??2009-03-02?10:46??GPS_INS位置組合程序——好\程序說明.txt

?????目錄??????????0??2013-07-31?08:42??GPS_INS位置組合程序——好

-----------?---------??----------?-----??----

???????????????756157????????????????????11