16-745: Assignment 2

State Space (Linear) Control and State Estimator Design, and Nonlinear Policy Design

This assignment has several parts: LQR control design, Kalman filter design, and nonlinear policy design using whatever technique you want to try out. We will again use the two-wheel inverted pendulum (TWIP) After doing this assignment you will have experience using "traditional" optimal control design techniques, and if you choose, reinforcement learning. I want you to use some kind of AI (ChatGPT, Gemini, Claude, ...) to do this assignment, in order to develop skills in using this new technology. I use AI to derive dynamic models and write code. A challenge for you is to guide the AI to get the right answer. In order to do this, you have to be able to tell when an answer is not correct. Looking at simulator output, and testing the data against a dynamic model derived in a different way are good ways to do this. Here is my chat about TWIPs with ChatGPT which shows how to linearize the dynamics, here is how to make the Kalman filter with hacks for a unicycle, and here is another example of using an AI: ChatGPT. The initial prompt.

1. Design and test an LQR controller for the TWIP model

Design and test an LQR controller for the TWIP model. To make this easier to grade, let's agree that the state is ( wheel_angle, wheel_angular_velocity, body_angle, body_angular_velocity ). To do this you should

Get the full nonlinear model for the dynamics (See sim-twip.zip or do your own derivation).
Linearize the model at the equilibrium point of the body (an inverted pendulum) straight up and all velocities zero.
Substitute in the numerical values of the parameters.
Choose an optimization criterion (the cost matrices Q and R).
Compute the optimal feedback gains K
Simulate the resulting controller. Ask the AI to write graphics code to animate the simulation so you can see the result. See ChatGPT to see how to get an AI to write graphics code.
Evaluate the controller, and potentially iterate by choosing a different optimization criterion.

The Q and R cost matrices are the design variables used to control how the LQR technique designs the controller. Play with them. What happens if you increase or decrease R? What happens if you increase or decrease the relative cost of body angle states vs. wheel angle states? What happens if you increase or decrease the relative cost of positions vs. velocities?

As part of the writeup, please produce plots of the TWIP states vs. time to demonstrate the results and tuning. The TAs can process pictures effectively.

2. Design and test a Kalman filter for the TWIP model

Design and test a Kalman filter for the TWIP model. Assume that the only sensors are the wheel angle with respect to the body and an inertial measurement unit (IMU) that has an accelerometer and a gyro (something that measures angular velocity of the body in world coordinates). In addition to filtering out noise, the Kalman filter has to estimate the missing velocity measurement, and estimate the body angle with respect to vertical. The accelerometer gives you a low frequency estimate of the body angle with respect to world vertical, contaminated by fore/aft accelerations. So the C matrix is (1,0,0,0; 0,0,1,0; 0,0,0,1) and y = (wheel_encoder, accelerometer_signal, gyro). The accelerometer actually measures the acceleration forwards and downwards in the body frame of reference: ax = g*sin(body_angle), az = g*cos(body_angle). You can either generate an accelerometer body_angle "measurement" of a tan2( ax, az ) or just ax (which assumes sin(body_angle) is approximately body_angle). Google "complementary filter" to understand more about body angle estimation, but implement a full Kalman filter (estimating all four states).

You need to choose the variance of the process noise (Qf) and measurement noise (Rf). These variables are the design variables for the filter, rather than representing realistic estimates of the sensor variances. To choose Qf and Rf, you should simulate the filter with some added process and measurement noise (you can drive the TWIP around using the LQR controller to do these tests). What happens when you increase or decrease Rf? What happens when you increase or decrease the variance of the accelerometer relative to the encoder that measures wheel angle?

As part of the writeup, please produce plots of the estimated and actual TWIP states over time to demonstrate filter performance and any improvements due to tuning. The TAs can process pictures effectively.

One of the TAs recommended this lecture with these slides.

3. Design and test a nonlinear policy

Using any method you want, design a nonlinear policy to balance the robot with the goal wheel_angle = 0, body_angle = 0, and zero velocities that takes into account the trigonometric terms, a friction coefficient of 1 for the tire-ground contact, and a motor torque limit of TBA. The rules are: 1) If the wheels slip that is a fatal error for which there is an infinite cost. 2) If the motor torque limit is exceeded that is a fatal error for which there is an infinite cost.

You need to make up an optimization criterion (it can be the same or different from your LQR cost function). Methods you could try include some variant of dynamic programming, or some variant of reinforcement learning like PPO. Get the AI to write the code to create this controller.

It would be nice to compare the LQR (linear) and your nonlinear policy. Under what conditions is there a significant difference in performance. Plots comparing performance are useful.

4. Design and test a nonlinear policy that follows a trajectory

We want to do TWIP drag racing, driving the robot to go to a location and attain the equilibrium point as fast as possible, starting from the upright equilibrium point. You could just use whatever you did for part 3, setting the wheel_angle state variable to be relative to the wheel_angle goal, but getting to the equilibrium point as fast as possible is usually a bad design goal for a regulator, as it makes the policy sensitive to noise and borderline unstable. A much better approach is to design a time dependent trajectory x_desired(t) with a feedforward command u_ff(t) and potentially a special time variant feedback controller K_special(t) instead of the default regulator K. After the trajectory is executed the control can be switched back to the default regulator.

Using any method you want, including reinforcement learning, output a time-optimal trajectory to any desired wheel_angle (and corresponding location) obeying the no-slip and torque limit constraints.

It would be nice to compare your LQR (linear) controller, your nonlinear regulator, and your nonlinear trajectory follower. Plots are appreciated.

Rant: In the machine learning world this is often called a "goal conditioned" policy. I find this terminology confusing. It arises from the probabilistic nature of many RL policies p(action|state,goal), where the probability of an action is conditioned on a state and a goal. Given that the probabilistic nature of many RL policies is a hack to encourage exploration, high performance policies with known or learned models after exploration is done are typically deterministic and it doesn't make sense to use any term like "X conditioned".

What to turn in: Your (or an AI's) writeup of what you did, how you did it, and what you learned is what is important here. Provide any prompts and responses from any AI you used, as well as code you or an AI wrote.