% This program controls an inverted pendulum. A Kalman filter is used 
% for state estimation.

% Written by John L. Crassidis 9/13

% Other Required Routines: none

% Parameters
M=2;m=0.1;Mp=M+m/4;ell=0.1;c=2;g=9.81;

% Time
dt=0.01;tf=2;
t=[0:dt:tf]';m=length(t);

% State Model
a=[0 0 1 0;0 0 0 1;0 -3*m*g/(4*Mp') -c/Mp 0;0 3*(M+m)*g/(4*Mp*ell) 3*c/(4*Mp*ell) 0];
b=[0;0;1/Mp;-3/(4*Mp*ell)];c=[1 0 0 0;0 1 0 0];d=[0;0];
[phi,gam]=c2d(a,b,dt);

desired_pole=c2d(-10,1,dt);
d2=conv([1 -desired_pole],[1 -desired_pole]);
d4=conv(d2,d2);
gain=[0 0 0 1]*inv([gam phi*gam phi^2*gam phi^3*gam])*(d4(1)*phi^4+d4(2)*phi^3+d4(3)*phi^2+d4(4)*phi+d4(5)*eye(4));

% Initial Conditions for True System
x0=[0 10*pi/180 0 0]';
x=zeros(m,4);x(1,:)=x0';u=zeros(m,1);

% Initial Conditions for Kalman Filter
xe=zeros(m,4);xe(1,:)=x0';p=diag([1e-4^2 (0.15*pi/180)^2 0.1^2 (0.1*pi/180)^2]); 

% Standard Deviation of Noise
sig_pos=0.001;sig_ang=0.001*pi/180;r=diag([sig_pos^2 sig_ang^2]);
h=[1 0 0 0;0 1 0 0];

% Main Loop
for i = 1:m-1
% Measurements
y=[x(i,1)+sig_pos*randn(1);x(i,2)+sig_ang*randn(1)];
    
% Update State and Covariance
gain_kal=p*h'*inv(h*p*h'+r);
xe(i,:)=xe(i,:)+(gain_kal*(y-h*xe(i,:)'))';
p=(eye(4)-gain_kal*h)*p;

% Controller Using Estimated Quantities
u(i)=-gain(1)*xe(i,1)-gain(2)*xe(i,2)-gain(3)*xe(i,3)-gain(4)*xe(i,4);

% True State 
x(i+1,:)=(phi*x(i,:)'+gam*u(i))';

% Propagate Estimate and Covariance 
xe(i+1,:)=(phi*xe(i,:)'+gam*u(i))';
p=phi*p*phi';

end

% Final Control Point
u(i+1)=-gain(1)*xe(i+1,1)-gain(2)*xe(i+1,2)-gain(3)*xe(i+1,3)-gain(4)*xe(i+1,4);
 
% Plots
plot(t,x(:,2)*180/pi)
set(gca,'fontsize',12)
axis([0 2 -5 10])
set(gca,'ytick',[-5 0 5 10])
ylabel('Angle (Deg)')
xlabel('Time (Sec)')

pause

plot(t,x(:,1))
set(gca,'fontsize',12)
axis([0 2 -0.1 0.5]) 
set(gca,'ytick',[-0.1 0 0.1 0.2 0.3 0.4 0.5])
ylabel('Position (m)')
xlabel('Time (Sec)')