UKF学习笔记之匀速直线运动目标跟踪
UKF匀速直线运动目标跟踪
- 无迹卡尔曼滤波
- 运行结果
无迹卡尔曼滤波
代码:
function UKF
clc;
clear;
T=1; % 采样周期
N=60/T; % 采样次数
X=zeros(4,N); % 初始化真实轨迹矩阵
X(:,1)=[100 2 200 20]; % 目标初始位置、速度
Z=zeros(1,N); % 初始化 观测距离矩阵
w =1e-5;
Q = 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];% 状态转移矩阵
Xstation=[200,300]; % 观测位置
V = sqrt(R)*randn(1,N); % v均值为0,协方差为R的高斯白噪声 %sqrt(A)矩阵每个元素分别开方,与矩阵点乘有关;sqrtm(A)矩阵为整体,与矩阵相乘有关
W = sqrt(Q)*randn(2,1); % W均值为0,协方差为Q的高斯白噪声
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N X(:,t)=F*X(:,t-1)+G*W; % 目标的真实轨迹
end
for t=1:NZ(t)=Distance(X(:,t),Xstation)+V(t); % 真实观测目标位置
end
%%%%%%%%%%%%%%%%%%%%%%%%%% UKF滤波 %%%%%%%%%%%%%%%%%%
L=4; % L为状态维度
a=1; % a控制采样点的分布状态
k=1; % k为待选参数 没有界限,但是要保证(n+lamda)*P为半正定矩阵
b=2; % b非负的权系数
lambda=a*a*(L+k)-L; % lambda为缩放比列参数,用于降低总的预测误差
%%%%%%%%%%%%%%%%%%%%%% sigma点的权值 %%%%%%%%%%%%%%%%%
Wm=zeros(1,2*L+1);
Wc=zeros(1,2*L+1);
Wm(1)=lambda/(L+lambda);
Wc(1)=lambda/(L+lambda)+1-a*a+b; % 第一个采样点的均值和协方差的权值
for j=2:2*L+1 % 剩下2n个sigma点的均值和协方差的权值Wm(j)= lambda/(2*(L+lambda)); % 下标m为均值的权值Wc(j)= lambda/(2*(L+lambda)); % 下标c为协方差的权值
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Xukf = zeros(4,N);
Xukf(:,1)=X(:,1); %无迹Kalman滤波状态初始化
P0=eye(4); %协方差阵初始化
for t=2:Nxestimate = Xukf(:,t-1);P=P0;
%%%%%%% 第一步:获得一组采样点,Sigma点集 %%%%%%%%%%%%%%cho=(chol(P*(L+lambda)))'; % cho=chol(X)用于对矩阵X进行Cholesky分解,产生一个上三角阵cho,使cho'cho=X。若X为非对称正定,则输出一个出错信息。for k=1:LxgamaP1(:,k)=xestimate+cho(:,k);xgamaP2(:,k)=xestimate-cho(:,k);endXsigma=[xestimate xgamaP1 xgamaP2]; % 获得 2L+1 个Sigma点
%第二步:对Sigme点集进行一步预测Xsigmapre=F*Xsigma;
%第三步:利用第二部结果计算均值和协方差Xpred=zeros(4,1);for k=1:2*L+1Xpred=Xpred+Wm(k)*Xsigmapre(:,k);endPpred=zeros(4,4); %协力差阵预测for k=1:2*L+1Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)';endPpred=Ppred+G*Q*G';
%第四步:根据预测值,再次使用UT变换,得到新的sigma点集chor=(chol((L+lambda)*Ppred))';for k=1:LXaugsigmaP1(:,k)=Xpred+chor(:,k);XaugsigmaP2(:,k)=Xpred-chor(:,k);endXaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2];
%第五步:观测预测for k=1:2*L+1 %观测预测Zsigmapre(1,k)=h(Xaugsigma(:,k),Xstation);end
%第六步:计算观测预测均值和协方差Zpred=0; %观测预测的均值for k=1:2*L+1Zpred=Zpred+ Wm(k)*Zsigmapre(1,k);endPzz=0;for k=1:2*L+1Pzz=Pzz+Wc(k)*(Zsigmapre(1,k)-Zpred)*(Zsigmapre(1,k)-Zpred)';endPzz=Pzz+R; %得到协方差PzzPxz= zeros(4,1);for k=1:2*L+1Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(1,k)-Zpred)';end
%第七步:计算Kalman增益K=Pxz*inv(Pzz); % Kalman
%第八步:状态和方差更新Xukf(:,t)=Xpred+K*(Z(t)-Zpred);% 状态更新P=Ppred-K*Pzz*K'; %方差更新P0=P;
end
%6误差分析
Err_KalmanFilter=zeros(2,N);
for i=1:NErr_KalmanFilter(1,i)=X(1,i)-Xukf(1,i); %滤波后的误差Err_KalmanFilter(2,i)=X(3,i)-Xukf(3,i);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
hold on;box on;
plot(X(1,:),X(3,:),'-B.'); %真实轨迹
plot(Xukf(1,:),Xukf(3,:),'-rs'); %无迹Kalman滤波轨迹
legend('真实轨迹','UKF轨迹')
xlabel('横向坐标/m')
ylabel('纵向坐标');
figure
hold on;
box on;
plot(Err_KalmanFilter(1,:),'-kd','MarkerFace','r');
xlabel('时间/s');
ylabel('横向坐标误差/m');
figure
plot(Err_KalmanFilter(2,:),'-bd','MarkerFace','g');
xlabel('时间/s');
ylabel('纵向坐标误差/m');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%子函数:求两点间的距离
function d=Distance(X1,X2)d=sqrt((X1(1)- X2(1))^2 + (X1(3)-X2(2))^2);
end
%观测子函数:观测距离
function [y]=h(x,xx)
y=sqrt((x(1)-xx(1))^2+(x(3)-xx(2))^2);
end
end
运行结果
参考:参考书籍:卡尔曼滤波原理及应用
UKF学习笔记之匀速直线运动目标跟踪相关推荐
- 使用matlab将UKF应用于六维匀加速直线运动目标跟踪系统
将UKF应用于六维匀加速直线运动目标跟踪系统 % UKF在六维匀加速直线运动目标跟踪系统中的应用 % ukf_for_track_6_div_system %% 初始化参数设定 % 状态位数 n = ...
- hough变换直线检测_CV学习笔记(十五):直线检测
在这一篇文章中我们将学习使用OpenCV中的 HoughLines 函数和 HoughLinesP 函数来检测图像中的直线. 在这个函数中,使用的是霍夫变换(Hough Transform) 这是计算 ...
- Sharepoint学习笔记---Debug--使用ULS Log跟踪Solution错误信息
在开发Sharepoint Solution时,我们可以使用Attach to process来Debug我们的方案,然而一旦我们把Solution部署到了生产机上,我们就难以再使用这个最直接的方法了 ...
- Opencv学习笔记(十一)目标跟踪
原创文章,转载请注明:http://blog.csdn.net/crzy_sparrow/article/details/7414851 如果摄像机是固定的,那么我们可以认为场景(背景)大多数情况下是 ...
- OpenCV学习笔记(三十六)——Kalman滤波做运动目标跟踪 OpenCV学习笔记(三十七)——实用函数、系统函数、宏core OpenCV学习笔记(三十八)——显示当前FPS OpenC
OpenCV学习笔记(三十六)--Kalman滤波做运动目标跟踪 kalman滤波大家都很熟悉,其基本思想就是先不考虑输入信号和观测噪声的影响,得到状态变量和输出信号的估计值,再用输出信号的估计误差加 ...
- OpenCV学习笔记(二十六)——小试SVM算法ml OpenCV学习笔记(二十七)——基于级联分类器的目标检测objdect OpenCV学习笔记(二十八)——光流法对运动目标跟踪Video Ope
OpenCV学习笔记(二十六)--小试SVM算法ml 总感觉自己停留在码农的初级阶段,要想更上一层,就得静下心来,好好研究一下算法的东西.OpenCV作为一个计算机视觉的开源库,肯定不会只停留在数字图 ...
- OpenCV 3 Tracking API目标跟踪学习笔记——定义、物体跟踪常用算法、demo
今天开始接触目标跟踪 本文翻译自https://www.learnopencv.com/object-tracking-using-opencv-cpp-python/#opencv-tracking ...
- OpenCV学习笔记(二十一)——绘图函数core OpenCV学习笔记(二十二)——粒子滤波跟踪方法 OpenCV学习笔记(二十三)——OpenCV的GUI之凤凰涅槃Qt OpenCV学习笔记(二十
OpenCV学习笔记(二十一)--绘图函数core 在图像中,我们经常想要在图像中做一些标识记号,这就需要绘图函数.OpenCV虽然没有太优秀的GUI,但在绘图方面还是做得很完整的.这里就介绍一下相关 ...
- OpenCV学习笔记(十六)——CamShift研究 OpenCV学习笔记(十七)——运动分析和物体跟踪Video OpenCV学习笔记(十八)——图像的各种变换(cvtColor*+)imgproc
OpenCV学习笔记(十六)--CamShift研究 CamShitf算法,即Continuously Apative Mean-Shift算法,基本思想就是对视频图像的多帧进行MeanShift运算 ...
最新文章
- python曲线拟合笔记
- 使用BIOS以及直接写显存绘制图形
- 静态方法什么时候执行?_面试官:知道类在什么时候加载吗?
- 中原证券携手神策数据,筑就线上线下融合的数字化运营体系
- mysql url格式,关于MySql链接url参数的设置
- node.js 代码修改 自动识别重启工具
- OJ1007: 鸡兔同笼
- springsecurity 认证之授权码模式
- 利用curl去hack他人博客
- 2020杭电多校赛第四场1007 Go Running(hdu6808)
- excel 比对字符
- python实现泊松分布_Python实现概率分布(二项分布、伯努利分布、泊松分布、几何分布、正态分布等)...
- 二叉树的python实现
- 可重入锁 ReentrantLock
- 事情永远做不完,却什么都带不走
- 【SDN】普通路由器刷OpenWrt+OpenFlow教程完美版_搭建SDN OpenFlow1.3协议的路由器(Flash<16M)
- 小米随身wif linux平台配置
- IR-CUT选择须知
- 纯JS实现slideToggle动画,慢慢下拉打开
- v模拟器(华为、H3C)点滴