%=================本程序為捷聯慣導系統靜基座對準卡爾曼濾波程序================
%===========================觀測量為水平速度誤差===========================
%==========================================================================
clear?all
close?all
clc
%------------------------------常量定義區----------------------------------
L=45*pi/180;?????%初始緯度
g0=9.78049;
g=g0*（1+0.0052884*sin（L）*sin（L）-0.0000059*sin（2*L）*sin（2*L））;????%實驗當地重力加速度計算
wie=0.00007272205216643039903848711535368;	??????????%地球自轉角速度??單位：弧度每秒
wie_e=0;
wie_n=wie*cos（L）;????%導航坐標系選為當地地理坐標系?東北天方向
wie_u=wie*sin（L）;

Re=6378137.0;	?????%地球半徑:1984年WGS-84全球
T_align=5*60;????????%對準時間為5分鐘
T_samp=0.05;?????????%慣性器件的采樣時間?0.05s
number_data=T_align/T_samp;??????%數據個數?6000個

%--------------------------設定初始捷聯矩陣-----------------------------------
%姿態角設

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

?????文件???????7256??2006-12-20?15:43??KF_SINS.m

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

?????????????????7256????????????????????1