LQR control using factor graphs

This code is meant to provide a reference for implementing LQR control using GTSAM. For a discussion of the intuition behind the code, please see our corresponding blog post.

Provided in this folder are 5 files:

• minimum_lqr_fg.py - A MWE of using factor graphs to solve an LQR problem. This example was displayed in the blog post.
• dynamics_lti.py - A module for formulating and simulating linear, time invariant (LTI) dynamics problems.
• lqr.py - A module for formulating and solving LQR problems.
• example0_ricatti.py - Example code which solves an LQR problem using the traditional dynamic, Discrete Algebraic Ricatti Equation method. This code is just provided as a reference to compare the “standard” solution to the factor graph solution.
• example1_factorgraph.py - Example code which solves an LQR problem using GTSAM/factor graphs.

Please note: this code was tested on the 4.1.1 version of GTSAM on pypi:

pip install gtsam==4.1.1

For versions older than 4.1.0, please use this alternate download.