Lab 12b: LQG on the Inverted Pendulum on a Cart

Objective

The purpose of this lab is to familiarize myself with the Kalman Filter by simulating it with an inverted pendulum on a cart from Lab 11b.

Kalman Filter

Kalman filter function:
- As shown in the code:
```
Ad = scipy.linalg.expm(P.A*P.T_update)
Bd = P.B*P.T_update
                                
```
  The A and B matrices are updated using T_update which is defined as the controller and estimator update rate in seconds. Using this update rate, the A and B matrices are changed for a discrete system from a continuous system to be used with the Kalman filter.
- Considering that the states are:
```
x = [z, zdot, theta, thetadot]
                                
```
  The C matrix below shows that we are only measuring the fourth position, where the the state is thetadot, whereas, the rest of the states are being estimated.
```
C = np.matrix([[0.0, 0.0, 0.0, 1.0]]) 
                                
```
Initially the code would not run and would give me an error regarding np.sin(theta), but the errors were gone once I replaced theta with theta[0] as suggested on campuswire. Once I made these changes, the behavior is as expected (as shown below) and the KF performs the state estimation properly:

As shown above, the system is stable.
To find the observability matrix, I imported the control library and used control.obsv:
```
 
import control
print("O")
O = control.obsv(Ad,P.C)
print(O)
print("Rank")
print(np.linalg.matrix_rank(O))
                        
```
If you use the obsv(A,C)-command with this system, one will find that it is actually not observable since the rank(observability matrix) is not equal to n = 3 (should be 4):

As shown in the observablity matrix above, the z state is the one that is unobservable. we are simulating close to a perfect system so the Kalman filter can estimate states properly and since z is one of the states being estimated, the system is still stable.

Add imperfections and adjust the Kalman Filter

Please note that after a noise is added, it is not removed for the next step so we can continue testing the Kalman filter under all conditions.

Adding discrepancy to the initial state

To add this discrepancy to the intial state, I altered the inputs to the line: states = [np.array([[P.z0], [P.zdot0], [P.theta0], [P.thetadot0]])] in runSimulation.py. I started by adding an offset value of 0.1 to each of the states and incremented by 0.2, up to a value of 0.5.
Format:


offset = 0.5
states = [np.array([[P.z0+offset], [P.zdot0+offset], [P.theta0+offset], [P.thetadot0+offset]])]

Offset = 0.1
The system is stable for this offset value, the time to stabilize has not increased a significant amount.
Offset = 0.3
The system is still stable for this offset value, but the time to stabilize has increased a significant amount and the cart ends up leaving the screen for a small period of time but returns to stabilize.
Offset = 0.5
The system is still stable for this offset value, however,the time to stabilize has increased a lot and the cart leaves the screen for a longer period of time after which it returns to stabilize. This increase in settling time makes sense considering that the intial theta value is being increased and the cart takes time responding to this.

I stopped at this value because when I increased the offset any higher, the cart would jump out of the screen and the plotting method would start freaking out by opening multiple figures and the cart would disappear.

Saturation and Deadband

I picked deadband and saturation values using the following relations:
deadband = motordeadband/255*acceleration*mass = (75/255)*(3.33)*(0.46) = 0.45
Saturation = mass * acceleration = 0.46*3.33 = 1.532

To add saturation and deadband to u, I altered the following code in runSimulation.py:


u = -Kr.dot(mu-des_state)

# Adding a realistic saturation value and deadband to u
saturation = 1.532
deadband = 0.45

if u > 0:
    if u < deadband:
        u = 0
    elif u > saturation:
        u = saturation
else:
    if abs(u) < deadband:
        u = 0
    elif abs(u) > saturation:
        u = -1*saturation

As shown below, this resulted in a stable system with the inverted pendulum being stabilized relatively quickly which proves that kalman filter is still able to estimate the states properly:

Process and Measurement Noise

Process Noise

The process noise in the simulation is added by uncommenting the following lines in pendulumNonLinearDynamics.py:


dydt = [ydot0 + np.random.randn()*0.01, ydot1 + np.random.randn()*0.01, ydot2 + np.random.randn()*0.01, ydot3 + np.random.randn()*0.01]

As shown below, even with the process noise, the system is still stable but the settling time of the system is relatively higher since the system reacts slower. :

Measurement Noise

To add the measurement noise, I tried to change sigma = np.eye(4)*0.00001 but the noise did not make any difference so then I changed the following code to add random noise to the sensor measurements:


#Update controller and sensors every  seconds
if (t % P.T_update) < dt:
    u=-Kr.dot(mu-des_state)
    y_kf = P.C.dot(old_state) + np.random.normal(0,0.1)
    mu,sigma = kalmanFilter(mu,sigma,u,y_kf)

I added noise based on a gaussian (normal) distribution with mean = 0, and variance = 0.1. I chose these parameters as to not make the system unstable while also applying a reasonable and realistic measurement noise to the system. As shown below, the system was still stable, however, the performance was worse than before as expected:

Adjusting the parameters of the A and B matrices

The values of A and B were adjusted in Kalmanfilter.py as shown below:


factor = 1.0001
Ad = scipy.linalg.expm(P.A*P.T_update)*factor
Bd = P.B*P.T_update*factor

Initially, I adjusted all the values by multiplying them by factor of 1.0001 from the original and the system was stable as shown below:

Then I increased the factor by a magnitude higher: factor = 1.001 which was an unstable system as shown below. The inverted pendulum fell to the left since the cart was not able to accelerate quick enough to stabilize it.

Then I chose a factor value in between: 1.0005 ,which also failed, I repeated the process until a factor of 1.0002 provided a stable system as shown below which I believe is the maximum I could fugde up to before the system becomes unstable. The cart moves a lot but the system is still stable:

Changing the update time of the sensor and the controller (T_update)

I changed T_update in pendulumParam.py to the following values:

T_update = 0.005 s
Result = Stable system
T_update = 0.01 s
Result = Stable system
T_update = 0.03 s
Result = Stable system
T_update = 0.04 s
Result = Stable system
T_update = 0.05 s
Result = Unstable system

These test show that the maximum I can increase the value of T_update is 0.04s before the system becomes unstable.

Real robot

I do not believe that the real robot will be able to stabilize the pendulum because even though the Kalman filter does a great job at stabilizing the system with the added simulated process and measurement noise, or with imperfect initial conditions. There are still factors that would need be to considered in the real robot that have not been considered in the simulation. One of which is the motion of the robot in the 3rd dimension. We are assuming that the robot will always travel in a straight line which is not always the case. There are methods to make the motion of the robot as straight as possible, however, that depends on the ability of the robot to rotate and in previous labs, I found this issue challenging and the solution was not completely perfect. On another note, the frictional components have not be considered: ex. between the pendulum and the cart, or the cart and the floor. The lack of friction may also work against the controller, i.e. if the robot tries to stop and slips, it may cause a delay in the response. Finally, the robot may have to make small motions to stabilize the system which I have experienced to be a difficult task.

[Extra Credit] Changing the measurements

First I changed the C matrix such that it measures both z and thetadot, and then I changed the noise to be a 2x1 rather than a 1x1.


C = np.matrix([[1.0, 0.0, 0.0, 0.0],[0.0, 0.0, 0.0, 1.0]])

After changing these values, I still had to change the code such that I will only measure thetadot at 100Hz and z at 10 Hz and so I changed T_update to 0.01 and I added a variable that would count the number of times the Kalman filter has completed the prediction and update step. I added that counter as the input to the kalman filter function along with two conditional statements as shown below:


if (np.remainder(c,10)): # 10 Hz
    C_Upd = P.C
    y_Upd = y
else: # 100 Hz
    C_Upd = P.C[1, :]
    y_Upd = y[1, :]

mu_p = Ad.dot(mu) + Bd.dot(u) 
sigma_p = Ad.dot(sigma.dot(Ad.transpose())) + sigma_u

y_m = y_Upd-C_Upd.dot(mu_p)
sigma_m = C_Upd.dot(sigma_p.dot(C_Upd.transpose())) + sigma_n
kkf_gain = sigma_p.dot(C_Upd.transpose().dot(np.linalg.inv(sigma_m)))

mu = mu_p + kkf_gain.dot(y_m)    
sigma=(np.eye(4)-kkf_gain.dot(C_Upd)).dot(sigma_p)

return mu,sigma

The conditional statements check if the counter has a remainder once divided by 10. This ensures that both z and thetadot will be measured at 10 Hz, and only thetadot will be measured at 100 Hz. As shown below, after changing these measurement values, the system is still stable: