目录
- 前言
- 一、观测器设计
- 二、输出反馈MPC设计
-
- [2.1 预测模型](#2.1 预测模型)
- [2.2 代价函数设计](#2.2 代价函数设计)
- [2.3 约束构建](#2.3 约束构建)
-
- [2.3.1 系统约束](#2.3.1 系统约束)
- [2.3.2 终端约束](#2.3.2 终端约束)
- [2.4 构建二次规划求解](#2.4 构建二次规划求解)
- 三、系统稳定性分析
-
- [3.1 构造李雅普诺夫函数](#3.1 构造李雅普诺夫函数)
- [3.2 证明李雅普诺夫函数递减](#3.2 证明李雅普诺夫函数递减)
- 四、MATLAB实例
前言
致谢 【模型预测控制（2022春）lecture 3-2 Output feedback MPC】
本文需要是使用先前博客的知识，
控制器求解参考【MPC】模型预测控制笔记 (2)：约束MPC 、
观测器设计参考【MPC】模型预测控制笔记 (3)：无约束输出反馈MPC
以下内容针对系统 (1) 展开：
x k + 1 = A x k + B u k y k = C x k (1) \begin{align*} x_{k+1} &= Ax_k + Bu_k \\ y_k &= Cx_k \end{align*} \tag{1} xk+1yk=Axk+Buk=Cxk(1)
其中， x k ∈ R n x_k \in \mathbb{R}^n xk∈Rn 是系统的全状态， y k ∈ R m y_k \in \mathbb{R}^m yk∈Rm 是可知的系统输出， u k ∈ R p u_k \in \mathbb{R}^p uk∈Rp 是系统的输入， l b ≤ u k ≤ u b lb \le u_k \le ub lb≤uk≤ub。
系统能控且能观， m < n m<n m<n，部分状态不可直接测量，需要增加观测器来观测系统的全状态。
一、观测器设计
可设计观测器为：
x ^ k + 1 = A x ^ k + B u k + L ( y k − C x ^ k ) (2) \hat{x}{k+1} = A\hat{x}{k} + Bu_k + L(y_k - C\hat{x}_{k}) \tag{2} x^k+1=Ax^k+Buk+L(yk−Cx^k)(2)
观测误差 x ~ k = x k − x ^ k \tilde{x}k = x_k - \hat{x}{k} x~k=xk−x^k，其状态方程为：
x ~ k + 1 = ( A − L C ) x ~ k (3) \tilde{x}_{k+1} = (A-LC)\tilde{x}_k \tag{3} x~k+1=(A−LC)x~k(3)
其中满足 ∣ e i g ( A − L C ) ∣ < 1 |\mathrm{eig}(A-LC)|<1 ∣eig(A−LC)∣<1.
二、输出反馈MPC设计
由于全状态 x k x_{k} xk 不可直接测量，需使用观测状态 x ^ k \hat{x}_{k} x^k 完成控制器设计.
2.1 预测模型
在使用式 (2) 进行未来 N N N 步的预测时，由于未来的 y ( i ∣ k ) , i = 1 , 2 , ⋯ , N y_{(i|k)}, i=1,2,\cdots,N y(i∣k),i=1,2,⋯,N 不可知，且在观测过程中， y k − C x ^ k → 0 y_k - C\hat{x}_{k} \to \mathbf{0} yk−Cx^k→0.
故使用下式进行预测（ z ^ 0 = x ^ 0 \hat{z}0 = \hat{x}0 z^0=x^0）：
z ^ k + 1 = A z ^ k + B u k (4) \hat{z}{k+1} = A\hat{z}{k} + Bu_k \tag{4} z^k+1=Az^k+Buk(4)
未来 N N N 步的状态可表示为：
Z = G z ( 0 ∣ k ) + H U (5) Z = \mathcal{G}z_{(0|k)} + \mathcal{H}U \tag{5} Z=Gz(0∣k)+HU(5)
其中，
Z = [ z ( 1 ∣ k ) z ( 2 ∣ k ) ⋯ z ( N ∣ k ) ] T U = [ u ( 0 ∣ k ) u ( 1 ∣ k ) ⋯ u ( N − 1 ∣ k ) ] T G = [ A A 2 ⋯ A N ] T H = [ B 0 0 ⋯ 0 A B B 0 ⋯ 0 A 2 B A B B ⋯ 0 ⋮ ⋮ ⋮ ⋱ ⋮ A N − 1 B A 2 B A B ⋯ B ] \begin{align*} Z &= [z_{(1|k)} ~ z_{(2|k)} ~ \cdots~z_{(N|k)}]^T \\ U &= [u_{(0|k)} ~ u_{(1|k)} ~ \cdots~u_{(N-1|k)}]^T \\ \mathcal{G} &= \left[ A ~ A^2 ~\cdots ~ A^N \right]^T \\ \mathcal{H} &= \begin{bmatrix} B & 0 & 0 & \cdots & 0\\ AB & B & 0 & \cdots & 0\\ A^2B & AB & B & \cdots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ A^{N-1}B & A^2B & AB & \cdots & B \end{bmatrix} \end{align*} ZUGH=[z(1∣k) z(2∣k) ⋯ z(N∣k)]T=[u(0∣k) u(1∣k) ⋯ u(N−1∣k)]T=[A A2 ⋯ AN]T= BABA2B⋮AN−1B0BAB⋮A2B00B⋮AB⋯⋯⋯⋱⋯000⋮B
2.2 代价函数设计
使用 z k z_k zk 代替 x k x_k xk 来设计代价函数，其矩阵形式为：
J k = Z k T Q Z k + U k T R U k (6) J_k = Z_k^T\mathcal{Q}Z_k + U_k^T\mathcal{R}U_k \tag{6} Jk=ZkTQZk+UkTRUk(6)
其中 Q = d i a g ( Q , Q , ⋯ , Q , P ) \mathcal{Q} = \mathrm{diag}(Q,Q, \cdots, Q, P) Q=diag(Q,Q,⋯,Q,P)， R = d i a g ( R , R , ⋯ , R ) \mathcal{R} = \mathrm{diag}(R, R, \cdots, R) R=diag(R,R,⋯,R).
将式 (5) 代入上式，有：
J k = ( G z ( 0 ∣ k ) ) T Q ′ G z ( 0 ∣ k ) + 2 z ( 0 ∣ k ) T G T Q ′ H U k + U k T ( H T Q ′ H + R ) U k J_k = (\mathcal{G}z_{(0|k)})^T \mathcal{Q}^\prime \mathcal{G}z_{(0|k)} + 2z_{(0|k)}^T\mathcal{G}^T \mathcal{Q}^\prime \mathcal{H} U_k + U_k^T (\mathcal{H}^T \mathcal{Q}^\prime \mathcal{H} + \mathcal{R})U_k Jk=(Gz(0∣k))TQ′Gz(0∣k)+2z(0∣k)TGTQ′HUk+UkT(HTQ′H+R)Uk
2.3 约束构建
2.3.1 系统约束
输入约束：
U m i n ≤ U k ≤ U m a x (7) U_{min} \le U_k \le U_{max} \tag{7} Umin≤Uk≤Umax(7)
其中 U m i n = [ l b , l b , , ⋯ , l b ] U_{min} = [lb,~lb,~, \cdots, ~lb] Umin=[lb, lb, ,⋯, lb]， U m a x = [ u b , u b , , ⋯ , u b ] U_{max} = [ub,~ub,~, \cdots, ~ub] Umax=[ub, ub, ,⋯, ub].
2.3.2 终端约束
为保证系统的稳定性，增加终端约束，使优化的终端进去不变集 Ω \Omega Ω，且要求该集合内存在最优输入 u k = − K x k u_k=-Kx_k uk=−Kxk 始终满足输入约束 U \mathcal{U} U：
z ^ ( N ∣ k ) ∈ Ω (8) \hat{z}_{(N|k)} \in \Omega \tag{8} z^(N∣k)∈Ω(8)
在 【模型预测控制（2022春）lecture 3-2 Output feedback MPC】 中，考虑的是真实状态 x k x_k xk 的终端约束，即：
x ( N ∣ k ) ∈ Ω (A.1) x_{(N|k)} \in \Omega \tag{A.1} x(N∣k)∈Ω(A.1)假设初始的误差 x ~ 0 ∈ X 0 \tilde{x}_0 \in \mathcal{X}0 x~0∈X0，保守地通过以下形式来满足式 (A.1)：
z ^ ( N ∣ k ) ∈ Ω ∖ ( A − L C ) N X 0 \hat{z}{(N|k)} \in \Omega \setminus (A-LC)^N \mathcal{X}_0 z^(N∣k)∈Ω∖(A−LC)NX0其中 ∖ \setminus ∖ 为集合减，即认为 x ( N ∣ k ) ∈ z ^ ( N ∣ k ) + ( A − L C ) N X 0 x_{(N|k)} \in \hat{z}_{(N|k)} + (A-LC)^N \mathcal{X}_0 x(N∣k)∈z^(N∣k)+(A−LC)NX0.
但目前本人暂无法给出证明，因为实际观测是由式 (2) 进行的，但预测时只能按式 (4) 进行，其中仍然需要分析。
本文认为式 (8) 的终端约束即可保证系统的稳定性。
2.4 构建二次规划求解
综上，最优控制序列可通过二次规划问题求解：
U ∗ = a r g min U k J k s . t . U m i n ≤ U k ≤ U m a x z ^ ( N ∣ k ) ∈ Ω \begin{align*} & \hspace{0.4cm }U^* = \mathrm{arg} \min_{U_k} J_k \\ \mathrm{s. t.}& \quad U_{min} \le U_k \le U_{max} \\ & \hspace{0.9cm} \hat{z}_{(N|k)} \in \Omega \end{align*} s.t.U∗=argUkminJkUmin≤Uk≤Umaxz^(N∣k)∈Ω
三、系统稳定性分析
3.1 构造李雅普诺夫函数
选取李雅普诺夫函数 V ( x k ) V(x_k) V(xk)：
V ( x k ) = J k ∗ + E e r r + c o x ~ k T P o x ~ k \begin{align*} V(x_k) &= J_k^* + E_{err} + c_o\tilde{x}_k^TP_o\tilde{x}_k \end{align*} V(xk)=Jk∗+Eerr+cox~kTPox~k
其中， J k ∗ = min U k J k J_k^* = \min_{U_k} J_k Jk∗=minUkJk， x k T Q x k = z k T Q z k + E e r r x_k^TQx_k = z_k^TQz_k + E_{err} xkTQxk=zkTQzk+Eerr （ V ( x k ) V(x_k) V(xk) 包含 x k x_k xk），常数 c o > 0 c_o>0 co>0，
P o P_o Po 为正定对称矩阵，满足 ( A − L C ) T P o ( A − L C ) − P o < 0 (A-LC)^T P_o (A-LC) - P_o<0 (A−LC)TPo(A−LC)−Po<0.
证明：针对系统 (3)，必然存在正定对称矩阵 P o P_o Po，满足 ( A − L C ) T P o ( A − L C ) − P o < 0 (A-LC)^T P_o (A-LC) - P_o<0 (A−LC)TPo(A−LC)−Po<0
设存在对称矩阵 Q o > 0 Q_o > 0 Qo>0，构造 P o P_o Po，使得 ( A − L C ) T P o ( A − L C ) − P o = − Q o (A-LC)^T P_o (A-LC) - P_o = -Q_o (A−LC)TPo(A−LC)−Po=−Qo，有：
( A − L C ) T P o ( A − L C ) − P o = − Q o ( A − L C ) 2 T P o ( A − L C ) 2 − ( A − L C ) T P o ( A − L C ) = − ( A − L C ) T Q o ( A − L C ) ( A − L C ) 3 T P o ( A − L C ) 3 − ( A − L C ) 2 T P o ( A − L C ) 2 = − ( A − L C ) 2 T Q o ( A − L C ) 2 ⋮ ( A − L C ) n + 1 T P o ( A − L C ) n + 1 − ( A − L C ) n T P o ( A − L C ) n = − ( A − L C ) n T Q o ( A − L C ) n \begin{align*} (A-LC)^T P_o (A-LC) - P_o &= -Q_o \\ {(A-LC)^2}^T P_o (A-LC)^2 - (A-LC)^T P_o (A-LC) &= -(A-LC)^T Q_o (A-LC) \\ {(A-LC)^3}^T P_o (A-LC)^3 - {(A-LC)^2}^T P_o (A-LC)^2 &= -{(A-LC)^2}^T Q_o (A-LC)^2 \\ \quad\vdots& \\ {(A-LC)^{n+1}}^T P_o (A-LC)^{n+1} - {(A-LC)^n}^T P_o (A-LC)^n &= -{(A-LC)^n}^T Q_o (A-LC)^n \end{align*} (A−LC)TPo(A−LC)−Po(A−LC)2TPo(A−LC)2−(A−LC)TPo(A−LC)(A−LC)3TPo(A−LC)3−(A−LC)2TPo(A−LC)2⋮(A−LC)n+1TPo(A−LC)n+1−(A−LC)nTPo(A−LC)n=−Qo=−(A−LC)TQo(A−LC)=−(A−LC)2TQo(A−LC)2=−(A−LC)nTQo(A−LC)n以上各式相加可得：
( A − L C ) n + 1 T P o ( A − L C ) n + 1 − P o = − ∑ k = 1 n ( ( A − L C ) k T Q o ( A − L C ) k ) {(A-LC)^{n+1}}^T P_o (A-LC)^{n+1} - P_o = -\sum_{k=1}^n \left({(A-LC)^k}^T Q_o (A-LC)^k \right) (A−LC)n+1TPo(A−LC)n+1−Po=−k=1∑n((A−LC)kTQo(A−LC)k)因为 ∣ e i g ( A − L C ) ∣ < 1 |\mathrm{eig}(A - LC)| < 1 ∣eig(A−LC)∣<1，有
lim n → + ∞ ( A − L C ) n + 1 T P o ( A − L C ) n + 1 = 0 {\lim_{n \to +\infty} (A-LC)^{n+1}}^T P_o (A-LC)^{n+1} = 0 n→+∞lim(A−LC)n+1TPo(A−LC)n+1=0所以有：
P o = ∑ k = 1 ∞ ( A − L C ) k T Q o ( A − L C ) k > 0 P_o = \sum_{k=1}^{\infty} {(A-LC)^k}^T Q_o (A-LC)^k > 0 Po=k=1∑∞(A−LC)kTQo(A−LC)k>0故 P o P_o Po 对称且正定。
有
c o x ~ k T P o x ~ k + E e r r = c o ( x k T P o x k + z k T P o z k − 2 z k T P o x k ) + ( x k T Q x k − z k T Q z k ) = x k T ( c o P o + Q ) x k + z k T ( c o P o − Q ) z k − 2 z k T P o x k ≥ x k T ( c o P o + Q ) x k + z k T ( c o P o − Q ) z k − max ( x k T ( 2 P o ) x k , z k T ( 2 P o ) z k ) \begin{align*} c_o\tilde{x}_k^TP_o\tilde{x}k + E{err} &= c_o \left( x_k^T P_o x_k + z_k^T P_o z_k - 2z_k^T P_o x_k \right) + \left( x_k^TQx_k - z_k^TQz_k \right) \\ &= x_k^T (c_oP_o + Q) x_k + z_k^T (c_oP_o - Q) z_k - 2z_k^T P_o x_k \\ &\ge x_k^T (c_oP_o + Q) x_k + z_k^T (c_oP_o - Q) z_k - \max(x_k^T (2P_o) x_k,~z_k^T (2P_o) z_k) \end{align*} cox~kTPox~k+Eerr=co(xkTPoxk+zkTPozk−2zkTPoxk)+(xkTQxk−zkTQzk)=xkT(coPo+Q)xk+zkT(coPo−Q)zk−2zkTPoxk≥xkT(coPo+Q)xk+zkT(coPo−Q)zk−max(xkT(2Po)xk, zkT(2Po)zk)
显然存在有限常数 c o c_o co，使得 c o P o + Q − 2 P 0 c_oP_o + Q - 2P_0 coPo+Q−2P0 或 c o P o − Q − 2 P 0 c_oP_o - Q - 2P_0 coPo−Q−2P0 正定， c o x ~ k T P o x ~ k + E e r r > 0 c_o\tilde{x}_k^TP_o\tilde{x}k + E{err}>0 cox~kTPox~k+Eerr>0.
又因 J k ∗ > 0 J_k^*>0 Jk∗>0，故 V ( x k ) > 0 V(x_k) > 0 V(xk)>0.
3.2 证明李雅普诺夫函数递减
V ( x k + 1 ) − V ( x k ) = J k + 1 ∗ − J k ∗ + c o ( x ~ k + 1 T P o x ~ k + 1 − x ~ k T P o x ~ k ) + E e r r ′ = J k + 1 ∗ − J k ∗ + c o ( x ~ k T ( A − L C ) T P o ( A − L C ) x ~ k − x ~ k T P o x ~ k ) + E e r r ′ = J k + 1 ∗ − J k ∗ + c o x ~ k T ( ( A − L C ) T P o ( A − L C ) − P o ) x ~ k + E e r r ′ \begin{align*} V(x_{k+1}) - V(x_{k}) &= J_{k+1}^* - J_{k}^*+ c_o \left( \tilde{x}{k+1}^TP_o\tilde{x}{k+1} - \tilde{x}k^TP_o\tilde{x}k \right) + E{err}^\prime \\ &= J{k+1}^* - J_{k}^*+ c_o \left( \tilde{x}{k}^T (A-LC)^T P_o (A-LC)\tilde{x}{k} - \tilde{x}k^TP_o\tilde{x}k \right) + E{err}^\prime \\ &= J{k+1}^* - J_{k}^*+ c_o \tilde{x}_{k}^T \left( (A-LC)^T P_o (A-LC) - P_o \right) \tilde{x}k + E{err}^\prime \end{align*} V(xk+1)−V(xk)=Jk+1∗−Jk∗+co(x~k+1TPox~k+1−x~kTPox~k)+Eerr′=Jk+1∗−Jk∗+co(x~kT(A−LC)TPo(A−LC)x~k−x~kTPox~k)+Eerr′=Jk+1∗−Jk∗+cox~kT((A−LC)TPo(A−LC)−Po)x~k+Eerr′
其中 ( A − L C ) T P o ( A − L C ) − P o < 0 (A-LC)^T P_o (A-LC) - P_o<0 (A−LC)TPo(A−LC)−Po<0， c o > 0 c_o>0 co>0，故 c o x ~ k T ( ( A − L C ) T P o ( A − L C ) − P o ) x ~ k < 0 c_o \tilde{x}_{k}^T \left( (A-LC)^T P_o (A-LC) - P_o \right) \tilde{x}_k < 0 cox~kT((A−LC)TPo(A−LC)−Po)x~k<0，
当 x ~ k → 0 \tilde{x}k \to 0 x~k→0， E e r r ′ → 0 E{err}^\prime \to 0 Eerr′→0，否则存在有限常数 c o > 0 c_o>0 co>0，使得 c o x ~ k T ( ( A − L C ) T P o ( A − L C ) − P o ) x ~ k + E e r r ′ < 0 c_o \tilde{x}_{k}^T \left( (A-LC)^T P_o (A-LC) - P_o \right) \tilde{x}k + E{err}^\prime< 0 cox~kT((A−LC)TPo(A−LC)−Po)x~k+Eerr′<0.
故只需保证 J k + 1 ∗ − J k ∗ < 0 J_{k+1}^* - J_{k}^*<0 Jk+1∗−Jk∗<0 即可证明 V ( x k ) V(x_{k}) V(xk) 递减。
参考【MPC】模型预测控制笔记 (1)：无约束MPC、【MPC】模型预测控制笔记 (2)：约束MPC，
J k + 1 ∗ − J k ∗ < 0 J_{k+1}^* - J_{k}^*<0 Jk+1∗−Jk∗<0 成立，故
V ( x k + 1 ) − V ( x k ) = J k + 1 ∗ − J k ∗ + c o x ~ k T ( ( A − L C ) T P o ( A − L C ) − P o ) x ~ k + E e r r ′ < 0 V(x_{k+1}) - V(x_{k}) = J_{k+1}^* - J_{k}^*+ c_o \tilde{x}_{k}^T \left( (A-LC)^T P_o (A-LC) - P_o \right) \tilde{x}k + E{err}^\prime < 0 V(xk+1)−V(xk)=Jk+1∗−Jk∗+cox~kT((A−LC)TPo(A−LC)−Po)x~k+Eerr′<0
综上，在观测器和输出反馈MPC联合作用下，系统 (1) 渐近稳定。
四、MATLAB实例
针对系统 (1)，设系统中 A = [ 1.1 2 0 0.95 ] A = \begin{bmatrix} 1.1 & 2 \\ 0 & 0.95 \end{bmatrix} A=[1.1020.95]， B = [ 0 0.079 ] B = \begin{bmatrix} 0 \\ 0.079 \end{bmatrix} B=[00.079]， C = [ 1 0 ] C = \begin{bmatrix} 1 & 0 \end{bmatrix} C=[10]， − 4 ≤ u k ≤ 4 -4 \le u_k \le 4 −4≤uk≤4.
设计式 (2) 形式的观测器，其中最优增益 L = [ 1.8342 0.3571 ] L = \begin{bmatrix} 1.8342 \\ 0.3571 \end{bmatrix} L=[1.83420.3571].
终端约束中取不变集 Ω = [ − 0.2 , 0.2 ] × [ − 0.1 , 0.1 ] \Omega = [-0.2,~ 0.2] \times [-0.1,~ 0.1] Ω=[−0.2, 0.2]×[−0.1, 0.1]，写为关于 U k U_k Uk 的不等式约束：
A i n U k ≤ b i n A_{in}U_k \le b_{in} AinUk≤bin其中，
A i n = [ 1 0 − 1 0 0 1 0 − 1 ] [ 0 , 0 , ⋯ , I 2 × 2 ] H b i n = [ 0.2 0.2 0.1 0.1 ] − [ 1 0 − 1 0 0 1 0 − 1 ] [ 0 , 0 , ⋯ , I 2 × 2 ] G z ^ ( 0 ∣ k ) \begin{align*} A_{in} &= \begin{bmatrix} 1 & 0 \\ -1 & 0 \\ 0 & 1 \\ 0 &-1 \end{bmatrix} [0,~0,~\cdots, ~I_{2 \times 2}] \mathcal{H} \\ b_{in} &= \begin{bmatrix} 0.2 \\ 0.2 \\ 0.1 \\ 0.1 \end{bmatrix}-\begin{bmatrix} 1 & 0 \\ -1 & 0 \\ 0 & 1 \\ 0 &-1 \end{bmatrix}[0,~0,~\cdots, ~I_{2 \times 2}] \mathcal{G} \hat{z}_{(0|k)} \end{align*} Ainbin= 1−100001−1 [0, 0, ⋯, I2×2]H= 0.20.20.10.1 − 1−100001−1 [0, 0, ⋯, I2×2]Gz^(0∣k).
取 Q = d i a g ( 1 , 1 ) Q = \mathrm{diag}(1,~1) Q=diag(1, 1)， R = 0.1 R = 0.1 R=0.1， N = 20 N = 20 N=20.
计算得 P = d i a g ( 1 , 1 ) P = \mathrm{diag}(1,~1) P=diag(1, 1)：
实现代码如下：
matlab
%% 观测器
%%
A = [1.1 2;0 0.95];
B = [0; 0.079];
C = [1, 0];
Ap = A';
Bp = C';
%% LQR 最优增益
K = LQR(Ap, Bp, eye(2), 0.1, 500, 1e-6);
L = K';
disp(eig(A - L*C))
disp(abs(eig(A - L*C)))
%% 控制器
%% 选取K并检验系统是否稳定
A = [1.1 2;0 0.95];
B = [0; 0.079];
K = [1.4 5.76];
eigSys = eig(A - B*K);
disp(abs(eigSys))
%% 求解P
K = [1.4 5.76];
Q = eye(2);
R = 0.1;
syms P [2 2] % P 为2*2的矩阵
equ = P - (A - B*K)' * P * (A - B*K) == Q + K'*R*K;
Psol = solve(equ, P);
Psol = [Psol.P1_1, Psol.P2_1; Psol.P2_1, Psol.P2_2];
Psol = double(Psol);
disp(Psol)
%% 计算G、H、Q、R
N = 20;
[G, H] = getGH(N, A, B);
[Qp, Rp] = getQR(N, Q, Psol, R);
%% 约束条件
lb = -4 * ones(N, 1);
ub = 4 * ones(N, 1);
n = size(A, 2);
tmpReshape = kron(ones(1, N-1), zeros(n));
tmpReshape = [tmpReshape, eye(n)];
tmpAin = [1 0;
-1 0;
0 1;
0 -1];
tmpbin = [0.2; 0.2; 0.1; 0.1];
% Ain = tmpAin * tmpReshape * H;
% bin = tmpbin - tmpAin * tmpReshape * G * xCur;
%% 效果演示
A = [1.1 2;0 0.95];
B = [0; 0.079];
C = [1, 0];
L = [1.8342; 0.3571];
options = optimoptions('quadprog', 'MaxIterations', 200, 'Display','none');
xCur = [1.2;-0.7]; % 设初始状态为[1;1]
xLog = xCur;
xHatCur = [0;0]; % 设初始状态为[0;0]
xHatLog = xHatCur;
uLog = [];
step = 0:50;
u = 0;
for i = step
Hp = 2 * (H' * Qp * H + Rp);
fp = 2 * xHatCur' * G' * Qp * H;
Hp = 0.5 * (Hp + Hp');
Ain = tmpAin * tmpReshape * H;
bin = tmpbin - tmpAin * tmpReshape * G * xHatCur;
U = quadprog(Hp, fp, Ain, bin, [], [], lb, ub, u, options);
u = U(1);
yCur = C * xCur; % y_k
xHatCur = A * xHatCur + B*u + L * (yCur - C * xHatCur); % xHat_k+1
xCur = A*xCur + B*u; % x_k+1
xHatLog = [xHatLog, xHatCur];
xLog = [xLog, xCur];
uLog = [uLog, u];
end
figure(1)
subplot(3,1,1)
hold on
plot(step, xLog(1,1:end-1))
plot(step, xHatLog(1,1:end-1))
title('x1')
grid on
subplot(3,1,2)
hold on
plot(step, xLog(2,1:end-1))
plot(step, xHatLog(2,1:end-1))
title('x2')
grid on
subplot(3,1,3)
plot(step, uLog)
title('u')
grid on
%%
function [Qp, Rp] = getQR(N, Q, P, R)
Qp = eye(N);
Qp(end) = 0;
Qp = kron(Qp, Q) + kron(eye(N)-Qp, P);
Rp = eye(N);
Rp = kron(Rp, R);
end
function [G, H] = getGH(N, A, B) % N>1
tmp = A;
G = tmp;
for i=2:N
tmp = A*tmp;
G = [G; tmp];
end
r = size(B, 1);
c = size(B, 2);
H = zeros(r * N, c * N);
tmp = B;
for j = N:-1:1
H( (j-1)*r+1:j*r, (j-1)*c+1:j*c ) = tmp;
end
for i = 2:N
tmp = A*tmp;
for j = i:N
H( (j-1)*r+1:j*r, (j-i)*c+1:(j-i+1)*c ) = tmp;
end
end
end
function K = LQR(A, B, Q, R, maxIter, eps)
% A、B分别为系统矩阵和输入矩阵，Q和R分别为状态误差和输入的对角权重矩阵
% maxIter为最大迭代步数N，eps为迭代精度C
i = 1; P = Q; delta = 1e9;
while i < maxIter && delta > eps
Pn = Q + A' * (P - P*B* inv(R+B'*P*B) *B'*P) * A;
delta = max(abs(Pn-P), [], "all");
P = Pn;
i = i+1;
end
K = inv(R + B' * P * B) * B' * P * A;
end最终效果：
