diff --git a/Kalman_Filter_Tutorial/Kalman_filt_for_Ninjam.m b/Kalman_Filter_Tutorial/Kalman_filt_for_Ninjam.m new file mode 100644 index 0000000000000000000000000000000000000000..2e163346a387ed3b3800d68cfe37e96ce2fda5aa --- /dev/null +++ b/Kalman_Filter_Tutorial/Kalman_filt_for_Ninjam.m @@ -0,0 +1,140 @@ +% written by StudentDave +%for licensing and usage questions +%email scienceguy5000 at gmail. com + +%Bayesian Ninja tracking Quail using kalman filter + +clear all +%% define our meta-variables (i.e. how long and often we will sample) +duration = 10; %how long the Quail flies +dt = .1; %The Ninja continuously looks for the birdy, +%but we'll assume he's just repeatedly sampling over time at a fixed interval + +%% Define update equations (Coefficent matrices): A physics based model for where we expect the Quail to be [state transition (state + velocity)] + [input control (acceleration)] +A = [1 dt; 0 1] ; % state transition matrix: expected flight of the Quail (state prediction) +B = [dt^2/2; dt]; %input control matrix: expected effect of the input accceleration on the state. +C = [1 0]; % measurement matrix: the expected measurement given the predicted state (likelihood) +%since we are only measuring position (too hard for the ninja to calculate speed), we set the velocity variable to +%zero. + +%% define main variables +u = 1.5; % define acceleration magnitude +Q= [0; 0]; %initized state--it has two components: [position; velocity] of the Quail +Q_estimate = Q; %x_estimate of initial location estimation of where the Quail is (what we are updating) +QuailAccel_noise_mag = 0.05; %process noise: the variability in how fast the Quail is speeding up (stdv of acceleration: meters/sec^2) +NinjaVision_noise_mag = 10; %measurement noise: How mask-blinded is the Ninja (stdv of location, in meters) +Ez = NinjaVision_noise_mag^2;% Ez convert the measurement noise (stdv) into covariance matrix +Ex = QuailAccel_noise_mag^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % Ex convert the process noise (stdv) into covariance matrix +P = Ex; % estimate of initial Quail position variance (covariance matrix) + +%% initize result variables +% Initialize for speed +Q_loc = []; % ACTUAL Quail flight path +vel = []; % ACTUAL Quail velocity +Q_loc_meas = []; % Quail path that the Ninja sees + + +%% simulate what the Ninja sees over time +figure(2);clf +figure(1);clf +for t = 0 : dt: duration + + % Generate the Quail flight + QuailAccel_noise = QuailAccel_noise_mag * [(dt^2/2)*randn; dt*randn]; + Q= A * Q+ B * u + QuailAccel_noise; + % Generate what the Ninja sees + NinjaVision_noise = NinjaVision_noise_mag * randn; + y = C * Q+ NinjaVision_noise; + Q_loc = [Q_loc; Q(1)]; + Q_loc_meas = [Q_loc_meas; y]; + vel = [vel; Q(2)]; + %iteratively plot what the ninja sees + plot(0:dt:t, Q_loc, '-r.') + plot(0:dt:t, Q_loc_meas, '-k.') + axis([0 10 -30 80]) + hold on + pause +end + +%plot theoretical path of ninja that doesn't use kalman +plot(0:dt:t, smooth(Q_loc_meas), '-g.') + +%plot velocity, just to show that it's constantly increasing, due to +%constant acceleration +%figure(2); +%plot(0:dt:t, vel, '-b.') + + +%% Do kalman filtering +%initize estimation variables +Q_loc_estimate = []; % Quail position estimate +vel_estimate = []; % Quail velocity estimate +Q= [0; 0]; % re-initized state +P_estimate = P; +P_mag_estimate = []; +predic_state = []; +predic_var = []; +for t = 1:length(Q_loc) + % Predict next state of the quail with the last state and predicted motion. + Q_estimate = A * Q_estimate + B * u; + predic_state = [predic_state; Q_estimate(1)] ; + %predict next covariance + P = A * P * A' + Ex; + predic_var = [predic_var; P] ; + % predicted Ninja measurement covariance + % Kalman Gain + K = P*C'*inv(C*P*C'+Ez); + % Update the state estimate. + Q_estimate = Q_estimate + K * (Q_loc_meas(t) - C * Q_estimate); + % update covariance estimation. + P = (eye(2)-K*C)*P; + %Store for plotting + Q_loc_estimate = [Q_loc_estimate; Q_estimate(1)]; + vel_estimate = [vel_estimate; Q_estimate(2)]; + P_mag_estimate = [P_mag_estimate; P(1)] +end + +% Plot the results +figure(2); +tt = 0 : dt : duration; +plot(tt,Q_loc,'-r.',tt,Q_loc_meas,'-k.', tt,Q_loc_estimate,'-g.'); +axis([0 10 -30 80]) + + +%plot the evolution of the distributions +figure(3);clf +for T = 1:length(Q_loc_estimate) +clf + x = Q_loc_estimate(T)-5:.01:Q_loc_estimate(T)+5; % range on x axis + + %predicted next position of the quail + hold on + mu = predic_state(T); % mean + sigma = predic_var(T); % standard deviation + y = normpdf(x,mu,sigma); % pdf + y = y/(max(y)); + hl = line(x,y,'Color','m'); % or use hold on and normal plot + + %data measured by the ninja + mu = Q_loc_meas(T); % mean + sigma = NinjaVision_noise_mag; % standard deviation + y = normpdf(x,mu,sigma); % pdf + y = y/(max(y)); + hl = line(x,y,'Color','k'); % or use hold on and normal plot + + %combined position estimate + mu = Q_loc_estimate(T); % mean + sigma = P_mag_estimate(T); % standard deviation + y = normpdf(x,mu,sigma); % pdf + y = y/(max(y)); + hl = line(x,y, 'Color','g'); % or use hold on and normal plot + axis([Q_loc_estimate(T)-5 Q_loc_estimate(T)+5 0 1]); + + + %actual position of the quail + plot(Q_loc(T)); + ylim=get(gca,'ylim'); + line([Q_loc(T);Q_loc(T)],ylim.','linewidth',2,'color','b'); + legend('state predicted','measurement','state estimate','actual Quail position') + pause +end \ No newline at end of file diff --git a/MSc_thesis_for_LUT/MSc_thesis_Alijani_Farid.docx b/MSc_thesis_for_LUT/MSc_thesis_Alijani_Farid.docx index 9cef5125c7068d440fa89e70388382b109a8dce4..bc405cf41206a08d6c9d422ec1bc8ea45e7e3435 100644 Binary files a/MSc_thesis_for_LUT/MSc_thesis_Alijani_Farid.docx and b/MSc_thesis_for_LUT/MSc_thesis_Alijani_Farid.docx differ diff --git a/MSc_thesis_for_LUT/Presentation_Alijani_Farid.pptx b/MSc_thesis_for_LUT/Presentation_Alijani_Farid.pptx new file mode 100644 index 0000000000000000000000000000000000000000..77f55a427ad571b26071c56f3d9c2441ed1329ae Binary files /dev/null and b/MSc_thesis_for_LUT/Presentation_Alijani_Farid.pptx differ diff --git a/MSc_thesis_for_LUT/Press_release_Alijani_Farid.docx b/MSc_thesis_for_LUT/Press_release_Alijani_Farid.docx new file mode 100644 index 0000000000000000000000000000000000000000..b9c1bbd4d33fede695872d4137fcafeff01f48c0 Binary files /dev/null and b/MSc_thesis_for_LUT/Press_release_Alijani_Farid.docx differ diff --git a/MSc_thesis_for_LUT/Seminar_Enrolment.pdf b/MSc_thesis_for_LUT/Seminar_Enrolment.pdf new file mode 100644 index 0000000000000000000000000000000000000000..46e3c71a28dbd7d6301a185bda9cf901bfffb9d3 Binary files /dev/null and b/MSc_thesis_for_LUT/Seminar_Enrolment.pdf differ diff --git a/MSc_thesis_for_LUT/enrolling form new.pdf b/MSc_thesis_for_LUT/enrolling form new.pdf new file mode 100644 index 0000000000000000000000000000000000000000..966bb716df05cadbb25dcd50010ee7d1eba2aaa5 Binary files /dev/null and b/MSc_thesis_for_LUT/enrolling form new.pdf differ