Scribd
Upload
16 views3 pages

Homework 3

Uploaded by

MR E-N
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
16 views3 pages

Homework 3

Uploaded by

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

Thái Nguyễn Nghĩa

MSSV:106200136
20DTCLC2.

HOMEWORK 3
Môn: Kỹ thuật Robot
Problem 1:
(a) MATLAB program
% Given data
true_height = 50; % True building height (not used in
calculations)
initial_estimate = 60; % Initial guess of the height
initial_uncertainty = 225; % Initial estimate uncertainty
measurement_error_std = 5; % Measurement error standard deviation
measurements = [49.03, 48.44, 55.21, 49.98, 50.6, 52.61, 45.87, 42.64, 48.26, 55.84];
% Measurements
% Initializing variables
x_hat = initial_estimate; % Initial state estimate
p = initial_uncertainty; % Initial estimate uncertainty
r = measurement_error_std^2; % Measurement variance
num_measurements = length(measurements); % Number of measurements
% Preallocate arrays to store the results
x_estimates = zeros(1, num_measurements); % State estimates
p_estimates = zeros(1, num_measurements); % Estimate uncertainties
% Kalman filter algorithm
for i = 1:num_measurements
% Measurement update (correction)
k = p / (p + r); % Kalman gain
x_hat = x_hat + k * (measurements(i) - x_hat); % Update estimate with
measurement
p = (1 - k) * p; % Update uncertainty
% Store results
x_estimates(i) = x_hat;
p_estimates(i) = p;
end
% Display the results
disp('State estimates (building height):');
disp(x_estimates);
disp('Estimate uncertainties:');
disp(p_estimates);
(b) Fill in the table
Unit: meters (m)

n 1 2 3 4 5 6 7 8 9 10
zn 49.0 48.4 55.2 49.9 50.6 52.6 45.8 42.6 48.2 55.8
3 4 1 8 1 7 4 6 4
^x n ,n 50.1 49.3 51.2 50.9 50.8 51.1 50.4 49.4 49.3 49.9
3 3 1 6 4 4 1 6
pn , n 22.5 11.85 8.04 6.08 4.89 4.09 3.51 3.1 2.74 2.47

(c) Plot the true value, measured values, estimates, and 95% confidence intervals

Add these codes to plot value:


ci_upper = x_estimates + 1.96 * sqrt(p_estimates);
ci_lower = x_estimates - 1.96 * sqrt(p_estimates);
% Plotting
figure;
hold on;
plot(1:num_measurements, true_height * ones(1, num_measurements), 'g-',
'LineWidth', 1.5); % True value
plot(1:num_measurements, measurements, 'r-', 'MarkerSize', 6, 'DisplayName',
'Measurements'); % Measurements
plot(1:num_measurements, x_estimates, 'b-', 'LineWidth', 1.5, 'DisplayName',
'Estimates');
% Estimates fill([1:num_measurements, fliplr(1:num_measurements)], [ci_upper,
fliplr(ci_lower)], 'y', 'FaceAlpha', 0.2, 'EdgeColor', 'none'); % Confidence interval
% Labels and Legend
xlabel('Measurement Number');
ylabel('Height (m)');
title('Kalman Filter Estimates with 95% Confidence Interval');
legend('True Height', 'Measurements', 'Estimates', '95% Confidence Interval');
grid on;
hold off;

Result:

You might also like