Location via proxy:   [ UP ]  
[Report a bug]   [Manage cookies]                
Scribd
Upload
2 views

Position_Control_Simulation_Code

This document presents a position control simulation code for a line follower robot using a Kalman filter. It defines simulation parameters, system matrices, and noise covariances, and then simulates the true system while estimating the position and velocity. The results are plotted to compare the true position, estimated position, and measurement errors over time.

Uploaded by

ademasfaw222
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
2 views

Position_Control_Simulation_Code

This document presents a position control simulation code for a line follower robot using a Kalman filter. It defines simulation parameters, system matrices, and noise covariances, and then simulates the true system while estimating the position and velocity. The results are plotted to compare the true position, estimated position, and measurement errors over time.

Uploaded by

ademasfaw222
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
You are on page 1/ 2

Position Control Simulation Code for

Line Follower Robot


% Define simulation parameters
dt = 0.1; % Time step
T = 100; % Total time
time = 0:dt:T;

% System matrices (assuming constant velocity model)


A = [1 dt; 0 1];
B = [0; 1];
C = [1 0]; % Position measurement

% Noise covariances
Q = [0.001 0; 0 0.001]; % Process noise
R = 0.05; % Measurement noise

% Initialization
x = [0; 0]; % Initial true state [position; velocity]
x_est = [0; 0]; % Estimated state
P = eye(2); % Initial covariance

% Storage
X = zeros(2, length(time));
X_est = zeros(2, length(time));
Z = zeros(1, length(time));

% Simulate
for k = 2:length(time)
% True system
u = 1; % Constant acceleration input
x = A * x + B * u + mvnrnd([0;0], Q)'; % Add process noise
z = C * x + normrnd(0, sqrt(R)); % Measurement with noise

% Prediction
x_pred = A * x_est + B * u;
P_pred = A * P * A' + Q;

% Kalman Gain
K = P_pred * C' / (C * P_pred * C' + R);
% Correction
x_est = x_pred + K * (z - C * x_pred);
P = (eye(2) - K * C) * P_pred;

% Store results
X(:,k) = x;
X_est(:,k) = x_est;
Z(k) = z;
end

% Plotting
figure;
subplot(2,1,1);
plot(time, X(1,:), 'k', time, X_est(1,:), 'g--', time, Z, 'r:');
legend('True Position','Estimated Position','Measurement');
title('Estimated vs Real Position');

subplot(2,1,2);
plot(time, X(1,:) - X_est(1,:), 'b', time, X(1,:) - Z, 'r');
legend('Estimation Error', 'Measurement Error');
title('Error');

You might also like