% We implement a particle filter to estimate the state of the system given % in exercise 1. clear all global L L = 0.15; load('measurement_data.mat'); load('input_data.mat'); load('True_trajectory.mat'); N_particles = 1200; X_particles = zeros(4, N_particles); % storage for the particles representing the state. P_particles = zeros(4,4, N_particles); weights = zeros(1, N_particles); X_est = zeros(4, length(Z)); %storage for saving the estimated state. P_k = zeros(4,4, length(Z)); P0 = eye(4,4)*0.05; X0 = [0; 1.5; 0; 1.5]; std_meas = 0.5; P_k(:,:,1) = P0; X_est(:,1) = X0; dt = 0.01; R = [std_meas*std_meas 0; 0 std_meas*std_meas]; H = [1, 0, 0, 0; 0, 1, 0, 0]; Q = eye(4,4)*0.0001; %Initial particles and weights for i=1:N_particles X_particles(:,i) = X0 ;%+ chol(Q)*randn(4,1); weights(i) = 1.0/N_particles; end for k=2:1:length(Z) %Propagate the particles for i=1:N_particles X_particles(:,i) = nominal_state_transition(X_particles(:,i), U(k), dt); X_particles(:,i) = X_particles(:,i) + chol(Q)*randn(4,1); end % The predicted particles can be used to compute the predicted mean and % covariance. % Compute the weights mean_Z = H*X_particles; for i=1:N_particles weights(i) = exp((-1/2)*(Z(:,k) - mean_Z(:,i))'*inv(R)*(Z(:,k) - mean_Z(:,i))); end weights = weights./sum(weights); % Do resampling ind = resampstr(weights); X_particles = X_particles(:, ind); % Compute estimated state X_est(:, k) = mean(X_particles, 2); end figure plot(Z(1,:), Z(2,:), 'k.', 'MarkerSize', 6) hold on plot(true_states(1,:), true_states(2,:),'b', X_est(1,:), X_est(2,:),'r-.', 'LineWidth', 2.5) legend("Measurement", "True","Estimated") axis equal hold off figure plot(1:length(true_states), true_states(4,:),'b', 1:length(X_est(4,:)), X_est(4,:),'r', 'LineWidth', 2.5) legend("True speed","Estimated speed") figure plot(1:length(true_states), true_states(3,:),'b', 1:length(X_est(3,:)), X_est(3,:),'r', 'LineWidth', 2.5) legend("True heading","Estimated heading") %% rmse = 0; for i=1:length(true_states) rmse = rmse + (true_states(:,i) - X_est(:,i)).*(true_states(:,i) - X_est(:,i)); end rmse = sqrt(rmse./length(Z)) %% function [state_next] = nominal_state_transition(state, u, dt) theta = state(3); v = state(4); global L state_next = [1, 0, 0, dt*v*cos(theta); 0, 1, 0, dt*v*sin(theta); 0, 0, 1, dt*tan(u)/L; 0, 0, 0, 1] * state; end