Error-State LQR Formulation for Quadrotor UAV Trajectory Tracking

The state of the quadrotor UAV is defined as follows, inspired by [1, 4]:

Name	True	Nominal	Error	Composition
Full state	$\bm{x}_{t}$	$\bm{x}$	$\delta\bm{x}$	$\bm{x}_{t}=\bm{x}\oplus\delta\bm{x}$
Position	$\bm{p}_{t}$	$\bm{p}$	$\delta\bm{p}$	$\bm{p}_{t}=\bm{p}+\delta\bm{p}$
Velocity	$\bm{v}_{t}$	$\bm{v}$	$\delta\bm{v}$	$\bm{v}_{t}=\bm{v}+\delta\bm{v}$
Orientation	$\bm{q}_{t}$	$\bm{q}$	$\delta\bm{q}$	$\bm{q}_{t}=\bm{q}\otimes\delta\bm{q}$
Rotation Matrix	$\bm{R}_{t}$	$\bm{R}$	$\delta\bm{R}$	$\bm{R}_{t}=\bm{R}\,\delta\bm{R}$
Exponential Vector			$\delta\bm{\theta}$	$\delta\bm{R}=\exp\left([\delta\bm{\theta}]_{\times}\right)$

The full state of the quadrotor UAV includes the position and velocity of the center of mass, expressed in the inertial frame, as well as the orientation represented as a unit quaternion:

$$\bm{x}=\begin{pmatrix}\bm{p}&\bm{q}&\bm{v}\end{pmatrix}^{T}\in\mathbb{R}^{3}\times S^{3}\times\mathbb{R}^{3}.$$

The error-state contains the $\delta$-values for these quantities, with the important distinction that the orientation error component is represented in exponential coordinates:

$$\delta\bm{\theta}\in\mathbb{R}^{3},\quad\delta\bm{x}=\begin{pmatrix}\delta\bm{p}&\delta\bm{\theta}&\delta\bm{v}\end{pmatrix}^{T}\in\mathbb{R}^{9}.$$

The control of the quadrotor UAV is defined as follows:

Magnitude	True	Nominal	Error	Composition
Full control	$\bm{u}_{t}$	$\bm{u}$	$\delta\bm{u}$	$\bm{u}_{t}=\bm{u}\oplus\delta\bm{u}$
Collective Thrust	$c_{t}$	$c$	$\delta c$	$c_{t}=c+\delta c$
Angular Velocity	$\bm{\omega}_{t}$	$\bm{\omega}$	$\delta\bm{\omega}$	$\bm{\omega}_{t}=(\delta\bm{R})^{T}\bm{\omega}+\delta\bm{\omega}$

The full control of the quadrotor UAV contains the collective thrust and desired angular velocity expressed in the current body frame, $\bm{u}=\begin{pmatrix}c&\bm{\omega}\end{pmatrix}^{T}\in\mathbb{R}^{4}$. The error-state contains the $\delta$-values for these quantities, taking care to ensure that the angular velocity error is represented in the current body frame (with the nominal angular velocity represented in the nominal frame’s coordinates), with $\delta\bm{u}=\begin{pmatrix}\delta c&\delta\bm{\omega}\end{pmatrix}^{T}\in\mathbb{R}^{4}$.

The nominal-state kinematics and dynamics are identical to those of the true state, with appropriate substitutions:

$$\dot{\bm{p}}=\bm{v}$$

(4)

$$\dot{\bm{v}}=\bm{g}+\frac{1}{m}\bm{R}\begin{pmatrix}0\\ 0\\ c\end{pmatrix}$$

(5)

$$\dot{\bm{q}}=\frac{1}{2}\bm{q}\otimes\begin{pmatrix}0\\ \bm{\omega}\end{pmatrix}$$

(6)

We now derive the time derivatives of the error-state $\dot{\delta\bm{x}}$ for each component. Throughout this derivation, we assume the error-state is small, ignore second-order terms, and employ small-angle approximations. First, for the position error:

	$\displaystyle\bm{p}_{t}$	$\displaystyle=\bm{p}+\delta\bm{p},$
	$\displaystyle\delta\bm{p}$	$\displaystyle=\bm{p}_{t}-\bm{p},$
	$\displaystyle\dot{\delta\bm{p}}$	$\displaystyle=\dot{\bm{p}_{t}}-\dot{\bm{p}},$
		$\displaystyle=\bm{v}_{t}-\bm{v},$
		$\displaystyle=\delta\bm{v}.$

We next solve for $\dot{\delta\bm{v}}$, using the approximation $\bm{R}_{t}=\bm{R}(I+[\delta\bm{\theta}]_{\times})+O(\left\lVert\delta\bm{\theta}\right\lVert^{2})$:

	$\displaystyle\bm{v}_{t}$	$\displaystyle=\bm{v}+\delta\bm{v}$
	$\displaystyle\delta\bm{v}$	$\displaystyle=\bm{v}_{t}-\bm{v}$
	$\displaystyle\dot{\delta\bm{v}}$	$\displaystyle=\dot{\bm{v}_{t}}-\dot{\bm{v}}$
		$\displaystyle=\bm{g}+\frac{1}{m}\bm{R}_{t}\begin{pmatrix}0\\ 0\\ c_{t}\end{pmatrix}-\left(\bm{g}+\frac{1}{m}\bm{R}\begin{pmatrix}0\\ 0\\ c\end{pmatrix}\right)$
		$\displaystyle=\frac{1}{m}\bm{R}_{t}\begin{pmatrix}0\\ 0\\ c_{t}\end{pmatrix}-\frac{1}{m}\bm{R}\begin{pmatrix}0\\ 0\\ c\end{pmatrix}$
		$\displaystyle=\frac{1}{m}\left(\bm{R}\,\delta\bm{R}\begin{pmatrix}0\\ 0\\ c_{t}\end{pmatrix}-\bm{R}\begin{pmatrix}0\\ 0\\ c\end{pmatrix}\right)$
		$\displaystyle\approx\frac{1}{m}\left(\bm{R}(I+[\delta\bm{\theta}]_{\times})\begin{pmatrix}0\\ 0\\ c_{t}\end{pmatrix}-\bm{R}\begin{pmatrix}0\\ 0\\ c\end{pmatrix}\right)$		(Small angle approximation of $\delta\bm{R}$)
		$\displaystyle=\frac{1}{m}\left((\bm{R}+\bm{R}[\delta\bm{\theta}]_{\times})\begin{pmatrix}0\\ 0\\ c_{t}\end{pmatrix}-\bm{R}\begin{pmatrix}0\\ 0\\ c\end{pmatrix}\right)$
		$\displaystyle=\frac{1}{m}\left(\bm{R}\begin{pmatrix}0\\ 0\\ c_{t}\end{pmatrix}-\bm{R}\begin{pmatrix}0\\ 0\\ c\end{pmatrix}+\bm{R}[\delta\bm{\theta}]_{\times}\begin{pmatrix}0\\ 0\\ c_{t}\end{pmatrix}\right)$
		$\displaystyle=\frac{1}{m}\bm{R}\left(\begin{pmatrix}0\\ 0\\ \delta c\end{pmatrix}+[\delta\bm{\theta}]_{\times}\begin{pmatrix}0\\ 0\\ c+\delta c\end{pmatrix}\right)$

Lastly, we solve for $\dot{\delta\bm{\theta}}$. We note that the angular velocity of a rigid body expressed in the body frame and the exponential coordinates for the body frame orientation are related by:

$\displaystyle\dot{\bm{\theta}}$

$\displaystyle=J_{r}^{-1}(\bm{\theta})\bm{\omega}\quad\mathrm{with}\quad J_{r}^{-1}(\bm{\theta})=I+\frac{1}{2}[\bm{\theta}]_{\times}+\left(\frac{1}{\left\lVert\bm{\theta}\right\lVert^{2}}-\frac{1+\cos\left\lVert\bm{\theta}\right\lVert}{2\left\lVert\bm{\theta}\right\lVert\sin\left\lVert\bm{\theta}\right\lVert}\right)[\bm{\theta}]_{\times}^{2}$

where $J_{r}^{-1}(\bm{\theta})$ is the inverse of the right Jacobian of $SO(3)$ [5]. Using this fact, we may write:

	$\displaystyle\dot{\delta\bm{\theta}}$	$\displaystyle=J_{r}^{-1}(\delta\bm{\theta})\delta\bm{\omega}$
		$\displaystyle\approx(I+\frac{1}{2}[\delta\bm{\theta}]_{\times})\delta\bm{\omega}$		(Small angle approximation of $J_{r}^{-1}(\delta\bm{\theta})$)

For a nonlinear function $f:\mathbb{R}^{n_{x}}\times\mathbb{R}^{n_{u}}\to\mathbb{R}^{n_{x}}$, the first-order Taylor approximation of $f(\bm{x},\bm{u})$ about some point $(\bm{\bar{x}},\bm{\bar{u}})$ is written as:

	$\displaystyle f(\bm{x},\bm{u})$	$\displaystyle\approx f(\bm{\bar{x}},\bm{\bar{u}})+\frac{\partial f}{\partial\bm{x}}\Bigg{|}_{\bar{\bm{x}},\bar{\bm{u}}}(\bm{x}-\bm{\bar{x}})+\frac{\partial f}{\partial\bm{u}}\Bigg{|}_{\bar{\bm{x}},\bar{\bm{u}}}(\bm{u}-\bm{\bar{u}})$
		$\displaystyle=\left(f(\bm{\bar{x}},\bm{\bar{u}})-\frac{\partial f}{\partial\bm{x}}\Bigg{|}_{\bar{\bm{x}},\bar{\bm{u}}}\bm{\bar{x}}-\frac{\partial f}{\partial\bm{u}}\Bigg{|}_{\bar{\bm{x}},\bar{\bm{u}}}\bm{\bar{u}}\right)+\frac{\partial f}{\partial\bm{x}}\Bigg{|}_{\bar{\bm{x}},\bar{\bm{u}}}\bm{x}+\frac{\partial f}{\partial\bm{u}}\Bigg{|}_{\bar{\bm{x}},\bar{\bm{u}}}\bm{u}$

Notice that the expression:

$$f(\bm{\bar{x}},\bm{\bar{u}})-\frac{\partial f}{\partial\bm{x}}\Bigg{|}_{\bar{\bm{x}},\bar{\bm{u}}}\bm{\bar{x}}-\frac{\partial f}{\partial\bm{u}}\Bigg{|}_{\bar{\bm{x}},\bar{\bm{u}}}\bm{\bar{u}}$$

is nothing but the first-order Taylor approximation of $f$ about $(\bm{\bar{x}},\bm{\bar{u}})$ evaluated at the point $(\bm{0},\bm{0})$, which we can take to be near-zero for $f$ with $f(\bm{0},\bm{0})=\bm{0}$ and a linearization point $(\bm{\bar{x}},\bm{\bar{u}})$ sufficiently close to 0.

Thus, for $f(\bm{0},\bm{0})=\bm{0}$ and $(\bm{\bar{x}},\bm{\bar{u}})$ sufficiently close to 0, we may approximate $f(\bm{x},\bm{u})$ as:

$$f(\bm{x},\bm{u})\approx A\bm{x}+B\bm{u}\quad\textrm{where}\quad A\in\mathbb{R}^{n_{x}\times n_{x}},\,\,B\in\mathbb{R}^{n_{x}\times n_{u}}$$

with:

$$A=\frac{\partial f}{\partial\bm{x}}\Bigg{|}_{\bar{\bm{x}},\bar{\bm{u}}}\quad\textrm{and}\quad B=\frac{\partial f}{\partial\bm{u}}\Bigg{|}_{\bar{\bm{x}},\bar{\bm{u}}}$$

For our purposes, we will write $\dot{\delta\bm{x}}=f(\delta\bm{x},\delta\bm{u})$ and write the linearized system as:

$$\dot{\delta\bm{x}}\approx\frac{\partial f}{\partial\delta\bm{x}}\Bigg{|}_{\overline{\delta\bm{x}},\overline{\delta\bm{u}}}\delta\bm{x}+\frac{\partial f}{\partial\delta\bm{u}}\Bigg{|}_{\overline{\delta\bm{x}},\overline{\delta\bm{u}}}\delta\bm{u}$$

where $\overline{\delta\bm{x}}$ is the current error state and $\overline{\delta\bm{u}}$ is the current error control.

We write out the relevant Jacobians to construct the $A$ and $B$ matrices of the linearized error-state dynamics. These matrices take the form:

$$\frac{\partial f}{\partial\delta\bm{x}}=\begin{pmatrix}0&0&\partial\dot{\delta\bm{p}}/\partial\delta\bm{v}\\ 0&\partial\dot{\delta\bm{\theta}}/\partial\delta\bm{\theta}&0\\ 0&\partial\dot{\delta\bm{v}}/\partial\delta\bm{\theta}&0\end{pmatrix}\quad\frac{\partial f}{\partial\delta\bm{u}}=\begin{pmatrix}0&0\\ 0&\partial\dot{\delta\bm{\theta}}/\partial\delta\bm{\omega}\\ \partial\dot{\delta\bm{v}}/\partial\delta c&0\end{pmatrix}$$

From the above expressions for the error-state dynamics, we take derivatives and arrive at:

	$\displaystyle\frac{\partial\dot{\delta\bm{p}}}{\partial\delta\bm{v}}$	$\displaystyle=I\in\mathbb{R}^{3\times 3}$
	$\displaystyle\frac{\partial\dot{\delta\bm{\theta}}}{\partial\delta\bm{\theta}}$	$\displaystyle=-\frac{1}{2}[\delta\bm{\omega}]_{\times}\in\mathbb{R}^{3\times 3}$
	$\displaystyle\frac{\partial\dot{\delta\bm{v}}}{\partial\delta\bm{\theta}}$	$\displaystyle=-\frac{1}{m}\bm{R}\left[\begin{pmatrix}0\\ 0\\ c+\delta c\end{pmatrix}\right]_{\times}\in\mathbb{R}^{3\times 3}$
	$\displaystyle\frac{\partial\dot{\delta\bm{\theta}}}{\partial\delta\bm{\omega}}$	$\displaystyle=I+\frac{1}{2}[\delta\bm{\theta}]_{\times}\in\mathbb{R}^{3\times 3}$
	$\displaystyle\frac{\partial\dot{\delta\bm{v}}}{\partial\delta c}$	$\displaystyle=\frac{1}{m}\bm{R}\left(I+[\delta\bm{\theta}]_{\times}\right)\begin{pmatrix}0\\ 0\\ 1\end{pmatrix}\in\mathbb{R}^{3\times 1}$

We design an LQR controller to drive the error state to zero, where these quantities are defined in section 2.1. The LQR full-state feedback gain matrix $\bm{K}$ is obtained by minimizing the inifinite-horizon cost function $J(\delta\bm{x},\delta\bm{u})$:

$$J(\delta\bm{x},\delta\bm{u})=\int_{0}^{\infty}(\delta\bm{x}^{T}Q\delta\bm{x}+\delta\bm{u}^{T}R\delta\bm{u})dt$$

for $Q,R\succ 0$. The cost function is minimized by the control policy $\delta\bm{u}=-\bm{K}\delta\bm{x}$. where $\bm{K}$ is given by:

$\displaystyle\bm{K}$

$\displaystyle=R^{-1}B^{T}P$

with $P$ being the solution to the continuous-time Algebraic Riccati Equation. The full control at time $t$ is then given by:

$$\bm{u}_{t}=\bm{u}-\bm{K}\delta\bm{x}$$

As a practical implementation consideration, one must ensure that the $A$ matrix of the linearized error-state system is stable, i.e. $\Re(\lambda_{i})<0$ for all eigenvalues $\lambda_{i}$ in the spectrum of $A$. A small amount of regularization may be added to $A$ to ensure the ARE has a solution.

In a cascaded fashion, we use a simple bodyrate P-controller with feedback linearization to track the desired angular velocities produced by the LQR controller. The bodyrate controller is given by:

$$\bm{\tau}=J(K_{p}(\bm{\omega}-\bm{\omega}_{t}))+\bm{\omega}_{t}\times J\bm{\omega}_{t}$$

where $J$ is the inertia tensor of the quadrotor UAV and $K_{p}=\operatorname{diag}(K_{p,1},K_{p,2},K_{p,3})$ is the proportional gain matrix.