UKF、CKF的目标跟踪仿真.rar

clear all; close all; monte = 100; % Monte-Carlo times tf = 100; dt = 1; Q = [dt^3/3 dt^2/2 0 0 0; dt^2/2 dt 0 0 0; 0 0 dt^3/3 dt^2/2 0; 0 0 dt^2/2 dt 0; 0 0 0 0 1.75/10^3*dt]; R = diag([1000;100/10^6]); xk = zeros(5,1); Pkk1 = zeros(5,5); xk0 = [1000;300;1000;0;-2]; Pkk1 = diag([100;10;100;10;100/10^6]); y = zeros(2,1); rms_ukf = zeros(5,101,monte); for i = 1:1:monte xk = xk0; xkk1 = sqrt(Pkk1)*randn(5,1)+xk; xkarr = xk; % UKF xkhatarr_ukf = xkk1; xkk1_ukf = xkk1; Pkk1_ukf = Pkk1; l = 1; for t = dt:dt:tf %% true state value %tic; F = [1 sin(xk(5))/xk(5) 0 (cos(xk(5))-1)/xk(5) 0; 0 cos(xk(5)) 0 -sin(xk(5)) 0; 0 (1-cos(xk(5)))/xk(5) 1 sin(xk(5))/xk(5) 0; 0 sin(xk(5)) 0 cos(xk(5)) 0; 0 0 0 0 1]; x = F*xk + sqrt(Q)*randn(5,1); xk = x; y(1,1) = sqrt(xk(1)^2+xk(3)^2); y(2,1) = atan2(xk(3),xk(1)); y = y+sqrt(R)*randn(2,1); %t0(i,l) = toc; %% nonlinear filters % UKF or CKF [ xkk1_ukf,Pkk1_ukf] = UKF( xkk1_ukf, Pkk1_ukf, y, Q, R); % [ xkk1_ukf,Pkk1_ukf] = CKF( xkk1_ukf, Pkk1_ukf, y, Q, R); xkarr = [xkarr xk]; xkhatarr_ukf = [xkhatarr_ukf xkk1_ukf]; l = l+1; end rms_ukf(:,:,i) = xkarr-xkhatarr_ukf; end mse_ukf = zeros(5,101); mse_ukf_pos = zeros(101,1); mse_ukf_vel = zeros(101,1); for i = 1:1:101 for j=1:1:monte mse_ukf(1,i) = mse_ukf(1,i)+(rms_ukf(1,i,j))^2; mse_ukf(2,i) = mse_ukf(2,i)+(rms_ukf(2,i,j))^2; mse_ukf(3,i) = mse_ukf(3,i)+(rms_ukf(3,i,j))^2; mse_ukf(4,i) = mse_ukf(4,i)+(rms_ukf(4,i,j))^2; mse_ukf(5,i) = mse_ukf(5,i)+(rms_ukf(5,i,j))^2; end mse_ukf_pos(i) = sqrt((mse_ukf(1,i)+mse_ukf(3,i))/monte); mse_ukf_vel(i) = sqrt((mse_ukf(2,i)+mse_ukf(4,i))/monte); mse_ukf(5,i) = sqrt(mse_ukf(5,i)/monte); end k=1:1:101; figure;plot(k,mse_ukf_pos(k)); figure;plot(k,mse_ukf_vel(k)); figure;plot(k,mse_ukf(5,k));