I am trying to implement this QCQP optimisation problem in a more efficient way.
The optimisation aims to smooth an existing 3D trajectory represented by "P" which contains "n" 3D coordinates. The "n" variables called "Q" are initialised to be equal to "P". We want therefore optimise those 3D coordinates according to the following constraints and objective function:
\begin{align} \min_{Q_3,\ldots,Q_{n-2}} \quad& \sum_{k=2}^{n-1}\|2Q_k-Q_{k-1}-Q_{k+1}\|^2\\ \mathrm{s.t.} \quad & Q_1=P_1, \quad Q_2 = P_1 + d\cdot\frac{v_1}{\|v_1\|}\\ \quad &Q_n = P_n,\quad Q_{n-1} = P_n - d\cdot\frac{v_{n-1}}{\|v_{n-1}\|}\\ \quad &Q_k\in\mathcal B_k, \quad k=3,\ldots, n-1\\ \quad& \|2Q_k-Q_{k-1}-Q_{k+1}\|\leqslant\min\left\{\frac{d^2}{R_{\mathrm{min}}}, \alpha_k\left(\frac d{v_k}\right)^2\right\}, \quad k=2,\ldots,n-1. \end{align}
I have managed to implement this in Matlab using CVX but now I need better performances to work in real time. I was trying to convert my code from Matlab to C++ but CVX does not support Matlab Coder so that the only solution would be either to use a different matlab toolbox that supports it or better a c++ library to solve this problem directly.
I don't have much experience in this field and I find very difficult to implement it in matrix form and without a user-friendly interface.
I would be grateful for any help
thanks