clf
subplot(311), plot(t,yt,'b',t,y_hat,'r--'), 
xlabel('Number of Samples'), ylabel('Output')
title('Kalman Filter Response')
legend('True','Filtered')
subplot(312), plot(t,yt-y,'g',t,yt-y_hat,'r--'),
xlabel('Number of Samples'), ylabel('Error')
legend('True - measured','True - filtered')
subplot(313), plot(t, state,'g',t,[x1_hat, x2_hat,x3_hat],'r--'),
xlabel('Number of Samples'), ylabel('State')
legend('True - state','True - filtered')

....
figure;
plot(t, filtered, t, v);
ylim([-2 2]);
xlabel('time (in seconds)');
legA = legend('filtered','real');
set(legA,'FontSize',12)
title('signal vs. estimation');
grid on

 [y_o,t_o,x_o] = lsim( sys, [u 0*w], t);     % IDEAL behavior for comparison
figure(1)
for k = 1:length(sys.A)
    subplot( round((length(sys.A)+1)/2 ),2,k)
    plot(t,x_o(:,k),'k','linewidth',2); hold on;
    plot(t,x_(:,k),'bo:','linewidth',2); 
    stairs(t,x(:,k),'r','linewidth',1.5);
    xlabel('Time (s)'); ylabel('Amplitude'); title(['State x_' num2str(k) '(t)']); grid on;
    legend('ideal','real','estimated'); hold off;
end
subplot( round((length(sys.A)+1)/2 ),2,k+1)
plot(t,y_o,'k','linewidth',2); hold on;
plot(t,z,'bo:','linewidth',2);
stairs(t,y,'r','linewidth',1.5);
xlabel('Time (s)'); ylabel('Amplitude'); title('Output y(t)'); grid on;
legend('ideal','measured','estimated');

%Generate process noise and sensor noise vectors using the same noise covariance values Q and R that you used to design the filter.
rng(10,'twister');
w1 = sqrt(Q)*randn(length(t),1);
v1 = sqrt(R)*randn(length(t),1);
%Finally, simulate the response using lsim.
out = lsim(SimModel,[u,w1,v1]);
%lsim generates the response at the outputs yt and ye to the inputs applied at w, v, and u. Extract the yt and ye channels and compute the measured response.
yt = out(:,1);   % true response
ye = out(:,2);  % filtered response
y = yt + v1;     % measured response
%Compare the true response with the filtered response.
clf
subplot(211), plot(t,yt,'b',t,ye,'r--'), 
xlabel('Number of Samples'), ylabel('Output')
title('Kalman Filter Response')
legend('True','Filtered')
subplot(212), plot(t,yt-y,'g',t,yt-ye,'r--'),
xlabel('Number of Samples'), ylabel('Error')
legend('True - measured','True - filtered')
%covariance of the error before filtering (measurement error covariance)
MeasErr = yt-yt;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)  %=0
% and after filtering (estimation error covariance)
EstErr = yt-ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr)
% so the Kalman filter reduces the error yt - y due to measurement noise

% Kalman filter
[y_hat, ~, ~, P_last, K_last, x_hat, K_steps, P_steps] = kalmanfilter(y, Q, R, StopTime);
e_real = x - x_hat; %state error
figure();
plot(t,y, t, y_hat);
ylim([-1.5,1.5]);
legA = legend('real','estimated');
title('true signal and filtered signal without attack');grid on;