clc
%Для работы требуется предварительно загрузить в рабочую область список переменных (файл discrete_model_variables)
R = 0.01;                               %Матрица ковариации измерительного шума

%ИЗМЕРЕНИЯ
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_sm
Kalman_1B_sm

%СРАВНЕНИЕ ПРЯМОГО И ОБРАТНОГО ФИЛЬТРОВ, А ТАКЖЕ МОДЕЛИ ДЛЯ СКОРОСТИ
% 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 = 600;
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 = 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_sm
% Kalman_1B_sm
% 
% %СРАВНЕНИЕ ПРЯМОГО, ОБРАТНОГО ФИЛЬТРОВ И МОДЕЛИ ДЛЯ СКОРОСТИ
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 = 0;
plot1 = plot(time', dV(:,1),'green', time(1+exclude_num:N - exclude_num)', dV_Smoothed(1+exclude_num:N-exclude_num,1), 'red');               
title({'delta V - green, delta V smoothed - red';[' R(measurment) = ',num2str(R)]});
xlabel('sec');
ylabel('m/s');
figure
exclude_num = 600;
plot2 = plot(time', Fi(:,1),'green', time(1+exclude_num:N - exclude_num)', Fi_Smoothed(1+exclude_num:N-exclude_num,1), 'red');               
title({'delta Fi - green, delta Fi smoothed - red';[' R(measurment) = ',num2str(R)]});
xlabel('sec');
ylabel('rad/s');
figure
exclude_num = 600;
plot3 = plot(time', dW(:,1),'green', time(1+exclude_num:N - exclude_num)', dW_Smoothed(1+exclude_num:N-exclude_num,1), 'red');               
title({'delta W - green, delta W smoothed - red';[' R(measurment) = ',num2str(R)]});
xlabel('sec');
ylabel('rad/s');