Objective

The objective of this lab is to finish up control by implementing a Kalman Filter in the simulated inverted pendulum.

Ideal System

First, I ran the code out of the box. It didn't slow to a halt like the previous week's code, but it was fairly stable
i2c1

We calculate new A and B matrices for use with the Kalman Filter, why is this?
The new Kalman filter uses new A and B matrics compared to last week's code because the Kalman filter discretizes the system as opposed to last week, which was based on continuous time.
What state(s) are we measuring directly, and which states are we estimating
In the previous week we were measuring all, with a C matrix of 
                            C = np.matrix([[1.0, 0.0, 0.0, 0.0],
                                     [0.0, 1.0, 0.0, 0.0],
                                     [0.0, 0.0, 1.0, 0.0],
                                     [0.0, 0.0, 0.0, 1.0]])
whereas this week the C matrix is only 
                          #Measure one state only
                      C = np.matrix([[0.0, 0.0, 0.0, 1.0]])
Indicating that the only state being measured is the fourth, which is theta dot in this code.
Using the function print(control.obsv(P.A,P.C)) I see that the matrix is
i2c1
I think that because the first row has no value in the first columm, the x state is not observable. However, we already knew fromm the C matrix that only theta dot is being observed, and the rest are being estimated. it is only important that the theta dot in this matrix is observable, which it is.

Adding Noise


First to simulate process noise in the Kalman filter, I commented out the line 
                        #with process noise:
                         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]
The Kalman filter still worked, though slightly less smoothly
i2c1
I next added in measurement noise, sigma_n. First I tried changing the line sigma = np.eye(4)*0.00001 in runSimulation.py, but that didnt appear to actually affect the filter. Next, I tried changing it within the loop that calculates each state, by adding an element of randomness to the y_kf value that gets passed to the kaulman filter, and that resulted in a noisier output.
i2c1
I used the same level of noise as in the process noise, and it added what looked like a decent amount of noise after looking at the plot. Much more would be difficult to tell the signal over the noise, and much less wouldnt have much impact.

Changing the Initial State

Next, I worked on changing the initial measurements to see if that would throw the filter off. This time I did need to change the value outside the loop, as I was only trying to change the initial condition and not give consistent error for each state of the pendulum running. I again started with the level of noise, which didnt do a heck of a lot (.01) mu = np.array([[P.z0+.01], [P.zdot0+.01], [P.theta0+.01], [P.thetadot0+.01]]). Next, I pumped it up by a factor of 10 to see what would happen, adding .1 to each state.
i2c1
Then, I bumped it up to .5, and that was a real struggle to balance again, but it was able to measure that. That seems a little bit overly high, as the entire range of x is only -2 to +2, so its good to know the filter will still work with such bad initial conditions.
i2c1

Saturation and Deadband

Next, I again added in saturation and deadband as we did in the previous week's lab. Again I went into the loop and found u=-Kr.dot(mu-des_state). I added the same limitations as last week, with a deadband of .275 and a saturation of 1.85 

                        u=-Kr.dot(mu-des_state)
        
                        if(u>0): 
                            if(u>1.85): 
                                u = 1.85 
                            elif(u<.275): 
                                u = 0 #deadband

                        else: 

                            if(u<-1.85): 
                                u = -1.85 
                            elif(u>-.275): 
                               u = 0 #deadband

i2c1
To my great relief the kalman filter also did not struggle with this, and kept on just fine.

AB Matrix Adjustments

I next worked on "fudging the numbers" for the A and B matrices. 
                      A = np.matrix([[0.0, 1.0, 0.0, 0.0],
                          [0.0, -b/m2, -m1*g/m2, 0.0],
                          [0.0, 0.0, 0.0, 1.0],
                          [0.0, -b/(m2*ell), -(m1+m2)*g/(m2*ell), 0.0]])

              B = np.array([[0.0], [1.0/m2], [0.0], [1.0/(m2*ell)]])
I increased the matrix by .01 to start with, just as I did with noise and others. That worked fine, so I increased to .1, which also worked. Eventually, pushing it to .5 caused it to slowly drift off screeen, although apparently still be stableKr = control.place(P.A*1.5,P.B*1.5,dpoles) Below are the (left) .1 increase and (right) .5 increase
i2c1i2c1
These numbers seem as though they can be fudged significantly without screwing up the filter too much.

Decreasing Update Time

Finally, I changed the update time T_update in pendulumParam.py. I first cut it to a tenth, down to .01. This still stayed stable.
i2c1
Next, I upped it again to .05, and still it was stable, although very jittery.
i2c1
I again increased to .1, and that caused the pendulum to become unstable. This is not all that surprising, as ten updates a second is nothing in terms of control needs. Also, our artemis would be more than capable of 20 times a second.

More checks

Now that I had confirmed the filter works with my robots parameters, I decided to see how far I could push the simulation by exaggerating known parameters. I started with deadband, incrasing it to almost double because I know the wheels can stick sometimes.
i2c1
The deadband actually worked up until around a deadband of u=1, which is pretty crazy and way better than last week's method.
i2c1
Next, I put deadband back to a reasonable value of .4 and played with noise. I increased process noise by a factor of 10, and that still managed to stabilize the pendulum. The pendulum at this state, with all the possible variables just a little wrong, is very jittery, which is what I would expect if I were to try and put this on my real robot, but it still works.

Conclusions

In the end, with all the changes, I was still able to make the pendulum stabilize. Although this might lead the naive reader to guess that this would work on the actual robot, I know it better than that and still don't believe it would work on the real robot, especially if Professor Peterson herself couldn't get it working.