EKF_ExtRefSys_angle

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];