# LQG with bias rejection for quadcopter attitude control

We're trying to design an attitude controller for a quadcopter.
The system dynamics are given: $$\boldsymbol{\dot{q}} = \frac{1}{2} \boldsymbol{q} \otimes \begin{pmatrix} 0 \\ \vec{\omega} \end{pmatrix} \\ \dot{\vec{\omega}} = \Gamma_n \vec{n} + \Gamma_u \vec{u} - I^{-1} \left(\vec{\omega} \times I \vec{\omega} \right) \\ \dot{\vec{n}} = k_2 \left(k_1 \vec{u} - \vec{n}\right)$$ $$\boldsymbol{q}$$ is the orientation, expressed as a quaternion, $$\otimes$$ is the Hamiltonian quaternion product, $$\vec{\omega}$$ is the angular velocity of the drone, $$\Gamma_n$$ and $$\Gamma_u$$ are constant 3×3 matrices that relate the torque of the motors to the motor speed $$n$$ and the motor control signal $$u$$. $$k_1$$ and $$k_2$$ are constants specific to the motors. $$I$$ is the inertia matrix of the drone.

We've linearized the system around the equilibrium $$\boldsymbol{q} = (1, 0, 0, 0),\ \vec{\omega} = \vec{0},\ \vec{n} = \vec{0}$$, and we've discretized it using zero order hold. This results in system matrices $$A$$, $$B$$, $$C$$, $$D$$.
The first quaternion state is not independent from the other three quaternion components, so the number of states is reduced from 10 to 9 (remove the first row and column of $$A$$ and $$C$$, and remove the first row of $$B$$ and $$D$$).
The state $$x = \begin{pmatrix} q_1 & q_2 & q_3 & \omega_x & \omega_y & \omega_z & n_x & n_y & n_x \end{pmatrix}^T \in \mathbb{R}^{9×1}$$, the output (measurement) $$y = \begin{pmatrix} q_1 & q_2 & q_3 & \omega_x & \omega_y & \omega_z \end{pmatrix}^T \in \mathbb{R}^{6×1}$$, the input (control) $$u = \begin{pmatrix} u_x & u_y & u_z \end{pmatrix}^T \in \mathbb{R}^{3×1}$$.

A full-state LQR controller is used to stabilize the drone, and a Kalman filter is used as an observer.

The first Kalman filter we used is $$\hat{x}_{k+1} = A \hat{x}_k + B u_k + L \left(y_k - C\hat{x}\right)$$ For the system with process disturbances $$\delta_x$$, input disturbances $$\delta_u$$ and sensor noise $$v$$: $$x_{k+1} = A x_k + B u_k + \begin{pmatrix} I_9 & B\end{pmatrix} \begin{pmatrix} \delta_x \\ \delta_u \end{pmatrix}$$
$$y_k = C x_k + D u_k + v$$

Then we used MATLAB to calculate matrix $$L$$:

L = dlqe(A, [ eye(9), B ], C, Q, R);


where:$$\quad Q = \mathbb{E}\left(w w^T\right) \quad w = \begin{pmatrix} \delta_x \\ \delta_u \end{pmatrix} \\ \quad R = \mathbb{E}\left(v v^T\right)$$

Now our goal is to add bias rejection, in other words, find an observer for the system with constant bias $$d$$: $$\begin{pmatrix}x_{k+1} \\ d_{k+1} \end{pmatrix} = \begin{pmatrix} A & 0 \\ 0 & I_6 \end{pmatrix} \begin{pmatrix} x_k \\ d_k \end{pmatrix} + \begin{pmatrix} B \\ 0 \end{pmatrix} u_k + \begin{pmatrix} I_9 & B \\ 0 & 0 \end{pmatrix} \begin{pmatrix} \delta_x \\ \delta_u \end{pmatrix} \\ y_k = \begin{pmatrix} C & I_6 \end{pmatrix} \begin{pmatrix}x_k \\ d_k \end{pmatrix} + D u_k + v$$ We need bias rejection, because our measurements for $$\boldsymbol{q}$$ and $$\vec{\omega}$$ are the result of numerically integrating the angular acceleration measurement. On top of that, these measurements are rather temperature sensitive.

We're pretty much stuck, because the pair $$\left(\begin{pmatrix} C & I_6 \end{pmatrix}, \begin{pmatrix} A & 0 \\ 0 & I_6 \end{pmatrix}\right)$$ is not observable.

If we only want to estimate the bias on $$\vec{\omega}$$, the pair $$\left(\begin{pmatrix} C & 0 \\ & I_3\end{pmatrix}, \begin{pmatrix} A & 0 \\ 0 & I_3 \end{pmatrix}\right)$$ is observable, but MATLAB throws an error when using this system with the dlqe command:

Error using dlqe (line 91)
(A-G*N/R*C,G*(Q-N/R*N')*G') or (C,A) has non minimal modes near unit circle.


Are there any obvious flaws in our reasoning? Is there anything we can do to estimate the bias or drift of our sensor readings?