博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，Matlab项目合作可私信。
个人主页：海神之光
代码获取方式：
海神之光Matlab王者学习之路—代码获取方式
️座右铭：行百里者，半于九十。

更多Matlab仿真内容点击
Matlab图像处理（进阶版）
路径规划（Matlab）
神经网络预测与分类（Matlab）
优化求解（Matlab）
语音处理（Matlab）
信号处理（Matlab）
车间调度（Matlab）

一、强跟踪滤波IMU+GNSS组合导航简介

强跟踪滤波（Strong Tracking Filter）是一种基于卡尔曼滤波的IMU+GNSS组合导航算法，用于提高导航的精度和鲁棒性。该算法通过融合IMU和GNSS的数据，利用卡尔曼滤波来估计系统的状态，并根据系统的动态模型和测量数据进行递归更新。

以下是强跟踪滤波IMU+GNSS组合导航的步骤：
（1）初始化：设置初始状态估计值和协方差矩阵。
（2）预测：根据系统的动态模型，使用IMU的测量数据进行状态预测，并更新状态的协方差矩阵。
（3）更新：使用GNSS的测量数据来校正状态估计值和协方差矩阵。
（4）融合：将预测和更新的结果进行融合，得到最终的状态估计值和协方差矩阵。

1 以 GNSS+IMU 的紧耦合技术为基础，结合环境特征匹配 的综合方案将成为主流
我们认为，L3 级别自动驾驶的定位精度误差要控制在 30cm 以内。一方面，高精度 地图的绘制精度要求达到 10cm，定位精度应与地图精度匹配。另一方面，L3 级别 需要满足车道级定位精度。从国家颁布的城市道路标准可以得到相关数据，城市道 路、交叉路口、干线公路（包括高速公路）、路肩（高速公路紧急停车带）对应的 车道宽度分别为 3.5 米、 2.3-2.5 米、3.75 米以及 1.5-2.5 米，轿车的宽度一般约 2 米。由此，我们分析 L3 级别需要 10-30cm 的定位精度。在技术路线上，我们认为 自动驾驶的成熟定位方案，应该是以 GNSS+IMU 的紧耦合技术为基础，结合环境 特征匹配的综合方案。

2 GNSS 精度的提升主要依赖于增强系统的建设
传统的 GNSS 单点定位精度为米级，精度提升的主要路径是通过 RTK 技术以及建 立增强系统。 GNSS 的全称是全球导航卫星系统，包括 GPS、Glonass、Galileo、北斗及对应的增 强系统和区域系统。卫星定位原理是通过接收器与至少 4 颗卫星通讯来确定该接收 器的位臵，由于存在大气离层干扰，这种方法是伪距测量，精度只能达到米级。RTK 是一种利用 GPS 载波相位观测值进行实时动态相对定位的技术，流动站利用基准 站数据和 GPS 观测数据做差分处理，可以在野外实时得到厘米级定位结果。RTK 已经是一项成熟的技术，随着增强系统不断建设，在大多场景下车辆可以通过 GNSS 定位系统获得满足自动驾驶需要的精度。

自动驾驶车辆通过安装的车载 GNSS 接收机接收信号，比较基站接收的卫星信号做 差分处理得到高精度定位。最早利用单独的 GPS 做厘米级 RTK 固定解需要几分钟， 现在通过 GNSS 的多星多频联合结算，时间缩短到几十秒。

地基增强系统：通过在地面建立固定的参考站(CORS 站)，来对比卫星定位坐标与 自身已知坐标的误差，将差分修正发送给接收机，最终使得卫星导航精度达到亚米 级。 星基增强系统：包含了 GNSS 系统中的五个增强系统，由地面参考基站，主控站， 上行注入站和地球同步卫星共同组成。差分站将监测导航卫星获得的原始定位数据 送至主控站，主控站计算出各卫星的定位修正信息，地球静止轨道卫星最后将注入 站发来的信息播发给广大用户。

3 惯性导航系统是应对车辆复杂行驶环境的必要补充
惯性导航系统（Internal Navigation System, INS）由陀螺仪、加速度计等惯性传感器 和导航解算系统集成而成。陀螺仪和加速度计是系统的核心器件，陀螺仪测量物体 的角速度，加速度计测量物体的加速度。典型的惯导产品包含 3 组陀螺仪和加速度 计，分别测量三个自由度的角速度和加速度，通过积分即可获得物体在三维空间的 运动速度和轨迹。在实际应用中，需要利用 GPS、北斗等方式产生的信号进行初始 化，结合惯导信号和卫星导航信号进行进行卡尔曼滤波处理，得出其最佳推算的定 位信息。 惯导比起卫星定位具有自身的技术优势，测量方法不依赖外界，短期精度高，能稳 定高频地输出信号。工作原理是通过感知物体在空间的角速度、线速度，进而获取 物体的姿态、位臵和速度等信息，实现对运动物体姿态和运动轨迹的测量，可以实 现全天候全地点地工作。但惯导也有自身的缺陷，由于采用积分算法，定位误差随 载体运行不断累积。

4 GNSS+IMU 紧密耦合方案有望成为定位技术主流
GNSS 和 INS 都存在着自身难以克服的缺点，但两者具有很强的互补性，组合定位 可以各取所长。按照信息交换或组合程度的不同，分为松散组合和紧耦合。两种组 合方案都需要用到卡尔曼滤波器，区别在于松散组合只是 GNSS 信息单向对 INS 信号进行反馈校正，而紧耦合是双向信息传输，INS 信号也用于计算载体相对于 GNSS 卫星的伪距和伪距率, 来辅助 GNSS 信号的接收过程，以此提高精度和动态 性能。

总体来说，松耦合方案相对紧耦合来说结构简单，在 GPS 工作良好时，组合方案 输出精度较好，当 GPS 受影响而长期不工作时，组合精度急剧下降。紧耦合方案 在动态工作下精度和可靠性更高，即使 GPS 信号无法跟踪时也可以利用 INS 独立 导航，而且利用 INS 可以提高 GPS 信号重新捕获速度，改进跟踪回路能力，提高 抗干扰性和保密性。

在实际当中，组合导航在城市复杂路况的表现显著优于单独 GNSS 导航。从示例中 可以看到，组合导航行驶轨迹更圆滑，在天桥、树林或是遮挡环境下都能有持续性 输出。组合导航可以保障高动态、高频率定位输出的稳定性，在辅助高速过弯方面 优势明显。

二、部分源代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SINS协方差仿真
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%常数设置%%%%%%%%%%%
deg_rad=0.01745329252e0;% Transfer from angle degree to rad
g=9.7803698; %重力加速度 （单位：米/秒/秒）

%%%%%%%%仿真时间设置%%%%%%
t=0;
T=1;
t_stop=5000.0;

%%%%%%%%%%%%%%%%航迹发生器%%%%%%%%%%%%%%%%%
atti=zeros(3,1); %横滚、俯仰、航向（单位：度）
atti_rate=zeros(3,1);%横滚速率、俯仰速率、航向速率（单位：度/秒）
veloB=zeros(3,1); %飞机运动速度——X右翼、Y机头、Z天向（单位：米/秒）
acceB=zeros(3,1); %飞机运动加速度——X右翼、Y机头、Z天向（单位：米/秒/秒）
posi=zeros(3,1); %航迹发生器初始位置经度、纬度、高度（单位：度、度、米）
posi=[106.491;29.528;300.0];

atti(1,1)=0.0;
atti(2,1)=0.0;
atti(3,1)=90.0; %初始航向角度（单位：度）

veloB(2,1)=0.0; %飞机初始运动速度——机头（单位：米/秒）

%%%%%%%%%%%%%%%%%%%%IMU输出%%%%%%%%%%
Wibb=zeros(3,1); %机体系陀螺仪输出 （单位：度/秒）
Fb=zeros(3,1); %机体系加速度计输出 （单位：米/秒/秒）

Gyro_fix=zeros(3,1);%机体系陀螺仪固定误差输出 （单位：弧度/秒）
Acc_fix=zeros(3,1); %机体系加速度计固定误差输出 （单位：米/秒/秒）
Gyro_b=zeros(3,1); % 陀螺随机常数（弧度/秒）
Gyro_r=zeros(3,1); % 陀螺一阶马尔可夫过程（弧度/秒）
Gyro_wg=zeros(3,1); %陀螺白噪声（弧度/秒）
Acc_r =zeros(3,1); % 加速度一阶马尔可夫过程（米/秒/秒）

%%%%%%%%%%%%%%%%%%GPS仿真输出%%%%%%%%%%%%%%
posiG = zeros(3,1); %GPS输出的飞行器位置（经度（度）、纬度（度）、高度（米））；
veloG = zeros(3,1); %GPS输出的飞行器速度（东向（米/秒），北向（米/秒），天向（米/秒））

%%%%%%%%%%%%%%%%%%%%%捷联惯导仿真%%%%%%%%%%%%%
attiN=zeros(3,1); %飞行器初始姿态
veloN=zeros(3,1); %飞行器初始速度（相对于导航系）
posiN=zeros(3,1); %飞行器初始位置
WnbbA_old=zeros(3,1); %角速度积分输出（单位：弧度）

posiN=posi; %初始位置与航迹位置一致
attiN=atti; %初始姿态与航迹姿态一致（可以用初始对准函数替换）

%%%%%%%%%%%%%%%%%%%%%%%KALMAN滤波输出%%%%%%%%%%%%%%%%%
T_D = 1; %离散周期；
T_M = 0; %滤波量测产生时间（秒）；
Xc = zeros(18,1); %综合模型状态量；(可用于误差修正)全部为国际单位制
PK = zeros(18,18); %协方差阵；
Xerr =zeros(1,18); %状态估计量的误差值（记录某个时刻的）
kflag=0; %GPS信息有效标志位（1－有效）

Acc_modi = zeros(3,1); %加速度计误差修正值（米/秒/秒）（X,Y,Z）
Gyro_modi= zeros(3,1); %陀螺误差修正值(弧度/秒)(X,Y,Z)

%%%%%%%%%%%%%%%%%%%初始对准%%%%%%%%%%%%%%%%%%%
kc=0;
tmp_Fb=zeros(3,1);
tmp_Wibb=zeros(3,1);
t_alig = 0;
while t<=120
[t_alig,atti,atti_rate,veloB,acceB]=trace(0,T,atti,atti_rate,veloB,acceB);
%航迹发生器产生飞行轨迹参数
[Wibb,Fb,posi]=IMUout(T,posi,atti,atti_rate,veloB,acceB);
%陀螺和加速度仿真

[Gyro_fix,Acc_fix]=imu_err_fix(Wibb,Fb);
%产生非随机性误差
%Fb=Fb+Acc_fix;
%Wibb=Wibb+Gyro_fix/deg_rad; % deg/s
%加入非随机性误差

[Gyro_b,Gyro_r,Gyro_wg,Acc_r]=imu_err_random(t,T,Gyro_b,Gyro_r,Gyro_wg,Acc_r);
%产生随机性误差初始值（此时IMU进入正常工作状态，则随机常数已为常值）
Fb=Fb+Acc_r;
Wibb=Wibb+Gyro_b/deg_rad+Gyro_r/deg_rad+Gyro_wg/deg_rad; % deg/s
%加入随机性误差

tmp_Fb=tmp_Fb+Fb;
tmp_Wibb=tmp_Wibb+Wibb;
kc=kc+1;

t=t+T;

disp(t);

end

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]李永生,董光焰,陈凯,谢亚峰,唐金力.基于卡尔曼滤波的无源定位精度分析[J].弹箭与制导学报. 2022,42(04)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除

仿真咨询
1 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化

2 机器学习和深度学习方面
卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

4 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化

5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配

6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化

7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化

8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置

9 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长

10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合