公式推导参考链接，以下工作来自于b站Dr_can博士

一般情况下，状态空间，或者叫做动力学模型离散表达为：

x(k+1)=Ax(k)+Bu(k)

MPC是目前自动驾驶领域比较流行的控制框架，主要分为动力学预测，优化求解，滚动控制三个部分。其中，优化求解方法有多种，譬如说凸优化、二次规划等等。凸优化要求状态约束满足凸集的形式，也有相关凸化的方法，但是求解过程往往十分复杂，非常耗时。二次规划方法在求解方面有天然优势，因此受到了MPC控制框架的青睐。

对于约束部分，在二次规划问题中，只能处理线性约束，譬如等式约束或者是不等式约束，需要考虑实际的情况。在简单情况下，也可以不考虑相关线性约束。

MPC需要使用动力学模型对状态量进行预测，设定预测步数为N，那么状态量表示为：

{X(k)}=\left[\begin{array}{c}x(k \mid k) \\x(k+1 \mid k) \\\vdots \\x(k+i \mid k) \\\vdots \\x(k+N \mid k)\end{array}\right]

控制量表示为：

\boldsymbol{U}(k)=\left[\begin{array}{c}\boldsymbol{u}(k \mid k) \\\boldsymbol{u}(k+1 \mid k) \\\vdots \\\boldsymbol{u}(k+i \mid k) \\\vdots \\\boldsymbol{u}(k+N-1 \mid k)\end{array}\right]

将预测过程写入到矩阵中，方便进行优化求解，则有如下：

X(k)=Mx(k)+CU(k)

其中，

\boldsymbol{M}=\left[\begin{array}{c}\boldsymbol{I}_{n \times n} \\\boldsymbol{A}_{n \times n} \\\boldsymbol{A}^{2} \\\vdots \\\boldsymbol{A}^{N}\end{array}\right]

C=\left[\begin{array}{cccc}0 & 0 & \ldots & 0 \\\vdots & \vdots & \ldots & \vdots \\0 & 0 & & 0 \\B & 0 & \ldots & 0 \\A B & B & \ldots & 0 \\\vdots & \vdots & \ddots & 0 \\A^{N-1} B & A^{N-2} B & \ldots & B\end{array}\right]

因此，考虑到控制燃料最优，最终点约束和预测状态约束等问题，惩罚函数为：

J=\sum_{i=0}^{N-1}\left(\boldsymbol{x}(k+i \mid k)^{T} \boldsymbol{Q} \boldsymbol{x}(k+i \mid k)+\boldsymbol{u}(k+i \mid k)^{T} \boldsymbol{R} \boldsymbol{u}(k+i \mid k)\right)+\boldsymbol{x}(k+N \mid k)^{T} \boldsymbol{F} \boldsymbol{x}(k+N \mid k)

转换成为二次规划的形式，如下：

J=\boldsymbol{x}(k)^{T} \boldsymbol{G} \boldsymbol{x}(k)+\boldsymbol{U}(k)^{T} \boldsymbol{H} \boldsymbol{U}(k)+2 \boldsymbol{x}(k)^{T} \boldsymbol{E} \boldsymbol{U}(k)

其中，

\begin{array}{l} \bar{G}={M^{T} \bar{Q} M} \\ \bar{E}=C^{T} \bar{Q} M \\ \bar{H}=C^{T} \bar{Q} C+\bar{R} \end{array}

其中，

\overline{\boldsymbol{Q}}=\left[\begin{array}{ccc}\boldsymbol{Q} & \cdots & \\\vdots & \boldsymbol{Q} & \vdots \\& \cdots & \boldsymbol{F}\end{array}\right] \quad \overline{\boldsymbol{R}}=\left[\begin{array}{ccc}\boldsymbol{R} & \cdots & \\\vdots & \ddots & \vdots \\& \cdots & \boldsymbol{R}\end{array}\right]

在代价函数中，第一项对于二次规划无关紧要可以忽略。第二、三项即为二次规划的一般形式。通过二次规划求解，就能获得优化控制量U_k，选取第一个控制量作为系统的输入，进行求解即可。

举例，具体形式为：

\left[\begin{array}{l}x_{1}(k+1) \\x_{2}(k+1)\end{array}\right]=\left[\begin{array}{cc}1 & 0.1 \\0 & 2\end{array}\right]\left[\begin{array}{l}x_{1}(k) \\x_{2}(k)\end{array}\right]+\left[\begin{array}{c}0 \\0.5\end{array}\right] u(k)

设定初始值，并进行求解控制量即可，相关代码如下。

如果有完整的动力学模型，可以进行第三步—滚动优化，将每次优化得到的u_k 作为控制量实现滚动控制。这里省略相关动力学模型更新，传感器观测等建模过程。

A = [1,0.1;0,2];
B = [0;0.5];
N = 10;
x_k = [5;5];
Q = [1,0;0,1];
R = [0.1];
F = Q;
[M,C,Q_bar,R_bar,G,E,H,U_k] = MPC_Zero_Ref1(A,B,N,x_k,Q,R,F)
function [M,C,Q_bar,R_bar,G,E,H,U_k] = MPC_Zero_Ref1(A,B,N,x_k,Q,R,F)
n = size(A,1);
p = size(B,2);
M = [eye(n);zeros(N*n,n)];
C = zeros((N+1)*n,N*p);

tmp = eye(n);
for i = 1:N
   rows = i*n+(1:n);
   C(rows,:) = [tmp*B,C(rows-n,1:end-p)];
   tmp = A*tmp;
   M(rows,:) = tmp;
end
S_q = size(Q,1);
S_r = size(R,1);
Q_bar = zeros((N+1)*S_q,(N+1)*S_q);
for i = 0:N
    Q_bar(i*S_q+1:(i+1)*S_q,i*S_q+1:(i+1)*S_q) = Q; 
end
Q_bar(N*S_q+1:(N+1)*S_q,N*S_q+1:(N+1)*S_q) = R;
R_bar = zeros(N*S_r,N*S_r);
for i = 0:N-1
   R_bar(i*S_r+1:(i+1)*S_r,i*S_r+1:(i+1)*S_r) = R;
end
G = M'*Q_bar*M;
E = C'*Q_bar*M;
H = C'*Q_bar*C + R_bar;
f = (x_k'*E')';
U_k = quadprog(H,f);
end