%%Observer
%%Blog P53 Example
%%Example Mass Spring Damper
%%JLJ 2014 wwww.basicaridata.eu
%%Equilibrium position= step value/K
clear all;
close;
timespan=60; %%s
ts=1/10;%% tsample
nosteps=timespan/ts
k=0.3; %%N/m
c=0.2; %%Ns/m
m=1; %%kg
A=[0 1;-k/m -c/m];
B=[0;1/m];
C=[1 0];
D=[0];
xzero=[1;0];
t=linspace(0,timespan,nosteps)
in=ones(1,nosteps);
in(1)=0
O=[C;C*A];
eig(A)
wn=(k/m)^0.5
MSP=ss(A,B,C,D);
[y,t,x]=lsim(MSP,in*0,t,xzero);
figure('name','Mass Spring Damper system, State estimator','numbertitle','off')
%%Position and velocity of mass

subplot(231)
plot(t,y)
grid
subplot(234)
plot(t,x(:,2))
grid
%%Continuous observer and
%%discrete time observer is calculated
L=[0.5;0.5]
MSP_observer=ss((A-L*C),[L],C,[0])
MSP_observerd=c2d(MSP_observer,ts,'zoh')
MSP_observerdt=c2d(MSP_observer,ts,'tustin')
[yo,to,xo]=lsim(MSP_observer,y,t)
[yod,tod,xod]=lsim(MSP_observerd,y)
subplot(232)
plot(t,yo)
hold on 
plot(t,yod,'--r')
grid
subplot(235)
plot(t,xo(:,2))
hold on
plot(t,xod(:,2),'--r')
grid
%%Plot of error from integration and from theory

MSP_error=ss((A-L*C),B,C,D)
[ye,te,xe]=lsim(MSP_error,0*y,t,xzero)
subplot(233)
plot(t,(y-yo))
hold on;
plot(t,ye,'--r')
grid
subplot(236)
plot(t,(x(:,2)-xo(:,2)))
hold on;
plot(t,xe(:,2),'--r')
grid()