Control Scheme
I am interested in putting together a method to determine actuator outputs for a multirotor.
In the ideal case the system can be represented as
eqn 1: $B \vec{v} = \vec{u}$
Where $\vec{v}$ is the vector of our commanded torques and thrusts. $\vec{v} = \begin{bmatrix} \vec{m} \\ \vec{t}\end{bmatrix}$, $B$ is the vehicle dependent matrix that maps our commands to our actuator outputs, and $\vec{u}$ represents the outputs for each actuator.
Unfortunately the values of $\vec{u}$ are limited to a certain range so it is not always possible to satisfy eqn 1 for any value $\vec{v}$.
For this reason it is necessary to make an adjustment to $\vec{v}$ so that $\vec{u}$ lies within the allowed range. The new system looks like:
eqn 2: $\vec{u} = B (\vec{v} + \vec{a})$
Where $\vec{a}$ is a vector that adjusts each of the inputs from $\vec{v}$
Under ideal conditions $\vec{a}$ should be a $0$ vector but when the actuator outputs are saturated then the $\vec{a}$ should take on a value such that none of the outputs are saturated.
For example, I would to find $\vec{a}$ such that $u$ lies within the available space $-1 \leq U_i \leq 1$ and also such that $\bigl|\vec{a}\bigr|$ is minimized.
Can anyone suggest a method to determine $\vec{a}$?
As I understand, this is the problem you are trying to solve:
$\underset{\vec{a}}{min}~|\vec{a}|\\ \text{s.t.} \\ -1~\leq ~B(\vec{v}+\vec{a})~\leq1 $
We can change the objective function for $|a|^2$. This is a quadratic objective function that is convex. The problem then becomes:
$\underset{\vec{a}}{min}~|\vec{a}|^2\\ \text{s.t.} \\ -1~\leq ~B(\vec{v}+\vec{a})~\leq1 $
The constraints are all linear, the objective function is convex and quadratic. I would choose an Interior-Point method to solve it (such as IPOPT or quadprog in Matlab).
Also the problem should not have problems converging. It is also possible to obtain an analytical jacobian and hessian which would allow quicker convergence.
Depending on the size of the problem and if B is sparse one could look for more efficient implementations.