%Рассмотрим дискретную модель объекта вида
%x[n+1]=Ax[n]+B(u[n]+w[n]);
%y[n]=Cx[n];
%с аддитивным гауссовым шумом W[n]по входу и следующими данными:
A = [0, -0.01; 0.01, 0];  
B = [1,1]';
C=[1,0];
D=[0];
%Задача заключается в том, чтобы спроектировать фильтр Калмана для оценки
%вектора выхода y[n] при условии, что известны вход u[n] и измерения 
%выхода  ,y[n],замешанные с шумом v[n].
%yv[n]=Cx[n]+v[n],

%где v[n]- вектор последовательностей гауссова "белого шума".
%Это можно реализовать с помощью следующего оператора, установив   что
%Ts=-1 означает ввод дискретной модели
%
%
Plant = ss(A,[B B],C,0,-1 ,'inputname',{'u' 'w'},'outputname','y')  ;
%Полагая
Q=1; R=1;
%синтезируем фильтр Калмана, применяя оператор
[kalmf, L, P, M]= kalman(Plant,Q,R)
pause
%На выходе оператора получаем ss-модель фильтра Калмана kalmf и обновленную
%матрицу коэффициентов М:
M 
pause
%Входами фильтра Калмана являются сигналы u и yv , а выходами – вектор 
%выхода ye и оценка вектора состояния x  (рис.1).

%Фильтр Калмана

%Поскольку интерес представляет только оценка вектора выхода, сохраним 
%только первую строку ss–объекта  kalmf:
kalmf=kalmf(1,:)
pause
% Для того,чтобы убедиться, как функционирует фильтр, необходимо 
%сгенерировать входные сигналы и шумы и сравнить выход объекта y с выходом
%фильтра ye . Можно сгенерировать каждую реакцию отдельно либо 
%сгенерировать их вместе. Для моделирования раздельных реакций следует 
%использовать функцию lsim для моделирования сначала только объекта,
%а затем объединения объекта и фильтра.
%Совместное моделирование поясняется структурной схемой (рис.2).
%В соответствии с этой структурной схемой можно сформировать ss-модель,
%используя функции parallel  и feedback. Сначала построим модель объекта P
%(рис.3), входами которой являются сигналы u, w, v, а выходами – 
%измерения y и yv :
a=A;  
b=[B B 0*B]; 
c=[C;C]; 
d=[0 0 0; 0 0 1];  
P = ss(a,b,c,d,-1,'inputname',{'u' 'w' 'v'},'outputname',{'y' 'yv'}); 
sys=parallel(P,kalmf,1,1,[],[]);
pause
%Соединить вход #4 с выходом #2
SimModel=feedback(sys, 1,4,2,1);
pause
%Удалить yv из списка входов-выходов
SimModel = SimModel([1 3], [1 2 3]);  
SimModel.inputname   
 SimModel.outputname 
 pause
 %Теперь можно промоделировать поведение фильтра Калмана. Сгенерируем
 %входной сигнал     и векторы шумов w, v: 
 t=[0:100]';
u=sin(t/5);
n=length(t)

randn('seed',0)
w=sqrt(Q)*randn(length(t),1);
v=sqrt(R)*randn(length(t),1);
%Промоделируем с помощью функции lsim:
[out,x]=lsim(SimModel,[w,v,u]);  

y=out(:,1);  
ye=out(:,2);  
yv=y+v;
%и сравним выходы объекта и фильтра на графике:
subplot(211),plot(t,y,'-',t,ye,'-'),
xlabel('Количество измерений'),ylabel('Выход')
subplot(212),plot(t,y-yv,'-',t,y-ye,'-'),
xlabel('Количество измерений'),ylabel('Ошибка')
%На первом графике рис.4 показан выход объекта (пунктирная линия) и выход
%фильтра (сплошная линия); на втором графике показаны ошибка измерения 
%(пунктирная линия) и ошибка оценки (сплошная линия). Из графиков следует,
%что уровень шума после фильтрации существенно уменьшился. Это
%подтверждается и оценкой дисперсий:
pause
MeasErr=y-yv;
MeasErrCov=sum(MeasErr.*MeasErr)/length(MeasErr)
EstErr=y-ye;
EstErrCov=sum(EstErr.*EstErr)/length(EstErr) 
pause
%Дисперсия ошибки оценки почти в четыре раза меньше погрешности измерения.