交互式多模型-扩展卡尔曼滤波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主动雷达
在二维情况下,雷达量测为距离和角度
rkm=rk+r~kbkm=bk+b~k{r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde{b}_krkm=rk+r~kbkm=bk+b~k
其中
rk=(xk−x0)+(yk−y0)2)bk=tan−1yk−y0xk−x0r_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
[x0,y0][x_0,y_0][x0,y0]为雷达坐标,一般情况为0。雷达量测为zk=[rk,bk]′z_k=[r_k,b_k]'zk=[rk,bk]′。雷达量测方差为
Rk=cov(vk)=[σr200σb2]R_k=\text{cov}(v_k)=\begin{bmatrix}\sigma_r^2 & 0 \\0 & \sigma_b^2 \end{bmatrix}Rk=cov(vk)=[σr200σb2]且σr=70m\sigma_r=70mσr=70m, σb=0.3o\sigma_b=0.3^oσb=0.3o。
三、性能评估
RMSE(Root mean-squared error):蒙塔卡罗次数M=500M=500M=500,x^k∣ki\hat{x}_{k|k}^ix^k∣ki为第iii次仿真得到的估计。
RMSE(x^)=1M∑i=1M(xk−x^k∣ki)(xk−x^k∣ki)′\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^)=1M∑i=1M(xk−x^k∣ki)2+(yk−y^k∣ki)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^)=1M∑i=1M(x˙k−x˙^k∣ki)2+(y˙k−y˙^k∣ki)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),nnn 为状态维数,Pk∣ki\mathbf{P}_{k|k}^iPk∣ki为第iii次仿真滤波器输出的估计协方差
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, CTclose 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_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;xk_EKF1=X_aver_zero; %X(0|0)= X_aver_zeroPk_EKF1=P_zero; %P(0|0)= P_zeroxk_EKF2=X_aver_zero;Pk_EKF2=P_zero;xk_EKF3=X_aver_zero;Pk_EKF3=P_zero;
原创不易,路过的各位大佬请点个赞
交互式多模型-扩展卡尔曼滤波IMM-EKF——机动目标跟踪中的应用相关推荐
- 交互式多模型-粒子滤波IMM-PF—在机动目标跟踪中的应用/matlab实现
交互式多模型-粒子滤波IMM-PF-在机动目标跟踪中的应用/matlab实现 原创不易,路过的各位大佬请点个赞 WX: ZB823618313 交互式多模型-粒子滤波IMM-PF-在机动目标跟踪中的应 ...
- 交互式多模型算法IMM——机动目标跟踪中的应用
机动目标跟踪--交互式多模型算法IMM 原创不易,路过的各位大佬请点个赞 WX: ZB823618313 机动目标跟踪--交互式多模型算法IMM 机动目标跟踪--交互式多模型算法IMM 1. 对机动目 ...
- 机动目标跟踪——匀加速运动CA模型(二维)
机动目标跟踪--匀加速运动CA模型(二维) 原创不易,路过的各位大佬请点个赞 WX: ZB823618313 机动目标跟踪--目标模型概述 机动目标跟踪--匀加速运动CA模型(二维) 1. 对机动目标 ...
- 机动目标跟踪—匀速转弯CT模型/匀速圆周运动
机动目标跟踪-匀速转弯CT模型/匀速圆周运动 原创不易,路过的各位大佬请点个赞 针对机动目标跟踪的探讨.技术支持欢迎联系,也可以站内私信 WX: ZB823618313 机动目标跟踪-匀速转弯CT模型 ...
- 无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分)
无迹卡尔曼滤波UKF-目标跟踪中的应用(仿真部分) 原创不易,路过的各位大佬请点个赞 机动目标跟踪/非线性滤波/传感器融合/导航等探讨联系WX: ZB823618313 算法部分见博客: [无迹卡尔曼 ...
- 交互式多模型-无迹卡尔曼滤波IMM-UKF算法matlab实现(跟踪场景二)
交互式多模型-无迹卡尔曼滤波IMM-UKF算法matlab实现--机动目标跟踪 原创不易,路过的各位大佬请点个赞 针对机动目标跟踪的探讨.技术支持欢迎联系,也可以站内私信 WX: ZB82361831 ...
- 扩展卡尔曼滤波(EKF)理论讲解与实例(matlab、python和C++代码)
扩展卡尔曼滤波(EKF)理论讲解与实例(matlab.python和C++代码) 文章目录 扩展卡尔曼滤波(EKF)理论讲解与实例(matlab.python和C++代码) 理论讲解 KF和EKF模型 ...
- 行驶车辆状态估计,无迹卡尔曼滤波,扩展卡尔曼滤波(EKF UKF)
行驶车辆状态估计,无迹卡尔曼滤波,扩展卡尔曼滤波(EKF UKF) 软件使用:Matlab Simulink 适用场景:采用扩展卡尔曼滤波和无迹卡尔曼滤波EKF UKF进行行驶车辆的"车速, ...
- 扩展卡尔曼滤波(EKF)算法详细推导及仿真(Matlab)
前言 扩展卡尔曼滤波算法是解决非线性状态估计问题最为直接的一种处理方法,尽管EKF不是最精确的"最优"滤波器,但在过去的几十年成功地应用到许多非线性系统中.所以在学习非线性滤波问题 ...
最新文章
- jsp mysql 图片路径,请教JSP中怎么向MySql中存入和取出图片
- PHP ob_start() 函数介绍
- ArrayList各方法的时间复杂度
- Junit4常用注解
- 浏览器跨域访问解决方案
- C++文件操作与文件流
- antimalware可以关闭吗_“对方正在输入...”什么时候会出现?可以关闭吗?
- [大学回忆录-思想]为博乎?为专乎?
- Windows Live Writer配置步骤
- 微软最强命令行工具发布,强势霸榜GitHub
- Numpy中高维axis的操作个人理解
- 高数常见的符号及其读法
- 安装sql2000提示html,安装sql2000数据库提示:command line option syntax error
- python sasl_python用sasl的方式连接ldap提示
- vac服务器未响应,csgo国服游戏停止工作、未响应的解决方法
- tf.extract_image_patches
- Memcache教程
- 你会成为屋里最有趣的人
- 整合mybatis-plus必须避开的大坑
- 【私人备忘录】Android P 去电代码流程
热门文章
- pythonturtle画圆形螺旋风管_圆形风管节点大样图
- 教育知识与能力·中学
- 华为云Linux部署深度学习项目
- 光遇显示服务器已满怎么办,光遇服务器已满怎么办 光遇服务器已满您正在登陆队列中解决方法...
- tf.nn.conv2d和tf.contrib.slim.conv2d的区别
- 2※、封装流(包装流派)、缓冲流【字节缓冲流、字符缓冲流】-->【字节缓冲流、字符缓冲流】 、字符集、转换流以及打印流
- 任你和QQ陌生人聊天
- 无聊的游戏 Beta 0.1
- 网络摄像机中的IR-CUT详解
- c# 不可访问 因为它受保护级别限制