本文介紹的MATLAB程序可以實現：基于交互式多模型（IMM）的無跡卡爾曼濾波（UKF）方法，用于二維平面中目標的運動狀態估計。該算法結合了兩個運動模型：勻速直線模型（CV）和勻速轉彎模型（CT），可在不同運動模式間自適應切換，從而提高目標跟蹤精度。

程序詳解

🧠 程序核心思想

在目標跟蹤問題中，由于目標可能存在不同的運動模式（如直線、轉彎等），單一模型很難始終適應。IMM算法通過在多個運動模型間加權融合預測結果，并實時調整模型權重，提升整體濾波魯棒性和精度。

在每一步濾波中，IMM流程如下：

1. 模型交互（混合）：基于前一時刻模型概率和轉移矩陣計算混合初始狀態。
2. 狀態預測：每個模型獨立進行 UKF 預測。
3. 觀測更新：每個模型基于當前觀測進行 UKF 校正，得到各自的狀態估計和置信度。
4. 模型概率更新：根據每個模型的觀測匹配程度（似然）更新模型概率。
5. 估計融合：加權融合所有模型的狀態估計，得到當前最終估計值。

📊 主要功能模塊

• 模擬真實軌跡
• 單模型 UKF 濾波
• IMM UKF 濾波

運行結果

軌跡真值與不同方法得到的對比：

位置誤差對比曲線：

速度誤差對比曲線：

模型的概率曲線：

命令行窗口輸出的誤差統計特性對比：

MATLAB源代碼

部分代碼如下：

% CV和CT模型組成的IMM UKF
% 2024-11-06/Ver1
% 2025-08-07/Ver2：添加誤差統計特性計算與輸出、優化非線性情況下的權重更新
%% 建模
clear; %清空工作區
clc; %清空命令行
close all; %關閉所有窗口（主窗口除外）
rng(0); %固定隨機種子，讓每次運行得到的結果相同
N = 600; %定義仿真時間為600
T = 1; %定義采樣間隔為1
x0 = [1000,10,1000,10]'; %狀態初始化，四項為別為x軸位置、速度、y軸位置、速度
xA = []; %預定義輸出的狀態
% CV勻速運動
% x = A1*x + G1*sqrt(Q1)*[randn,randn]';
A1 = [1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1]; %定義勻速運動時的狀態轉移矩陣
G1=[T^2/2,0;T,0;0,T^2/2;0,T] ; %設置勻速運動時的輸入向量轉移矩陣
Q1=0.01*diag([1,1]); %設置狀態轉移協方差矩陣
% CT勻速圓周運動
A2=CreatCTF(-pi/360,T); %設置勻速圓周運動時的狀態轉移矩陣
G2=CreatCTT(T); %設置勻速圓周運動時的輸入向量轉移矩陣
Q2=0.144^2*diag([1,1]); %設置勻速圓周運動時的% 產生真實數據
x = x0; %在迭代產生真值前，給x賦初值
for k = 1:140%勻速直線x = A1*x + G1*sqrt(Q1)*randn(size(Q1,1),1); %狀態一步轉移，后面的是誤差xA =[xA x]; % 將迭代得到的x加在xA后面一列
end
for k = 1:260%勻速圓周轉彎x = A2*x + G2*sqrt(Q2)*randn(size(Q2,1),1); %狀態一步轉移，后面的是誤差xA =[xA x]; % 將迭代得到的x加在xA后面一列
end
for k = 1:N-400%勻速直線x = A1*x + G1*sqrt(Q1)*randn(size(Q1,1),1); %狀態一步轉移，后面的是誤差xA =[xA x]; % 將迭代得到的x加在xA后面一列
end

完整代碼的下載鏈接：
https://download.csdn.net/download/callmeup/91611532

如需幫助，或有導航、定位濾波相關的代碼定制需求，請點擊下方卡片聯系作者