Scribd
Upload
6 views

Localization Assignment

The document details the implementation of an Extended Kalman Filter (EKF) for localizing a differential drive robot within a binary occupancy map. It includes sections on creating the occupancy map, defining robot and simulation parameters, initializing state and covariance, and executing the main simulation loop with noise modeling. Additionally, it provides helper functions for robot motion modeling and visualizing the results of the localization process.

Uploaded by

mdehtemam0511
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
6 views

Localization Assignment

The document details the implementation of an Extended Kalman Filter (EKF) for localizing a differential drive robot within a binary occupancy map. It includes sections on creating the occupancy map, defining robot and simulation parameters, initializing state and covariance, and executing the main simulation loop with noise modeling. Additionally, it provides helper functions for robot motion modeling and visualizing the results of the localization process.

Uploaded by

mdehtemam0511
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

Table of Contents

EKF Localization for Differential Drive Robot ......................................................................................... 1


Create Binary Occupancy Map .............................................................................................................. 1
Robot Parameters ................................................................................................................................ 2
Simulation Parameters ......................................................................................................................... 2
Initialize Robot State and Covariance ...................................................................................................... 2
Process and Measurement Noise ............................................................................................................ 3
Arrays to store data ............................................................................................................................. 3
Main Simulation Loop ......................................................................................................................... 3
Helper Functions ................................................................................................................................. 5

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
Published with MATLAB® R2023b

You might also like