"Classical control" theory does not produce a stable system for an inverted pendulum, observer-feedback control is needed. To construct an observer, first we need to model the system. Here is a block diagram of that model:
The leftmost block of this model, denoted r[n], is the input to the system - what we would like it to do while it is balancing the pendulum. In our system, we will probably just set this to zero, but for the model it is currently a step function - we are asking the cart to move 1 meter forward while balancing the pendulum. The major component of this diagram is the block titled "State-Space", which models the system response to input. This block uses the acceleration of the cart, in conjunction with the previous states of the system, to determine the current characteristics such as pendulum angle and cart velocity. This will not be included in our program, it is only needed to model the system.
Our program will concern the block titled "Observer" as well as the two gain blocks. The observer block takes the current acceleration, velocity, and pendulum position, and generates the appropriate acceleration for the next time step. The two zero-order hold blocks convert between discrete-time parts of the system (the computer, currently set at 30 Hz refresh time - the refresh rate of the camera), and the continuous-time parts of the system (the pendulum). The two random number blocks represent noise going into the system (slight aberrant movements of the cart), and noise observing the robot (errors in the slope given by our camera). The observer block is a subsystem which contains the following block diagram:
The observer simply uses the previous state of the robot (xhat[n] being the previous observer output, and y[n] being the previous system output) in conjuction with the current state of the system (the current acceleration given by u[n]) to determine what the robot's acceleration should be for the next time step. The various matrices used in the matrix gain steps are simply weightings specified either by our system or by what we want the system to do. L is a weighting vector of which parameters it is most important to minimize. This observer finds the optimum solution to minimize the weighted sum given by L.
In this case, we are optimizing the response of the system under no-noise conditions. This response looks like:
The top curve represents the pendulum angle, while the bottom curve represents cart position. In this case, we are asking the cart to move 1 meter forward, so it starts by backing up, then accelerating to the exact 1 meter position. The pendulum starts falling in the forward direction, then balances out at 1 meter.
Unfortunately, our system is likely to have significant noise, especially in determining the pendulum angle. The pendulum response under such noise conditions is likely to look something like this:
Under noisy conditions, it still manages to move 1 meter forward, but the pendulum angle varies considerably (angle units are in radians). If this method is not sufficient to stabilize the pendulum, we will have to get an idea of the relative magnitudes of the noise, and do an optimization based on that, called Kalman filtering, though the simulation will be essentially the same, just with different matrix weights.