% Початкові дані
Noc=[9 25 51 101 151 201 301 401 501]';
Type_V=3; % рух ОВ 1 - з постійною швидкістю 2- стрибкоподібна зміна швидкості від 0 до макс значення
 % 3 - переміщення на задану відстань(для 3 час спостереження 200 с)
FK_P=4; % порядок матриць для фільтра Калмана
min 3
EKS_P=2; % кількість попередніх значення дляекспоненціального згладжування
ksi=0.7; % коефіцієнт згладжування
jjj=4; % виведення графіків параметрів руху для Noc=jjj
FKMean=2; % усереднення відліків для оцінкиточності ФК
v=11; % максимальна швидкість, мм/с
a_max=1; % максимальна швидкість, мм/с^2
x0=35; % початкова координата ОВ ФК
v0=v; % початкова швидкість ОВ ФК
a0=a_max; % початкове прискорення ОВ ФК
PARAM=[x0 v0 a0 0 0 0];
dt1=0.04; % крок дискретності
t1=(0:dt1:100)'; % час спостереження
SigmaNoise=0.6/sqrt(3); % СКЗ шуму вимірювань координати
Sigma_Dist_x=0;%3.33; % СКЗ збурень координати 5%
Sigma_Dist_v=0;%1.67; % СКЗ збурень швидкості 5%
Sigma_Dist_a=0; % СКЗ збурень прискорення
Sigma_Dist_x0=0;%0.1; % СКЗ початкових умов координатиФК
Sigma_Dist_v0=0;%0.1; % СКЗ початкових умов швидкостіФК
Sigma_Dist_a0=0;%0.0001; % СКЗ початкових умовприскорення ФК
TWx=0.12; TWv=0.12; % постійні часу (секунд)періодичної ланки для фільтрації збурень - корельванізбурення
Tu=1; % постійні часу (секунд) аперіодичної ланки 2-го порядку для фільтрації упр сигналу - врахуванняінерційності ОВ
randn('state',0);
% Математична модель руху неперервного ОВ 2-го порядку -неперервна система 2-го порядку 
% в просторі стану. Вхід - швидкість і прискорення.Збурення по входу - для швидкості і прискорення
% Моделюються рівноприскорений рух (U1=0 U2=a(t)) і рівномірний рух (U1=v(t) U2=0)
% dX/dt=AX+BU+GW
% Y= CX+DU+HW+V
% U (відсутній) - вхід Y (позначення в розділі 4 xi*)- вихід=поточній координаті ОВ
% X (Z) - внутрішній стан системи координата швидкість
% W (Lamnda Greek) - збурення в системі швидкостіприскорення - це ще 2 входи
% V (Delta Greek x) - шум вимірювань виходу тобто поточної координати ОВ
Z0=[x0; 0]; % початковий стан системи координата швидкість
A=[0 1 % матриці системи в просторі стану
 0 0];
 B=[1 0 % 1 0 - задання швидкості ОВ
 0 0]; % 0 0
 G=[1 0 % G (P Greek) - вхідні збурення в системі - швидкість
 0 0];
C=[1 0]; % H - матриця вимірювань формує вихід з першого елементу вектора стану тобто поточної кординати
D=[0 0]; % вхід не впливає на вихід
H=[0 0]; % збурення на вихід не діють
[t1M,t1N] = size(t1);
U=zeros(t1M,2); % вхід=0
W=zeros(t1M,2); % Lamnda Greek=0 - збурення в системі швидкість і прискорення
V=zeros(t1M,1); % Delta Greek xi шум вимірювань координати
OV_CTime=ss(A,[B G],C,[D H]); % ОВ в просторі стану
% Моделювання неперервної системи 2-го порядку
if Type_V==1 % вхідна швидкість
 U(:,1)=v; U(:,2)=0; % швидкість та прискорення
end;
if Type_V==2 % вхідна швидкість
 U(:,1)=v; U(1:1000,1)=0; U(:,1)=lsim(tf([1],[Tu^22*Tu 1]),U(:,1),t1);
end;
if Type_V==3 % вхідна швидкість
 U(:,1)=v; U(1:500,1)=0; U(2000:2500,1)=0;
 for iii=1:500 U(iii,1)=v*iii/500; end;
 for iii=1:500 U(iii+2000,1)=v*(500-iii)/500; end; 
 U(:,1)=lsim(tf([1],[Tu^2 2*Tu 1]),U(:,1),t1);
end;
for i=2:t1M-1
 U(i,2)=(U(i+1,1)-U(i-1,1))/2/dt1; % прискорення
end;
U(t1M,2)=U(t1M-1,2);
W(:,1)=randn(t1M,1).*(Sigma_Dist_v); % Lamnda Greek=0 - збурення в системі швидкість
SysWx=tf([1],[TWx 1]);
Wx=lsim(SysWx,randn(t1M,1).*(Sigma_Dist_x),t1);
SysWv=tf([1],[TWv 1]); W(:,1)=lsim(SysWv,W(:,1),t1);
V=randn(t1M,1).*(SigmaNoise)+Wx; % шум вимірювань координати і збурення координати
[Y,t2,X]=lsim(OV_CTime,[U W],t1,Z0); % моделювання - вихід (координата) і вектор стану (координата і швидкість)
% figure; plot(t1,W(:,1),'g', t1,V,'m');
% title('Noise CTime System: W1=v g, V m');
% figure; plot(t2,Y,'k', t2,Y+V,'g', t2,U(:,1),'k',t2,U(:,1)+W(:,1),'g'); % для рівномірного руху
% title('Output and State Vector CTime System: Y=x, kY+V=x+v g, U1=v k, U1+W1=v+w g');
% Математична модель для вимірювань за відеозображеннями
% дискретна система в просторі стану
% x[n+1] = Ax[n] + Bu[n] + Gw[n]
% y[n] = Cx[n] + Du[n] + Hw[n] + v[n]
% U (відсутній) - вхід=0 Y (позначення в розділі 4xi*) - вихід=поточній координаті ОВ
% X (Z) - внутрішній стан системи координати швидкістьприсокрення
% W (Lamnda Greek) - збурення в системі координати швидкості прискорення - це ще 3 входи додатково до U
% V (Delta Greek x) - шум вимірювань виходу тобто поточної координати ОВ
% початковий стан системи координата швидкість прискорення
ZD0=zeros(1,FK_P);
for i=1:FK_P
 ZD0(i)=PARAM(i);
end;
% матриці системи в просторі стану
% А (F Greek) - неперевна модель рівноприсокреного руху ОВ
A_=zeros(FK_P, FK_P);
for i=1:FK_P
 for j=i:FK_P
 A_(i,j)=dt1^(j-i)/factorial(j-i);
 end;
end;
BD=zeros(FK_P,1); % вхід відсутній
GD=A_; % G (P Greek) - передача збурень в системі
CD=zeros(1,FK_P); CD(1)=1; % H - матриця вимірювань формує вихід з першого елементу вектора стану тобто поточної кординати
DD=[0]; % вхід відсутній
HD=zeros(1,FK_P); % збурення на вихід не діють
[t1M,t1N] = size(t1);
% Дискретний фільтр Калмана для відеозображень
ZD=zeros(FK_P,t1M); GI=zeros(FK_P,t1M);
P=zeros(FK_P,FK_P,t1M);
I3_3=zeros(FK_P,FK_P);
for i=1:FK_P
 I3_3(i,i)=1;
end;
ZD(:,1)=ZD0;
QN=zeros(FK_P,FK_P);
if FK_P>=1 QN(1,1)=Sigma_Dist_x^2; end;
if FK_P>=2 QN(2,2)=Sigma_Dist_v^2; end;
if FK_P>=3 QN(3,3)=Sigma_Dist_a^2; end;
P=zeros(FK_P,FK_P);
for i=1:FK_P
 for j=1:FK_P
 P(i,j)=ZD(i)*ZD(j);
 end;
end;
if FK_P>=1 P(1,1)=P(1,1)+Sigma_Dist_x0^2; end;
if FK_P>=2 P(2,2)=P(2,2)+Sigma_Dist_v0^2; end;
if FK_P>=3 P(3,3)=P(3,3)+Sigma_Dist_a0^2; end;
for i=2:t1M
 Pi1=A_*P(:,:,(i-1))*A_'+GD*QN*GD';
 GI(:,i)=Pi1*CD'*(CD*Pi1*CD'+SigmaNoise^2)^(-1);
 ZD(:,i)=A_*ZD(:,i-1)+GI(:,i)*(Y(i)+V(i)-CD*(A_*ZD(:,i-1)));
 P(:,:,i)=Pi1*(I3_3-GI(:,i)*CD);
end;
% експоненційне згладжування
ZDEKS=zeros(3,t1M);
for i=EKS_P+1:t1M
ZDEKS(1,i)=(1-ksi)*(Y(i)+V(i));% згладжування по координаті
%ZDEKS(2,i)=(1-ksi)*ZDDIFRV(2,i,jjj);%згладжування по швидкості
%ZDEKS(3,i)=(1-ksi)*ZDDIFRV(3,i,jjj);%згладжування по прискоренню

for j=1:EKS_P
 ZDEKS(:,i)=ZDEKS(:,i)-ksi.*(-1).^j.*EKS_P./j.*ZDEKS(:,i-j);
 end;
end;
% Оцінка похибок
DZDFull=zeros(3,t1M,size(Noc,1));
DZDEKS=zeros(3,t1M);
for j=1:size(Noc,1)
 DZDFull(1,:,j)=ZD(1,:)-Y'; DZDFull(2,:,j)=ZD(2,:)-U(:,1)'; DZDFull(3,:,j)=ZD(3,:)-U(:,2)';
end;
DZDEKS(1,:)=ZDEKS(1,:)-Y'; DZDEKS(2,:)=ZDEKS(2,:)-U(:,1)'; DZDEKS(3,:)=ZDEKS(3,:)-U(:,2)';
if Type_V==1
 DZD=zeros(3,FKMean+1,size(Noc,1));
 for j=1:size(Noc,1)
 DZD(:,:,j)=DZDFull(:,Noc(j):Noc(j)+FKMean,j);
 end;
else
 DZD=zeros(3,size(U,1)-510,size(Noc,1));
 for j=1:size(Noc,1)
 DZD(:,:,j)=DZDFull(:,Noc(j):size(U,1)-510+Noc(j)-1,j);
 end;
 DZD=DZDFull(:,510:size(U,1)-510,:);
end;
DZDEKS=DZDEKS(:,510:size(U,1)-510);
DZD_Max1=zeros(size(Noc,1),1);
DZD_Max2=zeros(size(Noc,1),1);
DZD_Max3=zeros(size(Noc,1),1);
DZD_Std1=zeros(size(Noc,1),1);
DZD_Std2=zeros(size(Noc,1),1);
DZD_Std3=zeros(size(Noc,1),1);
for j=1:size(Noc,1)
DZD_Max1(j)=max(abs(DZD(1,:,j)));
DZD_Max2(j)=max(abs(DZD(2,:,j)));
DZD_Max3(j)=max(abs(DZD(3,:,j)));
DZD_Std1(j)=std(DZD(1,:,j));
DZD_Std2(j)=std(DZD(2,:,j));
DZD_Std3(j)=std(DZD(3,:,j));
end;
DZDEKS_Max1=zeros(size(Noc,1),1);
DZDEKS_Max2=zeros(size(Noc,1),1);
DZDEKS_Max3=zeros(size(Noc,1),1);
DZDEKS_Std1=zeros(size(Noc,1),1);
DZDEKS_Std2=zeros(size(Noc,1),1);
DZDEKS_Std3=zeros(size(Noc,1),1);
DZDEKS_Max1(:)=max(DZDEKS(1,:));
DZDEKS_Max2(:)=max(DZDEKS(2,:));
DZDEKS_Max3(:)=max(DZDEKS(3,:));
DZDEKS_Std1(:)=std(DZDEKS(1,:));
DZDEKS_Std2(:)=std(DZDEKS(2,:));
DZDEKS_Std3(:)=std(DZDEKS(3,:));
% Теретичний розрахунок похибок
% Фільтр Калмана
DZD_StdTx=zeros(size(Noc,1),1);
DZD_StdTv=zeros(size(Noc,1),1);
DZD_StdTx(:)=sqrt(SigmaNoise*dt1*sqrt(2*Sigma_Dist_a*SigmaNoise+Sigma_Dist_v^2));
DZD_StdTv(:)=sqrt(Sigma_Dist_a*dt1*sqrt(2*Sigma_Dist_a*SigmaNoise+Sigma_Dist_v^2));
%DZD_StdTa(:)=0;
figure; % графіки параметрів руху для Noc=jjj
plot(t1,Y,'b',t1,Y+V,'g',t1,ZD(1,:),'r',t1,ZDEKS(1,:),'c');
xlabel('Час, с'); ylabel('Координата, мм');
grid on;
% графіки похибок параметрів руху
figure;
plot(Noc*dt1,DZDEKS_Std1,'m-',Noc*dt1,DZD_Std1,'b-.','LineWidth',3);
xlabel('Кількість відліків координати'); ylabel('СКЗпохибки, мм');
grid on;