扩展卡尔曼滤波代码

%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