Kalman-Filter.rar_made

clc close all clear all % Set true trajectory Nsamples=60; a=2; % acceleration of ship v0=6; %initial velocity of ship dt = 0.1; % sampling time t=0:dt:dt*Nsamples; Vtrue = v0 + a*t; %m/s (true velocity of ship) Xinitial = 1000; % initial position of ship Xtrue = Xinitial -(v0*t + (a/2) * t.^2);% Xtrue is a vector of true positions of the ship %%% Motion equations % Previous state (initial guess): Our guess is that the ship starts at 1000 m with velocity % that equals to 70% of the real velocity Xk_prev = [1000; 0.7*v0; a]; % Current state estimate Xk=[]; % Motion equation: Xk = Phi*Xk_prev + Noise, that is Xk(n) = Xk(n-1) + Vk(n-1) * dt % here, velocity and acceleration is not measured, but it is estimated % Phi represents the dynamics of the system: it is the motion equation Phi = [1 -dt -dt*dt/2; 0 1 dt; 0 0 1]; % The error matrix (or the confidence matrix): P states whether we should % give more weight to the new measurement or to the model estimate sigma_model = 1; % P = sigma^2*G*G'; P = [sigma_model^2 0 0; 0 sigma_model^2 0; 0 0 sigma_model^2]; % Q is the process noise covariance Q = [0 0 0; 0 0 0; 0 0 0]; % M is the measurement matrix. % We measure X, so M(1) = 1 % We do not measure V and a, so M(2)= M(3)= 0 M = [1 0 0]; % R is the measurement noise covariance. Generally R and sigma_meas can % vary between samples. sigma_meas = 3; % 1 m/sec R = sigma_meas^2; %%% Kalman iteration % Buffers for later display Xk_buffer = zeros(3,Nsamples+1); Xk_buffer(:,1) = Xk_prev; Z_buffer = zeros(1,Nsamples+1); Z_buffer(1,1)=1000; for k=1:Nsamples % Z is the measurement vector. In our % case, Z = TrueData + RandomGaussianNoise Z = Xtrue(k+1)+sigma_meas*randn; Z_buffer(k+1) = Z; % Kalman iteration P1 = Phi*P*Phi' + Q; S = M*P1*M' + R; % K is Kalman gain. If K is large, more weight goes to the measurement. % If K is low, more weight goes to the model prediction. K = P1*M'*inv(S) P = P1 - K*M*P1 Xk = Phi*Xk_prev + K*(Z-M*Phi*Xk_prev); Xk_buffer(:,k+1) = Xk; % For the next iteration Xk_prev = Xk; end; %%% Plot resulting graphs figure; plot(t,Xtrue-Z_buffer,'g','Linewidth',2); hold on; % plot(t,Z_buffer,'c'); plot(t,Xtrue-Xk_buffer(1,:),'--m','Linewidth',2); title('Position estimation results'); xlabel('Time (s)'); ylabel('Error (m)'); % legend('True position','Measurement','Kalman estimated'); legend('Possition Error in case of Without Filtering','Error in case of Kalman Estimation'); grid on; figure; plot(t,Xtrue,'--g','Linewidth',2); hold on; plot(t,Z_buffer,'c','Linewidth',2); plot(t,Xk_buffer(1,:),'.m','Linewidth',2); title('Position estimation results'); xlabel('Time (s)'); ylabel('Position (m)'); legend('True position','Without Filtering','Kalman estimated'); % legend('Error of Measurement','Error of estimation'); grid on; %%% Velocity analysis % The instantaneous velocity as derived from 2 consecutive position % measurements InstantaneousVelocity = [-9 (Z_buffer(2:Nsamples+1)-Z_buffer(1:Nsamples))/dt]; figure; plot(t,Vtrue,'m','Linewidth',2); hold on; plot(t,-InstantaneousVelocity,'--g','Linewidth',2); plot(t,Xk_buffer(2,:),'.k','Linewidth',2); title('Velocity estimation results'); xlabel('Time (s)'); ylabel('Velocity (m/s)'); legend('True velocity','Estimated velocity by raw consecutive samples','Estimated velocity by Kalman filter'); %%% Position analysis % % figure; % plot(t,Xtrue,'g'); % hold on; % plot(t,Z_buffer,'c'); % % plot(t,Xk_buffer(1,:),'m'); % title('Position estimation results'); % xlabel('Time (s)'); % ylabel('Position (m)'); % legend('True position','Measurement','Kalman estimated'); % % % %%% Velocity analysis % % % % % The instantaneous velocity as derived from 2 consecutive position % % % measurements % InstantaneousVelocity = [-9 (Z_buffer(2:Nsamples+1)-Z_buffer(1:Nsamples))/dt]; % % figure; % plot(t,Vtrue,'m'); % hold on; % plot(t,-InstantaneousVelocity,'g'); % plot(t,Xk_buffer(2,:),'k'); % title('Velocity estimation results'); % xlabel('Time (s)'); % ylabel('Velocity (m/s)'); % legend('True velocity','Estimated velocity by raw consecutive samples','Estimated velocity by Kalman filter');