使用matlab将UKF应用于六维匀加速直线运动目标跟踪系统

将UKF应用于六维匀加速直线运动目标跟踪系统

% UKF在六维匀加速直线运动目标跟踪系统中的应用
% ukf_for_track_6_div_system
%% 初始化参数设定
% 状态位数
n = 6;
% 采样时间
t = 0.5;
% 过程噪声协方差矩阵
Q = [1 0 0 0 0 0;0 1 0 0 0 0;0 0 0.01 0 0 0;0 0 0 0.01 0 0;0 0 0 0 0.0001 0;0 0 0 0 0 0.0001];
% 测量噪声协方差矩阵
R = [100 0;0 0.001^2];
% 状态方程
f = @(x)[x(1) + t*x(3) + 0.5*t^2*x(5);x(2) + t*x(4) + 0.5*t^2*x(6);x(3) + t*x(5);x(4) + t*x(6);x(5);x(6)];
% 观测方程
h = @(x)[sqrt(x(1)^2 + x(2)^2);atan(x(2)/x(1))];
s = [1000; 5000; 10; 50; 2; -4];
% 初始化状态
x0 = s + sqrtm(Q) * randn(n, 1);
% 初始化协方差
P0 = [100 0 0 0 0 0;0 100 0 0 0 0;0 0 1 0 0 0;0 0 0 1 0 0;0 0 0 0 0.1 0;0 0 0 0 0 0.1];
% 总时间
N = 50;
% UKF滤波状态初始化
Xukf = zeros(n, N);
% 真实状态
X = zeros(n, N);
% 测量值
Z = zeros(2, N);
for i = 1: NX(:,i) = f(s) + sqrtm(Q) * randn(6, 1);s = X(:, i);
end
% ux为中间变量
ux = x0;
for k = 1: N% 测量值Z(:, k) = h(X(:,k)) + sqrtm(R) * randn(2, 1);% 调用UKF滤波算法[Xukf(:,k), P0] = ukf(f, ux, P0, h, Z(:,k), Q, R);ux = Xukf(:,k);
end
% 跟踪误差分析
for k = 1: NRMS(k) = sqrt((X(1,k) - Xukf(1,k))^2 + (X(2,k) - Xukf(2,k))^2);
end
%% 画图，轨迹图
figure
t = 1:N;
hold on;
box on;
plot(X(1,t), X(2,t), 'k-');
plot(Z(1,t).*cos(Z(2,t)), Z(1,t).*sin(Z(2,t)), '-b.');
plot(Xukf(1,t), Xukf(2,t), '-r.');
legend('实际值', '测量值', 'ukf估计值');
xlabel('x方向位置/m');
ylabel('y方向位置/m');
% 误差分析图
figure
box on;
plot(RMS, '-ko', 'MarkerFace', 'r');
xlabel('t/s');
ylabel('偏差/m');
title('跟踪位置偏差');
%% UKF子函数
function [X, P] = ukf(ffun, X, P, hfun, Z, Q, R)
% 非线性系统中UKF算法
% 状态维数
L = numel(X);
% 观测维数
m = numel(Z);
alpha = 1e-2;
ki = 0;
beta = 2;
lambda = alpha^2 * (L + ki) - L;
c = L + lambda;
Wm = [lambda/c, 0.5/c+zeros(1,2*L)];
Wc = Wm;
Wc(1) = Wc(1) + (1 - alpha^2 + beta);
c = sqrt(c);
% 第一步： 获得一组Sigma点集
% Sigma点集，在状态X附近的点集，X是6*13矩阵，每列为1样本
Xsigmaset = sigmas(X, P, c);
% 第二、三、四步：对Sigma点集进行一步预测，得到均值X1means和方差P1和新sigma点集X1
% 对状态UT变换
[X1means, X1, P1, X2] = ut(ffun, Xsigmaset, Wm, Wc, L, Q);
% 第五、六步： 得到观测预测，Z1为X1集合的预测，Zpre为Z1的均值，Pzz为协方差
% 对观测UT变换
[Zpre, Z1, Pzz, Z2] = ut(hfun, X1, Wm, Wc, m, R);
% 协方差Pxz
Pxz = X2 * diag(Wc) * Z2';
% 第七步： 计算Kalman增益
K = Pxz * inv(Pzz);
% 第八步： 状态和方差更新
X = X1means + K * (Z - Zpre);
P = P1 - K * Pxz';
end%% UT变换子函数
% 输入：fun为函数句柄，Xsigma为样本集，Wm和Wc为权值，n为状态维数（n=6），COV为方差
% 输出：Xmeans为均值
function [Xmeans, Xsigma_pre, P, Xdiv] = ut(fun, Xsigma, Wm, Wc, n, COV)
% 得到Xsigma样本个数
LL = size(Xsigma, 2);
% 均值
Xmeans = zeros(n, 1);
Xsigma_pre = zeros(n, LL);
for k = 1: LL% 一步预测Xsigma_pre(:,k) = fun(Xsigma(:,k));Xmeans = Xmeans + Wm(k) * Xsigma_pre(:,k);
end
% 预测减去均值
Xdiv = Xsigma_pre - Xmeans(:, ones(1,LL));
% 协方差
P = Xdiv * diag(Wc) * Xdiv' + COV;
end%% 产生Sigma点集函数
function Xset = sigmas(X, P, c)
% Cholesky分解
A = c * chol(P)';
Y = X(:, ones(1, numel(X)));
Xset = [X Y+A Y-A];
end

最终结果：

使用matlab将UKF应用于六维匀加速直线运动目标跟踪系统相关推荐

UKF学习笔记之匀速直线运动目标跟踪
UKF匀速直线运动目标跟踪无迹卡尔曼滤波运行结果无迹卡尔曼滤波代码: function UKF clc; clear; T=1; % 采样周期 N=60/T; % 采样次数 X=zeros(4 ...
随机数字信号处理期末大报告——基于卡尔曼滤波的自由落体运动目标跟踪MATLAB实现
完整的实验报告下载随机数字信号处理期末大报告-基于卡尔曼滤波的自由落体运动目标跟踪.docx-机器学习文档类资源-CSDN下载程序包及所需数据下载 target tracking us ...
智慧交通day02-车流量检测实现05：小车匀加速案例
""" 现在利用卡尔曼滤波对小车的运动状态进行预测.主要流程如下所示:导入相应的工具包小车运动数据生成参数初始化利用卡尔曼滤波进行小车状态预测可视化:观察参数的变化与结果 ...
基于matlab单目视觉焊缝跟踪系统,基于激光线结构光3D视觉的机器人轨迹跟踪方法与应用...
摘要: 本文利用工控机.图像采集卡.工业相机和激光器,设计了激光线结构光3D视觉系统;利用工控机.运动控制卡.伺服驱动器和六轴串联机器人组成机器人运动系统;两者共同构成基于激光线结构光的机器人轨迹跟踪 ...
matlab对摄像头实时运动目标检测,基于Matlab实时运动目标跟踪检测系统
2012.03 COMPUTER SECURITY 17 学术.技术基于 Matlab 实时运动目标跟踪检测系统汪惠兰,林航飞,李赔龙 (同济大学交通运输与工程学院,上海 201804) 摘要 ...
matlab目标跟踪物流应用,目标跟踪系统的MATLAB 源程序包
[实例简介] 目标跟踪系统的MATLAB 源程序包目标跟踪系统的MATLAB 源程序包 [实例截图] [核心代码] 目标跟踪系统的MATLAB源程序包 ├── auc.m ├── auction_2D ...
hough变换检测直线 matlab,Matlab实现Hough变换检测图像中的直线
Hough变换的原理: 将图像从图像空间变换至参数空间,变换公式如下: 变换以后,图像空间与参数空间存在以下关系: 图像空间中的一点在参数空间是一条曲线,而图像空间共线的各点对应于参数空间交于一点的各 ...
基于MATLAB的视频运动目标跟踪与检测定位系统
一.课题背景视频运动目标检测与跟踪算法是计算机视觉领域的一个核心课题,也是智能视频监控系统的关键底层技术.它融合了图像处理.人工智能等领域的研究成果,已经广泛应用于安保监控.智能武器.视频会议.视频 ...
matlab为什么要升维数,基于MATLAB和升维投影法的手写字符识别输入系统
256 现代交际·2019年6期作者简介:王思达,长春师范大学学生,研究方向:计算机科学与技术:刘勇,长春师范大学学生,研究方向:计算机科学与技术:袁汇灵, 长春师范大学学生,研究方向:计算机科学与 ...

使用matlab将UKF应用于六维匀加速直线运动目标跟踪系统

使用matlab将UKF应用于六维匀加速直线运动目标跟踪系统相关推荐

最新文章

热门文章