Tutorial: Kalman Filter with MATLAB example part3



Ive created a website with more content and codes! go here. enjoy!

MATLAB example of Bayesian NINJA using KALMAN FILTER to hunt QUAIL

this tutorial features MATLAB® programming language, go here of you wanna get it…

31 Comments

  • 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.

  • 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.

  • 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?

  • Line 79 and 80 of your example code reads

        % Predict next state of the quail with the last state and predicted motion.
        Q_estimate = A * Q_estimate + B * u;
        
    When the ninja runs this line of code, the value of u used is 0.5.

    So if the ninja already knows the value of the acceleration of the quail he can predict its path over time without the need of any filter at all, or for that matter without the need for any observations as well.

    Could you please elaborate how the ninja already knows the exact value of u?

  • 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. 

  • 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

  • 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 🙂

Leave a Reply

Your email address will not be published. Required fields are marked *