GPS和INS联合导航MATLAB程序

一个GPS和INS联合导航MATLAB不错的程序，与大家一起分享，希望对您有所帮助

%GPS/INS无反馈位置组合 卡尔曼滤波器

%%%%%%%%%%%%%%%%%%
%edit by horsejun
%%%%%%%%%%%%%%%%%%

%每秒更新一次速度位置误差
%连续状态系统方程
%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 [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输出位置，作为滤波器系统参数
%v      ins输出速度，作为滤波器系统参数
%fn     ins输出导航系下比力，作为滤波器参数
%quat   ins输出四元数，作为滤波器参数
%Q      系统噪声方差
%R      测量噪声方差
%Ta     加表误差漂移相关时间
%Tg     陀螺仪误差漂移相关时间
%tao    迭代步长
%%%%%%%输入向量均为行向量%%%%%%%%%%%%%

%输出
%E_position     位置预测值
%E-velocity     速度预测值

%各参数初始化
Re 		= 6378245;   %地球长半径
e 		= 1/298.257;  %地球扁率
wie 	= 7.292e-5;  %地球自转角速度

%   东北天速度
Ve0   = v（:1）;
Vn0  

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       4651  2009-04-21 10:37  GPS和INS联合导航matlab程序\kalman_GPS_INS_position_sp_NFb.m

     文件     631454  2009-04-21 10:47  GPS和INS联合导航matlab程序\ode500.mat

     文件       4032  2009-04-21 11:18  GPS和INS联合导航matlab程序\s_GPS_INS_position_sp_demo.m

     目录          0  2009-04-21 18:34  GPS和INS联合导航matlab程序

----------- ---------  ---------- -----  ----

               640137                    4