State Estimator/Controller Example: Cart-Pole


State estimators and controllers can interact badly

Here is Matlab code for a continous time linearized model of a cart pole.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% state = [cart-position 
%          pendulum-angle
%          cart-velocity
%          pendulum-angular-velocity]
% xdot = A*x + B*u
% y = C*x + D*u

A = [ 0 0 1 0
      0 0 0 1
      0 0 0 0
      0 10 0 0 ]

B = [ 0
      0
      1
      1 ]

C = [ 1 1 0 0 ]

D = [ 0 ]

sys1 = ss( A, B, C, D )

pzmap( sys1 )

[mypoles,myzeros] = pzmap( sys1 )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

This results in:

mypoles =
    3.1623
   -3.1623
         0
         0
myzeros =
   -2.2361
    2.2361

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


Now let's design an LQR controller. Use simple choices for Q and R, which are definitely not the best choices.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Qc = eye(4)

Rc = eye(1)

[kc,sc,ec] = lqr(A,B,Qc,Rc)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

This results in:

kc =
   -1.0000   33.0796   -2.3698   10.5036


sc =
    2.3698  -10.5036    2.3080   -3.3080
  -10.5036  174.5317  -21.5835   54.6631
    2.3080  -21.5835    4.4190   -6.7888
   -3.3080   54.6631   -6.7888   17.2925


ec =
  -3.6999
  -2.7061
  -0.8639 + 0.5024i
  -0.8639 - 0.5024i

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulate the Cart

sys3 = ss( A - B*kc, B, eye(4), D )

x0 = [ 1 0 0 0 ]'

tt = 0:0.01:6;

uu = zeros( size(tt) );

[y,t] = lsim(sys3,uu,tt,x0);

f = -kc*y';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
plot(y(:,1))
title('cart position')
xlabel('centiseconds')        
ylabel('m')

plot(y(:,2))
title('pendulum angle')
xlabel('centiseconds')        
ylabel('radians')

plot(y(:,3))
title('cart velocity')
xlabel('centiseconds')        
ylabel('m/s')

plot(y(:,4))
title('pendulum angular velocity')
xlabel('centiseconds')        
ylabel('r/s')

plot(f)
title('force on cart')
xlabel('centiseconds')        
ylabel('arbitrary units')


Let's assess robustness by using the same gains Kc, but changing parameters in A and B.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test robustness of LQR gains by increasing mgl/I

A = [ 0 0 1 0
      0 0 0 1
      0 0 0 0
      0 10 0 0 ]

newplot;
hold on;
for v = 10:1.0:23
          A(4,2) = v;
 sys4 = ss( A-B*kc, B, C, D );
 pzmap( sys4 );
end
hold off;

title('Pole-Zero Map: A(4,2) = 10:1.0:23')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

We can increase mgl/I by a factor of 2.3.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test robustness of LQR gains by decreasing mgl/I

A = [ 0 0 1 0
      0 0 0 1
      0 0 0 0
      0 10 0 0 ]

newplot;
hold on;
for v = 10:-1.0:1
          A(4,2) = v;
 sys4 = ss( A-B*kc, B, C, D );
 pzmap( sys4 );
end
hold off;

title('Pole-Zero Map: A(4,2) = 10:-1.0:1')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

We can decrease mgl/I by a factor of 10, at least.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test robustness of LQR gains by decreasing the cart mass

A = [ 0 0 1 0
      0 0 0 1
      0 0 0 0
      0 10 0 0 ]

B = [ 0
      0
      1
      1 ]

newplot;
hold on;
for v = 1:-0.1:0.5
 B(3,1) = v;
 B(4,1) = v;
 sys4 = ss( A-B*kc, B, C, D );
 pzmap( sys4 );
end
hold off;

title('Pole-Zero Map: B = 1:-0.1:0.5')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Looks like we can decrease mass by about a factor of 2.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test robustness of LQR gains by increasing the cart mass

B = [ 0
      0
      1
      1 ]

newplot;
hold on;
for v = 1:1.0:10
 B(3,1) = v;
 B(4,1) = v;
 sys4 = ss( A-B*kc, B, C, D );
 pzmap( sys4 );
end
hold off;

title('Pole-Zero Map: B = 1:1.0:10')

% zoom in on origin
axis([-5 1 -1 1])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Looks like we remain stable with a factor of 10 increase in mass.

Zoom in on origin.


Now let's design an observer. Again, use simple choices for Q and R, which are definitely not the best choices.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now design observer

Cf = [ 1 0 0 0
      0 1 0 0 ]

Qf = eye(4)

Rf = eye(2)

% You can use the duality of LQR and Kalman Filter design:
[kf,sf,ef] = lqr(A',Cf',Qf,Rf)

% Or you can use the overly complicated Matlab Kalman design routine:
G = eye(4)
D = zeros(2,1)
H = zeros(2,4)
sys6 = ss(A,[B G],C,[D H]);
[kest,kf,sf] = kalman(sys6,Qf,Rf)

% To crudely try to move the poles to the right, I will scale Q and R
s = 1
[kf,sf,ef] = lqr(A',C',s*Qf,Rf/s)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

This results in

s = 1
kf =
    1.7321   -0.0000    1.0000   -0.0000
   -0.0000    6.4109   -0.0000   20.0499
sf =
    1.7321   -0.0000    1.0000   -0.0000
   -0.0000    6.4109   -0.0000   20.0499
    1.0000   -0.0000    1.7321   -0.0000
   -0.0000   20.0499   -0.0000   64.4288
ef =
  -0.8660 + 0.5000i
  -0.8660 - 0.5000i
  -3.6799
  -2.7310

s = 10
kf =
   10.9545   -0.0000   10.0000   -0.0000
   -0.0000   12.1772    0.0000   24.1421
sf =
    1.0954   -0.0000    1.0000   -0.0000
   -0.0000    1.2177    0.0000    2.4142
    1.0000    0.0000   10.9545   -0.0000
   -0.0000    2.4142   -0.0000   17.2212
ef =
   -9.9494
   -1.0051
  -10.8770
   -1.3002

s = 1000000
ef =
  -999999.999999500
  -1000000.000009500
  -1.000000000
  -1.000000000

Note that in the above changing the ratio of Qf and Rf did not move all the poles to the left.


Now combine the state estimator and controller.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now see if workable combination exists

AA = [ A -B*kc
kf'*Cf A-B*kc-kf'*Cf ]

BB = [ B
       B ]

CC = [ 1 1 0 0 0 0 0 0 ]

DD = [ 0 ]

sys2 = ss( AA, BB, CC, DD )

pzmap( sys2 )

[p,z] = pzmap( sys2 )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

This results in the following pole/zero map

Now we are going to plot the same thing at the same scale of some future plots.


Okay, now we introduce modeling error.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This is all setup

% Model dynamics
Am = A;
Bm = B;

AA = [ A -B*kc
kf'*Cf Am-Bm*kc-kf'*Cf ]

BB = [ B
       Bm ]

CC = [ 1 1 0 0 0 0 0 0 ]

DD = [ 0 ]

sys3 = ss( AA, BB, CC, DD )

[p,z] = pzmap( sys3 )

pzmap( sys3 )
axis([-7 3 -5 5])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


Let's actually do a sweep of different pendulum dynamics in both directions.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% cart2.jpg

% reset true dynamics
A = [ 0 0 1 0
      0 0 0 1
      0 0 0 0
      0 10 0 0 ]

B = [ 0
      0
      1
      1 ]

newplot;
hold on;
for v = 10:0.1:12
          A(4,2) = v;
 AA = [ A -B*kc
          kf'*Cf Am-Bm*kc-kf'*Cf ];
 BB = [ B
        Bm ];
 sys3 = ss( AA, BB, CC, DD );
 pzmap( sys3 );
end
hold off;

title('Pole-Zero Map: A(4,2) = 10:0.1:12')
axis([-7 3 -5 5])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

So increasing mgl/I drives the system unstable.


Now for decreasing mgl/I.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% cart3.jpg

% reset true dynamics
A = [ 0 0 1 0
      0 0 0 1
      0 0 0 0
      0 10 0 0 ]

B = [ 0
      0
      1
      1 ]

newplot;
hold on;
for v = 10:-0.5:1
          A(4,2) = v;
 AA = [ A -B*kc
          kf'*Cf Am-Bm*kc-kf'*Cf ];
 BB = [ B
        Bm ];
 sys3 = ss( AA, BB, CC, DD );
 pzmap( sys3 );
end
hold off;

title('Pole-Zero Map: A(4,2) = 10:-0.5:1')
axis([-7 3 -5 5])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

This drove poles to the origin, but not unstable.


Now let's play with the mass of the cart.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% cart4.jpg

% reset true dynamics
A = [ 0 0 1 0
      0 0 0 1
      0 0 0 0
      0 10 0 0 ]

B = [ 0
      0
      1
      1 ]

newplot;
hold on;
for v = 1:-0.01:0.85
 B(3,1) = v;
 B(4,1) = v;
 AA = [ A -B*kc
          kf'*Cf Am-Bm*kc-kf'*Cf ];
 BB = [ B
        Bm ];
 sys3 = ss( AA, BB, CC, DD );
 pzmap( sys3 );
end
hold off;

title('Pole-Zero Map: B = 1:-0.01:0.85')
axis([-7 3 -5 5])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Didn't take much decrease of the mass of the cart (1 to 0.85) to cause the system to go unstable.


Let's try increasing the mass of the cart.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% cart5.jpg

% reset true dynamics
A = [ 0 0 1 0
      0 0 0 1
      0 0 0 0
      0 10 0 0 ]

B = [ 0
      0
      1
      1 ]

newplot;
hold on;
for v = 1:0.1:2
 B(3,1) = v;
 B(4,1) = v;
 AA = [ A -B*kc
          kf'*Cf Am-Bm*kc-kf'*Cf ];
 BB = [ B
        Bm ];
 sys3 = ss( AA, BB, CC, DD );
 pzmap( sys3 );
 % [p,z] = pzmap( sys3 );
 % p
end
hold off;

title('Pole-Zero Map: B = 1:0.1:2')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Doubling the mass of the cart drives the system unstable.