UKF学习笔记_匀速直线运动目标跟踪

原创

已于 2025-06-19 17:45:30 修改 · 1.1k 阅读

14 ·

CC 4.0 BY-SA版权

文章标签：

#matlab

于 2021-03-30 19:36:31 首次发布

该博客是关于无迹卡尔曼滤波（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:N
    Z(t)=Distance(X(:,t),Xstation)+V(t);  % 真实观测目标位置
end
%%<