Tutorial: Kalman Filter with MATLAB example part3 

Check his website for the codes. It's written in the description people. %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

I want to tell you that you are doing a great job for a lot of people. I really like the way you explained Kalman and particle filters. A lot of complicated math equations are well undertood after your explanations. I am looking forward to the next show. Thanks Dave!

This is by far the best way to explain the Kalman filter. Is there any tutorials on Extended kalman filter (EKF)?

The elements in the matrix are: sigma(p)*sigma(p) sigma(p)*sigma(v) sigma(v)sigma(p) sigma(v)*sigma(v) Since p is proportional to dt^2/2*a, then sigma(p)=dt^2/2*sigma(a); Since v is proportional to a*dt, then sigma(v)=dt*sigma(a); From these eqs. you find that the matrix gets the form: dt^4/4 dt^3/2 dt^3/2 dt^2 Hope it helps!

Kalman filter explained with ninjas... This is great thank you!

This is fantastic! The best explanation I have seen. I'm beginning to understand how, and to some extent, why Kalman filters work!

Hi Dave. Thank you very much for your explanations. It is much easier to understand. It saved me a lot of time and frustration. I want to point out some correction about the code. Since the same accelation noise propagates to both postion and velocity, I suggest that the code below should be modified this way. QuailAccel_noise = QuailAccel_noise_mag * [(dt^2/2)*randn; dt*randn]; => randn_sample= randn; QuailAccel_noise = QuailAccel_noise_mag * [(dt^2/2)*randn_sample; dt*randn_sample];

Wow, I think this is the most understandable tutorial about Kalman Filter. The others have many math formulas which are very confusing. Thanks a lot.

I can see that the input control matrix B is [dt^2/2; dt]. But I can also see that a covariance matrix (relating to process noise) was created from the input control matrix, which is odd, because process noise 'w' is usually not related to the input control matrix.

Your explanation is very clear. Thank you, Dave

Please, I could see the equation on line 76 in your code, Q_estimate=A*Q_estimate+B*u; I guess, it is a noise-free model. Then this is the noise-free (true) location and there is then no need to use Kalman filter and its correction gain to correct it. However, your explanation is great and I appreciate your efforts.

Hey, this tutorial is by far the best tutorial on Kalman filter I have seen! But could you explain more about the predermined acceleration magnitude. How do we determine the quails acceleration magnitude beforehand? And it looks like, while knowing that information (acceleration u), we can just use Newtons laws of motion equations and ignore measurements... Thanks for your help in advance :)

Ex is the covariance matrix of the process noise (acceleration noise). How the variance of the acceleration affects the variance of the state vector (position and velocity) is described by this covariance matrix. state = [dt^2/s dt] * acceleration => var(state) = var([dt^2/s dt] * acceleration) => var(state) = [dt^2/s dt] * [dt^2/s dt]^T * var(acceleration) This is due to the linearity between the state and the acceleration, see eg. wolframs page on variance (cannot give link here) eq. (7)-(12)

Great set of tutorials! I really enjoyed them and they helped improve my understanding of the Kalman filter. Maybe you could do one on the EM algorithm?

Hey, Can you elaborate on this line: 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 I don't quite understand that part. Thanks

Hey Dave! Thanks for the video, really made it clear! I'll put you in my works cited :D

Great video bro! Do you have any recommended method to estimate the matrixes A and B when you're not able to modeling the system dynamics?

Quick question: so the Kalman filter assumes knowledge of the system and how it is moving? How would you then use the Kalman filter to project values for a stock price if you do not know the underlying equations.? Thanks for any responses.

QuailAccel_noise = QuailAccel_noise_mag * [(dt^2/2)*randn; dt*randn];

Hi, Dave, this is an excellent illustration on Kalman filter. Just one question, how do you determine the magnitude of the acceleration? like the 'u=1.5' in your scripts. Sincerely

thankyou for making this video so relatable with the ninja and all :)

Good tutorial, i didn't get why the Co-variance is related to acceleration term

You are right. That is better. Thanks Anyway, the code should be corrected, Right?

nice..thanks..u just saved me so much stress....... plz may i have to the code to play around with and try implementing myself?.

Ex = QuailAccel_noise_mag^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; I dont get this step

Hi Dave, would it be possible to make some videos on Extended Kalman filter, and the comparison with particle filter? Love your stuff! Salute!

Thank you for uploading this tutorial for us!

Very nice mate, but singular it's a "matrix" like the movie. Not a "matrices".

Hi Student Dave, love yours tutorials! Bayes theorem Hipster example really made me laugh :) So... I’m trying to make variable input, but it doesn't work. I just put u=1.5 * t where you simulate what the Ninja sees over time, a then again u=1.5 * t where you do Kalman filtering. Does this make sense? It seems I don’t understand something. Any help would be appreciated.

Thanks for the video and code, Dave. What is the function "smooth" in Matlab?

you explain excellent but i don't know where putted actual data in this program just puted initial state not a list of data which want to simulate

thanks a lot! this is very very helpful. can you explain particle filter after this?

I want to know why you are using dt for time in your first for loop. dt is just 0.1. I would have thought you would need to use the current value of t within the for loop. Why dt?

HI, thank you for the Tutorial. is there any way to track position and orientation of an object using kalman filter ?

hi.. am trying to use kalman filter for my project, i did manage to implement it ( i got my state equations,n measurement).. but how do you see the response or simulate the response n noise... it would help if you could share ur code..

very nice explanation thanks a million

Great explanation, thanks mate.

How to apply this for financial data? I do not have kinematics equations there.

A very good tutorial, thank you very much

It's pretty cool! Very helpful!

fantastic tutorial! thanks very much!

thank you sir, I have sent you an email to the one you provided on the website but no answer till now, i would like to get the code if that possible, thank you

thanx dave it isreally helpfull

Thank you for posting!

Help me a lot, thanks!

i'm impressed !

Good evening Dave, if it is possible could you please send a copy of the matlab code. Thanking you very much

hello WHAT do you mean Bulletin i can t find it, could you tell with more detail i really need the code

the example on the page doesnt work :(

is the interval dt must be fixed every iteration?

Very nice! thank you!

Thank you!

hey guys! i'm not sure what exactly your idea is here, but the randomized will just add random acceleration input..which might be cool but isn't necessary. i got something fun coming soon! :)

Good one!

Sir can this process be done in eviews. if yes how can one go about it

hmmm....there could be a number of reasons for that..like different versions, missing functions etc... message me at my account with your specific error k?..k..cool..

dude, u r freaking awesome! lol

Thank you very match!

very intersting explanation! could you send me the matlab code please

Helpful ! Thanks brother

oh! haha crap! you're right...mannnnnnn you get a cookie sir, and I get a lemon :( lol thx!

Hello internet! :) See channel bulletin for links to code!

he did, smart guy.

Can anyone test this out and give feedback? Spot: 'Circuit Solver' by Phasor Systems on Google Play.

Is there a link to the code?

Hello! Thanks for the class! Would you mind sending me this Matlab code? Thanks in advance.

Sir please provide the code

ThanX

I don't suggest you post your email address on a public domain. Just my 2 cents suggestion.

your code does not work: Vectors must be the same lengths.

the feed

where is the code???

guys the code please !! if u have it contact me in my profile

dt^2/s = dt^2/2

Or make life easy: QuailAccel_noise = QuailAccel_noise_mag * [(dt^2/2); dt] .* randn;

Sent a message to ur inbox. please check. highly appreciated! =D =D

I HATE IT SO MUCH!!!

Thank you!

Matlab code doesn't work btw. Gets stuck in a loop