機動目標跟蹤——交互式多模型算法IMM

原創不易，路過的各位大佬請點個贊

WX: ZB823618313

1.交互式多模型算法流程

核心思想： IMM算法的基本思想是用多個不同的運動模型匹配機動目標的不同運動模式，不同模型間的轉移概率是–個馬爾可夫矩陣，目標的狀態估計和模型概率的更新使用卡爾曼濾波。其算法流程圖如圖5.3所示。

emd算法基本思想，

2. 交互式多模型：具體推導

3. IMM+KF目標跟蹤

WX: ZB823618313

4. IMM+UKF目標跟蹤

4.1. 仿真參數

一、目標模型：CV CT CT
第一階段：1:39s，勻速運動CV
第二階段：40:71s，勻速圓周運動CT，角速度：$2?\pi /180;$
第三階段：72:99s，勻速運動CV
第四階段：100:131s，勻速圓周運動CT，角速度：$?3?\pi /180;$
第五階段：72:99s，勻速運動CV

CV CT 模型的具體方程形式見另一個博客

二、測量模型：2D主動雷達
在二維情況下，雷達量測為距離和角度
$$\begin{gathered} r_k^m=r_k+{\tilde{r}}_k \\ {b}_k^m=b_k+{\tilde{b}}_k \end{gathered}$$
其中
$$\begin{gathered} r_k=\sqrt{(x_k?x_0{)}^{+}(y_k?y_0{)}^2)} \\ {b}_k={\tan }^{?1}\frac{y_k?y_0}{x_k?x_0} \end{gathered}$$
$[x_0,y_0]$為雷達坐標，一般情況為0。雷達量測為$z_k=[r_k,b_k{]}^{\prime }$。雷達量測方差為
$$R_k=\text{cov}(v_k)=\left[\begin{array}{cc}{\sigma }_r^2& 0\\ 0& {\sigma }_b^2\end{array}\right]$$且${\sigma }_r=120m$， ${\sigma }_b=80mrad$。

算法與模型的區別？三、性能評估
RMSE(Root mean-squared error):蒙塔卡羅次數$M=500$，${\hat{x}}_{k|k}^i$為第$i$次仿真得到的估計。
$$\text{RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum _{i=1}^M({\mathbf{x}}_k?{\hat{\mathbf{x}}}_{k|k}^i)({\mathbf{x}}_k?{\hat{\mathbf{x}}}_{k|k}^i{)}^{\prime }}$$
$$\text{Position?RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum _{i=1}^M(x_k?{\hat{x}}_{k|k}^i{)}^2+(y_k?{\hat{y}}_{k|k}^i{)}^2}$$
$$\text{Velocity?RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum _{i=1}^M({\dot{x}}_k?{\hat{\dot{x}}}_{k|k}^i{)}^2+({\dot{y}}_k?{\hat{\dot{y}}}_{k|k}^i{)}^2}$$
ANEES（average normalized estimation error square）,$n$ 為狀態維數，${\mathbf{P}}_{k|k}^i$為第$i$次仿真濾波器輸出的估計協方差
$$\text{ANEES}(\hat{x})=\frac{1}{Mn}\sum _{i=1}^M({\mathbf{x}}_k?{\hat{\mathbf{x}}}_{k|k}^i{)}^{\prime }({\mathbf{P}}_{k|k}^i{)}^{?1}({\mathbf{x}}_k?{\hat{\mathbf{x}}}_{k|k}^i)$$

4.2. 跟蹤軌跡

4.3. 位置/速度RMSE

4.4. 模型概率

4.5. 部分代碼

% Interacting Multiple Model2% created by: 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 二維目標跟蹤，IMM-UKF
% 目標模型：近似勻速運動模型CV 近似勻角速度運動模型CT
% 狀態 x=[x位置， x速度， y位置， y速度]'
% 量測模型：三維量測(距離，角度)
% 算法: 單雷達的IMM-UKF
% 性能指標：真實軌跡，RMSE均方根誤差，
% 使用三個模型進行交互，分別為CV, CT, CTclose all;
clear all;
clc;
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%系統參數設置
runs=300; %蒙特卡洛實驗次數，濾波將進行100次
steps=150; %每次濾波進行80次采樣%模型1的動態方程參數設置,Xk+1=fk(Xk)+G*uk+Gk*Wk,CV模型
T=1;
Fk1=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1;];q1=0.01; % 目標運動學標準差，過程噪聲
Qk1=q1*[T^3/3, T^2/2,        0,     0;T^2/2, T,            0,     0;0,     0,     T^3/3,   T^2/2;0,     0,     T^2/2,   T  ;];% 過程噪聲協方差
Gk= eye(4); %過程噪聲增益矩陣%模型2的動態方程參數設置,Xk+1=fk(Xk)+G*uk+Gk*Wk,CT模型
w1 = 2*pi/180;
Fk2=[1       sin(w1*T)/w1        0     -(1-cos(w1*T))/w1         ;0         cos(w1*T)        0            -sin(w1*T)        ;0   (1-cos(w1*T))/w1        1           sin(w1*T)/w1        ;0         sin(w1*T)        0             cos(w1*T)        ;];
q2 = 0.01;Qk2= q2*[2*(w1*T-sin(w1*T))/w1^3       (1-cos(w1*T))/w1^2                    0           (w1*T-sin(w1*T))/w1^2     ;(1-cos(w1*T))/w1^2                     T       -(w1*T-sin(w1*T))/w1^2            0        ;0         -(w1*T-sin(w1*T))/w1^2       2*(w1*T-sin(w1*T))/w1^3           (1-cos(w1*T))/w1^2  ;(w1*T-sin(w1*T))/w1^2                     0             (1-cos(w1*T))/w1^2                         T;];%模型3的動態方程參數設置,Xk+1=fk(Xk)+G*uk+Gk*Wk,CT模型
w = -3*pi/180;
Fk3=[1       sin(w*T)/w        0     -(1-cos(w*T))/w         ;0         cos(w*T)        0            -sin(w*T)        ;0   (1-cos(w*T))/w        1           sin(w*T)/w        ;0         sin(w*T)        0             cos(w*T)        ;];
Qk3= q2*[2*(w*T-sin(w*T))/w^3       (1-cos(w*T))/w^2                    0           (w*T-sin(w*T))/w^2     ;(1-cos(w*T))/w^2                     T       -(w*T-sin(w*T))/w^2            0        ;0         -(w*T-sin(w*T))/w^2       2*(w*T-sin(w*T))/w^3           (1-cos(w*T))/w^2  ;(w*T-sin(w*T))/w^2                     0             (1-cos(w*T))/w^2                         T;];%量測方程參數設置，Zk=H*Xk+Vk
v_mu=[0,0]';
% 雷達傳感器標準差，即測量噪聲
sigma_r=200; sigma_b=0.3*pi/180;%雷達1
% 雷達坐標
xp(:,1)=[20000, 0, 3 ,0 ]; %第1個傳感器的位置，可設置[x坐標， 0， y坐標， 0]，xy對應傳感器二維位置中間的0不能變，只是為了適應維數%濾波初始化設置
X_aver_zero=[30000,80,20000,50]';
P_zero=diag([1e6,1e3,1e6,1e3]);%模型概率初始化
m=[0.8;0.1;0.1];
%模型轉移概率
Pa=[0.8 0.1 0.1;0.1 0.8 0.1;0.1 0.1 0.8;];%誤差存儲
X_true=zeros(4,steps,runs);
Z_true=zeros(2,steps,runs);
X_err_IMM=zeros(4,steps,runs);
X_IMM=zeros(4,steps,runs);
PI=zeros(3,steps,runs);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%randn('state',sum(100*clock));  %每次給不同的狀態重置隨機數產生器（因為clock每次都不同）%%for index=1:runs %蒙特卡洛次數index         %顯示運行次數%濾波初始化X=X_aver_zero+sqrtm(P_zero)*randn(4,1);%產生真實X0%三個濾波器的初始化   xk_UKF1=X_aver_zero;             %X(0|0)= X_aver_zeroPk_UKF1=P_zero;                  %P(0|0)= P_zeroxk_UKF2=X_aver_zero;Pk_UKF2=P_zero;xk_UKF3=X_aver_zero;Pk_UKF3=P_zero;%% 產生真實軌跡for k=1:39X=Fk1*X+Gk*sqrtm(Qk1)*randn(4,1);     %產生真實軌跡X_true(:,k,index)=X;endfor k=40:71X=Fk2*X+Gk*sqrtm(Qk2)*randn(4,1);X_true(:,k,index)=X;endfor  k=72:99X=Fk1*X+Gk*sqrtm(Qk1)*randn(4,1);X_true(:,k,index)=X;endfor  k=100:131X=Fk3*X+Gk*sqrtm(Qk3)*randn(4,1);X_true(:,k,index)=X;endfor  k=132:stepsX=Fk1*X+Gk*sqrtm(Qk1)*randn(4,1);X_true(:,k,index)=X;end%% 狀態遞歸實現for k=1:steps

原創不易，路過的各位大佬請點個贊