• 大小: 683KB
    文件类型: .zip
    金币: 1
    下载: 0 次
    发布日期: 2021-05-19
  • 语言: 其他
  • 标签: 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  新建文件夹\

评论

共有 条评论