%% -------------------------------------------------------------------- %%
% Target tracking model
% Version: 20140618
% Last updated : 2014-6-18
% All rights reserved By Xin-Chun Zhang
% University of Electronic Science and Technology of China, Chengdu, China
% E-mail��irving_zhang@163.com
%
% ------------------------------------------------------------------------------- %
% References (you may cite one of the articles in your paper):
% [1] X. C. Zhang, C. J. Guo, "Cubature Kalman filters: Derivation and
% extension," Chinses Physics B, vol.22, no.12, 128401, DOI: 10.
% 1088/1674-1056/22/12/128401
% [2] X. C. Zhang, Y. L. Teng, "A new derivation of the cubature Kalman
% filters," Asian Journal of Control, DOI: 10.1002/asjc.926
% [3] X. C. Zhang, "Cubature information filters using high-degree and embedded
% cubature rules," Circuits, Systems, and Signal Processing,vol.33, no.6,
% pp.1799-1818,DOI: 10.1007/s00034-013-9730-0
%% -------------------------------------------------------------------- %%
clear all;
close all;
clc;
global Q R T kesi w n m kesi_5 b_5 w2_5 w_5 w4_5;
% "Perms.exe" can be used to generate the cubature point set in the form [1,
% 1] or [1, -1] for any model
system('Perms.exe');
h = waitbar(0, '1', 'Name', 'Please wait ...', 'WindowStyle', 'modal', ...
'CreateCancelBtn', 'setappdata(gcbf,''canceling'',1)');
setappdata(h, 'canceling', 0);
% some parameters
T = 1;
q1 = 0.1;
q2 = 1.75e-4;
sigma_r = 10;
sigma_th = sqrt(10) * 10 ^ (-3);
M = [T ^ 3 / 3 T ^ 2 / 2 ; T ^ 2 / 2 T];
Q = [q1 * M zeros(2 , 3) ; zeros(2 , 2) q1 * M zeros(2 , 1) ; ...
zeros(1 , 4) q2 * T];
R = [sigma_r ^ 2 0 ; 0 sigma_th ^ 2];
% dimension
n = 5;
m = 2 * n;
w = 1 / m;
% generate the cubature point set [1]
kesi1 = eye(n);
kesi2 = -eye(n);
kesi = [kesi1 , kesi2] * (sqrt(m / 2));
kesip = [kesi1 , kesi2] * (sqrt(3));
yitao = zeros(n , 1);
yita_temp1 = (load('second.txt'))';
yita_temp2 = (load('first.txt'))';
yitakp = [yita_temp1, yita_temp2, -yita_temp2] * (sqrt(3));
kesi_5 = [yitao , kesip , yitakp];
[a_5 , b_5] = size(kesi_5);
w_5 = (4 - n) / 18;
w2_5 = 1 - (7 - n) * n / 18;
w4_5 = 1 / 36;
% total time in seconds
num = 100;
xarray_sum = zeros(n , num + 1);
xhatarray_sum = zeros(n , num + 1);
error_sum = zeros(n , num + 1);
xhatarray_5_sum = zeros(n , num + 1);
error_5_sum = zeros(n , num + 1);
counter = 0;
counter_5 = 0;
accuracy = 0;
accuracy_5 = 0;
vel = 0;
vel_5 = 0;
ome = 0;
ome_5 = 0;
% Monte Carlo runs
len = 100;
for iter = 1 : len
if (getappdata(h,'canceling'))
iter = iter - 1;
break;
end
waitbar(iter / len, h, sprintf('%s%%', num2str(100 * iter / len)));
x = [1000 , 300 , 1000 , 0 , -3 * pi / 180]';
% Initial data, you may choose a random vector
xhat = x;
xhat_5 = x;
Pplus = diag([100 , 10 , 100 , 10 , 100e-6]);
Pplus_5 = Pplus;
xarray = [x];
zarray = [[sqrt(x(1) ^ 2 + x(3) ^ 2) atan(x(3) / x(1))]' ...
+ sqrtm(R) * randn(2 , 1)];
xhatarray = [x];
xhatarray_5 = [x];
for i = 1 : num
fai1 = [1 sin(x(5) * T) / x(5) 0 -(1 - cos(x(5) * T)) / x(5) 0;
0 cos(x(5)*T) 0 -sin(x(5) * T) 0;
0 (1 - cos(x(5) * T)) / x(5) 1 sin(x(5) * T) / x(5) 0;
0 sin(x(5) * T) 0 cos(x(5) * T) 0;
0 0 0 0 1];
x = fai1 * x + sqrtm(Q) * randn(5 , 1);
z = [sqrt(x(1) ^ 2 + x(3) ^ 2) atan(x(3) / x(1))]' ...
+ sqrtm(R) * randn(2 , 1);
xarray = [xarray x];
zarray = [zarray z];
[xhat , Pplus] = Three_degree_CKF(xhat , Pplus , z);
xhatarray = [xhatarray xhat];
[xhat_5 , Pplus_5] = Fifth_degree_CKF(xhat_5 , Pplus_5 , z);
xhatarray_5 = [xhatarray_5 xhat_5];
end
%% ---------------------------------------------------------------
xarray_sum = xarray_sum + xarray;
error = xarray - xhatarray;
accuracy = accuracy + (error(1 , :) .^ 2 + error(3 , :) .^ 2);
xhatarray_sum = xhatarray_sum + xhatarray;
error_sum = error_sum + error;
error_5 = xarray - xhatarray_5;
accuracy_5 = accuracy_5 + (error_5(1 , : ) .^ 2 + error_5(3 , :) .^ 2);
vel = vel + (error(2 , :) .^ 2 + error(4 , : ) .^ 2);
vel_5 = vel_5 + (error_5(2 , : ) .^ 2 + error_5(4 , : ) .^ 2);
ome = ome + error(5 , : ) .^ 2;
ome_5 = ome_5 + error_5(5 , :) .^ 2;
xhatarray_5_sum = xhatarray_5_sum + xhatarray_5;
error_5_sum = error_5_sum + error_5;
end
delete(h);
xarray_avg = xarray_sum / iter;
xhatarray_avg = xhatarray_sum / iter;
error_avg = error_sum / iter;
xhatarray_5_avg = xhatarray_5_sum / iter;
error_5_avg = error_5_sum / iter;
accuracy = sqrt(accuracy / iter);
accuracy_5 = sqrt(accuracy_5 / iter);
vel = sqrt(vel / iter);
vel_5 = sqrt(vel_5 / iter);
ome = sqrt(ome / iter);
ome_5 = sqrt(ome_5 / iter);
CKF_RMS = log(rms(error_avg , 2));
col = CKF_RMS;
CKF_RMS_5 = log(rms(error_5_avg , 2));
col_5 = CKF_RMS_5;
figure('Units' , 'Normalized' , 'Color' , [1 1 1] , 'Name' , 'RMSEs' , ...
'NumberTitle' , 'off' , 'Position' , [0.1 0.1 0.8 0.8] , 'Tag' , 'newplot') , hold on;
box on;
subplot(311),
plot(50 : num , accuracy(51 : num + 1) , 'k:' , 50 : num , accuracy_5(51 : num + 1) , 'k-');
set(gca , 'fontname' , 'Arial' , 'fontsize' , 12);
set(gcf , 'Color' , 'White');
legend('3rd-degree CKF' , '5th-degree CKF' , 'Orientation' , 'vertical' , 'Location' , 'NorthWest');
legend('boxoff');
xlabel('(a)' , 'fontname' , 'Arial' , 'fontsize' , 16);
ylabel('RMSE_p_o_s(m)' , 'fontname' , 'Arial' , 'fontsize' , 16 , 'fontangle' , 'normal');
axis tight;
subplot(312),
plot(50 : num , vel(51 : num + 1) , 'k:' , 50 : num , vel_5(51 : num + 1) , 'k-');
set(gca , 'fontname' , 'Arial' , 'fontsize' , 12);
set(gcf , 'Color' , 'White');
legend('3rd-degree CKF' , '5th-degree CKF' , 'Orientation' , 'vertical' , 'Location' , 'NorthWest');
legend('boxoff');
xlabel('(b)' , 'fontname' , 'Arial' , 'fontsize' , 16);
ylabel('RMSE_v_e_l(m/s)' , 'fontname' , 'Arial' , 'fontsize' , 16 , 'fontangle' , 'normal');
axis tight;
subplot(313),
plot(50 : num , ome(51 : num + 1) * 180 / pi , 'k:' , 50 : num , ome_5(51 : num + 1) * 180 / pi , 'k-');
set(gca , 'fontname' , 'Arial' , 'fontsize' , 12);
set(gcf , 'Color' , 'White');
legend('3rd-degree CKF' , '5th-degree CKF' , 'Orientation' , 'vertical' , 'Location' , 'NorthWest');
legend('boxoff');
xlabel({'(c)'; 'Time (sec)'} , 'fontname' , 'Arial' , 'fontsize' , 16 , 'fontangle' , 'normal');
ylabel('RMSE_o_m_g(Deg/s)' , 'fontname' , 'Arial' , 'fontsize' , 16 , 'fontangle' , 'normal');
axis tight;
if (iter < len)
warndlg({'The program has been terminated by your actions!'; ...
['Note that the total Monte Carlo runs are ', num2str(iter), ' !!!']},'Warning!', 'modal');
end
- 1
- 2
前往页