robotics专项课程Estimation and Learning Assignment 2: Kalman Filter⎼Target Tracking

Where:

• X is state (for ex., 4x1 vector: {x ;y; vx ;vy} )
• P is state covariance matrix (for ex., 4x4 matrix)
• A is transition matrix for velocity term (for ex., 4x4 matrix)
• B is transition matrix for acceleration term (for ex., 4x2 matrix)
• u is acceleration matrix (for ex., 2x1 matrix)
• w is matrix representing noise in the process (for ex., 4x1 matrix)
• Q is system noise covariance matrix (for ex., 4x4 matrix)
• K is Kalman Gain (for ex., 4x2 matrix)
• C is observation matrix (for ex. 2x4 matrix)
• R is measurement noise covaiance matrix (for ex., 2x2 matrix)
• Y is measurement matrix (for ex., 2x1 matrix: {z_x; z_y})

So it takes   X(k-1)  and   P(k-1) from previous step, then finds new predicted   Xkp  and   Pkp, then finds the value of Kalman Gain   K, and finally calculates "better predicted" state   Xk and covariance matrix   Pk taking into account the measurements values   Y. So   Xk  and   Pk  becomes output for current step and input for the next step.

All you need to do is define matrices in green ( A,C, Sigma_m, Sigma_o ) and use mentioned formulas. Finding  A  and  C  is quite easy. Sigma_m  and  Sigma_o  are digonal matrices, which diagonal terms are objects to tune (random blind tuning, hell yeah!). 

function [ predictx, predicty, state, param ] = kalmanFilter( t, x, y, state, param, previous_t )
%UNTITLED Summary of this function goes here
%   Four dimensional state: position_x, position_y, velocity_x, velocity_y

    %% Place parameters like covarainces, etc. here:
    % P = eye(4)
    % R = eye(2)
    dt = ;
       
	  % Noise covariance matrices
    Sigma_m = diag([]);
    Sigma_o = diag([]); 
	
	  % Transition matrix
    A = eye(4);
    A(1,3) = dt;
    A(2,4) = dt;
    
    % Observation matrix
    C =zeros(2,4);
    C(1,1) = 1;
    C(2,2) = 1;
    

    % Check if the first time running this function
    if previous_t<0
        state = [x, y, 0, 0];
        param.P = 0.1 * eye(4);
        predictx = x;
        predicty = y;
        return;
    end

    %% TODO: Add Kalman filter updates
    Xkp = A*state';
    Pkp = A*param.P*A'+ Sigma_m;
    
    % Kalman gain
    K = Pkp*C'*inv(C*Pkp*C'+Sigma_o);
        
    % Z measrement
    Z = [x y]';
    
    % Update covariance matrix  
    param.P = Pkp-K*C*Pkp;
    
    % Update state
    state = Xkp+K*(Z-C*Xkp);
    state = state';
    
    % Prediction
    predictx = state(1);
    predicty = state(2);
end