God morgon matematikälskare!
Jag har hållit på med kvadratisk programmering förut. Allt handlar om att hitta lambda-uttrycken \(\lambda_i\) och det ska uppfylla dessa Karush Kuhn Tucker villkor.
\(\bigtriangledown f(\bar x) + \sum_{i=1}^m(\lambda_i)\bigtriangledown g_i(\bar x) = 0\)
\(g_i(\bar x) \leq 0 \forall i\)
\(\lambda_i g_i(\bar x) = 0 \forall i\)
\(\lambda_i \geq \forall i\)
Detta betyder att om vi har vektorn \(\bar x\) och vi tror att det är våran lokala minimum. Men om Karush Kuhn Tucker villkoren uppfylls, så har vi hittat vårat globala minimum. Uttrycket \(g_i(\bar x)\) är våra begränsningar.
Villkoren löser inte det kvadratiska problemet. För att lösa problemet så använder vi exempelvis Frank-Wolfe's metod, eller modifierad Simplex. Enklaste är om vi använder qpOASES_e. Vilket vi ska göra!
Kvadratisk programmering på en mikrokontroller - Optimering
Re: Kvadratisk programmering på en mikrokontroller - Optimer
Nu börjar vi igen.
Jag har nyss laddat ned qpOASES_e och sett två intressanta mappar - include och src. Mappen src innehåller .c filer och include innehåller .h filer.
Jag har ett litet problem att "länka" till .h filerna. Vet ni hur man gör?
Jag har nyss laddat ned qpOASES_e och sett två intressanta mappar - include och src. Mappen src innehåller .c filer och include innehåller .h filer.
Jag har ett litet problem att "länka" till .h filerna. Vet ni hur man gör?
Du har inte behörighet att öppna de filer som bifogats till detta inlägg.
Re: Kvadratisk programmering på en mikrokontroller - Optimer
Det finns ju trevlig cmake-definition så att bygga för host torde vara enkelt (på mac fungerar det klockrent) se t.ex. https://asciinema.org/a/x5NTAqv5iRgYauabqYM2VqQtf
Provade sen att dundra hem cross compile toolchain för ARM (typ för STM/blue-pill m.m.) https://asciinema.org/a/L7cUHeHVIZvd6314lYZU7ocYO
Värt att notera att ett "install" target skapas som paketerar ihop headerfiler följande filer är vad som kopieras:
Under förutsättningen att du lägger till mappen där du har headerfilerna ovan till vart gcc letar dvs med växeln
du kan också lägga headerfiler och allt tillsammans med ditt program bör fungera lika bra. Och glöm inte att länka med
ps. från asciinema så är det bara att markera, copy-paste om det är något av värde som du ser 
ds.
Provade sen att dundra hem cross compile toolchain för ARM (typ för STM/blue-pill m.m.) https://asciinema.org/a/L7cUHeHVIZvd6314lYZU7ocYO
Värt att notera att ett "install" target skapas som paketerar ihop headerfiler följande filer är vad som kopieras:
- Bounds.h
Constraints.h
Matrices.h
QProblem.h
UnitTesting.h
Constants.h
Flipper.h
MessageHandling.h
QProblemB.h
Utils.h
ConstraintProduct.h
Indexlist.h
Options.h
Types.h
extras/OQPinterface.h
Kod: Markera allt
#include <qpOASES_e.h>Kod: Markera allt
-IKod: Markera allt
-l qpOASES_eRe: Kvadratisk programmering på en mikrokontroller - Optimer
Tackar! Synd att jag inte hann säga till men jag har faktiskt fått min QP-lösare att fungera och den stämmer överens med MATLAB's Quadprog!
Mycket glädjande.
Då minimerar jag:
\(\frac{1}{2}x^THx + x^Tg\)
med begränsingarna
\(lb \leq x \leq ub\)
\(lbA \leq Ax \leq ubA\)
Matris \(H\) är en hessianmatris om ni undrar. Det betyder i praktisk mening att \(H \geq 0\) och \(H\) är symmetrisk.
Detta betyder att jag kan optimera in och utsignaler hos ett system och även begränsa hastigeterna i systemet. Har ni något förslag på en reglermodell? Tänkte om ni är intresserade? Ge mig något förslag på en linjär modell, eller olinjär om den inte alls är för svår. Föredrar linjär helst
För er som inte har en MATLAB licens från Mathworks, kan använda GNU Octave's QP-lösare. Den fungerar lika bra.
Här är ett exempel på körning i GNU Octave
Här är ett exempel på en körning i C.
Och resultatet blir xOpt likadant som GNU Octave visar sin \(x\) vektor.
Jag bifogar koden och projektet. Kan ni testa om det går att komplimera på en liten värre mikroprocessor? Jag talar inte om 8-bits processorer. Jag talar om 32-bits processorer som har så kallat lite mera kräm att ge?
Mycket glädjande.
Då minimerar jag:
\(\frac{1}{2}x^THx + x^Tg\)
med begränsingarna
\(lb \leq x \leq ub\)
\(lbA \leq Ax \leq ubA\)
Matris \(H\) är en hessianmatris om ni undrar. Det betyder i praktisk mening att \(H \geq 0\) och \(H\) är symmetrisk.
Detta betyder att jag kan optimera in och utsignaler hos ett system och även begränsa hastigeterna i systemet. Har ni något förslag på en reglermodell? Tänkte om ni är intresserade? Ge mig något förslag på en linjär modell, eller olinjär om den inte alls är för svår. Föredrar linjär helst
För er som inte har en MATLAB licens från Mathworks, kan använda GNU Octave's QP-lösare. Den fungerar lika bra.
Här är ett exempel på körning i GNU Octave
Kod: Markera allt
>> H = [1 4; 4 5]
H =
1 4
4 5
>> g = [1.5; 1.0]
g =
1.5000
1.0000
>> lb = [0.5; -2.0]
lb =
0.50000
-2.00000
>> ub = [10.0; 2.0]
ub =
10
2
>> lbA = -1
lbA = -1
>> ubA = 2
ubA = 2
>> [x, obj, info, lambda] = qp([], H, g, [], [], lb, ub, lbA, A, ubA)
x =
3
-2
obj = -7
info =
scalar structure containing the fields:
solveiter = 7
info = 0
lambda =
0.00000
0.00000
1.25000
0.00000
0.00000
7.00000
>>
Kod: Markera allt
#include <stdio.h>
#include <stdlib.h>
#include "qpOASES/Header/qpOASES_e.h"
int main(void) {
USING_NAMESPACE_QPOASES
/* Setup data of first QP. */
real_t H[2*2] = { 1.0, 4.0,
4.0, 5.0};
real_t A[1*2] = { 0.5, -0.25};
real_t g[2] = { 1.5,
1.0 };
real_t lb[2] = { 0.5,
-2.0 };
real_t ub[2] = { 10.0,
2.0 };
real_t lbA[1] = { -1.0 };
real_t ubA[1] = { 2.0 };
/* Setting up QProblem object. */
static Options options;
static QProblem example;
int nWSR;
real_t xOpt[2];
real_t yOpt[2+1];
QProblemCON( &example,2,1,HST_UNKNOWN );
Options_setToDefault( &options );
QProblem_setOptions( &example,options );
/* Solve first QP. */
nWSR = 6;
QProblem_init( &example,H,g,A,lb,ub,lbA,ubA, &nWSR,0 );
/* Get and print solution of first QP. */
QProblem_getPrimalSolution( &example,xOpt );
QProblem_getDualSolution( &example,yOpt );
printf( "\nxOpt = [ %e, %e ]; yOpt = [ %e, %e, %e ]; objVal = %e\n\n",
xOpt[0],xOpt[1],yOpt[0],yOpt[1],yOpt[2], QProblem_getObjVal( &example ) );
return 0;
}
Kod: Markera allt
xOpt = [ 3.000000e+00, -2.000000e+00 ]; yOpt = [ 0.000000e+00, 1.250000e+00, -7.000000e+00 ]; objVal = -7.000000e+00
#################### qpOASES -- QP NO. 1 #####################
Iter | StepLength | Info | nFX | nAC
----------+------------------+------------------+---------+---------
0 | 1.250000e-01 | REM BND 1 | 1 | 0
1 | 8.571429e-02 | REM BND 0 | 1 | 0
2 | 1.898734e-02 | ADD BND 1 | 2 | 0
3 | 1.035824e-01 | REM BND 0 | 1 | 0
4 | 1.422414e-01 | ADD CON 0 | 1 | 1
5 | 1.000000e+00 | QP SOLVED | 1 | 1
Du har inte behörighet att öppna de filer som bifogats till detta inlägg.
Re: Kvadratisk programmering på en mikrokontroller - Optimer
Antar att vi har våran tillståndsmodell som vi har antingen modulerat fram via mätdata eller skrivit den för hand. Det rekommenderas att bygga från mätdata - alla gånger.
\(x(k+1) = Ax(k) + Bu(k)\)
\(y(k) = Cx(k)\)
Där efter så bygger vi \(\Gamma\) och \(\Phi\) från
\(x(2) = Ax(1) + Bu(1)\\
x(3) = Ax(2) + Bu(2) = A(Ax(1) + Bu(1)) + Bu(2)\\
x(4) = Ax(3) + Bu(3) = A(A(Ax(1) + Bu(1)) + Bu(2)) + Bu(3)\\
x(5) = Ax(4) + Bu(4) = A(A(A(Ax(1) + Bu(1)) + Bu(2)) + Bu(3)) + Bu(3)\\
x(6) = Ax(5) + Bu(5) = A(A(A(A(Ax(1) + Bu(1)) + Bu(2)) + Bu(3)) + Bu(3)) + Bu(3)\\ \vdots \\ x(n+1) = A^{n} + A^{n}B + A^{n-1}B + A^{n-2}B + \dots + AB + B\)
Därför att vi använder våran tillståndsmodell igenom att återanvända \(x(k+1)\) nu nuvarande \(x(k)\). Vi kan sammanfatta hela med.
\(\begin{bmatrix}
x_1\\
x_2\\
x_3\\
x_4\\
\vdots\\
x_{n}
\end{bmatrix} = \begin{bmatrix}
CA\\
CA^2\\
CA^3\\
CA^4\\
\vdots\\
CA^{n-1}
\end{bmatrix}x_0+\begin{bmatrix}
CB & 0 & 0 & 0 & \dots & 0\\
CAB & CB & 0 & 0 & \dots & 0\\
CA^2B & CAB & CB & 0 & \dots& 0\\
CA^3B & CA^2B & AB & CB & \dots& 0\\
\vdots & \vdots & \vdots &\ddots &\ddots & 0\\
CA^{n-2}B & CA^{n-3}B & CA^{n-4}B & CA^{n-5}B & \dots & CB
\end{bmatrix} \begin{bmatrix}
u_1\\
u_2\\
u_3\\
u_4\\
\vdots\\
u_n
\end{bmatrix}\)
Där \(\Gamma\) är den stora lågtriangulära toeplitz matrisen och \(\Phi\) är den avlånga och smala matrisen. Vårat mål är att hitta \(U = \begin{bmatrix}
u_1\\
u_2\\
u_3\\
u_4\\
\vdots\\
u_n
\end{bmatrix}\)
När vi sätter \(R = \begin{bmatrix}
x_1\\
x_2\\
x_3\\
x_4\\
\vdots\\
x_{n}
\end{bmatrix}\)
Vi sätter in exakt allt på denna formel:
\(J = \frac{1}{2}U^TQU + c^TU\)
Där \(Q = \Gamma^T \alpha I \Gamma\) och \(c = \Gamma^T Q \Phi x_0 - \Gamma^T Q R\)
\(\alpha I\) ska vara mellan 0 och 1 för att undvika hård styrning av insignalerna.
Notera att vectorn \(c\) skall alltid uppdateras med nya tillståndsvektorn \(x_0\) för varje iteration. Du ska estimera \(x_0\) från ett kalman filter.
Nu fokuserar vi på begränsingarna:
\(ulb \leq U \leq uub\)
\(ylb \leq \Gamma U \leq yub\)
Där \(yub = MAX_Y - \Phi x_0\) och \(ylb = MIN_Y - \Phi x_0\)
För \(ulb, uub\) så är det bara sätta en enkel vektor t.ex. rada upp 1,1,1,1,1,1 om \(U\) ej får vara över 1 för alla element.
För enkelhetens skull så har jag varit snäll och gjort ett MATLAB-skript till er som är kopatibelt med GNU Octave.
Låt oss säga att jag använder dessa matriser som ska symbolisera ett dynamiskt beteende.
Jag sätter \(yub = 0.7\) och \(ylb = 0\) för hela \(U\)
Jag kör en beräkning. Jag väljer att jag vill titta 10 iterationer i framtiden, 0.5 som mitt "börvärde" och jag ska stå på tillståndet\(x_0 = [0;0]\) från början och alpha är 0.9
Sedan kör jag ett exempel i C.
Resultatet \(U\) är exakt samam i MATLAB och GNU Octave.
Nu använder vi \(U\) för att simulera. Det går fintrimma mer också om man vill.
\(x(k+1) = Ax(k) + Bu(k)\)
\(y(k) = Cx(k)\)
Där efter så bygger vi \(\Gamma\) och \(\Phi\) från
\(x(2) = Ax(1) + Bu(1)\\
x(3) = Ax(2) + Bu(2) = A(Ax(1) + Bu(1)) + Bu(2)\\
x(4) = Ax(3) + Bu(3) = A(A(Ax(1) + Bu(1)) + Bu(2)) + Bu(3)\\
x(5) = Ax(4) + Bu(4) = A(A(A(Ax(1) + Bu(1)) + Bu(2)) + Bu(3)) + Bu(3)\\
x(6) = Ax(5) + Bu(5) = A(A(A(A(Ax(1) + Bu(1)) + Bu(2)) + Bu(3)) + Bu(3)) + Bu(3)\\ \vdots \\ x(n+1) = A^{n} + A^{n}B + A^{n-1}B + A^{n-2}B + \dots + AB + B\)
Därför att vi använder våran tillståndsmodell igenom att återanvända \(x(k+1)\) nu nuvarande \(x(k)\). Vi kan sammanfatta hela med.
\(\begin{bmatrix}
x_1\\
x_2\\
x_3\\
x_4\\
\vdots\\
x_{n}
\end{bmatrix} = \begin{bmatrix}
CA\\
CA^2\\
CA^3\\
CA^4\\
\vdots\\
CA^{n-1}
\end{bmatrix}x_0+\begin{bmatrix}
CB & 0 & 0 & 0 & \dots & 0\\
CAB & CB & 0 & 0 & \dots & 0\\
CA^2B & CAB & CB & 0 & \dots& 0\\
CA^3B & CA^2B & AB & CB & \dots& 0\\
\vdots & \vdots & \vdots &\ddots &\ddots & 0\\
CA^{n-2}B & CA^{n-3}B & CA^{n-4}B & CA^{n-5}B & \dots & CB
\end{bmatrix} \begin{bmatrix}
u_1\\
u_2\\
u_3\\
u_4\\
\vdots\\
u_n
\end{bmatrix}\)
Där \(\Gamma\) är den stora lågtriangulära toeplitz matrisen och \(\Phi\) är den avlånga och smala matrisen. Vårat mål är att hitta \(U = \begin{bmatrix}
u_1\\
u_2\\
u_3\\
u_4\\
\vdots\\
u_n
\end{bmatrix}\)
När vi sätter \(R = \begin{bmatrix}
x_1\\
x_2\\
x_3\\
x_4\\
\vdots\\
x_{n}
\end{bmatrix}\)
Vi sätter in exakt allt på denna formel:
\(J = \frac{1}{2}U^TQU + c^TU\)
Där \(Q = \Gamma^T \alpha I \Gamma\) och \(c = \Gamma^T Q \Phi x_0 - \Gamma^T Q R\)
\(\alpha I\) ska vara mellan 0 och 1 för att undvika hård styrning av insignalerna.
Notera att vectorn \(c\) skall alltid uppdateras med nya tillståndsvektorn \(x_0\) för varje iteration. Du ska estimera \(x_0\) från ett kalman filter.
Nu fokuserar vi på begränsingarna:
\(ulb \leq U \leq uub\)
\(ylb \leq \Gamma U \leq yub\)
Där \(yub = MAX_Y - \Phi x_0\) och \(ylb = MIN_Y - \Phi x_0\)
För \(ulb, uub\) så är det bara sätta en enkel vektor t.ex. rada upp 1,1,1,1,1,1 om \(U\) ej får vara över 1 för alla element.
För enkelhetens skull så har jag varit snäll och gjort ett MATLAB-skript till er som är kopatibelt med GNU Octave.
Kod: Markera allt
function [H, Ay, g0, g1, ylb, yub, ulb, uub] = QP(A, B, C, x0, r, u_lb, u_ub, y_lb, y_ub, horizon, alpha)
% Compute the PHI matrix now!
PHI = phiMat(A, C, horizon);
% Compute the GAMMA matrix now
GAMMA = gammaMat(A, B, C, horizon);
% Compute the tuning matrix - Set it to identity matrix
Q = alpha*eye(size(GAMMA, 1), size(GAMMA, 1));
% Compute H matrix
H = GAMMA'*Q*GAMMA;
% Create g vector. g = g0*x - g1*r where x is the state vector and r is the reference vector
g0 = GAMMA'*Q*PHI;
g1 = GAMMA'*Q;
% Create the matrix for saturation on inputs
Ay = GAMMA;
% Comput best inputs - Try a computation
g = g0*x0 - g1*repmat(r, horizon, 1)
ylb = repmat(y_lb, horizon, 1) - PHI*x0;
yub = repmat(y_ub, horizon, 1) - PHI*x0;
ulb = repmat(u_lb, horizon, 1);
uub = repmat(u_ub, horizon, 1);
u = qp([], H, g, [], [], ulb, uub, ylb, Ay, yub) % Replace qp with quadprog if you are using MATLAB
end
function PHI = phiMat(A, C, N)
% Create the special Observabillity matrix
PHI = [];
for i = 1:N
PHI = vertcat(PHI, C*A^i);
end
end
function GAMMA = gammaMat(A, B, C, N)
% Create the lower triangular toeplitz matrix
GAMMA = [];
for i = 1:N
GAMMA = horzcat(GAMMA, vertcat(zeros((i-1)*size(C*A*B, 1), size(C*A*B, 2)),cabMat(A, B, C, N-i+1)));
end
end
function CAB = cabMat(A, B, C, N)
% Create the column for the GAMMA matrix
CAB = [];
for i = 0:N-1
CAB = vertcat(CAB, C*A^i*B);
end
end
Kod: Markera allt
A =
0.69922 0.34629
-1.03887 0.35293
B =
0.20052
0.69258
C =
1 0
Jag kör en beräkning. Jag väljer att jag vill titta 10 iterationer i framtiden, 0.5 som mitt "börvärde" och jag ska stå på tillståndet\(x_0 = [0;0]\) från början och alpha är 0.9
Kod: Markera allt
[H, Ay, g0, g1, ylb, yub, ulb, uub] = QP(sysd.A, sysd.B, sysd.C, [0;0], 0.5, 0, 100, 0, 0.7, 10, 0.9);
>> u = 1.57215
0.00000
0.31200
1.41416
0.19002
1.22215
0.35191
1.08564
0.46700
0.98861
Kod: Markera allt
#include <stdio.h>
#include <stdlib.h>
#include "qpOASES/Header/qpOASES_e.h"
int main() {
/*
* In this example we have selected the following configuration inside Confiugration.h
* NOTICE THAT YOU NEED A CPU that support "double" data type here if you want correct calculations
* SELECTED_CONTROL_STRATEGY MANUAL
* ADIM 2
* YDIM 1
* RDIM 1
* HORIZON 10
*/
/**
* State space model that are being used with sample time 0.5
*
A =
0.69922 0.34629
-1.03887 0.35293
B =
0.20052
0.69258
C =
1 0
*/
// Create the objective function
real_t H[HORIZON*RDIM*HORIZON*RDIM] = {
0.2805315, 0.2018439, 0.0426243, -0.0759651, -0.1036495, -0.0619421, -0.0040178, 0.0286614, 0.0272949, 0.0098612,
0.2018439, 0.2778442, 0.1994990, 0.0429871, -0.0714696, -0.0964493, -0.0568636, -0.0070791, 0.0149778, 0.0086050,
0.0426243, 0.1994990, 0.2757980, 0.1998156, 0.0469099, -0.0651867, -0.0920177, -0.0595349, -0.0190196, -0.0013312,
-0.0759651, 0.0429871, 0.1998156, 0.2757491, 0.1992087, 0.0459379, -0.0658723, -0.0916045, -0.0576876, -0.0164965,
-0.1036495, -0.0714696, 0.0469099, 0.1992087, 0.2682287, 0.1871637, 0.0374421, -0.0607511, -0.0687136, -0.0264218,
-0.0619421, -0.0964493, -0.0651867, 0.0459379, 0.1871637, 0.2489367, 0.1735563, 0.0456445, -0.0240875, -0.0186362,
-0.0040178, -0.0568636, -0.0920177, -0.0658723, 0.0374421, 0.1735563, 0.2393389, 0.1793417, 0.0715045, 0.0112337,
0.0286614, -0.0070791, -0.0595349, -0.0916045, -0.0607511, 0.0456445, 0.1793417, 0.2358515, 0.1637535, 0.0502133,
0.0272949, 0.0149778, -0.0190196, -0.0576876, -0.0687136, -0.0240875, 0.0715045, 0.1637535, 0.1661745, 0.0685843,
0.0098612, 0.0086050, -0.0013312, -0.0164965, -0.0264218, -0.0186362, 0.0112337, 0.0502133, 0.0685843, 0.0361865};
real_t Ay[HORIZON*YDIM*HORIZON*RDIM] = {
0.20052, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000,
0.38004, 0.20052, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000,
0.27824, 0.38004, 0.20052, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000,
0.06225, 0.27824, 0.38004, 0.20052, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000,
-0.10327, 0.06225, 0.27824, 0.38004, 0.20052, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000,
-0.14641, -0.10327, 0.06225, 0.27824, 0.38004, 0.20052, 0.00000, 0.00000, 0.00000, 0.00000,
-0.09141, -0.14641, -0.10327, 0.06225, 0.27824, 0.38004, 0.20052, 0.00000, 0.00000, 0.00000,
-0.00738, -0.09141, -0.14641, -0.10327, 0.06225, 0.27824, 0.38004, 0.20052, 0.00000, 0.00000,
0.04768, -0.00738, -0.09141, -0.14641, -0.10327, 0.06225, 0.27824, 0.38004, 0.20052, 0.00000,
0.05464, 0.04768, -0.00738, -0.09141, -0.14641, -0.10327, 0.06225, 0.27824, 0.38004, 0.20052};
real_t g0[HORIZON*RDIM*ADIM] = {
0.0895555, 0.2675379,
-0.2126023, 0.1248676,
-0.2760081, -0.0300475,
-0.1621426, -0.1061071,
-0.0076856, -0.0926476,
0.0835982, -0.0338390,
0.0884757, 0.0180790,
0.0461764, 0.0363724,
0.0083306, 0.0259372,
-0.0022316, 0.0080912};
real_t g1[HORIZON*RDIM*HORIZON*YDIM] = {
0.18047, 0.34204, 0.25042, 0.05602, -0.09294, -0.13177, -0.08227, -0.00664, 0.04291, 0.04918,
0.00000, 0.18047, 0.34204, 0.25042, 0.05602, -0.09294, -0.13177, -0.08227, -0.00664, 0.04291,
0.00000, 0.00000, 0.18047, 0.34204, 0.25042, 0.05602, -0.09294, -0.13177, -0.08227, -0.00664,
0.00000, 0.00000, 0.00000, 0.18047, 0.34204, 0.25042, 0.05602, -0.09294, -0.13177, -0.08227,
0.00000, 0.00000, 0.00000, 0.00000, 0.18047, 0.34204, 0.25042, 0.05602, -0.09294, -0.13177,
0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.18047, 0.34204, 0.25042, 0.05602, -0.09294,
0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.18047, 0.34204, 0.25042, 0.05602,
0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.18047, 0.34204, 0.25042,
0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.18047, 0.34204,
0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.18047};
// Minimum input signal
real_t ulb[HORIZON*RDIM] = {
0,
0,
0,
0,
0,
0,
0,
0,
0,
0};
// Maximum input signal
real_t uub[HORIZON*RDIM] = {
100,
100,
100,
100,
100,
100,
100,
100,
100,
100};
// Minimum output signal
real_t ylb[HORIZON*YDIM] = {
0,
0,
0,
0,
0,
0,
0,
0,
0,
0};
// Maximum output signal
real_t yub[HORIZON*YDIM] = {
0.7,
0.7,
0.7,
0.7,
0.7,
0.7,
0.7,
0.7,
0.7,
0.7};
// Reference
double r = 0.5;
//Initial state vector - Start with zeros. That works. Let kalman filter estimate the states later
double x0[ADIM];
memset(x0, 0, ADIM*sizeof(float));
// Setting up QProblem object
static QProblem objective;
static Options options;
QProblemCON(&objective, HORIZON*RDIM,HORIZON*YDIM, HST_UNKNOWN);
Options_setToDefault(&options);
QProblem_setOptions(&objective, options);
// Declare best input vector
int nWSR;
real_t best_inputs[HORIZON*RDIM];
// g1r = g1*r
real_t g1R[HORIZON*RDIM];
real_t R[HORIZON*RDIM];
for(int i = 0; i < HORIZON*RDIM; i++)
R[i] = r;
dmul(g1, R, g1R, HORIZON*RDIM, HORIZON*YDIM, 1);
// g0x0 = g0*x0
real_t g0x0[HORIZON*RDIM];
dmul(g0, x0, g0x0, HORIZON*RDIM, ADIM, 1);
// Compute g = g0*x0 - g1*r
real_t g[HORIZON*RDIM];
for(int i = 0; i < HORIZON*RDIM; i++){
g[i] = g0x0[i] - g1R[i];
}
// Solve first QP
nWSR = 20;
QProblem_init(&objective, H, g, Ay, ulb, uub, ylb, yub, &nWSR, 0); // This can not be repeated. Need to be called only ONCE!
// Get the best_inputs
QProblem_getPrimalSolution(&objective, best_inputs);
// Print best_inputs
printf("Best inputs that we can implement into the system = \n");
for(int i = 0; i < HORIZON*RDIM; i++)
printf("%f\n", best_inputs[i]);
printf("\n\n");
// Let say that you are going to do another QP
// Update vector g = g0*x0 - g1*r where x0 is the state from kalman filter and r is our reference vector
// Now solve again
nWSR = 20;
QProblem_hotstart(&objective, g, ulb, uub, ylb, yub, &nWSR, 0); // This can be repeated
printf("Best inputs that we can implement into the system = \n");
for(int i = 0; i < HORIZON*RDIM; i++)
printf("%f\n", best_inputs[i]);
return EXIT_SUCCESS;
}
Kod: Markera allt
1.572167
0.000000
0.311909
1.414425
0.189581
1.222756
0.351202
1.086411
0.466265
0.989330Du har inte behörighet att öppna de filer som bifogats till detta inlägg.
