Optimal Control Using Factor

Graph Optimization
Kwangki Kim
iASL
2024-04-17
LQR Control Using Factor Graphs Introduction

자료출처: [Link]
LQR Control Using Factor Graphs Introduction
LQR Control Using Factor Graphs Variable Elimination
To optimize the factor graph, which represents minimizing the least squares objectives
above, we can simply eliminate the factors from right to left. In this section we
demonstrate the variable elimination graphically and algebraically, but the matrix
elimination will be also provided.
LQR Control Using Factor Graphs Variable Elimination
LQR Control Using Factor Graphs Variable Elimination
LQR Control Using Factor Graphs Variable Elimination
LQR Control Using Factor Graphs Variable Elimination
LQR Control Using Factor Graphs Variable Elimination
LQR Control Using Factor Graphs Variable Elimination
LQR Control Using Factor Graphs Variable Elimination
LQR Control Using Factor Graphs Variable Elimination
LQR Control Using Factor Graphs Variable Elimination
LQR Control Using Factor Graphs Variable Elimination
LQR Control Using Factor Graphs Variable Elimination
LQR Control Using Factor Graphs Intuition
LQR Control Using Factor Graphs Intuition
LQR Control Using Factor Graphs Equivalence to the Ricatti Equation
LQR Control Using Factor Graphs Implementation Using GTSAM [Link]
LQR Control Using Factor Graphs Implementation Using GTSAM [Link]
LQR Control Using Factor Graphs Implementation Using GTSAM [Link]
Nonlinear Optimal Control Using Factor Graphs

Future Work
The factor graph (Figure 1) for our finite horizon
discrete LQR problem can be readily extended to LQG,
iLQR, DDP, and Reinforcement Learning using
non-deterministic dynamics factors, nonlinear factors,
discrete factor graphs
Nonlinear Optimal Control Using Factor Graphs
• The ternary factors represent the dynamics model constraints.
<latexit sha1_base64="5YAG9MCWvXzJPsWGq046ijfblao=">AAAB7nicbVDLSgNBEOz1GeMr6tHLYBDiJeyKRI9BLx4jmAckS5iddJIhs7PrzKwYlnyEFw+KePV7vPk3TpI9aGJBQ1HVTXdXEAuujet+Oyura+sbm7mt/PbO7t5+4eCwoaNEMayzSESqFVCNgkusG24EtmKFNAwENoPRzdRvPqLSPJL3ZhyjH9KB5H3OqLFS84GUnrr8rFsoumV3BrJMvIwUIUOtW/jq9CKWhCgNE1TrtufGxk+pMpwJnOQ7icaYshEdYNtSSUPUfjo7d0JOrdIj/UjZkobM1N8TKQ21HoeB7QypGepFbyr+57UT07/yUy7jxKBk80X9RBATkenvpMcVMiPGllCmuL2VsCFVlBmbUN6G4C2+vEwa52WvUq7cXRSr11kcOTiGEyiBB5dQhVuoQR0YjOAZXuHNiZ0X5935mLeuONnMEfyB8/kDU5qO7Q==</latexit>
• The unary factors represent the state and control costs we seek to
q(xi ) minimize via least-squares.

<latexit sha1_base64="hDMKDbD7cvbjWCWkq4CQacB4k5I=">AAAB/nicbVDLSgNBEOyNrxhfq+LJy2AQIkrYFYlehKAXjxHMA5JlmZ3MJkNmH8zMSsIS8Fe8eFDEq9/hzb9xkuxBEwsaiqpuuru8mDOpLOvbyC0tr6yu5dcLG5tb2zvm7l5DRokgtE4iHomWhyXlLKR1xRSnrVhQHHicNr3B7cRvPlIhWRQ+qFFMnQD3QuYzgpWWXPNg6Kbs1B6ja+SXhi5DZ4nLTlyzaJWtKdAisTNShAw11/zqdCOSBDRUhGMp27YVKyfFQjHC6bjQSSSNMRngHm1rGuKASiednj9Gx1rpIj8SukKFpurviRQHUo4CT3cGWPXlvDcR//PaifKvnJSFcaJoSGaL/IQjFaFJFqjLBCWKjzTBRDB9KyJ9LDBROrGCDsGef3mRNM7LdqVcub8oVm+yOPJwCEdQAhsuoQp3UIM6EEjhGV7hzXgyXox342PWmjOymX34A+PzB8iDlBk=</latexit>

xi+1 = f (xi , ui )

<latexit sha1_base64="EBOvnPzYjNLaQkPzElSo/hcduc8=">AAAB7nicbVBNS8NAEJ34WetX1aOXxSLUS0lEqseiF48V7Ae0oWy2m3bpZhN2J0IJ/RFePCji1d/jzX/jts1BWx8MPN6bYWZekEhh0HW/nbX1jc2t7cJOcXdv/+CwdHTcMnGqGW+yWMa6E1DDpVC8iQIl7ySa0yiQvB2M72Z++4lrI2L1iJOE+xEdKhEKRtFKbU0qaV9c9Etlt+rOQVaJl5My5Gj0S1+9QczSiCtkkhrT9dwE/YxqFEzyabGXGp5QNqZD3rVU0YgbP5ufOyXnVhmQMNa2FJK5+nsio5ExkyiwnRHFkVn2ZuJ/XjfF8MbPhEpS5IotFoWpJBiT2e9kIDRnKCeWUKaFvZWwEdWUoU2oaEPwll9eJa3Lqler1h6uyvXbPI4CnMIZVMCDa6jDPTSgCQzG8Ayv8OYkzovz7nwsWtecfOYE/sD5/AFQj47r</latexit>

Future Work
r(ui )
The factor graph (Figure 1) for our finite horizon
discrete LQR problem can be readily extended to LQG,
iLQR, DDP, and Reinforcement Learning using
non-deterministic dynamics factors, nonlinear factors,
discrete factor graphs
 Nonlinear Optimal Control Using Factor Graphs
<latexit sha1_base64="hDMKDbD7cvbjWCWkq4CQacB4k5I=">AAAB/nicbVDLSgNBEOyNrxhfq+LJy2AQIkrYFYlehKAXjxHMA5JlmZ3MJkNmH8zMSsIS8Fe8eFDEq9/hzb9xkuxBEwsaiqpuuru8mDOpLOvbyC0tr6yu5dcLG5tb2zvm7l5DRokgtE4iHomWhyXlLKR1xRSnrVhQHHicNr3B7cRvPlIhWRQ+qFFMnQD3QuYzgpWWXPNg6Kbs1B6ja+SXhi5DZ4nLTlyzaJWtKdAisTNShAw11/zqdCOSBDRUhGMp27YVKyfFQjHC6bjQSSSNMRngHm1rGuKASiednj9Gx1rpIj8SukKFpurviRQHUo4CT3cGWPXlvDcR//PaifKvnJSFcaJoSGaL/IQjFaFJFqjLBCWKjzTBRDB9KyJ9LDBROrGCDsGef3mRNM7LdqVcub8oVm+yOPJwCEdQAhsuoQp3UIM6EEjhGV7hzXgyXox342PWmjOymX34A+PzB8iDlBk=</latexit>

xi+1 = f (xi , ui )
<latexit sha1_base64="EBOvnPzYjNLaQkPzElSo/hcduc8=">AAAB7nicbVBNS8NAEJ34WetX1aOXxSLUS0lEqseiF48V7Ae0oWy2m3bpZhN2J0IJ/RFePCji1d/jzX/jts1BWx8MPN6bYWZekEhh0HW/nbX1jc2t7cJOcXdv/+CwdHTcMnGqGW+yWMa6E1DDpVC8iQIl7ySa0yiQvB2M72Z++4lrI2L1iJOE+xEdKhEKRtFKbU0qaV9c9Etlt+rOQVaJl5My5Gj0S1+9QczSiCtkkhrT9dwE/YxqFEzyabGXGp5QNqZD3rVU0YgbP5ufOyXnVhmQMNa2FJK5+nsio5ExkyiwnRHFkVn2ZuJ/XjfF8MbPhEpS5IotFoWpJBiT2e9kIDRnKCeWUKaFvZWwEdWUoU2oaEPwll9eJa3Lqler1h6uyvXbPI4CnMIZVMCDa6jDPTSgCQzG8Ayv8OYkzovz7nwsWtecfOYE/sD5/AFQj47r</latexit>

q(xi ) r(ui )
<latexit sha1_base64="5YAG9MCWvXzJPsWGq046ijfblao=">AAAB7nicbVDLSgNBEOz1GeMr6tHLYBDiJeyKRI9BLx4jmAckS5iddJIhs7PrzKwYlnyEFw+KePV7vPk3TpI9aGJBQ1HVTXdXEAuujet+Oyura+sbm7mt/PbO7t5+4eCwoaNEMayzSESqFVCNgkusG24EtmKFNAwENoPRzdRvPqLSPJL3ZhyjH9KB5H3OqLFS84GUnrr8rFsoumV3BrJMvIwUIUOtW/jq9CKWhCgNE1TrtufGxk+pMpwJnOQ7icaYshEdYNtSSUPUfjo7d0JOrdIj/UjZkobM1N8TKQ21HoeB7QypGepFbyr+57UT07/yUy7jxKBk80X9RBATkenvpMcVMiPGllCmuL2VsCFVlBmbUN6G4C2+vEwa52WvUq7cXRSr11kcOTiGEyiBB5dQhVuoQR0YjOAZXuHNiZ0X5935mLeuONnMEfyB8/kDU5qO7Q==</latexit>

LQR Using FG

x⇤i {x0 , ( x⇤i , u⇤i )i=1:N x⇤N }

<latexit sha1_base64="8UIom39fuvlJL/GB5B/tLdBpZQw=">AAACK3icbVDLSgMxFM3UV62vUZdugkWopZYZkSqCUOrGValgH9Bph0yatqGZB0lGLMP8jxt/xYUufODW/zB9iNp6IHDOufdyc48TMCqkYbxpiYXFpeWV5GpqbX1jc0vf3qkJP+SYVLHPfN5wkCCMeqQqqWSkEXCCXIeRujO4HNXrt4QL6ns3chiQlot6Hu1SjKSybL1kRfDONnIwA60OYRIpRdvZ3LcKR+rQjuiFeV4+MuPcT1tUjttZaMW2njbyxhhwnphTkgZTVGz9yer4OHSJJzFDQjRNI5CtCHFJMSNxygoFCRAeoB5pKuohl4hWNL41hgfK6cCuz9XzJBy7vyci5AoxdB3V6SLZF7O1kflfrRnK7lkrol4QSuLhyaJuyKD04Sg42KGcYMmGiiDMqforxH3EEZYq3pQKwZw9eZ7UjvNmIV+4PkkXS9M4kmAP7IMMMMEpKIIrUAFVgME9eAQv4FV70J61d+1j0prQpjO74A+0zy/OLqRu</latexit>

x̄i x̄i + 1,
<latexit sha1_base64="kKkcfFjr9257fAJyEzPiDPmUK24=">AAACFHicbZA9SwNBEIb34leMX6eWNotBEAPhTiRaBm0sFYwKuRjmNnO6ZO+D3Tk1hPwIG/+KjYUithZ2/hvvYgSNvrDw8M4Ms/P6iZKGHOfDKkxMTk3PFGdLc/MLi0v28sqpiVMtsCFiFetzHwwqGWGDJCk8TzRC6Cs887sHef3sGrWRcXRCvQRbIVxGMpACKLPadsXzQfPbtuSewoBA6/iGf3sV7nVQEeR8sVVq22Wn6gzF/4I7gjIb6ahtv3udWKQhRiQUGNN0nYRafdAkhcJByUsNJiC6cInNDCMI0bT6w6MGfCNzOjyIdfYi4kP350QfQmN6oZ91hkBXZryWm//VmikFe62+jJKUMBJfi4JUcYp5nhDvSI2CVC8DEFpmf+XiCjQIynLMQ3DHT/4Lp9tVt1atHe+U6/ujOIpsja2zTeayXVZnh+yINZhgd+yBPbFn6956tF6s16/WgjWaWWW/ZL19AroCnVs=</latexit>

ūi
<latexit sha1_base64="GT6cFTS6w774pkNn3ua0reGQTQw=">AAACFHicbZDJSgNBEIZ7XGPcoh69NAZBFMKMSPQoevEYwSyQGYeaTo029ix01ygh+BBefBUvHhTx6sGbb+NMEsHth4aPv6qorj9IlTRk2x/WxOTU9Mxsaa48v7C4tFxZWW2ZJNMCmyJRie4EYFDJGJskSWEn1QhRoLAdXB0X9fY1aiOT+Iz6KXoRXMQylAIot/zKjhuA5pkvuaswJNA6ueFf3g53e6gICj7fLvuVql2zh+J/wRlDlY3V8Cvvbi8RWYQxCQXGdB07JW8AmqRQeFt2M4MpiCu4wG6OMURovMHwqFu+mTs9HiY6fzHxoft9YgCRMf0oyDsjoEvzu1aY/9W6GYUH3kDGaUYYi9GiMFOcEl4kxHtSoyDVzwGElvlfubgEDYLyHIsQnN8n/4XWbs2p1+qne9XDo3EcJbbONtgWc9g+O2QnrMGaTLA79sCe2LN1bz1aL9brqHXCGs+ssR+y3j4Bq62dUg==</latexit>

ūi + u⇤i or
{P0 , (Pi , Ki )i=1:N 1 , PN }
<latexit sha1_base64="6bPNgeCJWj/gTR5k2aeCQYmEAnc=">AAACEHicbVDLSsNAFJ3UV62vqEs3g0WsUEsiUkUQim4EoVSwD2hCmEwn7dDJg5mJUEI+wY2/4saFIm5duvNvnLZZaOuByz2ccy8z97gRo0IaxreWW1hcWl7JrxbW1jc2t/TtnZYIY45JE4cs5B0XCcJoQJqSSkY6ESfIdxlpu8Prsd9+IFzQMLiXo4jYPuoH1KMYSSU5+qGVwIZjlGFJNVqGtw49chJ6aV7Uj820rMSknkIrdfSiUTEmgPPEzEgRZGg4+pfVC3Hsk0BihoTomkYk7QRxSTEjacGKBYkQHqI+6SoaIJ8IO5kclMIDpfSgF3JVgYQT9fdGgnwhRr6rJn0kB2LWG4v/ed1Yeud2QoMoliTA04e8mEEZwnE6sEc5wZKNFEGYU/VXiAeIIyxVhgUVgjl78jxpnVTMaqV6d1qsXWVx5MEe2AclYIIzUAM3oAGaAINH8AxewZv2pL1o79rHdDSnZTu74A+0zx8ttJmU</latexit>

Linearize (A) and Quadratize (B) w.r.t.

{x0 , (x̄i , ūi )i=1:N 1 , x̄N }
<latexit sha1_base64="JAIlOR1IZDYslsr6ZNxsvXhAlA0=">AAACH3icbZDLSsNAFIYn9VbrLerSzWARKtSSiFQRhKIbV6WCvUATwmQ6aYdOLsxMxBLyJm58FTcuFBF3fRunbQRtPTDw8f/ncOb8bsSokIYx1nJLyyura/n1wsbm1vaOvrvXEmHMMWnikIW84yJBGA1IU1LJSCfiBPkuI213eDPx2w+ECxoG93IUEdtH/YB6FCOpJEevWgl8dIwyLEHLRVwxLc8oduixk9Ar87J+YqblHzupp9BKHb1oVIxpwUUwMyiCrBqO/mX1Qhz7JJCYISG6phFJO0FcUsxIWrBiQSKEh6hPugoD5BNhJ9P7UniklB70Qq5eIOFU/T2RIF+Ike+qTh/JgZj3JuJ/XjeW3oWd0CCKJQnwbJEXMyhDOAkL9ignWLKRAoQ5VX+FeIA4wlJFWlAhmPMnL0LrtGJWK9W7s2LtOosjDw7AISgBE5yDGrgFDdAEGDyBF/AG3rVn7VX70D5nrTktm9kHf0obfwPtpJ/f</latexit>

LQR in terms of perturbed variables

{x0 , ( xi , ui )i=1:N xN }
<latexit sha1_base64="+hQNQkM9WJ9JK4iSgbMyvRJ7lJw=">AAACJXicbVBLSwMxGMzWV62vVY9egkWoUMuuSBVRKHrxVCrYB3TLks1m29DsgyQrlmX/jBf/ihcPFhE8+VdMH6K2DgQmM/ORfONEjAppGB9aZmFxaXklu5pbW9/Y3NK3dxoijDkmdRyykLccJAijAalLKhlpRZwg32Gk6fSvR37znnBBw+BODiLS8VE3oB7FSCrJ1i+sBD7YRhEWoOUSJpG60eI3j216aCf00jyvHplp8SeSVFNopbaeN0rGGHCemFOSB1PUbH1ouSGOfRJIzJAQbdOIZCdBXFLMSJqzYkEihPuoS9qKBsgnopOMt0zhgVJc6IVcnUDCsfp7IkG+EAPfUUkfyZ6Y9Ubif147lt5ZJ6FBFEsS4MlDXsygDOGoMuhSTrBkA0UQ5lT9FeIe4ghLVWxOlWDOrjxPGscls1wq357kK1fTOrJgD+yDAjDBKaiAG1ADdYDBI3gGr2CoPWkv2pv2PolmtOnMLvgD7fMLEgeimg==</latexit>

1,
 Appendix
Least Squares Implementation in
GTSAM