Here the implementation of an Extended Kalman Filter (EKF) that:
- Predicts the position of a moving object using to this end just the information given by external sensors that tells the angle on which the moving object was observed.
- It doesn´t account with the information about the distance from the sensor to the moving object (distance meassure). Just the angle.
- It doesn´t account with Odometry (Dead Reckoning) information or information from oder sensors mounted on the device ( sonar, lidar, or cameras).
- At the moment are there just 2 external sensors in the scene, but it can be extended to more.
Matlab Code
Run function
function run(Ftype)
% Juan Carlos Rodríguez Moreno
% jcrm@uma.es / juancarlosrodriguezmoreno@gmail.com
% www.juancarlos-rodriguezmoreno.com
% In this function an Extended Kalman Filter is used to stimate the
% position of a moving object (Robot) thanks to the information given
% by two sensors about the angle in which this two sensors observe the
% moving object.
%
% - It doesn´t account with the information about the distance from the
% sensor to the moving object (distance meassure). Just the angle.
%
% - It doesn´t account with Odometry (Dead Reckoning) information or info
% from oder sensors mounted on the device (no sonar, no lidar, no
% cameras)
%
% - At the moment are there just 2 external sensors in the scene,
% but it can be extended to more.
% See also:
% ekf_predict, ekf_update, ekfFilter
global S1; %POSITION OF THE SENSOR 1 (in the MAP)
global S2; %POSITION OF THE SENSOR 2 (in the MAP)
global sd; %error in the meassures
global dt; %time slice
S1 = [1;1];
S2 = [5;6];
sd = 0.05;
dt = 0.02;
numSteps=750;
[X] = Trajectory(numSteps,dt); %Trajectory of the Robot
%Initialization values for mean (mu) and covariance (SIGMA)
%[pose_x pose_y velocity_in_axis_x velocity_in_axis_y]
mu_init = [4;5;0;0];
SIGMA_init = diag([0.1 0.1 10 10]);
Z = getMeassurement(X(:,1),sd);
aux_mat_mu = zeros(size(mu_init,1),size(Z,2));
stimated_trayectory = [];
clc;
fprintf('Starting the %s Filter \n',Ftype);
filter_type = sprintf ('%s Filter',Ftype);
pause on;
Z = getMeassurement(X(:,1),sd);
[mu,SIGMA]=ekfFilter(mu_init,SIGMA_init,Z);
for c=1:numSteps
for k = 1:size(Z,2)
Z = getMeassurement(X(:,c),sd);
[mu,SIGMA]=ekfFilter(mu,SIGMA,Z(:,k));
aux_mat_mu(:,k) = mu;
pause(0.01);
stimated_trayectory = [stimated_trayectory aux_mat_mu];
plot(X(1,c),X(2,c),'ko',...
stimated_trayectory(1,:),stimated_trayectory(2,:),'b-',...
X(1,1:c),X(2,1:c),'m:',...
S1(1),S1(2),'rd',S2(1),S2(2),'rd');
axis([0 7 0 7]);
legend('Robot´s Real Pose',...
'Filter´s Stimation',...
'Robot´s Real Trajectory',...
'Sensor´s Position',...
'Location', 'NorthWest');
title(filter_type);
end
end
EKF Filter function
function [mu,SIGMA]=ekfFilter(mu,SIGMA,Z)
% The Extended Kalman Filter itself
% Juan Carlos Rodríguez Moreno
% jcrm@uma.es / juancarlosrodriguezmoreno@gmail.com
% www.juancarlos-rodriguezmoreno.com
global S1;
global S2;
global dt;
global sd;
azimut_meassure = @Azimut;
jacobian = @Jacobian;
% Discretize the continous model
qx = 0.1;
qy = 0.1;
F = [0 0 1 0;
0 0 0 1;
0 0 0 0;
0 0 0 0];
[TDM,Q] = lti_disc(F,[],diag([0 0 qx qy]),dt); %DM: Discrete Model
R = sd^2; %For the noise covariance
[mu,SIGMA] = ekf_predict(mu,SIGMA,TDM,Q);
[mu,SIGMA] = ekf_update(mu,SIGMA,Z,jacobian,R*eye(2),azimut_meassure,[],[S1 S2]);
end
EKF Update function
%EKF_UPDATE 1st order Extended Kalman Filter update step
%
% Syntax:
% [mu,SIGMA,K,MU,S,LH] = EKF_UPDATE(mu,SIGMA,Y,H,R,[h,V,param])
%
% In:
% mu - Nx1 mean state estimate after prediction step
% SIGMA - NxN state covariance after prediction step
% Z - Dx1 measurement vector.
% H - Derivative of h() with respect to state as matrix,
% inline function, function handle or name
% of function in form H(x,param)
% R - Measurement noise covariance.
% h - Mean prediction (innovation) as vector,
% inline function, function handle or name
% of function in form h(x,param). (optional, default H(x)*X)
% V - Derivative of h() with respect to noise as matrix,
% inline function, function handle or name
% of function in form V(x,param). (optional, default identity)
% param - Parameters of h (optional, default empty)
%
% Out:
% mu - Updated state mean
% SIGMA - Updated state covariance
% K - Computed Kalman gain
% MU - Predictive mean of Y
% S - Predictive covariance of Y
% LH - Predictive probability (likelihood) of measurement.
%
% Description:
% Extended Kalman Filter measurement update step.
% EKF model is
%
% y[k] = h(x[k],r), r ~ N(0,R)
%
function [mu,SIGMA,K,MU,S,LH] = ekf_update(mu,SIGMA,Z,H,R,h,V,param)
%
% Check which arguments are there
%
if nargin < 5
error('Too few arguments');
end
if nargin < 6
h = [];
end
if nargin < 7
V = [];
end
if nargin < 8
param = [];
end
%
% Apply defaults
%
if isempty(V)
V = eye(size(R,1));
end
H = feval(H,mu,param);
MU = feval(h,mu,param);
% update step
S = (V*R*V' + H*SIGMA*H');
K = SIGMA*H'/S;
mu = mu + K * (Z-MU);
SIGMA = SIGMA - K*S*K';
if nargout > 5
LH = gauss_pdf(Z,MU,S);
end
EKF Prediction function
%EKF_PREDICT 1st order Extended Kalman Filter prediction step
%
% Syntax:
% [mu,SIGMA] = EKF_PREDICT(mu,SIGMA,[A,Q,a,W,param])
%
% In:
% mu - Nx1 mean state estimate of previous step
% SIGMA - NxN state covariance of previous step
% A - Derivative of a() with respect to state as
% matrix, inline function, function handle or
% name of function in form A(x,param) (optional, default eye())
% Q - Process noise of discrete model (optional, default zero)
% a - Mean prediction E[a(x[k-1],q=0)] as vector,
% inline function, function handle or name
% of function in form a(x,param) (optional, default A(x)*X)
% W - Derivative of a() with respect to noise q
% as matrix, inline function, function handle
% or name of function in form W(x,param) (optional, default identity)
% param - Parameters of a (optional, default empty)
%
% Out:
% mu - Updated state mean
% SIGMA - Updated state covariance
function [mu,SIGMA] = ekf_predict(mu,SIGMA,A,Q,a,W,param)
% Check arguments %
if nargin < 3
A = [];
end
if nargin < 4
Q = [];
end
if nargin < 5
a = [];
end
if nargin < 6
W = [];
end
if nargin < 7
param = [];
end
W = eye(size(mu,1),size(Q,2));
% Perform prediction %
mu = A*mu;
SIGMA = A * SIGMA * A' + W * Q * W';
%EKF_PREDICT 1st order Extended Kalman Filter prediction step
%
% Syntax:
% [M,P] = EKF_PREDICT1(M,P,[A,Q,a,W,param])
%
% In:
% M - Nx1 mean state estimate of previous step
% P - NxN state covariance of previous step
% A - Derivative of a() with respect to state as
% matrix, inline function, function handle or
% name of function in form A(x,param) (optional, default eye())
% Q - Process noise of discrete model (optional, default zero)
% a - Mean prediction E[a(x[k-1],q=0)] as vector,
% inline function, function handle or name
% of function in form a(x,param) (optional, default A(x)*X)
% W - Derivative of a() with respect to noise q
% as matrix, inline function, function handle
% or name of function in form W(x,param) (optional, default identity)
% param - Parameters of a (optional, default empty)
%
% Out:
% M - Updated state mean
% P - Updated state covariance
function [M,P] = ekf_predict(M,P,A,Q,a,W,param)
% Check arguments %
if nargin < 3
A = [];
end
if nargin < 4
Q = [];
end
if nargin < 5
a = [];
end
if nargin < 6
W = [];
end
if nargin < 7
param = [];
end
W = eye(size(M,1),size(Q,2));
% Perform prediction %
M = A*M;
P = A * P * A' + W * Q * W';
Jacobian function
% Juan Carlos Rodríguez Moreno
% jcrm@uma.es / juancarlosrodriguezmoreno@gmail.com
% www.juancarlos-rodriguezmoreno.com
% Jacobian of the measurement model function
%
% J_dx = -(y-sy) / (x-sx)^2 * 1 / (1 + (y-sy)^2 / (x-sx)^2)
% = -(y-sy) / ((x-sx)^2 + (y-sy)^2)
%
% J_dy = 1 / (x-sx) * 1 / (1 + (y-sy)^2 / (x-sx)^2)
% = (x-sx) / ((x-sx)^2 + (y-sy)^2)
%
function J = Jacobian(x,s)
% Define the zeros matrix for the Jacobian.
J = zeros(size(s,2),size(x,1));
% Loop through sensors
for i=1:size(s,2)
j_ = [-(x(2)-s(2,i)) / ((x(1)-s(1,i))^2 + (x(2)-s(2,i))^2);...
(x(1)-s(1,i)) / ((x(1)-s(1,i))^2 + (x(2)-s(2,i))^2);...
zeros(size(x,1)-2,1)]';
J(i,:) = j_;
end
Azimut function
% Juan Carlos Rodríguez Moreno
% jcrm@uma.es / juancarlosrodriguezmoreno@gmail.com
% www.juancarlos-rodriguezmoreno.com
% Azimuth measurement function for EKF
% Az = atan((y-sy) / (x-sx))
function MU = Azimut(x,s)
for i=1:size(s,2)
Az = atan2(x(2,:)-s(2,i),x(1,:)-s(1,i));
np = find(Az>0.5*pi);
nn = find(Az<-0.5*pi);
if length(nn)>length(np)
Az(np) = Az(np)-2*pi;
else
Az(nn) = Az(nn)+2*pi;
end
MU(i,:) = [Az]; %MU = [angle_to_S1 angle_to_S2]
LTI DISC function
%LTI_DISC Discretize LTI ODE with Gaussian Noise
%
% Syntax:
% [TDM,Q] = lti_disc(F,L,Qc,dt)
%
% In:
% F - NxN Feedback matrix
% L - NxL Noise effect matrix (optional, default identity)
% Qc - LxL Diagonal Spectral Density (optional, default zeros)
% dt - Time Step (optional, default 1)
%
% Out:
% TDM - Transition Discrete Matrix
% Q - Discrete Process Covariance
%
% Description:
% Discretize LTI ODE with Gaussian Noise. The original
% ODE model is in form
%
% dx/dt = F x + L w, w ~ N(0,Qc)
%
% Result of discretization is the model
%
% x[k] = A x[k-1] + q, q ~ N(0,Q)
%
% Which can be used for integrating the model
% exactly over time steps, which are multiples
% of dt.
function [TDM,Q] = lti_disc(F,L,Q,dt)
%
% Check number of arguments
%
if nargin < 1
error('Too few arguments');
end
if nargin < 2
L = [];
end
if nargin < 3
Q = [];
end
if nargin < 4
dt = [];
end
if isempty(L)
L = eye(size(F,1));
end
if isempty(Q)
Q = zeros(size(F,1),size(F,1));
end
if isempty(dt)
dt = 1;
end
%
% Closed form integration of transition matrix
%
TDM = expm(F*dt);
%
% Closed form integration of covariance
% by matrix fraction decomposition
%
n = size(F,1);
Phi = [F L*Q*L'; zeros(n,n) -F'];
AB = expm(Phi*dt)*[zeros(n,n);eye(n)];
Q = AB(1:n,:)/AB((n+1):(2*n),:);
getMeassurement function
% Juan Carlos Rodríguez Moreno
% jcrm@uma.es / juancarlosrodriguezmoreno@gmail.com
% www.juancarlos-rodriguezmoreno.com
function [Z]=getMeassurement(x,sd)
global S1;
global S2;
% Angular measurements for both sensors
z1 = atan2(x(2)-S1(2), x(1)-S1(1)) + sd * randn;
z2 = atan2(x(2)-S2(2), x(1)-S2(1)) + sd * randn;
Z = [z1; z2];