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


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

function?[Estatestate1state2state3K1]?=?EKF（DpDvv?pwins?Fn?Q?R?Tg?Ta?ttins）

%輸入
%Dp?????量測位置誤差，?作為濾波器輸入
%Dv?????量測速度誤差，?作為濾波器輸入，
%p??????ins輸出位置，作為濾波器系統參數
%v??????ins輸出速度，作為濾波器系統參數
%fn?????ins輸出導航系下比力，作為濾波器參數
%quat???ins輸出四元數，作為濾波器參數
%Q??????系統噪聲方差
%R??????測量噪聲方差
%Ta?????加表誤差漂移相關時間
%Tg?????陀螺儀誤差漂移相關時間
%tao????迭代步長
%%%%%%%輸入向量均為行向量%%%%%%%%%%%%%

%輸出
%E_position?????位置預測值
%E-velocity?????速度預測值

%各參數初始化
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
wie?=?7.292115147e-5;?%>?地球自傳角速度
Re?=?6378137;?%>?地球橢球長半徑
f?=?1/298.257223563;
e?=?sqrt（f*（2?-?f））;?
%>?重力加速度計算參數
g0?=?9.7803267714;
g