GPS+INS组合导航

ins+GPS组合导航，ins+GPS组合导航，ins+GPS组合导航，ins+GPS组合导航，ins+GPS组合导航ins+GPS组合导航，ins+GPS组合导航ins+GPS组合导航

%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   = v（:2）;
Vu0   = v（:3）;
%   导航位置
L0    = p（:1）;
h0    = p（:3）;

%卡尔曼滤波参数初始化
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）;   %初始误差协方差阵
PP0 					= PP;
X 						= zeros（181）;  %初始状态
E_attitude 		= zeros（13）;
E_position 		= zeros（13）;
E_velocity 		= zeros（13）;

n = size（Dp1）;
for i=1:n-1
    %参数赋值
    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）;
    %由四元数计算姿态阵
    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）];

    %连续系统状态转换阵 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） 

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

     目录          0  2008-10-20 20:59  GPS-SINS位置组合程序

     文件      78336  2008-05-20 16:48  GPS-SINS位置组合程序\GPS_INS位置组合结果.doc

     文件       4650  2008-05-22 17:20  GPS-SINS位置组合程序\kalman_GPS_INS_position_sp_NFb.m

     文件     631454  2008-05-20 15:14  GPS-SINS位置组合程序\ode500.mat

     文件       4172  2008-05-22 17:22  GPS-SINS位置组合程序\s_GPS_INS_position_sp_demo.m

     文件        428  2008-05-22 17:25  GPS-SINS位置组合程序\程序说明.txt

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

               719040                    6