无迹卡尔曼滤波(UKF)在单观测站目标跟踪中的应用
假定目标做匀速直线运动,在单个观测站对目标进行观测的前提下,再假设目标的初始状态已知。目标的运动方程可以写成如下形式:
其中:
设采样时间间隔T=1s,运行时间为N=60s,W(k)的均方差为,为一个可调节的参数,其远小于1,V(k)均方差为R=5。同时设置UT变换中的相关系数,,维数为9,雷达位置设为(200,300).假设目标运动各时刻的真实状态信息为:,而利用UKF滤波算法得到的目标状态为:
定义均方根误差(RMSE):
仿真结果:
程序:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%无迹卡尔曼滤波在目标追踪中的应用
%%%%%%%%%%%%%%%%%%%%%%%
close all;
clear all;
clc;
T=1; %雷达扫描周期
N=60/T; %总的采样次数
X=zeros(4,N); %目标真实位置、速度
X(:,1)=[-100,2,200,20]; %目标初始位置、速度
Z=zeros(1,N); %传感器对位置的观测
delta_w=1e-3; %如果增大这个参数,目标真实轨迹就是曲线
Q=delta_w*diag([0.5,1]); %过程噪声均值
G=[T^2/2,0;T,0;0,T^2/2;0,T]; %过程噪声驱动矩阵
R=5; %观测噪声方差
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1]; %状态转移矩阵
x0=200; %雷达站位置
y0=300;
Xstation=[x0,y0];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
w=sqrtm(R)*randn(1,N);
for t=2:N
X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1); %目标真实轨迹
end
for t=1:N
Z(t)=Dist(X(:,t),Xstation)+w(t); %对目标观测
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%UKF滤波,UT变换
L=4;
alpha=1;
kalpha=0;
belta=2;
% ramda=alpha^2*(L+kalpha)-L;
ramda=3-L;
for j=1:2*L+1
Wm(j)=1/(2*(L+ramda));
Wc(j)=1/(2*(L+ramda));
end
Wm(1)=ramda/(L+ramda);
Wc(1)=ramda/(L+ramda)+1-alpha^2+belta; %权值计算
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Xukf=zeros(4,N);
Xukf(:,1)=X(:,1); %无迹卡尔曼滤波初始状态
P0=eye(4); %协方差阵初始化
Xg=Xukf;
for t=2:N
xestimate= Xukf(:,t-1);
P=P0;
%第一步:获得一组Sigma点集
cho=(chol(P*(L+ramda)))';
for k=1:L
xgamaP1(:,k)=xestimate+cho(:,k);
xgamaP2(:,k)=xestimate-cho(:,k);
end
Xsigma=[xestimate,xgamaP1,xgamaP2]; %Sigma点集
%第二步:对Sigma点集进行一步预测
Xsigmapre=F*Xsigma;
%第三步:利用第二步的结果计算均值和协方差
Xpred=zeros(4,1); %均值
for k=1:2*L+1
Xpred=Xpred+Wm(k)*Xsigmapre(:,k);
end
Ppred=zeros(4,4); %协方差矩阵预测
for k=1:2*L+1
Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)';
end
Ppred=Ppred+G*Q*G';
%第四步:根据预测值,再一次使用UT变换,得到新的sigma点集
chor=(chol((L+ramda)*Ppred))';
for k=1:L
XaugsigmaP1(:,k)=Xpred+chor(:,k);
XaugsigmaP2(:,k)=Xpred-chor(:,k);
end
Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2];
%第五步:观测预测
for k=1:2*L+1 %观测预测
Zsigmapre(1,k)=hfun(Xaugsigma(:,k),Xstation);
end
%第六步:计算观测预测均值和协方差
Zpred=0; %观测预测的均值
for k=1:2*L+1
Zpred=Zpred+Wm(k)*Zsigmapre(1,k);
end
Pzz=0;
for k=1:2*L+1
Pzz=Pzz+Wc(k)*(Zsigmapre(1,k)-Zpred)*(Zsigmapre(1,k)-Zpred)';
end
Pzz=Pzz+R; %得到协方差Pzz
Pxz=zeros(4,1);
for k=1:2*L+1
Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(1,k)-Zpred)';
end
%第七步:计算卡尔曼增益
K=Pxz*inv(Pzz); %卡尔曼增益
Xg(:,t)=Xpred;
xestimate=Xpred+K*(Z(t)-Zpred); %状态更新
P=Ppred-K*Pzz*K'; %方差更新
P0=P;
Xukf(:,t)=xestimate;
end
%误差分析
for i=1:N
Err_kalmanFilter(i)=Dist(X(:,i),Xukf(:,i)); %滤波后的误差
end
%%%%%%%%%画图
figure(1)
hold on;box on;
plot(X(1,:),X(3,:),'-k.'); %真实轨迹
plot(Xukf(1,:),Xukf(3,:),'-r+'); %无迹卡尔曼滤波轨迹
xlabel('x坐标/m');
ylabel('y坐标/m');
grid on
% plot(Xg(1,:),Xg(3,:),'-bo');
legend('真实轨迹','UKF轨迹')
figure(2)
hold on;box on;
plot( Err_kalmanFilter,'-ks','MarkerFace','r');
xlabel('时间/s');
ylabel('位置估计误差/RMSE');
grid on
%%%%%%%%%%%%%%%%%%%%%%子函数:求两点间的距离
function d=Dist(X1,X2)
if length(X2)<=2
d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
else
d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
end
end
%%%%%%%%%%%%%观测子函数:观测距离
function[y]=hfun(x,xx)
y=sqrt((x(1)-xx(1))^2+(x(3)-xx(2))^2);
end
%%%%%%%%%%%%%%%%%%%%%%%%.
参考资料:
《卡尔曼滤波原理及应用》——黄小平,王岩
无迹卡尔曼滤波(UKF)在单观测站目标跟踪中的应用相关推荐
- 无迹卡尔曼滤波UKF—目标跟踪中的应用(算法部分)
无迹卡尔曼滤波UKF-目标跟踪中的应用(算法部分) 原创不易,路过的各位大佬请点个赞 机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313 仿真部分见博客: [无迹卡 ...
- 无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分)
无迹卡尔曼滤波UKF-目标跟踪中的应用(仿真部分) 原创不易,路过的各位大佬请点个赞 机动目标跟踪/非线性滤波/传感器融合/导航等探讨联系WX: ZB823618313 算法部分见博客: [无迹卡尔曼 ...
- 交互式多模型-无迹卡尔曼滤波IMM-UKF算法matlab实现(跟踪场景二)
交互式多模型-无迹卡尔曼滤波IMM-UKF算法matlab实现--机动目标跟踪 原创不易,路过的各位大佬请点个赞 针对机动目标跟踪的探讨.技术支持欢迎联系,也可以站内私信 WX: ZB82361831 ...
- 交互式多模型-扩展卡尔曼滤波IMM-EKF——机动目标跟踪中的应用
交互式多模型-扩展卡尔曼滤波IMM-EKF--机动目标跟踪中的应用 原创不易,路过的各位大佬请点个赞 针对机动目标跟踪的探讨.技术支持欢迎联系,也可以站内私信 WX: ZB823618313 机动目标 ...
- 容积卡尔曼滤波CKF—目标跟踪中的应用(算法部分—I)
容积卡尔曼滤波CKF-目标跟踪中的应用(算法部分) 原创不易,路过的各位大佬请点个赞 机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313 作者:823618313@ ...
- 容积卡尔曼滤波CKF—目标跟踪中的应用(仿真部分—II)
容积卡尔曼滤波CKF-目标跟踪中的应用(算法部分-II) 原创不易,路过的各位大佬请点个赞 机动目标跟踪/非线性滤波/传感器融合/导航等探讨联系WX: ZB823618313 作者:823618313 ...
- 【控制】《多无人机协同控制技术》周伟老师-第9章-单无人机目标跟踪飞行控制策略
第8章 回到目录 第10章 [控制]第9章-单无人机目标跟踪飞行控制策略 9.1 单无人机目标跟踪控制系统设计思路 9.2 无人机飞行姿态控制 9.2.1 无人机运动模型 9.2.2 非线性模型线性化 ...
- 对无迹卡尔曼滤波(UKF)和粒子滤波(PF)的理解
对无迹卡尔曼滤波(UKF)的总结: EKF通过在工作点附近进行一阶泰勒展开来线性化非线性的运动方程和观测方程.这样的线性近似在高度非线性的情况下存在较大误差.而UKF换了一个线性化的思路.1)选取2n ...
- 无迹卡尔曼滤波UKF和容积卡尔曼滤波CKF进行锂电池SOC估计的C语言版本实现,和matlab版本一样包含定参和FFRLS两种情况
(C语言版)无迹卡尔曼滤波UKF和容积卡尔曼滤波CKF进行锂电池SOC估计的C语言版本实现,和matlab版本一样包含定参和FFRLS两种情况,已在VS2019和Ubuntu 20.04.4版本中运行 ...
最新文章
- 模组使用之NB-IoT模组的工作模式、PSM、DRX和eDRX状态说明
- 在单缓冲区和双缓冲区结构下,读入并分析完该文件的时间分别是
- en55032最新标准下载_大型设备塔吊安装维保、安全检查及案例,94页PPT下载!
- 【IOI2018】狼人【Kruscal重构树】【主席树】
- 前端学习(2035)vue之电商管理系统电商系统之形成折线图
- 发一个招聘启事,成都有兴趣的朋友可以来看一看。
- weblogic创建多个域 及域的配置方法 war包部署及访问测试
- java jxl读取xlsx_Java添加、读取Excel公式
- JavaWeb——springMVC入门程序
- Sourcegraph 代码搜索
- easydarwin 安装_在Linux平台上搭建EasyDarwin,编译代码并简单部署
- css模拟title和alt的提示效果[转]
- hashset去重原理_快手:Druid精确去重的设计与实现
- 零信任时代企业如何提升权限的安全?从浏览器小程序说起
- Opencv surf算法
- PS学习笔记2-ps的基础操作
- Android 索引的实现
- 渥太华计算机读研的好学校,加拿大硕士留学:渥太华最好的大学硕士项目
- Android 滑动方向整理
- 电商运营竞品分析不会做?6个步骤教会你