将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%% 画图，轨迹图figuret = 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');% 误差分析图figurebox 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);% 协方差PxzPxz = 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
最终结果：
