【滤波跟踪】基于卡尔曼滤波实现ins与gps松组合导航附matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

个人主页：Matlab科研工作室

个人信条：格物致知。

⛄ 内容介绍

基于卡尔曼滤波实现INS(Inertial Navigation System)与GPS(Global Positioning System)组合导航，可以实现高精度的导航定位。具体实现步骤如下：

1. 将INS和GPS的输出数据进行预处理，包括数据对齐、数据格式转换、数据平滑等。
2. 建立卡尔曼滤波模型，包括状态方程、观测方程和卡尔曼滤波参数。状态方程描述了系统的运动模型，观测方程描述了GPS测量的位置信息，卡尔曼滤波参数包括初始状态、协方差矩阵、噪声模型等。
3. 根据卡尔曼滤波模型，执行滤波算法。卡尔曼滤波算法包括预测和更新两个步骤。预测步骤根据状态方程，预测下一时刻的状态和协方差矩阵；更新步骤根据观测方程，校正预测的状态和协方差矩阵，得到最终的状态和协方差矩阵。
4. 根据滤波结果，获得INS与GPS的组合导航定位结果。根据滤波结果，可以获得系统的位置、速度和姿态等信息，从而实现高精度的导航定位。

需要注意的是，INS和GPS的输出数据有时会存在误差和偏差，因此需要进行校正和补偿。INS的误差主要包括零偏、比例因数误差和随机游走误差，可以通过陀螺仪和加速度计的校准来消除。GPS的误差主要包括多径误差、钟漂误差和大气延迟误差，可以通过差分GPS和信号处理方法来消除

⛄ 部分代码

clear all;

% close all;

clc;

g0=9.78049;

rad_deg=0.01745329;

wie=7.2921158e-5;

Re=6378393.0;

time=50;

Hn=0.1;

G_Drift=[0.01*rad_deg/3600;   

         0.01*rad_deg/3600;  

         0.01*rad_deg/3600];

A_Bias=[1e-4*g0     

        1e-4*g0    

        1e-4*g0];

%%%%%%

T_p=7;

T_r=9;

T_y=12;

%%%%%%

pitchm=0*rad_deg;

rollm=0*rad_deg;

yawm=0*rad_deg;

%%%%初始角%%

yawk=90*rad_deg;

%%%%initial phase angle初始相位

p_pitch=0*rad_deg;

p_roll=0*rad_deg;

p_yaw=0*rad_deg;

%%%初始误差角%%平台坐标系到导航坐标系的角度

pitch_error = 0.05*rad_deg;

roll_error = 0.05*rad_deg;

yaw_error =0.05*rad_deg;

%%% n对应主惯导%%wie在导航坐标系的投影

lati=45.7796*rad_deg;

longi=126.6705*rad_deg;

wien=[0

      wie*cos(lati)

      wie*sin(lati)];

⛄ 运行结果

⛄ 参考文献

[1] 于德新,杨兆升,刘雪杰.基于卡尔曼滤波的GPS/DR导航信息融合方法[J].交通运输工程学报, 2006, 6(2):5.DOI:CNKI:SUN:JYGC.0.2006-02-014.

[2] 周红进,王秀森,许江宁.GFINS与GPS扩展松组合导航方法[J].中国惯性技术学报, 2011, 19(3):6.DOI:CNKI:SUN:ZGXJ.0.2011-03-017.

[3] 唐娟.基于联邦强跟踪卡尔曼滤波的组合导航关键技术研究[D].山东大学,2019.

[4] 王欣明.GPS/INS组合导航系统中卡尔曼滤波算法的研究与实现[D].北京交通大学[2023-06-10].

⛳️ 代码获取关注我

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料