How to implement a Kalman regulator in the control of classical pendulum problem

My final purpose is to realize an LQG control of inverted pendulum.

To this end, my first step I think is to build a Kalman regulator in Mathematica. The original Matlab version of Kalman filter is written by Steve Brunton. I try to follow his code, but I am stuck when using the Mathematica command KalmanEstimator which complains “The number of columns in * is not equal to the length of * ”.

Any suggestions will be greatly appreciated.

Thanks.

My Mathematica code is below.

Remove["Global`*"] // Quiet;  m = 1; M = 5; L = 2; g = -10; d = 1;  s = -1; (* pendulum up: s=1 *)  (* y={x[t], x'[t], \[Theta][t], \[Theta]'[t]} *) \[DoubleStruckCapitalA] = {{0, 1, 0, 0}, {0 , -d/M, -m g/M, 0}, {0, 0,0, 1}, {0, - ((s d )/(M L)), -((s (m + M) g)/(M L)), 0}}; \[DoubleStruckCapitalB] = {{0, 1/M, 0, s/(M L)}}\[Transpose]; \[DoubleStruckCapitalC] = {{1, 0, 0, 0}}; \[DoubleStruckCapitalD] = ConstantArray[0, {Dimensions[\[DoubleStruckCapitalC]][[1]], Dimensions[\[DoubleStruckCapitalB]][[2]]}];   (*Augment system with disturbances and noise*) vd = 0.1  IdentityMatrix[4]; (* disturbance covariance *) vn = 1 IdentityMatrix[1];(*disturbance covariance*)  (* augment inputs to include disturbance and noise *) \[DoubleStruckCapitalB]F = ArrayFlatten[{{\[DoubleStruckCapitalB], vd, 0 \[DoubleStruckCapitalB]}}];  sysC = StateSpaceModel[{\[DoubleStruckCapitalA], \[DoubleStruckCapitalB]F, \[DoubleStruckCapitalC], ArrayFlatten[{{0, 0, 0, 0, 0, vn}}]}, SystemsModelLabels -> {{"u", "-", "-", "-", "disturbance", "noise"}, {"x"}, {"x", "dx", "\[Theta]", "d\[Theta]"}}] sysFullOutput = StateSpaceModel[{\[DoubleStruckCapitalA], \[DoubleStruckCapitalB]F, IdentityMatrix[4], ConstantArray[0, {4, Dimensions[\[DoubleStruckCapitalB]F][[2]]}]}];  KalmanEstimator[{sysC, All, {1, 2, 3, 4(*dInputs*)}}, {vd, vn}]

Maybe the matlab code and its result is helpful, so I also pasted them here.

clear all, close all, clc m = 1; M = 5; L = 2; g = -10; d = 1; s = -1; % pendulum up (s=1) % y = [x; dx; theta; dtheta]; A = [0 1 0 0;     0 -d/M -m*g/M 0;     0 0 0 1;     0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0]; B = [0; 1/M; 0; s*1/(M*L)]; C = [1 0 0 0];   D = zeros(size(C,1),size(B,2)); %%  Augment system with disturbances and noise Vd = .1*eye(4);  % disturbance covariance Vn = 1;       % noise covariance BF = [B Vd 0*B];  % augment inputs to include disturbance and noise sysC = ss(A,BF,C,[0 0 0 0 0 Vn]);  % build big state space system... with single output sysFullOutput = ss(A,BF,eye(4),zeros(4,size(BF,2)));  % system with full state output, disturbance, no noise %%  Build Kalman filter [L,P,E] = lqe(A,Vd,C,Vd,Vn);  % design Kalman filter Kf = (lqr(A',C',Vd,Vn))';   % alternatively, possible to design using "LQR" code sysKF = ss(A-L*C,[B L],eye(4),0*[B L]);  % Kalman filter estimator %%  Estimate linearized system in "down" position (Gantry crane) dt = .01; t = dt:dt:50; uDIST = randn(4,size(t,2)); uNOISE = randn(size(t)); u = 0*t; u(100:120) = 100;     % impulse u(1500:1520) = -100;  % impulse uAUG = [u; Vd*Vd*uDIST; uNOISE]; [y,t] = lsim(sysC,uAUG,t); [xtrue,t] = lsim(sysFullOutput,uAUG,t); [x,t] = lsim(sysKF,[u; y'],t); plot(t,xtrue,'-',t,x,'--','LineWidth',2) figure plot(t,y) hold on plot(t,xtrue(:,1),'r') plot(t,x(:,1),'k--')

matlab result Mathematica Error