%% Low_Pass_Filter_Step.m
clear all; format compact; format short; clc; close all;
%% a. Define a time vector, create a pure step function input and plot it.
% Create the time vector
dt = 0.002;
t = 0 : dt : 0.2;
s0 = ones(size(t)); % Initialize the step function
s0(1:5) = 0;        % set data to zero before the step occurs.
% Plot the step function
figure;
plot(t,s0,'k');
title('Pure Step');
grid on;

%% b. Define the parameters for a 1st Order Low-Pass Filter
% A 1st Order Low-Pass Filter smooths the data
% Each iteration's output is a combination of the current measurement
%  plus the previous filter output.
bw    = 10           %(Hz) Bandwidth
tau   = 1/(2*pi*bw)  % (s) 0.0318  Time constant
% The output rises to a factor of (1-(1/e)) of the step in tau seconds
gain_y = exp(-2*pi*bw*dt)    % 0.8819
gain_s = 1-gain_y  % 0.1181
y0    = zeros(size(s0));  % initial the output to zeros
t_len = length(t)

%% c. Apply the 1st Order Low-Pass Filter to the pure step input
% using a for loop, to show the response without noise.
% Plot the result.
for k = 2:t_len
  y0(k) = gain_y*y0(k-1) + gain_s*s0(k);
end
%figure
hold on;
plot(t,y0,'r','LineWidth',2);
% { } allow a 2-line title
title({'First-Order Filter Step Response',...
      ['Bandwidth= ',num2str(bw),', Time Constant= ',num2str(tau)]});
xlabel('Time');
% y raises to 1- exp(-1) = 0.6321 in tau seconds

%% d. Create a 2nd input = step function + noise.
% Plot the noisy signal.
noise_std = 0.2;  % Standard deviation of the noise
s1 = s0 + noise_std*randn(size(s0)); 
hold on;
plot(t,s1,'b','LineWidth',1.5);
title('Step + Noisy Measurements');

%% e. Apply the 1st Order Low-Pass Filter to the noisy input using a for-loop. 
% Plot the result. Add a legend.
y1    = zeros(size(s1));  % initial the output to zeros
t_len = length(t)
% --- Note: Apply the fiter to the noisy s1 ---
for k = 2:t_len
  y1(k) = gain_y*y1(k-1) + gain_s*s1(k);
end
hold on;
plot(t,y1,'g','LineWidth',3);
% { } allow a 2-line title
title({'First-Order Filter Step Response',...
      ['Bandwidth= ',num2str(bw),', Time Constant= ',num2str(tau)]});
xlabel('Time');
legend('Pure Step', 'Filtered Pure Step', 'Noisy Step', 'Filtered Noisy Step')