Scribd
Upload
9 views9 pages

EKF

The document describes the implementation of an Extended Kalman Filter (EKF) for localization of a differential drive robot using a binary occupancy map. It includes the setup of the robot's parameters, simulation parameters, and the main simulation loop that updates the robot's state and covariance while visualizing the true and estimated positions. Additionally, it provides helper functions for the robot's motion model and error ellipse plotting.

Uploaded by

meenakshi sinha
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
9 views9 pages

EKF

The document describes the implementation of an Extended Kalman Filter (EKF) for localization of a differential drive robot using a binary occupancy map. It includes the setup of the robot's parameters, simulation parameters, and the main simulation loop that updates the robot's state and covariance while visualizing the true and estimated positions. Additionally, it provides helper functions for the robot's motion model and error ellipse plotting.

Uploaded by

meenakshi sinha
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 9

MAPPING THROUGH EKF LOCALIZATION

EKF Localization for Differential Drive Robot


clear all;
close all;

Create Binary Occupancy Map


rows = 20;
cols = 20;
occupancyMap = zeros(rows, cols);

% Define rooms and corridor


occupancyMap(1:5, 1:5) = 1; % Room 1 (top-left)
occupancyMap(1:5, 15:19) = 1; % Room 2 (top-right)
occupancyMap(14:18, 1:5) = 1; % Room 3 (bottom-left)
occupancyMap(14:18, 15:19) = 1; % Room 4 (bottom-right)
occupancyMap(8:12, 1:19) = 1; % Corridor

% Create binary occupancy grid


binaryMap = logical(occupancyMap);
occGrid = robotics.BinaryOccupancyGrid(binaryMap);

% Display the map


figure('Position', [100, 100, 500, 500]);
show(occGrid);
title('Binary Occupancy Map');
hold on;

1
Robot Parameters
wheelRadius = 0.1; % Wheel radius (m)
wheelDistance = 0.5; % Distance between wheels (m)

Simulation Parameters
dt = 0.1; % Time step (s)
totalTime = 30; % Total simulation time (s)
timeSteps = 0:dt:totalTime;
numSteps = length(timeSteps);

Initialize Robot State and Covariance


State: [x, y, theta]'

x_true = [10; 10; 0]; % True initial state (middle of map)


x_est = [10; 10; 0]; % Initial state estimate
P = diag([0.1, 0.1, 0.05].^2); % Initial covariance matrix

2
Process and Measurement Noise
Q = diag([0.1, 0.1, 0.05].^2); % Process noise covariance
R = diag([0.1, 0.1].^2); % Measurement noise covariance

Arrays to store data


x_true_history = zeros(3, numSteps);
x_est_history = zeros(3, numSteps);
P_history = zeros(3, numSteps);
z_history = zeros(2, numSteps);

Main Simulation Loop


for i = 1:numSteps
time = timeSteps(i);

% Define control inputs (wheel velocities)


if time < 10
vLeft = 0.5;
vRight = 0.7;
elseif time < 20
vLeft = 0.7;
vRight = 0.5;
else
vLeft = 0.6;
vRight = 0.6;
end

% Calculate robot velocities from wheel velocities


v = (vRight + vLeft) / 2; % Linear velocity
omega = (vRight - vLeft) / wheelDistance; % Angular velocity
u = [v; omega];

% Update true robot state with noise


process_noise = sqrt(Q) * randn(3, 1);
x_true = robot_motion_model(x_true, u, dt) + process_noise;

% Generate noisy measurement


measurement_noise = sqrt(R) * randn(2, 1);
z = [x_true(1); x_true(2)] + measurement_noise;

% EKF Prediction Step


[x_pred, F, G] = robot_motion_model_jacobian(x_est, u, dt);
P_pred = F * P * F' + Q;

% EKF Update Step


H = [1, 0, 0; 0, 1, 0]; % Measurement model Jacobian (position only)
y = z - H * x_pred; % Innovation
S = H * P_pred * H' + R; % Innovation covariance
K = P_pred * H' / S; % Kalman gain

3
% Update state and covariance
x_est = x_pred + K * y;
P = (eye(3) - K * H) * P_pred;

% Store data
x_true_history(:, i) = x_true;
x_est_history(:, i) = x_est;
P_history(:, i) = diag(P);
z_history(:, i) = z;

% Visualize every 10 steps


if mod(i, 10) == 0
% Plot true position
plot(x_true(1), x_true(2), 'ro', 'MarkerSize', 10);

% Plot estimated position with uncertainty ellipse


plot(x_est(1), x_est(2), 'bo', 'MarkerSize', 8);

% Plot uncertainty ellipse


error_ellipse(P(1:2, 1:2), x_est(1:2), 0.95);

% Plot measurements
plot(z(1), z(2), 'gx', 'MarkerSize', 6);

drawnow;
end
end

% Plot results
figure;
subplot(3, 1, 1);
plot(timeSteps, x_true_history(1, :), 'r-', timeSteps, x_est_history(1, :),
'b--');
legend('True', 'Estimated');
title('X Position');
grid on;

subplot(3, 1, 2);
plot(timeSteps, x_true_history(2, :), 'r-', timeSteps, x_est_history(2, :),
'b--');
legend('True', 'Estimated');
title('Y Position');
grid on;

subplot(3, 1, 3);
plot(timeSteps, x_true_history(3, :), 'r-', timeSteps, x_est_history(3, :),
'b--');
legend('True', 'Estimated');
title('Orientation (theta)');
grid on;

% Plot trajectory
figure;
show(occGrid);

4
hold on;
plot(x_true_history(1, :), x_true_history(2, :), 'r-', 'LineWidth', 2);
plot(x_est_history(1, :), x_est_history(2, :), 'b--', 'LineWidth', 2);
plot(z_history(1, :), z_history(2, :), 'g.', 'MarkerSize', 2);
legend('True Path', 'Estimated Path', 'Measurements');
title('Robot Trajectory with EKF Localization');

Helper Functions
function x_next = robot_motion_model(x, u, dt)
% Motion model for differential drive robot
% x = [x, y, theta]'
% u = [v, omega]'

v = u(1);
omega = u(2);

x_next = zeros(3, 1);

if abs(omega) < 1e-6


% Straight line motion
x_next(1) = x(1) + v * cos(x(3)) * dt;
x_next(2) = x(2) + v * sin(x(3)) * dt;
x_next(3) = x(3);
else
% Circular motion
x_next(1) = x(1) + (v/omega) * (sin(x(3) + omega*dt) - sin(x(3)));
x_next(2) = x(2) + (v/omega) * (cos(x(3)) - cos(x(3) + omega*dt));
x_next(3) = x(3) + omega * dt;
end

% Normalize angle
x_next(3) = mod(x_next(3) + pi, 2*pi) - pi;
end

function [x_next, F, G] = robot_motion_model_jacobian(x, u, dt)


% Motion model with Jacobians for EKF
% x = [x, y, theta]'
% u = [v, omega]'

v = u(1);
omega = u(2);

% Next state
x_next = robot_motion_model(x, u, dt);

% Jacobian with respect to state


F = eye(3);

if abs(omega) < 1e-6


% Straight line motion
F(1, 3) = -v * sin(x(3)) * dt;
F(2, 3) = v * cos(x(3)) * dt;

5
else
% Circular motion
F(1, 3) = (v/omega) * (cos(x(3) + omega*dt) - cos(x(3)));
F(2, 3) = (v/omega) * (sin(x(3) + omega*dt) - sin(x(3)));
end

% Jacobian with respect to control input


G = zeros(3, 2);

if abs(omega) < 1e-6


% Straight line motion
G(1, 1) = cos(x(3)) * dt;
G(2, 1) = sin(x(3)) * dt;
G(3, 2) = dt;
else
% Circular motion
G(1, 1) = (sin(x(3) + omega*dt) - sin(x(3))) / omega;
G(1, 2) = v * (sin(x(3)) - sin(x(3) + omega*dt)) / omega^2 + ...
v * cos(x(3) + omega*dt) * dt / omega;
G(2, 1) = (cos(x(3)) - cos(x(3) + omega*dt)) / omega;
G(2, 2) = v * (cos(x(3) + omega*dt) - cos(x(3))) / omega^2 + ...
v * sin(x(3) + omega*dt) * dt / omega;
G(3, 2) = dt;
end
end

function h = error_ellipse(P, mu, p)


% Plot error ellipse for covariance matrix P centered at mu
% p is the confidence level (e.g., 0.95 for 95% confidence)

s = -2 * log(1 - p);
[V, D] = eig(P);

t = linspace(0, 2*pi, 100);


a = sqrt(s * D(1, 1));
b = sqrt(s * D(2, 2));

ellipse_x = a * cos(t);
ellipse_y = b * sin(t);

R = [V(1, 1), V(1, 2); V(2, 1), V(2, 2)];


transformed = R * [ellipse_x; ellipse_y];

h = plot(transformed(1, :) + mu(1), transformed(2, :) + mu(2), 'b--');


end

6
7
8
9

You might also like