使用matlab将无迹卡尔曼应用于目标跟踪

将无迹卡尔曼应用于目标跟踪

% 无迹卡尔曼在目标跟踪中的应用
% 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

最终结果：