将无迹卡尔曼应用于目标跟踪
% 无迹卡尔曼在目标跟踪中的应用
% UKF_Dist_CV
clc
clear
%% 初始化参数
% 雷达扫描周期
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滤波参数初始化
% UKF滤波, UT变换
L = 4;
alpha = 1;
kalpha = 0;
belta = 2;
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);
for t = 2: N
xestimate = Xukf(:,t-1);
P = P0;
% 第一步:获得一组Sigma点集
% cho = (chol(P*(L+ramda)))';
cho = sqrtm(P*(L+ramda));
for k = 1: L
xgamaP1(:,k) = xestimate + cho(:,k);
xgamaP2(:,k) = xestimate - cho(:,k);
end
% Sigma点集
Xsigma = [xestimate, xgamaP1, xgamaP2];
% 第二步: 对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))';
chor = sqrtm((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;
Pxz = zeros(4,1);
for k = 1: 2*L+1
Pxz = Pxz + Wc(k) * (Xaugsigma(:,k) - Xpred) * (Zsigmapre(1,k) - Zpred)';
end
% 第七步: 计算Kalman增益
K = Pxz * inv(Pzz);
% 第八步: 状态和方差更新
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
hold on;
box on;
% 真实轨迹
plot(X(1,:), X(3,:), '-k.');
% 无迹卡尔曼滤波轨迹
plot(Xukf(1,:), Xukf(3,:), '-r+');
legend('真实轨迹', 'UKF轨迹');
figure
hold on;
box on;
plot(Err_KalmanFilter, '-ks', 'MarkerFace', 'r')
%% Dist子函数: 求两点间距离
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
%% hfun观测子函数: 观测距离
function [y] = hfun(x, xx)
y = sqrt((x(1) - xx(1))^2 + (x(3) - xx(2))^2);
end
最终结果: