GPS/INS组合导航Matlab仿真源码包含实验数据

这份matlab代码是下载后可以直接运行的，包含了仿真数据，GPS与INS组合导航，亲测可用！

% 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（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;c
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）

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

     文件     631454  2008-05-20 15:14  ode500.mat

     文件       4811  2019-05-07 15:24  s_GPS_INSdemo.m

     文件      81920  2015-07-24 21:17  结果.doc

     文件        436  2015-05-25 22:45  说明.txt

     文件       4666  2019-05-07 15:24  kalman_GPS_INS.m

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

               723287                    5