%% initial setup
N = 200;         % number of data
om = 10;         % omega = k/m in physics
dt = 0.01;       % smapling time \Delta t

Ac = [0 1; -om^2 0];  % continuous time matrix
A= expm(Ac*dt);       % discrete time matrix

gamma = 0;       % damping coefficient
Ac2=Ac;        
Ac2(2,2) =-gamma;% continous time system with damping
A2= expm(Ac2*dt);% discrete time dynamics with damping

x = [10;0];      % initial condition

%% data generation
X = zeros(2,N);
for n=1:N
    x = A2*x;
    X(:,n) = x;
%     if n==100;          % step change in the middle
%         x(1)=x(1)+20;
%     end
end
y = X(1,:) + 1*randn(1,N); % add random noise to observations

%% setup estimation
x0 =[0;0];         % prior value of x0
P0 =10*eye(2);

KF.A=A;
KF.C=[1,0];
KF.Q=diag([0.,0]); % dispersion of the model -- should be changed!
KF.R=1;             % variance of the measurement
KF.x=x0;
KF.P=P0;

Xe=zeros(2,N);      % call kalman filter on the measured data
for n=1:N
    KF = kalman(KF,y(n));
    Xe(:,n) = KF.x; % store results 
end



%
fig=figure(1);
hold off
plot(X(1,:)')
hold on
plot(y,'x')
plot(Xe(1,:),'r')
title(['Q=[' num2str(KF.Q(1,1))  ','  num2str(KF.Q(2,2)) '], R=1, P_0=I, x_0=[' num2str(x0(1)) ',' num2str(x0(2)) ']' ])
legend('true x', 'estimated x')
% 
% % %%
% fig.PaperUnits = 'inches';
% fig.PaperPosition = [0 0. 4 3];
% fig.PaperSize = [4 3];
% print(fig,['sine2_g' [num2str(gamma)] '_q1' num2str(KF.Q(1,1)) '_q2' num2str(KF.Q(2,2)) ],'-dpdf')