unscented kalman filter for sensor fusion

18 vues (au cours des 30 derniers jours)
houssem alimi
houssem alimi le 11 Jan 2019
Commenté : Ilayat Ali le 20 Juin 2023
this code estimate signal using unscented kalman filter but I want to estimate two signals
  1 commentaire
Ilayat Ali
Ilayat Ali le 20 Juin 2023
This is the answer of your question to estimate two signals
clear all;
%% Before filter execution
% System properties
T = 0.1; % Sampling time
N = 600; % Number of time steps for filter
N1 = 20; % Station 1 North coordinate
E1 = 0; % Station 1 East coordinate
N2 = 0; % Station 2 North coordinate
E2 = 20; % Station 2 East coordinate
% Step 1: Define UT Scaling parameters and weight vectors
L = 8; % Size of state vector
alpha = 1; % Primary scaling parameter
beta = 2; % Secondary scaling parameter (Gaussian assumption)
kappa = 0; % Tertiary scaling parameter
lambda = alpha^2*(L+kappa) - L;
wm = ones(2*L + 1,1)*1/(2*(L+lambda));
wc = wm;
wm(1) = lambda/(lambda+L);
wc(1) = lambda/(lambda+L) + 1 - alpha^2 + beta;
% Step 2: Define noise assumptions
Q = diag([0 0 4 4 0 0 4 4]);
R = diag([1 1 1 1]);
% Step 3: Initialize state and covariance
x = zeros(8, N); % Initialize size of state estimate for all k
x(:,1) = [0; 0; 50; 50; 0; 0; 50; 50]; % Set initial state estimate
P0 = eye(8, 8); % Set initial error covariance
% Simulation Only: Calculate true state trajectory for comparison
% Also calculate measurement vector
w = sqrt(Q)*randn(8, N); % Generate random process noise (from assumed Q)
v = sqrt(R)*randn(4, N); % Generate random measurement noise (from assumed R)
xt = zeros(8, N); % Initialize size of true state for all k
xt(:,1) = [0; 0; 50; 50; 0; 0; 50; 50] + sqrt(P0)*randn(8, 1); % Set true initial state
yt = zeros(4, N); % Initialize size of output vector for all k
for k = 2:N
xt(:,k) = [1 0 T 0 0 0 0 0;
0 1 0 T 0 0 0 0;
0 0 1 0 0 0 0 0;
0 0 0 1 0 0 0 0;
1 0 T 0 1 0 T 0;
0 1 0 T 0 1 0 T;
0 0 1 0 0 0 1 0;
0 0 0 1 0 0 0 1]*xt(:,k-1) + w(:,k-1);
yt(:,k) = [sqrt((xt(1,k)-N1)^2 + (xt(2,k)-E1)^2);
sqrt((xt(5,k)-N1)^2 + (xt(6,k)-E1)^2);
sqrt((xt(1,k)-N2)^2 + (xt(2,k)-E2)^2);
sqrt((xt(5,k)-N2)^2 + (xt(6,k)-E2)^2)] + v(:,k);
end
%% Execute Unscented Kalman Filter
P = P0; % Set first value of P to the initial P0
for k = 2:N % Step 1: Generate the sigma-points
sP = chol(P, 'lower'); % Calculate square root of error covariance
% chi_p = "chi previous" = chi(k-1)
chi_p = [x(:,k-1), x(:,k-1)*ones(1,L)+sqrt(L+lambda)*sP, ...
x(:,k-1)*ones(1,L)-sqrt(L+lambda)*sP]; % Step 2: Prediction Transformation
% Propagate each sigma-point through prediction
% chi_m = "chi minus" = chi(k|k-1)
chi_m = [1 0 T 0 0 0 0 0;
0 1 0 T 0 0 0 0;
0 0 1 0 0 0 0 0;
0 0 0 1 0 0 0 0;
1 0 T 0 1 0 T 0;
0 1 0 T 0 1 0 T;
0 0 1 0 0 0 1 0;
0 0 0 1 0 0 0 1]*chi_p;
x_m = chi_m*wm; % Calculate mean of predicted state
% Calculate covariance of predicted state
P_m = Q;
for i = 1:2*L+1
P_m = P_m + wc(i)*(chi_m(:,i) - x_m)*(chi_m(:,i) - x_m)';
end
% Step 3: Observation Transformation
% Propagate each sigma-point through observation
psi_m = [sqrt((chi_m(1,:) - N1).^2 + (chi_m(2,:) - E1).^2);
sqrt((chi_m(5,:) - N1).^2 + (chi_m(6,:) - E1).^2);
sqrt((chi_m(1,:) - N2).^2 + (chi_m(2,:) - E2).^2);
sqrt((chi_m(5,:) - N2).^2 + (chi_m(6,:) - E2).^2)];
y_m = psi_m*wm; % Calculate mean of predicted output
% Calculate covariance of predicted output
% and cross-covariance between state and output
Pyy = R;
Pxy = zeros(L, 4);
for i = 1:2*L+1
Pyy = Pyy + wc(i)*(psi_m(:,i) - y_m)*(psi_m(:,i) - y_m)';
Pxy = Pxy + wc(i)*(chi_m(:,i) - x_m)*(psi_m(:,i) - y_m)';
end
% Step 4: Measurement Update
K = Pxy/Pyy; % Calculate Kalman gain
x(:,k) = x_m + K*(yt(:,k) - y_m); % Update state estimate
P = P_m - K*Pyy*K'; % Update covariance estimate
end
%% Display results
figure(1);
t = T*(1:N);
for i = 1:4
subplot(2,2,i);
plot(t, x(i,:), 'b-', t, xt(i,:), 'r--', 'LineWidth', 2);
xlabel('Time (s)');
ylabel(['x_',num2str(i)]);
grid on;
legend('UKF','True');
end

Connectez-vous pour commenter.

Réponses (1)

Elad Kivelevitch
Elad Kivelevitch le 31 Mai 2019
Houssem,
Your code seems to implement a single singal unscented Kalman filter (in fact, a cubature Kalman filter with the settings you use for alpha, beta, and kappa). You need to use either two such filters, one per each signal, or a tracker configured to use an unscented Kalman filter.
If you want to use an unscented Kalman filter from MathWorks, consider using unscentedKalmanFilter or trackingUKF. If you want to track multiple objects, consider using a tracker, such trackerGNN, trackerJPDA, or trackerTOMHT.
Elad

Tags

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by