Discrete State Space - Inverted Pendulum

Hello, I am looking for help in order to best build a simulink model of the closed loop system, including the state feedback control law, observer dynamics and the Nbar gain.

The model i have created so far is shown below along with my code. I am looking to simulate the response however cannot get the correct response even though the gain values i calculated are controllable.

By looking at my code would anyone be able to tell me what varibles are required in what block of the simulink model for correct operation? Please see below.

%% Linearised Dynamic Model of Rotary Inverted Pendulum

%% Motor % Resistance Rm = 8.4; % Current-torque (N-m/A) kt = 0.042; % Back-emf constant (V-s/rad) km = 0.042; % %% Rotary Arm % Mass (kg) Mr = 0.095; % Total length (m) Lr = 0.085; % Moment of inertia about pivot (kg-m^2) Jr = Mr*Lr^2/12; % Equivalent Viscous Damping Coefficient (N-m-s/rad) Dr = 0; % %% Pendulum Link % Mass (kg) Mp = 0.024; % Total length (m) Lp = 0.129; % Moment of inertia about pivot (kg-m^2) Jp = Mp*Lp^2/12; % Equivalent Viscous Damping Coefficient (N-m-s/rad) Dp = 0; % Gravity Constant g = 9.81;

% State Space Representation Jt = Jr*Jp + Mp*(Lp/2)^2*Jr + Jp*Mp*Lr^2; A = [0 0 1 0; 0 0 0 1; 0 Mp^2*(Lp/2)^2*Lr*g/Jt -Dr*(Jp+Mp*(Lp/2)^2)/Jt -Mp*(Lp/2)*Lr*Dp/Jt; 0 Mp*g*(Lp/2)*(Jr+Mp*Lr^2)/Jt -Mp*(Lp/2)*Lr*Dr/Jt -Dp*(Jr+Mp*Lr^2)/Jt];

B = [0; 0; (Jp+Mp*(Lp/2)^2)/Jt; Mp*(Lp/2)*Lr/Jt]; C = eye(2,4); D = zeros(2,1);

% Add actuator dynamics B = kt * B / Rm; A(3,3) = A(3,3) - kt*kt/Rm*B(3); A(4,3) = A(4,3) - kt*kt/Rm*B(4);

% Load into state-space system RPsys = ss(A,B,C,D); % %% Start of CW

State_Space_Model = c2d(RPsys, 1/15);

figure step(State_Space_Model);

stability = isstable(State_Space_Model);

figure zgrid(0.517, 22.224); grid on;

CLPZ1 = 0.1379 + i*0.4437; CLPZ2 = 0.1379 - i*0.4437; CLPOrigin1 = 0.05 + i*0.05; CLPOrigin2 = 0.05 - i*0.05; ObserverPole1 = 0.25*(CLPZ1); ObserverPole2 = 0.25*(CLPZ2); ObserverPole3 = 0.25*(CLPOrigin1); ObserverPole4 = 0.25*(CLPOrigin2);

[F,G,H,J] = ssdata(State_Space_Model);

K = place(F, G, [CLPZ1,CLPZ2, CLPOrigin1, CLPOrigin2]); L = place(F', H', [ObserverPole1,ObserverPole2, ObserverPole3, ObserverPole4])';

I = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];

 H1 = [1 0 0 0];
 J1 = 0;

 N =(inv([(F-I) G;
      H1 J1]));

N1 = N*[0; 0; 0; 0; 1]; Nx = zeros(4, 1); for count = 1:4 Nx(count, 1) = N1(count, 1); end Nu = N1(5, 1);

Nbar = Nu + (K*Nx);

%Task 8

zerofill1 = zeros(4,4);

zerofill2 = zeros(4,1);

Overall_CL1 = [ (F-(G*K)) (G*K); (zerofill1) (F-(L*H)) ];

Overall_CL2 = [(G*Nbar); zerofill2];

Overall_CL3 = [(H-(J*K)) (J*K)];

Overall_CL4 = (J*Nbar);

Controlability = ctrb(Overall_CL1, Overall_CL2);

Cx = eye(4); Dx = zeros(4,1); Rank = rank(Controlability);

Many thanks,

Andrew