交互式多模型-扩展卡尔曼滤波IMM-EKF——机动目标跟踪中的应用
交互式多模型-扩展卡尔曼滤波IMM-EKF——机动目标跟踪中的应用
原创不易，路过的各位大佬请点个赞
针对机动目标跟踪的探讨、技术支持欢迎联系，也可以站内私信
WX: ZB823618313
机动目标跟踪——交互式多模型算法IMM
- 交互式多模型-扩展卡尔曼滤波IMM-EKF——机动目标跟踪中的应用
-
- 1. IMM算法介绍
- 2. 目标运动模型概述
- 3. EKF介绍
- 4. IMM-EKF仿真实现：案列一
-
- 4.1. 仿真参数
- 4.2. 跟踪轨迹
- 4.3. 位置/速度RMSE
- 4.4. 模型概率
- 4.5. 部分代码
基于IMM机动目标跟踪算法设计最重要的核心部分主要包括：
- IMM框架
- 滤波器选择：(这里基于UKF)
- 目标运动模型：（这里基于CV CT）
1. IMM算法介绍
核心思想： IMM算法的基本思想是用多个不同的运动模型匹配机动目标的不同运动模式，不同模型间的转移概率是–个马尔可夫矩阵，目标的状态估计和模型概率的更新使用卡尔曼滤波。其算法流程图如图5.3所示。
具体算法推导见另一个博客，这里不再赘述
2. 目标运动模型概述
机动目标模型描述了目标状态随着时间变化的过程。一个好的模型抵得上大量的数据。当前几乎所有的目标跟踪算法都是基于模型进行状态估计的。在卡尔曼滤波器被引入目标跟踪领域后，基于状态空间的机动目标建模成为主要研究对象之一。
匀速运动模型CV
匀速转弯运动CT
匀加速运动CA
Singer模型
Jerk模型
当前统计模型CS
… …
这对这些模型的介绍，见博主其他博客
3. EKF介绍
从算法层面，在机动目标跟踪系统中，常用的滤波算法是以卡尔曼滤波器为基本框架的估计算法。卡尔曼滤波器是一种线性、无偏、以误差均方差最小为准则的最优估计算法，它有精确的数学形式和优良的使用效能。卡尔曼滤波方法实质上是一种数据处理方法，它采用递推滤波方法，根据获取的量测数据由递推方程递推给出新的状态估计。由于计算量和存储量小，比较容易满足实时计算的要求，在工程实践中得到广泛应用。
除此之外，非线性滤波也广泛应用与机动目标跟踪，比如：
扩展卡尔曼滤波EKF
无迹卡尔曼滤波UKF
容积卡尔曼滤波CKF
求积卡尔曼滤波QKF
中心差分卡尔曼滤波CDKF
Divided difference filter DDF
高斯混合滤波GSF
强跟踪滤波STF
粒子滤波PF
… …
扩展卡尔曼滤波是基于函数近似的非线性滤波算法，即对非线性系统进行泰勒级数展开，取一阶项，非线性系统则退化为线性系统。
优点：算法复杂度低，存在解析解
缺点：非线性系统必须可导、推导的地方比较多、噪声扰动比较大时容易发散EKF算法的推导及介绍，见博主其他博客
4. IMM-EKF仿真实现：案列一
4.1. 仿真参数
一、目标模型：CV CT CV
第一阶段：1:39s，匀速运动CV
第二阶段：40:91s，匀速圆周运动CT，角速度： 6 ∗ π / 180 ; 6*\pi/180; 6∗π/180;
第三阶段：92:99s，匀速运动CV
第四阶段：100:131s，匀速圆周运动CT，角速度： − 3 ∗ π / 180 ; -3*\pi/180; −3∗π/180;
第五阶段：132:150s，匀速运动CV
CV CT 模型的具体方程形式见另一个博客
二、测量模型：2D主动雷达
在二维情况下，雷达量测为距离和角度
r k m = r k + r ~ k b k m = b k + b ~ k {r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde{b}_k rkm=rk+r~kbkm=bk+b~k
其中
r k = ( x k − x 0 ) + ( y k − y 0 ) 2 ) b k = tan − 1 y k − y 0 x k − x 0 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}}\\ rk=(xk−x0)+(yk−y0)2)bk=tan−1xk−x0yk−y0
[ x 0 , y 0 ] [x_0,y_0] [x0,y0]为雷达坐标，一般情况为0。雷达量测为 z k = [ r k , b k ] ′ z_k=[r_k,b_k]' zk=[rk,bk]′。雷达量测方差为
R k = cov ( v k ) = [ σ r 2 0 0 σ b 2 ] R_k=\text{cov}(v_k)=\begin{bmatrix}\sigma_r^2 & 0 \\0 & \sigma_b^2 \end{bmatrix} Rk=cov(vk)=[σr200σb2]且 σ r = 70 m \sigma_r=70m σr=70m， σ b = 0. 3 o \sigma_b=0.3^o σb=0.3o。
三、性能评估
RMSE(Root mean-squared error):蒙塔卡罗次数 M = 500 M=500 M=500， x ^ k ∣ k i \hat{x}_{k|k}^i x^k∣ki为第 i i i次仿真得到的估计。
RMSE ( x ^ ) = 1 M ∑ i = 1 M ( x k − x ^ k ∣ k i ) ( x k − x ^ k ∣ k 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)'} RMSE(x^)=M1i=1∑M(xk−x^k∣ki)(xk−x^k∣ki)′
Position RMSE ( x ^ ) = 1 M ∑ i = 1 M ( x k − x ^ k ∣ k i ) 2 + ( y k − y ^ k ∣ k i ) 2 \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} Position RMSE(x^)=M1i=1∑M(xk−x^k∣ki)2+(yk−y^k∣ki)2
Velocity RMSE ( x ^ ) = 1 M ∑ i = 1 M ( x ˙ k − x ˙ ^ k ∣ k i ) 2 + ( y ˙ k − 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} Velocity RMSE(x^)=M1i=1∑M(x˙k−x˙^k∣ki)2+(y˙k−y˙^k∣ki)2
ANEES（average normalized estimation error square）, n n n 为状态维数， P k ∣ k i \mathbf{P}_{k|k}^i Pk∣ki为第 i i i次仿真滤波器输出的估计协方差
4.2. 跟踪轨迹
4.3. 位置/速度RMSE
4.4. 模型概率
4.5. 部分代码
% Interacting Multiple Model2
% created by:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 二维目标跟踪，IMM-EKF
% 目标模型：近似匀速运动模型CV 近似匀角速度运动模型CT
% 状态 x=[x位置， x速度， y位置， y速度]'
% 量测模型：三维量测(距离，角度，多普勒速度)
% 算法: 单雷达的IMM-UKF
% 性能指标：真实轨迹，RMSE均方根误差，
% 使用三个模型进行交互，分别为CV, CT, CT
close all;
clear all;
clc;
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%系统参数设置
runs=50; %蒙特卡洛实验次数，滤波将进行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 = 6*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([1e5,1e2,1e5,1e2]);
%模型概率初始化
m_ukf=[0.6;0.2;0.2];
m_ekf=[0.6;0.2;0.2];
%模型转移概率
Pa_ukf=[0.8 0.1 0.1;
0.1 0.8 0.1;
0.1 0.1 0.8;];
Pa_ekf=[0.8 0.1 0.1;
0.2 0.6 0.2;
0.2 0.2 0.6;];
%误差存储
X_true=zeros(4,steps,runs);
Z_true=zeros(2,steps,runs);
X_err_IMM_ukf=zeros(4,steps,runs);
X_IMM_ukf=zeros(4,steps,runs);
PI_ukf=zeros(3,steps,runs);
X_err_IMM_ekf=zeros(4,steps,runs);
X_IMM_ekf=zeros(4,steps,runs);
PI_ekf=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_zero
Pk_UKF1=P_zero; %P(0|0)= P_zero
xk_UKF2=X_aver_zero;
Pk_UKF2=P_zero;
xk_UKF3=X_aver_zero;
Pk_UKF3=P_zero;
xk_EKF1=X_aver_zero; %X(0|0)= X_aver_zero
Pk_EKF1=P_zero; %P(0|0)= P_zero
xk_EKF2=X_aver_zero;
Pk_EKF2=P_zero;
xk_EKF3=X_aver_zero;
Pk_EKF3=P_zero;
原创不易，路过的各位大佬请点个赞