✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
基于卡尔曼滤波实现INS（Inertial Navigation System）与GPS（Global Positioning System）组合导航，可以实现高精度的导航定位。具体实现步骤如下：
- 将INS和GPS的输出数据进行预处理，包括数据对齐、数据格式转换、数据平滑等。
- 建立卡尔曼滤波模型，包括状态方程、观测方程和卡尔曼滤波参数。状态方程描述了系统的运动模型，观测方程描述了GPS测量的位置信息，卡尔曼滤波参数包括初始状态、协方差矩阵、噪声模型等。
- 根据卡尔曼滤波模型，执行滤波算法。卡尔曼滤波算法包括预测和更新两个步骤。预测步骤根据状态方程，预测下一时刻的状态和协方差矩阵；更新步骤根据观测方程，校正预测的状态和协方差矩阵，得到最终的状态和协方差矩阵。
- 根据滤波结果，获得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].
