GPS_INS位置组合程序

GPS_INS位置组合程序GPS_INS位置组合程序GPS_INS位置组合程序

%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 [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）       = -fn;
    F（62）       = fe;
    F（64）       = 2

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2013-07-31 08:42  GPS_INS位置组合程序——好\
     文件       78336  2008-05-20 16:48  GPS_INS位置组合程序——好\GPS_INS位置组合结果.doc
     文件        4593  2008-09-19 21:41  GPS_INS位置组合程序——好\kalman_GPS_INS_position_sp_NFb.asv
     文件        4592  2008-11-05 20:28  GPS_INS位置组合程序——好\kalman_GPS_INS_position_sp_NFb.m
     文件        7256  2006-12-20 15:43  GPS_INS位置组合程序——好\KF_SINS.m
     文件      631454  2008-05-20 15:14  GPS_INS位置组合程序——好\ode500.mat
     文件       21292  2008-11-26 20:27  GPS_INS位置组合程序——好\shixiong.m
     文件        4104  2008-08-28 14:28  GPS_INS位置组合程序——好\s_GPS_INS_position_sp_demo.asv
     文件        4017  2009-03-02 11:03  GPS_INS位置组合程序——好\s_GPS_INS_position_sp_demo.m
     文件         162  2008-09-13 10:00  GPS_INS位置组合程序——好\~$S_INS位置组合结果.doc
     文件         351  2009-03-02 10:46  GPS_INS位置组合程序——好\程序说明.txt
     目录           0  2018-04-15 19:43  新建文件夹\