Kalman Filter (KF)
Linear dynamical system (Linear evolution functions)
Extended Kalman Filter (EKF)
Non-linear dynamical system (Non-linear evolution functions)
MATLAB CODE
########################################################################
function [x_next,P_next,x_dgr,P_dgr] = ekf(f,Q,h,y,R,del_f,del_h,x_hat,P_hat);
% Extended Kalman filter
%
% -------------------------------------------------------------------------
%
% State space model is
% X_k+1 = f_k(X_k) + V_k+1 --> state update
% Y_k = h_k(X_k) + W_k --> measurement
%
% V_k+1 zero mean uncorrelated gaussian, cov(V_k) = Q_k
% W_k zero mean uncorrelated gaussian, cov(W_k) = R_k
% V_k & W_j are uncorrelated for every k,j
%
% -------------------------------------------------------------------------
%
% Inputs:
% f = f_k
% Q = Q_k+1
% h = h_k
% y = y_k
% R = R_k
% del_f = gradient of f_k
% del_h = gradient of h_k
% x_hat = current state prediction
% P_hat = current error covariance (predicted)
%
% -------------------------------------------------------------------------
%
% Outputs:
% x_next = next state prediction
% P_next = next error covariance (predicted)
% x_dgr = current state estimate
% P_dgr = current estimated error covariance
%
% -------------------------------------------------------------------------
%
if isa(f,'function_handle') & isa(h,'function_handle') & isa(del_f,'function_handle') & isa(del_h,'function_handle')
y_hat = h(x_hat);
y_tilde = y - y_hat;
t = del_h(x_hat);
tmp = P_hat*t;
M = inv(t'*tmp+R+eps);
K = tmp*M;
p = del_f(x_hat);
x_dgr = x_hat + K* y_tilde;
x_next = f(x_dgr);
P_dgr = P_hat - tmp*K';
P_next = p* P_dgr* p' + Q;
else
error('f, h, del_f, and del_h should be function handles')
return
end
##############################################################################
For more
https://drive.google.com/folderview?id=0B2l8IvcdrC4oMzU3Z2NVXzQ0Y28&usp=sharing
Linear dynamical system (Linear evolution functions)
Extended Kalman Filter (EKF)
Non-linear dynamical system (Non-linear evolution functions)
Consider the following non-linear system:
Assume that we can somehow determine a reference trajectory
Then:
where
For the measurement equation, we have:
We can then apply the standard Kalman filter to the linearized model
How to choose the reference trajectory?
Idea of the extended Kalman filter is to re-linearize the model around the most recent state estimate, i.e.
The Extended Kalman Filter (EKF) has become a standard technique used in a number of
# nonlinear estimation and
# machine learning applications
#State estimation
#estimating the state of a nonlinear dynamic system
#Parameter estimation
#estimating parameters for nonlinear system identification
#e.g., learning the weights of a neural network
#dual estimation
#both states and parameters are estimated simultaneously
#e.g., the Expectation Maximization (EM) algorithm
MATLAB CODE
########################################################################
function [x_next,P_next,x_dgr,P_dgr] = ekf(f,Q,h,y,R,del_f,del_h,x_hat,P_hat);
% Extended Kalman filter
%
% -------------------------------------------------------------------------
%
% State space model is
% X_k+1 = f_k(X_k) + V_k+1 --> state update
% Y_k = h_k(X_k) + W_k --> measurement
%
% V_k+1 zero mean uncorrelated gaussian, cov(V_k) = Q_k
% W_k zero mean uncorrelated gaussian, cov(W_k) = R_k
% V_k & W_j are uncorrelated for every k,j
%
% -------------------------------------------------------------------------
%
% Inputs:
% f = f_k
% Q = Q_k+1
% h = h_k
% y = y_k
% R = R_k
% del_f = gradient of f_k
% del_h = gradient of h_k
% x_hat = current state prediction
% P_hat = current error covariance (predicted)
%
% -------------------------------------------------------------------------
%
% Outputs:
% x_next = next state prediction
% P_next = next error covariance (predicted)
% x_dgr = current state estimate
% P_dgr = current estimated error covariance
%
% -------------------------------------------------------------------------
%
if isa(f,'function_handle') & isa(h,'function_handle') & isa(del_f,'function_handle') & isa(del_h,'function_handle')
y_hat = h(x_hat);
y_tilde = y - y_hat;
t = del_h(x_hat);
tmp = P_hat*t;
M = inv(t'*tmp+R+eps);
K = tmp*M;
p = del_f(x_hat);
x_dgr = x_hat + K* y_tilde;
x_next = f(x_dgr);
P_dgr = P_hat - tmp*K';
P_next = p* P_dgr* p' + Q;
else
error('f, h, del_f, and del_h should be function handles')
return
end
##############################################################################
For more
https://drive.google.com/folderview?id=0B2l8IvcdrC4oMzU3Z2NVXzQ0Y28&usp=sharing
