使用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: N
    X(:,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: N
    RMS(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

最终结果：