clc
%Äëÿ ðàáîòû òðåáóåòñÿ ïðåäâàðèòåëüíî çàãðóçèòü â ðàáî÷óþ îáëàñòü ñïèñîê ïåðåìåííûõ (ôàéë discrete_model_variables)
R = 0.1;                               %Ìàòðèöà êîâàðèàöèè èçìåðèòåëüíîãî øóìà

%ÈÇÌÅÐÅÍÈß
prcnt10 = N/100;
p_count = 0;
for i = 1:1:N
    Xk(1,1) = dV(i,1);
    Xk(2,1) = Fi(i,1);
    Xk(3,1) = dW(i,1);
    
    Zk = H*Xk + sqrt(R).*randn;             %Âåêòîð èçìåðåíèé
    dV_measured(i,1) = Zk;                  %Ñîõðàíåíèå äàííûõ èçìåðåíèé
    if (mod(i,prcnt10) == 0)  
       clc;
       fprintf('Measuring complete %d%%', p_count + 1);
       p_count = p_count + 1;
    end    
end;                       

%ÇÀÏÓÑÊ ÔÈËÜÒÐÀÖÈÈ
Kalman_1F
Kalman_1B

%ÑÐÀÂÍÅÍÈÅ ÏÐßÌÎÃÎ È ÎÁÐÀÒÍÎÃÎ ÔÈËÜÒÐÎÂ, À ÒÀÊÆÅ ÌÎÄÅËÈ ÄËß ÑÊÎÐÎÑÒÈ
% figure
% plot1 = plot(time', dV(:,1),'green', time', Backward_dV_Filtered(:,1), 'red',time', dV_Filtered(:,1), 'blue');               
% title({'delta V - green, delta V estimated - blue, Backward delta V estimated - red';[' R(measurment) = ',num2str(R)]});
% xlabel('sec');
% ylabel('m/s');

%ÑÐÀÂÍÅÍÈÅ ÏÐßÌÎÃÎ, ÎÁÐÀÒÍÎÃÎ ÔÈËÜÒÐÀ È ÌÎÄÅËÈ ÄËß ÄÐÅÉÔÀ
% figure
% exclude_num = 400;
% plot2 = plot(time', dW(:,1),'green', time(1:N - exclude_num)', Backward_dW_Filtered(1:N-exclude_num,1), 'red',time(1+exclude_num:N)', dW_Filtered(1+exclude_num:N,1), 'blue');               
% title({'delta W - green, delta W estimated - blue, Backward delta W estimated - red';[' R(measurment) = ',num2str(R)]});
% xlabel('sec');
% ylabel('rad/s');

%ÈÇÌÅÐÅÍÈß ÄËß ÄÐÓÃÎÃÎ ÇÍÀ×ÅÍÈß ÌÀÒÐÈÖÛ ÊÎÂÀÐÈÀÖÈÈ ÂÕÎÄÍÛÕ ØÓÌÎÂ

R = 0.1;                               %Ìàòðèöà êîâàðèàöèè èçìåðèòåëüíîãî øóìà

%ÈÇÌÅÐÅÍÈß
prcnt10 = N/100;
p_count = 0;
for i = 1:1:N
    Xk(1,1) = dV(i,1);
    Xk(2,1) = Fi(i,1);
    Xk(3,1) = dW(i,1);
    
    Zk = H*Xk + sqrt(R).*randn;             %Âåêòîð èçìåðåíèé
    dV_measured(i,1) = Zk;                  %Ñîõðàíåíèå äàííûõ èçìåðåíèé
    if (mod(i,prcnt10) == 0)  
       clc;
       fprintf('Measuring complete %d%%', p_count + 1);
       p_count = p_count + 1;
    end    
end;                       

%ÇÀÏÓÑÊ ÔÈËÜÒÐÀÖÈÈ
Kalman_1F
Kalman_1B

%ÑÐÀÂÍÅÍÈÅ ÏÐßÌÎÃÎ È ÎÁÐÀÒÍÎÃÎ ÔÈËÜÒÐÎÂ, À ÒÀÊÆÅ ÌÎÄÅËÈ
% figure
% plot3 = plot(time', dV(:,1),'green', time', Backward_dV_Filtered(:,1), 'red',time', dV_Filtered(:,1), 'blue');               
% title({'delta V - green, delta V estimated - blue, Backward delta V estimated - red';[' R(measurment) = ',num2str(R)]});
% xlabel('sec');
% ylabel('m/s');

%ÑÃËÀÆÈÂÀÍÈÅ
Smoothing

figure
exclude_num = 400;
plot4 = plot(time', dW(:,1),'green', time(exclude_num:N-exclude_num)', dW_Smoothed(exclude_num:N-exclude_num,1), 'red');               
title({'delta W - green, delta W smoothed - red';[' R(measurment) = ',num2str(R)]});
xlabel('sec');
ylabel('m/s');