//Blog P53 Example
//Example Mass Spring Damper
//JLJ 2014 wwww.basicaridata.eu
//Equilibrium position= step value/K
clear;
close;
timespan=60; //s
ts=1/5;// 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];
spec(A)
wn=(k/m)^0.5
MSP=syslin('c',A,B,C,D)
[y x]=csim(in*0,t,MSP,xzero)
//Position and velocity of mass
figure('Figure_name','Mass spring damper state estimation')
title("Transient figures  www.basicairdata.eu","fontsize",3)
f=gcf();
f.background = -2
subplot(231)
plot(t,y)
xlabel('time s')
ylabel('Real position m')
xgrid()
subplot(234)
xlabel('time s')
ylabel('Velocity m/s')
plot(t,x(2,:))
xgrid()
//Continuous observer
L=[0.4;0.4]
//Pole placement
//ppol(A,L,[-1,-1])
L=[2.2647059;2.4852941]
//ppol(A,L,[-0.5,-0.5])
L=[0.0359675;0.3293551 ]
//ppol(A,L,[-0.8,-0.8])
L=[1.030303;4.6565657]
//State observer continuous and discrete time
MSP_observer=syslin('c',(A-L*C),[L],C,[0])
MSP_observerd=cls2dls(MSP_observer,ts)
//ind=[in;y]
[yo xo]=csim(y,t,MSP_observer)
[xd]=ltitr(MSP_observerd.A,MSP_observerd.B,y)
//(y,MSP_observerd,xzero)
//[yod,xod]=flts(y,MSP_observerd,xzero)

subplot(232)
plot(t,yo)
plot(t,xd(1,:),'--r')
xlabel('time s')
ylabel('Estimated position, blue continuous time, red discrete time m')
xgrid()
subplot(235)
plot(t,xo(2,:))
plot(t,xd(2,:),'--r')
xlabel('time s')
ylabel('Estimated velocity, blue continuous time, red discrete time m')
xgrid()
//Plot of error from integration and from theory
MSP_error=syslin('c',(A-L*C),B,C,D)
[ye xe]=csim(0*y,t,MSP_error,xzero)
subplot(233)
plot(t,(y-yo))
plot(t,ye,'--r')
xlabel('time s')
ylabel('Estimator position error, blue simulated, red calculated m')
xgrid()
subplot(236)
plot(t,(x(2,:)-xo(2,:)))
plot(t,xe(2,:),'--r')
xlabel('time s')
ylabel('Estimator velocity error, blue simulated, red calculated m')
xgrid()