【组合导航】基于卡尔曼滤波实现IMM-GNSS- UWB -INS子模型切换算法组合导航Matlab仿真

本文链接：https://blog.csdn.net/matlab_daizuo/article/details/145388809

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知，求助可私信。

🔥 内容介绍

组合导航系统因其高精度、高可靠性和高自主性在复杂环境下具有广泛应用前景。针对单一导航系统在特定场景下的不足，本文深入探讨了基于卡尔曼滤波框架的多源信息融合方法，重点研究了交互式多模型（IMM）算法在GNSS、UWB、INS三种导航子系统间切换的应用。该方案利用IMM算法动态估计系统状态并切换子模型，实现了在不同环境条件下对最优导航解的自适应选择，从而提高了组合导航系统的整体性能。论文详细阐述了IMM-GNSS-UWB-INS组合导航系统的数学模型、卡尔曼滤波过程以及模型切换策略，并通过仿真实验验证了所提出方法的有效性和优越性。结果表明，所提出的IMM子模型切换算法能够有效克服单一导航系统在特定环境下的局限性，显著提升了组合导航系统的精度和鲁棒性。

关键词：组合导航，卡尔曼滤波，交互式多模型，GNSS，UWB，INS，子模型切换

1. 引言

在现代社会，导航定位技术已成为诸多领域不可或缺的关键支撑。然而，单一导航系统往往存在固有的局限性。例如，全球导航卫星系统（GNSS）在遮挡或多径效应严重的环境下精度降低，惯性导航系统（INS）的误差会随时间积累，超宽带定位（UWB）的精度受非视距（NLOS）传播影响。为克服上述不足，组合导航技术应运而生，它将多种导航系统的优势互补，以期实现更精准、更可靠的定位导航。

本文关注的重点是基于卡尔曼滤波框架的IMM（交互式多模型）算法在GNSS、UWB和INS这三种子系统间的切换应用。卡尔曼滤波作为一种最优状态估计器，广泛应用于组合导航系统中。IMM算法则是一种能够处理系统模型不确定性的有效工具，它通过维护多个模型并根据模型概率动态切换，使得系统能够适应不同的运行状态和环境条件。本文将详细介绍IMM-GNSS-UWB-INS组合导航系统的建模、卡尔曼滤波实现以及模型切换策略，并进行仿真验证。

2. 理论基础

2.1 卡尔曼滤波

卡尔曼滤波是一种用于线性系统状态估计的递归算法，它基于系统动态模型和观测模型，通过预测和更新两个步骤，不断修正状态估计值，达到最优估计的目的。其基本公式可概括为：

预测步骤：
- 状态预测： 𝑥^𝑘∣𝑘−1=𝐹𝑘𝑥^𝑘−1∣𝑘−1+𝐵𝑘𝑢𝑘x^k∣k−1=Fkx^k−1∣k−1+Bkuk
- 协方差预测：𝑃𝑘∣𝑘−1=𝐹𝑘𝑃𝑘−1∣𝑘−1𝐹𝑘𝑇+𝑄𝑘Pk∣k−1=FkPk−1∣k−1FkT+Qk
更新步骤：
- 卡尔曼增益：𝐾𝑘=𝑃𝑘∣𝑘−1𝐻𝑘𝑇(𝐻𝑘𝑃𝑘∣𝑘−1𝐻𝑘𝑇+𝑅𝑘)−1Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1
- 状态更新：𝑥^𝑘∣𝑘=𝑥^𝑘∣𝑘−1+𝐾𝑘(𝑧𝑘−𝐻𝑘𝑥^𝑘∣𝑘−1)x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)
- 协方差更新：𝑃𝑘∣𝑘=(𝐼−𝐾𝑘𝐻𝑘)𝑃𝑘∣𝑘−1Pk∣k=(I−KkHk)Pk∣k−1

其中，𝑥^x^表示状态估计值，𝑃P表示状态协方差，𝐹F是状态转移矩阵，𝐵B是控制矩阵，𝑢u是控制输入，𝑄Q是过程噪声协方差，𝐻H是观测矩阵，𝑅R是观测噪声协方差，𝑧z是观测值。

2.2 交互式多模型（IMM）算法

IMM算法是一种处理系统模型不确定性的有效方法，其核心思想是在多个假设的模型下并行运行卡尔曼滤波器，并根据模型概率动态切换，以实现对系统状态的自适应估计。IMM算法主要包括以下步骤：

模型混合：根据上一时刻的模型概率，对当前时刻各个模型的初始状态和协方差进行加权混合。
卡尔曼滤波：对每个模型分别进行卡尔曼滤波预测和更新。
模型概率更新：根据每个模型的观测残差，更新各个模型的后验概率。
状态估计融合：对每个模型的滤波结果，根据其对应的后验概率进行加权融合，得到最终的状态估计。

3. IMM-GNSS-UWB-INS 组合导航系统建模

3.1 系统状态模型

3.3 子模型设定

根据实际应用场景，定义以下三个子模型：

模型1（GNSS-INS）： GNSS和INS工作，适用于开阔环境。
模型2（UWB-INS）： UWB和INS工作，适用于GNSS遮挡环境。
模型3（INS）：只有INS工作，适用于GNSS和UWB均不可用的环境。

每个子模型对应不同的状态转移矩阵和观测矩阵。

4. IMM-卡尔曼滤波算法实现

IMM-卡尔曼滤波算法的实现流程如下：

初始化：初始化每个模型的初始状态估计值、协方差矩阵和模型概率。
混合：根据上一时刻的模型概率，对当前时刻各个模型的初始状态和协方差进行加权混合。
滤波：对每个模型分别进行卡尔曼滤波预测和更新，利用各自对应的状态转移矩阵、观测矩阵和观测值。
模型概率更新：根据每个模型的观测残差，计算模型似然值，并更新模型后验概率。
融合：对每个模型的滤波结果，根据其对应的后验概率进行加权融合，得到最终的状态估计。
循环：将最终的状态估计值和协方差矩阵作为下一时刻的先验值，重复步骤2-5。

5. 仿真实验与结果分析

为了验证所提出方法的有效性，进行了仿真实验。仿真实验场景设定为：

初始状态：位置、速度、姿态均为已知，加速度计和陀螺仪零偏为随机值。
系统运动：模拟一段较为复杂的运动轨迹，包括直线、转弯等运动状态。
噪声：GNSS、UWB和INS均存在随机噪声，且UWB存在一定的NLOS误差。
子模型切换：模拟GNSS遮挡和UWB失效情况，实现子模型之间的切换。

实验结果表明：

IMM算法能够准确估计系统状态，并根据环境变化动态切换子模型。
与单一导航系统相比，所提出的IMM-GNSS-UWB-INS组合导航系统在所有场景下的定位精度均得到显著提升。
在GNSS遮挡和UWB失效场景下，系统仍能保持一定的定位精度，体现了较强的鲁棒性。

6. 结论

本文详细介绍了基于卡尔曼滤波的IMM-GNSS-UWB-INS子模型切换算法组合导航方法。该方法充分利用GNSS、UWB和INS三种导航系统的优势，并采用IMM算法进行动态模型切换，有效提高了组合导航系统的精度和鲁棒性。仿真实验结果验证了所提出方法的有效性和优越性。该方法为复杂环境下的高精度导航定位提供了一种有前景的解决方案。