View ECE4960-2020 on GitHub

Cornell University: ECE 4960

Return to main page

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

Objective

The purpose of this lab is to familiarize yourself with the Kalman Filter by simulating it with an inverted pendulum on a cart from Lab 11b. Although we do not expect you to make this code work on your actual robot, we expect you to approach the lab with that goal in mind. That means adapting parameters to fit your particular system, checking how sensitive the control is to imperfections in your model parameters and to realistic noise in your measurements.

Prelab

Check out lecture 18 for a brief recap on Kalman Filters.

Download lab code, and familiarize yourself with the functions in the folder. Note that to eliminate the crashing ODE from Lab11b, we changed some of the functions slightly - you may notice that we are now regulating around theta = 0 as up, instead of theta = pi as up.

  • pendulumParam.py: initializes physical pendulum parameters and state space equations.
  • pendulumNonlinearDynamics.py: describes the nonlinear dynamics of the system.
  • signalGenerator.py: computes various reference functions to test the pendulum.
  • pendulumAnimation.py: helper functions for animating the system.
  • plotDataZ.py: helper functions to plot state variables and reference values for debugging.
  • runSimulation.py: integrates the entire vector field using the controller and the true nonlinear dynamics, and plots the results.
  • kalmanFilter.py: A simple Kalman Filter.

Lab

Check out the Kalman Filter

  1. The Kalman Filter function should look familiar from Lecture 18. Notice:
    • We calculate new A and B matrices for use with the Kalman Filter, why is this?
    • What state(s) are we measuring directly, and which states are we estimating?
  2. Run the code. Notice that the initial Kalman Filter can easily estimate the states because we are simulating close to a perfect system.
  3. If you use the obsv(A,C)-command with this system, you will find that it is actually not observable. How come it still works?
    • Hint: Look at your A and C matrix, what/which state(s) are unobservable?
    • Hint: Try to see what happens if your initial guess of the states are wrong.
    • Hint: Try to plot the uncertainty for each of the states over time.
  4. You may want to change the default controller to an LQR controller.

Add imperfections and adjust the Kalman Filter

  1. Recall that the relative values of the process noise (sigma_u) and the measurement noise (sigma_n) can change how well the Kalman Filter is able to estimate the state. In the following we will implement realistic limitations/noise of our robots. Throughout this exercise:
    • Argue for your choice of parameters
    • If the Kalman Filter fails try to adjust sigma_u and sigma_n
    • If the controller keeps failing, consider running the controller on the real states and debug the Kalman Filter separately.
    • Feel free to try different reference functions
  2. It can be tricky to measure the initial state perfectly. Try adding discrepancy to the initial state and see how quickly the Kalman Filter is able to converge to the correct state.
  3. Try adding a realistic saturation value and deadband to u, is the Kalman Filter still able to estimate the state well?
  4. Try adding realistic measurement and process noise, is the Kalman Filter still able to estimate the state well?
  5. Try adjusting the parameters of the A- and B-matrix that is used in the Kalman Filter function. How much are you allowed to fudge these numbers before the Kalman Filter fails to track the state well?
  6. Try changing the update time of the sensor and the controller (T_update), how slow can the system update before the cart is no longer able to stabilize the pendulum?
    • Keep in mind that the measurement noise you implemented with a shorter update time may need to be decreased for a slower update time.
  7. With all of these imperfections implemented, do you think your specific robot will be able to stabilize the pendulum?

Change your measurements (optional)

  1. For 3 bonus points, change the code such that you can measure both thetadot (at appr. 100Hz) and z or zdot (at appr. 10Hz). This scenario corresponds to one in which you measure the angular velocity of the pendulum using the gyroscope and the position or velocity of the cart using a TOF sensor pointing towards a wall. Explain if and how this added measurement helps.

Write-up

To demonstrate that you’ve successfully completed the lab, please upload a brief lab report (<1.000 words), with code snippets (not included in the word count), photos, and/or videos documenting that everything worked and what you did to make it happen.