模型预测控制(Model Predict Control)利用一个已有的模型、系统当前的状态和未来的控制量去预测系统未来的输出;这个输出的长度是控制周期的整数倍;由于未来的控制量是未知的,需要根据一定的条件进行求解,以得到未来的控制量序列,并在每个控制周期结束后,系统根据当前实际状态重新预测系统未来的输出。因此模型预测控制有三个关键步骤,分别是:预测模型、滚动优化和反馈校正。
-
预测模型:预测模型是控制的基础,根据对象的历史信息和未来输入,预测系统未来的输出。预测模型有:状态空间方程、传递函数、阶跃响应、脉冲响应、神经网络模型等等。
-
滚动优化:模型预测控制通过控制某一性能指标的最优来确定控制序列,但优化不是一次离线进行,而是反复在线进行的。
-
反馈校正:为了防止模型失配或者干扰等引起控制对理想状态的偏差,在新的采样时刻,首先检测对象的实际输出,并利用这一实时信息对基于模型的预测结果进行修正,然后再进行新的优化。
在此,选择状态空间方程作为预测模型。
1 状态空间方程
状态空间方程为:
{x˙=Ax+Bu+Ddisy=Cx+Du(1)
\begin{cases}
\dot{x} = A x + B u + D_{dis} \\
y = C x + D u
\end{cases}
\tag{1}
{x˙=Ax+Bu+Ddisy=Cx+Du(1)
连续状态空间方程需要离散化,常用的离散化方法有:欧拉公式离散化,后向差分和双线性变换离散化
Matrix | Euler | Backward Rect | Bilinear Transform |
---|---|---|---|
AdA_dAd | I+A∗ΔtI + A*\Delta tI+A∗Δt | (I−A∗Δt)−1(I - A*\Delta t)^{-1}(I−A∗Δt)−1 | (I−A∗Δt2)−1(I+A∗Δt2)(I - \frac{A*\Delta t}{2})^{-1} (I + \frac{A*\Delta t}{2})(I−2A∗Δt)−1(I+2A∗Δt) |
BdB_dBd | B∗ΔtB* \Delta tB∗Δt | (I−A∗Δt)−1B∗Δt(I - A*\Delta t)^{-1} B * \Delta t(I−A∗Δt)−1B∗Δt | (I−A∗Δt2)−1B∗Δt(I - \frac{A*\Delta t}{2})^{-1} B * \Delta t(I−2A∗Δt)−1B∗Δt |
Dd,disD_{d,dis}Dd,dis | Ddis∗ΔtD_{dis} * \Delta tDdis∗Δt | (I−A∗Δt)−1Ddis∗Δt(I - A*\Delta t)^{-1} D_{dis} * \Delta t(I−A∗Δt)−1Ddis∗Δt | (I−A∗Δt2)−1Ddis∗Δt(I - \frac{A*\Delta t}{2})^{-1} D_{dis} * \Delta t(I−2A∗Δt)−1Ddis∗Δt |
CdC_dCd | CCC | C(I−A∗Δt)−1C (I - A*\Delta t)^{-1}C(I−A∗Δt)−1 | C(I−A∗Δt2)−1C (I - \frac{A*\Delta t}{2})^{-1}C(I−2A∗Δt)−1 |
DdD_dDd | DDD | D+C(I−A∗Δt)−1B∗ΔtD + C (I - A*\Delta t)^{-1} B * \Delta tD+C(I−A∗Δt)−1B∗Δt | D+C(I−A∗Δt2)−1B∗Δt2D + C (I - \frac{A*\Delta t}{2})^{-1} \frac{B * \Delta t}{2}D+C(I−2A∗Δt)−12B∗Δt |
离散的状态空间方程为:
{xk+1=Adxk+Bduk+Dd,disyk=Cdxk+Dduk(2)
\begin{cases}
x_{k+1} = A_d x_k + B_d u_k + D_{d,{dis}} \\
y_k = C_d x_k + D_d u_k
\end{cases}
\tag{2}
{xk+1=Adxk+Bduk+Dd,disyk=Cdxk+Dduk(2)
2 预测模型
根据经验模型(状态空间方程)和当前状态、未来控制量,可以预测未来的输出量。
{xk+1=Adxk+Bduk+Dd,disxk+2=Adxk+1+Bduk+1+Dd,dis=Ad(Adxk+Bduk+Dd,dis)+Bduk+1+Dd,dis=Ad2xk+(AdBduk+Bduk+1)+AdDd,dis+Dd,disxk+3=Adxk+2+Bduk+2+Dd,dis=Ad2xk+1+(AdBduk+1+Bduk+2)+AdDd,dis+Dd,dis=Ad3xk+(Ad2Bduk+AdBduk+1+Bduk+2)+Ad2Dd,dis+AdDd,dis+Dd,dis⋮xk+Np=AdNpxk+(AdNp−1Bduk+⋯+AdNp−Nc+1Bduk+1+AdNp−NcBduk+Nc−1)+(AdNp−1Dd,dis+⋯+AdDd,dis+Dd,dis)(3)
\begin{cases}
x_{k+1} &= A_d x_k + B_d u_k + D_{d,{dis}} \\
x_{k+2} &= A_d x_{k+1} + B_d u_{k+1} + D_{d,{dis}} \\
&= A_d(A_d x_k + B_d u_k + D_{d,{dis}}) + B_d u_{k+1} + D_{d,{dis}} \\
&= A^2_{d} x_k + (A_d B_d u_k + B_d u_{k+1}) + A_d D_{d,{dis}} + D_{d,{dis}} \\
x_{k+3} &= A_d x_{k+2} + B_d u_{k+2} + D_{d,{dis}} \\
&= A^2_{d} x_{k+1} + (A_dB_du_{k+1} + B_d u_{k+2}) + A_dD_{d,{dis}} + D_{d,{dis}} \\
&= A^3_d x_k + (A^2_dB_du_{k} + A_dB_du_{k+1} + B_d u_{k+2}) + A^2_d D_{d,{dis}} + A_dD_{d,{dis}} + D_{d,{dis}} \\
\vdots \\ x_{k+N_p} &= A^{N_p}_d x_{k} + (A^{N_p -1}_dB_du_{k} + \cdots + A^{N_p-N_c+1}_dB_du_{k+1} + A^{N_p-N_c}_d B_d u_{k+N_c-1}) \\
&+(A^{N_p -1}_d D_{d,{dis}} + \cdots + A_d D_{d,{dis}} + D_{d,{dis}})
\end{cases}
\tag{3}
⎩⎨⎧xk+1xk+2xk+3⋮xk+Np=Adxk+Bduk+Dd,dis=Adxk+1+Bduk+1+Dd,dis=Ad(Adxk+Bduk+Dd,dis)+Bduk+1+Dd,dis=Ad2xk+(AdBduk+Bduk+1)+AdDd,dis+Dd,dis=Adxk+2+Bduk+2+Dd,dis=Ad2xk+1+(AdBduk+1+Bduk+2)+AdDd,dis+Dd,dis=Ad3xk+(Ad2Bduk+AdBduk+1+Bduk+2)+Ad2Dd,dis+AdDd,dis+Dd,dis=AdNpxk+(AdNp−1Bduk+⋯+AdNp−Nc+1Bduk+1+AdNp−NcBduk+Nc−1)+(AdNp−1Dd,dis+⋯+AdDd,dis+Dd,dis)(3)
其中,NpN_pNp是预测时域,NcN_cNc是控制时域,并且Np≥NcN_p \geq N_cNp≥Nc,在Nc<k≤NpNc < k \leq N_pNc<k≤Np时域内,uk=0u_k = 0uk=0。
将公式(3)整理可得:
X=FX0+ΦU+E(4)
X = F X_{0} + \Phi U + E
\tag{4}
X=FX0+ΦU+E(4)
其中:
{X=[xk+1,xk+2,⋯ ,xk+NP]T;X0=xk;U=[uk,uk+2,⋯ ,uk+Nc−1]T;F=[Ad,Ad2,⋯ ,AdNp]T;Φ=[Bd0⋯0AdBdBd⋯0⋮⋮⋱⋮ANp−1dBdANp−2dBd⋯AdNp−NcBd]E=[Dd,dis,AdDd,dis+Dd,dis,⋯ ,∑i=0Np−1AkiDd,dis]T(5)
\begin{cases}
X = [x_{k+1},x_{k+2},\cdots,x_{k+N_P}]^T; \\
X_0 = x_k; \\
U = [u_{k},u_{k+2},\cdots,u_{k+N_c-1}]^T; \\
F = [A_d, A^{2}_d,\cdots, A^{N_p}_d]^T; \\
\Phi = \left[\begin{matrix}
B_d &0 & \cdots & 0 \\
A_d B_d & B_d & \cdots & 0 \\
\vdots & \vdots & \ddots & \vdots \\
A^{N_p -1}d B_d & A^{N_p -2}d B_d & \cdots & A^{N_p-N_c}_d B_d \\
\end{matrix} \right] \\
E = [D_{d,dis}, A_d D_{d,dis} + D_{d,dis},\cdots,\sum_{i=0}^{N_p-1}A^i_kD_{d,dis}]^T \\
\end{cases}
\tag{5}
⎩⎨⎧X=[xk+1,xk+2,⋯,xk+NP]T;X0=xk;U=[uk,uk+2,⋯,uk+Nc−1]T;F=[Ad,Ad2,⋯,AdNp]T;Φ=BdAdBd⋮ANp−1dBd0Bd⋮ANp−2dBd⋯⋯⋱⋯00⋮AdNp−NcBdE=[Dd,dis,AdDd,dis+Dd,dis,⋯,∑i=0Np−1AkiDd,dis]T(5)
假设Dd=0D_d=0Dd=0,由公(4)可以得到系统的预测输出量:
Y=CdFX0+CdΦU+CdE=FyX0+ΦyU+Ey=[yk+1,yk+2,⋯ ,yk+Np]T(6)
Y = C_d FX_{0} + C_d \Phi U + C_d E = F_yX_0 +\Phi _{y} U + E_y = [y_{k+1},y_{k+2},\cdots,y_{k+N_p}]^T
\tag{6}
Y=CdFX0+CdΦU+CdE=FyX0+ΦyU+Ey=[yk+1,yk+2,⋯,yk+Np]T(6)
3 代价函数
以系统的期望输出与预测输出的误差最小作为代价函数
J=(Y−Yref)TQe(Y−Yref)+UTReU=(FyX0+ΦyU+Ey−Yref)TQe(FyX0+ΦyU+Ey−Yref)+UTReU=UT(ΦyTQeΦy+Re)U+UT[2ΦyTQe(FyX0+Ey−Yref)]+(FyX0+Ey−Yref)TQe(FyX0+Ey−Yref)(7)
\begin{aligned}
J &= (Y - Y_{ref})^T Q_e (Y - Y_{ref}) + U^T R_e U \\
&= (F_yX_0 +\Phi _{y} U + E_y - Y_{ref})^T Q_e (F_yX_0 +\Phi _{y} U + E_y - Y_{ref}) + U^T R_e U \\
&= U^T(\Phi ^T_{y} Q_e \Phi _{y} + R_e)U + U^T[2\Phi ^T_{y} Q_e (F_y X_0 + E_y -Y_{ref})] +
(F_y X_0 + E_y -Y_{ref})^T Q_e (F_y X_0 + E_y -Y_{ref})
\end{aligned}
\tag{7}
J=(Y−Yref)TQe(Y−Yref)+UTReU=(FyX0+ΦyU+Ey−Yref)TQe(FyX0+ΦyU+Ey−Yref)+UTReU=UT(ΦyTQeΦy+Re)U+UT[2ΦyTQe(FyX0+Ey−Yref)]+(FyX0+Ey−Yref)TQe(FyX0+Ey−Yref)(7)
则代价函数可以简写为:
J=UTHU+2UTG+P(8)
J = U^T H U + 2U^TG+P
\tag{8}
J=UTHU+2UTG+P(8)
其中,QQQ是状态权重矩阵,RRR是控制输入权重矩阵,PPP是常量,显然代价函数是一个QPQPQP问题的求解。
H=ΦyTQeΦy+Re;G=ΦyTQeM;P=MTQeM;M=FyX0+Ey−YrefQe=[Q0⋯00Q⋯0⋮⋮⋱⋮00⋯Q]Np×NpRe=[R0⋯00R⋯0⋮⋮⋱⋮00⋯R]Nc×Nc(9)
\begin{array}{cc}
H = \Phi ^T_{y} Q_e \Phi _{y} + R_e; \\
G = \Phi ^T_{y} Q_e M; \\
P =M^T Q_e M; \\
M = F_y X_0 + E_y -Y_{ref} \\
Q_e=\left[\begin{matrix}
Q &0 &\cdots &0 \\
0 &Q &\cdots &0 \\
\vdots &\vdots &\ddots &\vdots \\
0 &0 &\cdots &Q \\
\end{matrix}
\right]_{N_p \times N_p} \\
R_e=\left[\begin{matrix}
R &0 &\cdots &0 \\
0 &R &\cdots &0 \\
\vdots &\vdots &\ddots &\vdots \\
0 &0 &\cdots &R \\
\end{matrix}
\right]_{N_c \times N_c}
\end{array}
\tag{9}
H=ΦyTQeΦy+Re;G=ΦyTQeM;P=MTQeM;M=FyX0+Ey−YrefQe=Q0⋮00Q⋮0⋯⋯⋱⋯00⋮QNp×NpRe=R0⋮00R⋮0⋯⋯⋱⋯00⋮RNc×Nc(9)
4 约束条件
假设只考虑控制变量的上下界约束,则矩阵UUU的约束为:
Umin≤U≤Umax(10)
U_{min} \leq U \leq U_{max}
\tag{10}
Umin≤U≤Umax(10)
或者写成以下形式:
[I−I]U≥[Umin−Umax](11)
\left[ \begin{matrix} I \\ -I \end{matrix}\right]U \geq \left[ \begin{matrix} U_{min} \\ -U_{max} \end{matrix}\right]
\tag{11}
[I−I]U≥[Umin−Umax](11)
5 QPQPQP问题
由上可知,MPCMPCMPC问题的求解最终转化为QPQPQP问题的求解,对于QPQPQP问题工程上可以求解的,有多种方法及开源库可以进行求解。
min: J=12UTHU+UTs.t. Umin≤U≤Umax(12)
\begin{matrix}
min: \; \; J = \frac{1}{2}U^T H U + U^T \\
s.t.\; \; U_{min} \leq U \leq U_{max}
\end{matrix}
\tag{12}
min:J=21UTHU+UTs.t.Umin≤U≤Umax(12)
将求解的控制量系列的第一个值作为控制量。
6 增量约束问题
将状态空间方程(2)改写为增量模式,以Δu\Delta uΔu为控制量:
{ξk+1=Amξk+BmΔuk+Dm,disθk=Cmξk(13)
\begin{cases}
\xi_{k+1} = A_m \xi_k + B_m \Delta u_k + D_{m,{dis}} \\
\theta_k = C_m \xi_k
\end{cases}
\tag{13}
{ξk+1=Amξk+BmΔuk+Dm,disθk=Cmξk(13)
其中:
ξ=[xk,uk]T;θk=[yk,uk]T;Am=[AdBd0I];Bm=[0I]T;Dm,dis=[Dd,dis0]T;Cm=[Cd00I];(14)
\begin{array}{cc}
{}\xi = [x_k,u_k]^T; \\
\theta _k = [y_k, u_k]^T; \\
A_m = \left[\begin{matrix} A_d &B_d \\ 0 &I \end{matrix}\right]; \\
B_m = \left[\begin{matrix} 0 &I \end{matrix}\right]^T; \\
D_{m,dis} = \left[\begin{matrix} D_{d,dis} &0 \end{matrix}\right]^T; \\
C_m = \left[\begin{matrix} C_d &0 \\ 0 &I \end{matrix}\right]; \\
\end{array}
\tag{14}
ξ=[xk,uk]T;θk=[yk,uk]T;Am=[Ad0BdI];Bm=[0I]T;Dm,dis=[Dd,dis0]T;Cm=[Cd00I];(14)
与上述相同的推到可以得到增量模型的QPQPQP形式:
min: J=12ΔUTHΔU+ΔUTs.t. Umin≤U≤Umax ΔUmin≤ΔU≤ΔUmax(15)
\begin{matrix}
min: \; \; J = \frac{1}{2} \Delta U^T H \Delta U + \Delta U^T \\
s.t.\; \; \; \; \; \; \; \; \; \; U_{min} \leq U \leq U_{max} \\
\; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \;
\Delta U_{min} \leq \Delta U \leq \Delta U_{max}
\end{matrix}
\tag{15}
min:J=21ΔUTHΔU+ΔUTs.t.Umin≤U≤UmaxΔUmin≤ΔU≤ΔUmax(15)
其中,RmR_mRm是ΔU\Delta UΔU的权重矩阵:
Qe=[Q0⋯000⋯00Q⋯000⋯0⋮⋮⋱⋮⋮⋮⋱⋮00⋯Q00⋯000⋯0R0⋯000⋯00R⋯0⋮⋮⋱⋮⋮⋮⋱⋮00⋯000⋯R](Np+NC)×(Np+NC)Re=[Rm0⋯00Rm⋯0⋮⋮⋱⋮00⋯Rm]Nc×Nc(16)
\begin{array}{cc}
Q_e=\left[\begin{matrix}
Q &0 &\cdots &0 &0 &0 &\cdots &0\\
0 &Q &\cdots &0 &0 &0 &\cdots &0\\
\vdots &\vdots &\ddots &\vdots &\vdots &\vdots &\ddots &\vdots\\
0 &0 &\cdots &Q &0 &0 &\cdots &0\\
0 &0 &\cdots &0 &R &0 &\cdots &0\\
0 &0 &\cdots &0 &0 &R &\cdots &0\\
\vdots &\vdots &\ddots &\vdots &\vdots &\vdots &\ddots &\vdots\\
0 &0 &\cdots &0 &0 &0 &\cdots &R\\
\end{matrix}
\right]_{(N_p + N_C) \times (N_p + N_C)} \\
R_e=\left[\begin{matrix}
R_m &0 &\cdots &0 \\
0 &R_m &\cdots &0 \\
\vdots &\vdots &\ddots &\vdots \\
0 &0 &\cdots &R_m \\
\end{matrix}
\right]_{N_c \times N_c}
\end{array}
\tag{16}
Qe=Q0⋮000⋮00Q⋮000⋮0⋯⋯⋱⋯⋯⋯⋱⋯00⋮Q00⋮000⋮0R0⋮000⋮00R⋮0⋯⋯⋱⋯⋯⋯⋱⋯00⋮000⋮R(Np+NC)×(Np+NC)Re=Rm0⋮00Rm⋮0⋯⋯⋱⋯00⋮RmNc×Nc(16)
根据ΔU\Delta UΔU来求解UUU,使UUU满足其约束:
uk=uk−1+Δuk=uk−1+[1 0 ⋯ 0]ΔUuk+1=uk+Δuk+1=uk−1+Δuk+Δuk+1=uk−1+[1 1 ⋯ 0]ΔU⋮uNc=uNc−1+ΔuNc=uNc−2+ΔuNc−1+ΔuNc=uk−1+[1 1 ⋯ 1]ΔU(17)
\begin{array}{cc}
u_{k} &= u_{k-1} + \Delta u_{k} &= u_{k-1} + [1\;0\;\cdots\;0] \Delta U \\
u_{k+1} &= u_{k} + \Delta u_{k+1} &= u_{k-1} + \Delta u_{k} + \Delta u_{k+1} &= u_{k-1} + [1\;1\;\cdots\;0] \Delta U \\
\vdots \\
u_{N_c} &= u_{N_c -1} + \Delta u_{N_c} &= u_{N_c -2} + \Delta u_{N_c - 1} + \Delta u_{N_c} &= u_{k-1} + [1\;1\;\cdots\;1] \Delta U \\
\end{array}
\tag{17}
ukuk+1⋮uNc=uk−1+Δuk=uk+Δuk+1=uNc−1+ΔuNc=uk−1+[10⋯0]ΔU=uk−1+Δuk+Δuk+1=uNc−2+ΔuNc−1+ΔuNc=uk−1+[11⋯0]ΔU=uk−1+[11⋯1]ΔU(17)
即:
[ukuk+1uk+2⋮uk+Nc]=[III⋮I]uk−1+[I00⋯0II0⋯0III⋯0⋮⋮⋮⋱⋮III⋯I][ΔukΔuk+1Δuk+2⋮Δuk+Nc](18)
\left[ \begin{matrix} u_k \\u_{k+1} \\u_{k+2} \\ \vdots \\u_{k+N_c} \end{matrix}\right] =
\left[ \begin{matrix} I \\I \\I \\ \vdots \\I \end{matrix}\right]u_{k-1} +
\left[ \begin{matrix}
I &0 &0 & \cdots &0 \\
I &I &0 & \cdots &0 \\
I &I &I & \cdots &0 \\
\vdots & \vdots & \vdots & \ddots & \vdots\\
I &I &I & \cdots &I
\end{matrix}\right]
\left[ \begin{matrix} \Delta u_k \\ \Delta u_{k+1} \\ \Delta u_{k+2} \\ \vdots \\ \Delta u_{k+N_c} \end{matrix}\right]
\tag{18}
ukuk+1uk+2⋮uk+Nc=III⋮Iuk−1+III⋮I0II⋮I00I⋮I⋯⋯⋯⋱⋯000⋮IΔukΔuk+1Δuk+2⋮Δuk+Nc(18)
简化可得:
[C1−C1]ΔU≥[Umin−C2uk−1−Umax+C2uk−1](19)
\left[ \begin{matrix} C_1 \\ -C_1 \end{matrix}\right] \Delta U \geq
\left[ \begin{matrix} U_{min} - C_2 u_{k-1} \\ -U_{max} + C_2 u_{k-1}\end{matrix}\right] \\
\tag{19}
[C1−C1]ΔU≥[Umin−C2uk−1−Umax+C2uk−1](19)
其中:
C1=[I00⋯0II0⋯0III⋯0⋮⋮⋮⋱⋮III⋯I];C2=[III⋮I](20)
C_1 = \left[ \begin{matrix}
I &0 &0 & \cdots &0 \\
I &I &0 & \cdots &0 \\
I &I &I & \cdots &0 \\
\vdots & \vdots & \vdots & \ddots & \vdots\\
I &I &I & \cdots &I
\end{matrix}\right];
C_2 =\left[ \begin{matrix} I \\I \\I \\ \vdots \\I \end{matrix}\right]
\tag{20}
C1=III⋮I0II⋮I00I⋮I⋯⋯⋯⋱⋯000⋮I;C2=III⋮I(20)
7 matlab求解
function [flag, control_out] = SolveLinearMpc(A, B, C, D, Q, R, sample_period, upper, lower, state_k, reference, Np, Nc)
if (size(A,1) ~= size(A,2) || ...
size(B,1) ~= size(A,1) || ...
size(D,1) ~= size(A,1) || ...
size(Q,1) ~= size(Q,2) || ...
size(R,1) ~= size(R,2) || ...
size(Q,1) ~= size(C,1) || ...
size(R,1) ~= size(B,2) || ...
size(C,2) ~= size(A,1) || ...
size(B,2) ~= size(lower,1) || ...
size(lower,1) ~= size(upper,1) || ...
size(state_k,1) ~= size(A,1))
flag = false;
return;
end
%% ÀëÉ¢»¯
matrix_i = eye(size(A,1));
% Ad = inv(matrix_i - sample_period * 0.5 * A) * (matrix_i + sample_period * 0.5 * A);
Ad = matrix_i + A * sample_period;
Bd = B * sample_period;
Dd = D * sample_period;
Cd = C;
%% Ô€²âŸØÕó
F = zeros(Np * size(Cd,1), size(Ad,2));
Phi = zeros(Np * size(Cd,1), Nc * size(Bd,2));
E = zeros(Np * size(Cd,1), size(Dd,2));
matrix_f = zeros(Np * size(C,1), size(A,2));
F(1:size(Cd,1), 1:size(Ad,2)) = Cd * Ad;
matrix_f(1:size(Cd,1), 1:size(Ad,2)) = Cd;
%% update F
for i = 2:1:Np
F((i-1) * size(Cd,1) + 1 : i * size(Cd,1), :) = ...
F((i-2) * size(Cd,1) + 1 : (i-1) * size(Cd,1), :) * Ad;
matrix_f((i-1) * size(Cd,1) + 1 : i * size(Cd,1), :) = ...
matrix_f((i-2) * size(Cd,1) + 1 : (i-1) * size(Cd,1), :) * Ad;
end
%% update Phi
% for i = 1:1:Np
% for j = 1:1:i
% Phi((i-1) * size(Cd,1) + 1 : i * size(Cd,1), (j-1) * size(Bd,2) + 1 : j * size(Bd, 2)) = ...
% matrix_f((i-j) * size(Cd,1) + 1 : (i-j+1) * size(Cd,1), 1 : size(matrix_f,2)) * Bd;
% if j == Nc
% break;
% end
% end
% end
matrix_phi = matrix_f * Bd;
Phi(: , 1 : size(Bd,2)) = matrix_phi;
for i = 1 : Nc - 1
Phi(:, (i * size(Bd,2) + 1) : (i + 1) * size(Bd,2)) = [zeros(i * size(Cd,1), size(Bd,2)); ...
matrix_phi(1 : (Np - i) * size(Cd,1), :)];
end
%% update E
E(1 : size(Cd,1), 1 : size(Dd,2)) = Cd * Dd;
for i = 2:1:Np
E((i-1) * size(Cd,1) + 1 : i * size(Cd,1), :) = ...
matrix_f((i-1) * size(Cd,1) + 1 : i * size(Cd,1), :) * Dd +...
E((i-2) * size(Cd,1) + 1 : (i-1) * size(Cd,1), :);
end
%% update Cost Function: min J = (Y_p - Ref)^T * Qe * (Y_p - Ref) + U^T * Re * U
%% s.t. matrix_inequality_constraint * U ¡Ü matrix_inequality_boundary
%% convert to QP problem: min J = 0.5 * U^T * H * U + U^T * G + P
%% where: H = Phi^T * Qe * Phi + Re
%% G = Phi^T * Qe * M
%% P = M^T Qe * M
%% M = F * X_k + E - Ref
Qe = zeros(Np * size(Q,1), Np * size(Q,2));
Re = zeros(Nc * size(R,1), Nc * size(R,2));
for i = 1:1:Np
Qe((i-1) * size(Q,1) + 1 : i * size(Q,1), (i-1) * size(Q,2) + 1 : i * size(Q,2)) = Q;
end
for i = 1:1:Nc
Re((i-1) * size(R,1) + 1 : i * size(R,1), (i-1) * size(R,2) + 1 : i * size(R,2)) = R;
end
M = F * state_k + E - reference;
H = Phi' * Qe * Phi + Re;
G = Phi' * Qe * M;
P = M' * Qe * M;
%% update constraint
matrix_ctrl_k = zeros(size(Bd,2), Nc * size(Bd,2));
matrix_ctrl_k(1 : size(Bd,2), 1 : size(Bd,2)) = eye(size(Bd,2)); %% use to get the fisrt contol value
Upper_boundary = zeros(Nc * size(Bd,2), 1);
Lower_boundary = zeros(Nc * size(Bd,2), 1);
for i = 1:1:Nc
Upper_boundary((i-1) * size(Bd,2) + 1 : i * size(Bd,2), :) = upper;
Lower_boundary((i-1) * size(Bd,2) + 1 : i * size(Bd,2), :) = lower;
end
inequality_boundary = [Upper_boundary; - Lower_boundary];
matrix_inequality_constraint_upper = eye(Nc * size(Bd,2), Nc * size(Bd,2));
matrix_inequality_constraint_lower = eye(Nc * size(Bd,2), Nc * size(Bd,2));
matrix_inequality_constraint = [matrix_inequality_constraint_upper; ...
-matrix_inequality_constraint_lower];
%% solve QP
solve = - H \ G; %% optimal slove with no constraint
is_satisfy_constraint = true;
for i=1:1:size(matrix_inequality_constraint,1)
if matrix_inequality_constraint(i, :) * solve > inequality_boundary(i)
is_satisfy_constraint = false;
end
end
if is_satisfy_constraint
control_out = matrix_ctrl_k * solve;
flag = true;
else
ppp = matrix_inequality_constraint * (H \ matrix_inequality_constraint');
ddd = (matrix_inequality_constraint * (H \ G) + inequality_boundary);
lambda = zeros(size(ddd,1), size(ddd,2));
tolerance = 10;
for i = 1:38
lambda_p = lambda;
for j = 1:size(ddd,1)
www = ppp(i,:) * lambda - ppp(i,i) * lambda(i,1);
www = www + ddd(i,1);
la = - www / ppp(i,i);
lambda(i,1) = max(0, la);
end
tolerance = (lambda - lambda_p)' * (lambda - lambda_p);
if tolerance < 10e-8
break;
end
end
solve = - H \ G - H \ matrix_inequality_constraint' * lambda;
control_out = matrix_ctrl_k * solve;
flag = true;
end