Linear Quadratic Tracking (LQT)#
Linear quadratic tracking (LQT) is a simple form of optimal control that trades off tracking and control costs expressed as quadratic terms over a time horizon, with the evolution of the state described in a linear form. The LQT problem is formulated as the minimization of the cost
where \(\boldsymbol{\mu}=\left[\boldsymbol{\mu}_{1}^{\top}, \boldsymbol{\mu}_{2}^{\top}, \ldots, \boldsymbol{\mu}_{T}^{\top}\right]^{\top}\) is the via-points planned by the pretrained DGform model and also the targets need to be tracked, \(\boldsymbol{x}=\left[\boldsymbol{x}_{1}^{\top}, \boldsymbol{x}_{2}^{\top}, \ldots, \boldsymbol{x}_{T}^{\top}\right]^{\top}\) refers to the evolution of the state variables, \(\boldsymbol{u}=\left[\boldsymbol{u}_{1}^{\top}, \boldsymbol{u}_{2}^{\top}, \ldots, \boldsymbol{u}_{T-1}^{\top}\right]^{\top}\) is the evolution of control commands. \(\boldsymbol{Q}\) represents the precision matrices, and \(\boldsymbol{R}\) is the control weight matrices.
1. Define parameters#
param = {
"nbData": 200, # Number of data points
"nbVarPos": 7, # Dimension of position data
"nbDeriv": 2, # Number of static and dynamic features (2 -> [x,dx])
"dt": 1e-2, # Time step duration
"rfactor": 1e-8 # Control cost
}
param["nb_var"] = param["nbVarPos"] * param["nbDeriv"] # Dimension of state vector
3. Define linear system#
The simplified linear system can be described as \(\boldsymbol{x}_{t+1}=\boldsymbol{A}_{t} \boldsymbol{x}_{t}+\boldsymbol{B}_{t} \boldsymbol{u}_{t}\). By converting the time sequential evolution into matrix form, we can yield a compact form, \(\boldsymbol{x}=\boldsymbol{S}_{\boldsymbol{x}} \boldsymbol{x}_{1}+\boldsymbol{S}_{\boldsymbol{u}} u\).
In detail,
def set_dynamical_system(param: Dict):
A1d = np.zeros((param["nbDeriv"], param["nbDeriv"]), dtype=np.float32)
B1d = np.zeros((param["nbDeriv"], 1), dtype=np.float32)
for i in range(param["nbDeriv"]):
A1d += np.diag(np.ones(param["nbDeriv"] - i), i) * param["dt"] ** i * 1 / factorial(i)
B1d[param["nbDeriv"] - i - 1] = param["dt"] ** (i + 1) * 1 / factorial(i + 1)
A = np.kron(A1d, np.identity(param["nbVarPos"], dtype=np.float32))
B = np.kron(B1d, np.identity(param["nbVarPos"], dtype=np.float32))
nb_var = param["nb_var"] # Dimension of state vector
# Build Sx and Su transfer matrices
Su = np.zeros((nb_var * param["nbData"], param["nbVarPos"] * (param["nbData"] - 1)))
Sx = np.kron(np.ones((param["nbData"], 1)), np.eye(nb_var, nb_var))
M = B
for i in range(1, param["nbData"]):
Sx[i * nb_var:param["nbData"] * nb_var, :] = np.dot(Sx[i * nb_var:param["nbData"] * nb_var, :], A)
Su[nb_var * i:nb_var * i + M.shape[0], 0:M.shape[1]] = M
M = np.hstack((np.dot(A, M), B)) # [0,nb_state_var-1]
return Su, Sx
4. Solve LQT#
Substituting \(\boldsymbol{x}=\boldsymbol{S}_{\boldsymbol{x}} \boldsymbol{x}_{1}+\boldsymbol{S}_{\boldsymbol{u}} \boldsymbol{u}\) into Equ. 1, we can get the solution
The residuals of this least squares solution provides information about the uncertainty of this estimate, in the form of a full covariance matrix (at control trajectory level)
def get_u_x(param: Dict, start_pose: np.ndarray, muQ: np.ndarray, Q: np.ndarray, R: np.ndarray, Su: np.ndarray, Sx: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
x0 = start_pose.reshape((14, 1))
u_hat = np.linalg.inv(Su.T @ Q @ Su + R) @ Su.T @ Q @ (muQ - Sx @ x0)
# x= S_x x_1 + S_u u
x_hat = (Sx @ x0 + Su @ u_hat).reshape((-1, param["nb_var"]))
return u_hat, x_hat