# 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 ]

% we combine cart-position and pendulum-angle to keep this a SISO system.
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')
```

```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:
[kft,sf,ef] = lqr(A',Cf',Qf,Rf)
% This produces the transpose of the Kalman gain: kft = kf'

% 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],Cf,[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
[kft,sf,ef] = lqr(A',Cf',s*Qf,Rf/s)

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

This results in

s = 1
kft =
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
kft =
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.