clc; clear; close all;

% Sample measured distance values (replace with actual data or simulation)
measured_dist = [277, 275, 277, 272, 274, 276, 275, 274, 276, 275, 273, 279, 275, 276, 275, 272, 274, 276, 274, 271, 275, 277, 271, 272, 269, 266, 270, 263, 258, 256, 252, 249, 244, 244, 244, 240, 238, 244, 237, 238, 234, 236, 240, 229, 228, 230, 234, 230, 226, 220, 218, 216, 216, 213, 212, 216, 213, 210, 211, 212, 210, 205, 206, 202, 202, 200, 201, 196, 192, 194, 192, 192, 182, 189, 186, 184, 179, 176, 179, 177, 174, 171, 173, 168, 168, 166, 161, 163, 161, 159, 159, 155, 149, 147, 148, 148, 147, 140, 143, 140, 142, 139, 137, 137, 136, 129, 128, 128, 125, 125, 120, 120, 116, 114, 114, 115, 109, 108, 104, 105, 102, 103, 100, 99, 94, 94, 95, 89, 88, 86, 85, 81, 85, 84, 80, 78, 80, 79, 76, 74, 74, 72, 72, 70, 69, 67, 69, 67, 64, 66, 58, 61, 63, 62, 59, 59, 59, 58, 57, 53, 52, 53];

% Kalman filter parameters
R = 2;  % Measurement noise
Q = 0.01; % Process noise
x_est = measured_dist(1); % Initial estimate
P = 1; % Initial error covariance

filtered_dist = zeros(size(measured_dist));
filtered_dist(1) = x_est;

for k = 2:length(measured_dist)
    % Prediction step
    x_pred = x_est;
    P_pred = P + Q;
    
    % Update step
    K = P_pred / (P_pred + R); % Kalman gain
    x_est = x_pred + K * (measured_dist(k) - x_pred);
    P = (1 - K) * P_pred;
    
    filtered_dist(k) = x_est;
end

% Correction formula (adjust based on actual calibration data)
corrected_dist = filtered_dist - (17.05 + 0.0803 * filtered_dist + 0.00000453 * filtered_dist.^2);

% Handling "too close" condition (threshold: 40mm)
corrected_dist(corrected_dist < 40) = NaN; % Represent "too close" as NaN

% Plot results
figure;
hold on;
plot(measured_dist, 'b-', 'LineWidth', 1.5, 'DisplayName', 'Raw Measured Distance');
plot(filtered_dist, 'g-', 'LineWidth', 1.5, 'DisplayName', 'Filtered Distance (Kalman)');
plot(corrected_dist, 'r-', 'LineWidth', 1.5, 'DisplayName', 'Corrected Distance');

xlabel('Measurement Index');
ylabel('Distance (mm)');
title('Comparison of Measured, Filtered, and Corrected Distances');
legend;
grid on;
hold off;
