%?????????????????????????????????????????????????????????? % unscented_particle_filter.m
%?????????????????????????????????????????????????????????? % Unscented particle filter application
%
% A particle filter based on the unscented transform that
% estimates the target state X_k = [x_k y_k x_dot_k y_dot_k]
% based on observations of its range and range rate from
% two independent radar/sonar sensors.
%
% Copyright Sandeep Sira 2008
%
%?????????????????????????????????????????????????????????? clear
close all;
clc;
clear all;
% Initialization
X_0 = [0 0 10 150]'; % Initial target state
a = [0 2000]; % x?y coordinates of Sensor A
b = [3000 1500]; % x?y coordinates of Sensor B
dT = 2; % Sampling interval in seconds
endk = 50; % Number of sampling epochs
R = diag([10 4 10 4]); % Covariance matrix of sensor observation
% error
q = 0.5; % Process noise intensity parameter
Ns = 10; % Number of particles
P_0 = diag([1000 100 1000 100]);% Initial covariance
% UPF parameters
na = 4; kappa = 4; alpha = 1; beta = 0;
lambda = alpha^2*( na+kappa) -na;
% State transition matrix
F = [1 0 dT 0;
0 1 0 dT;
0 0 1 0;
0 0 0 1];
% Process noise covariance matrix
Q = q*[ dT^3/3 0 dT^2/2 0;
0 dT^3/3 0 dT^2/2;
dT^2/2 0 dT 0;
0 dT^2/2 0 dT];
sqrtm_Q = sqrtm(Q);
inv_Q = inv(Q);
inv_R = inv(R);
sqrtm_R = sqrtm(R);
% Generate a target trajectory
X(:,1) = X_0;
for k = 2: endk
X(:,k) = F*X(:,k-1) + sqrtm_Q * randn(4 ,1);
end
% Sample an initial value of the target state. This is the mean of the
% density P(X_0)
XHat (:,1) = X_0 + sqrtm(P_0)* randn(4 ,1);
% Sample the particles from the initial density
Xp = XHat*ones(1,Ns) + sqrtm(P_0)*randn(4,Ns);
% Mean and covariance of particles
mx = mean(Xp ,2)* ones(1,Ns); err_X = Xp -mx;
for j = 1:Ns
P_xx(:,:,j) = err_X(:,j) * err_X(:,j)';
end
% Commence unscented particle filtering loop
for k = 2: endk
% Project the particles forward
Xp_in = F* Xp;
% Get the observation
zeta = X(:,k);
da_true = norm( zeta ([1:2]) -a');
db_true = norm( zeta ([1:2]) -b');
va_true = zeta ([3:4])' * (zeta ([1:2]) -a')/ da_true;
vb_true = zeta ([3:4])' * (zeta ([1:2]) -b')/ db_true;
Zk = [ da_true; va_true; db_true; vb_true] + sqrtm_R * randn (4 ,1);
for j = 1:Ns
% Obtain the sigma points
P(:,:) = P_xx(:,:,j);
sqrt_P_xx = real(sqrtm( (na + lambda )*P ));
chi_X (:,:) = mx(:,j)* ones (1 ,2* na+1) + ...
[ zeros(4,1) sqrt_P_xx -sqrt_P_xx ];
w_chi = [ kappa ones(1,2* na)/2 ] / (na + kappa );
% Propagate the sigma points
chi_X = F* chi_X + sqrtm_Q * randn (4 ,2* na +1);
% Calculate mean and variance
mx(:,j) = chi_X * w_chi';
err_chi_X = chi_X -mx(:,j)* ones(1 ,2* na +1);
PHat = err_chi_X *(err_chi_X.*( ones(4 ,1)* w_chi ))';
% Range and range rate of the sigma points as
% observed by the sensors
chi_X_da = sqrt( sum((chi_X([1 ,2] ,:) -a'* ones(1 ,9)).^ 2 ) );
chi_X_db = sqrt( sum((chi_X([1 ,2] ,:) -b'* ones(1 ,9)).^ 2 ) );
chi_X_va = diag(chi_X([3:4] ,:)' ...
*( chi_X([1:2] ,:) -a'* ones(1 ,9)))' ./ chi_X_da;
chi_X_vb = diag(chi_X([3:4] ,:)' ...
*( chi_X([1:2] ,:) -b'* ones(1 ,9)))' ./ chi_X_db;
chi_Z = [chi_X_da; chi_X_va; chi_X_db; chi_X_vb ];
m_chi_Z = chi_Z * w_chi';
err_chi_Z = chi_Z -m_chi_Z * ones(1 ,9);
% Obtain P_zz , P_xz
P_zz = zeros(4 ,4); P_xz = zeros(4 ,4);
P_zz = err_chi_Z *(err_chi_Z.*( ones(4 ,1)* w_chi ))';
P_xz = err_chi_X *(err_chi_Z.*( ones(4 ,1)* w_chi ))';
% Innovations covariance and Kalman gain
P_eta = P_zz + R; K = P_xz * inv(P_eta );
% Update
mx(:,j) = mx(:,j) + K *(Zk -m_chi_Z );
PHat = PHat -K * P_eta * K';
% Store for the next time step
P_xx(:,:,j) = PHat;
% Sample a new particle
Xp(:,j) = mx(:,j) + real(sqrtm(PHat ))* randn(4 ,1);
% Importance density component of the weight
w3(j) = exp(-0.5 *( Xp(:,j) -mx(:,j) )' ...
* inv(PHat) *( Xp(:,j) -mx(:,j) ) );
end
% Calculate the weights
da = sqrt( sum((Xp([1:2] ,:) -a'* ones(1,Ns)).^2 ));
db = sqrt( sum((Xp([1:2] ,:) -b'* ones(1,Ns)).^2 ));
va = diag(Xp([3:4] ,:)' *( Xp([1:2] ,:) -a'* ones(1,Ns)))' ./ da;
vb = diag(Xp([3:4] ,:)' *( Xp([1:2] ,:) -b'* ones(1,Ns)))' ./ db;
innov = Zk * ones(1,Ns) -[ da; va; db; vb];
% Likelihood component
w1 = diag( exp(-0.5 * innov' * inv_R * innov) )';
% Kinematic prior component
w2 = diag( exp(-0.5 *( Xp -Xp_in )' * inv_Q * ...
( Xp -Xp_in ) ) )';
w = w1 .* w2 ./ w3;
if sum(w) == 0
fprintf('Alarm weights = 0 at time step %d\n', k);
w = ones(1,Ns)/Ns;
end
w = w / sum(w);
N_eff = 1 /(w * w');
% Calculate the estimate
XHat(:,k) = Xp *w';
% Plot the results so far
figure(1); clf;
plot(X(1,:), X(2,:),'b*-', XHat(1,:), XHat(2,:),'co-', ...
Xp(1,:), Xp(2,:),'r.');
hold on
%plot(a(1), a(2),'rs', b(1), b(2),'gs');
title('X-Y position'); xlabel('X(m)'); ylabel('Y(m)');
legend('Target trajectory','Target estimate', ...
'Particles','Location','SouthEast')
figure(2); clf;
subplot(2,1,1),
plot([0:endk-1]*dT,X(3,:),'b*-',[0:k-1]*dT,XHat(3,:), ...
'co-',(k-1)*dT*ones(1,Ns),Xp(3,:),'r.');
title('Velocity'); ylabel('X m/s');
subplot(2,1,2),
plot([0:endk-1]*dT , X(4,:),'b*-', [0:k-1]*dT , XHat(4,:), ...
'co-',(k-1)*dT * ones(1,Ns), Xp(4,:),'r.');
xlabel('Sampling instant'); ylabel('Y m/s');
% Resample
[Xp w index] = importance_resample(Xp , w);
% Housekeeping
mx = mx(:,index );
P_xx = P_xx(:,:, index );
end
fprintf('\n\n');
% Plot the MSE
figure
trMSE = diag(( XHat -X)' *(XHat -X));
semilogy([0:endk-1]*dT , trMSE,'b*-');
xlabel('Time'); ylabel('MSE');
title('Total Mean Square Tracking Error');
- 1
- 2
前往页