iCub-main
Optimal Control

Class for optimal control based on Discrete Algebraic Riccati Equation (DARE). More...

Collaboration diagram for Optimal Control:

Classes

class  iCub::ctrl::Riccati
Classic Riccati recursive formula for optimal control in a LQ problem. More...

Detailed Description

Class for optimal control based on Discrete Algebraic Riccati Equation (DARE).

Description

The class Riccati can be used for optimal control based on Discrete Algebraic Riccati Equation (DARE). The DARE is used to compute the linear gain matrix of the feedback controls. In the following, the basics of a LQR (linear quadratic regulation) problem are reported.

Given the linear system:

$x_{i+1} = A x_i + B u_i \ , \ i=0,1,\ldots,N-1$

with the known initial state $$x_0 = \hat{x}$$, and the quadratic cost $$J$$:

$J = \sum^{N-1}_{i=0} \left[ x^\top_i V x_i + u^\top_i P u_i \right] + x^\top_N V_N x_N$

with $$V=V^\top \geq 0$$, $$V_N=V^\top_N \geq 0$$, $$P=P^\top>0$$, the problem is to find the sequence of optimal controls $$u^\circ_0, \ldots, u^\circ_{N-1}$$ minimizing $$J$$. The optimal controls can be found via dynamic programming, and a closed form solution can be found. At time instant $$i$$ the optimal cost-to-go and control are:

$J^\circ(x_{i}) = x^\top_{i} \ T_{i} \ x_{i} \ , \ u^\circ_{i} = - L_i \ x_{i}$

where $$L_i$$ is:

$L_i = (P+B^\top T_{i+1} B)^{-1} B^\top T_{i+1} A$

whilst $$T_i$$ is computed after the `‘discrete time algebraic Riccati equation’':

$T_N = V_N \ , \ T_i = V + A^\top [ T_{i+1} - T_{i+1} B (P+B^\top T_{i+1} B)^{-1} B^\top T_{i+1} ] A$

Example

Riccati r(A,B,V,P,VN,true);
r.solveRiccati(steps);
for(i=0; i<steps; i++)
{
u=r.doLQcontrol(i,x);
x=A*x+B*u;
}
A
Definition: sine.m:16