从LQR到iLQR的简明易懂过程（一）

LQR基础

状态转移：
$$x_{k+1}=A_k{x}_t+B_k{u}_t$$
需要使如下目标函数最优
$$J(x_0,U)=\frac{1}{2}{\Sigma }_{k=1}^{N-1}(x_k^T{Q}_k{x}_k+u_k^T{R}_k{u}_k)+\frac{1}{2}{x}_N^T{Q}_f{x}_N$$

其中三部分为状态成本，控制成本和终点状态成本。

在终端时刻 $N$，剩余成本即为终端惩罚：
$$V_N(x_N)=\frac{1}{2}{x}_N^T{Q}_f{x}_N$$
为便于递推，我们定义：
$${\mathbf{S}}_{\mathbf{N}}={\mathbf{Q}}_{\mathbf{f}}$$
所以：
$$V_N(x_N)=\frac{1}{2}{x}_N^T{S}_N{x}_N$$

$k=N-1$ 时刻的最优控制律 $u_{N-1}^{\ast }$

$$V_N(x_N)=\frac{1}{2}{x}_N^T{Q}_f{x}_N=\frac{1}{2}{x}_N^T{S}_N{x}_N$$

根据贝尔曼最优性原理
$$\begin{array}{rl}{V}_i(x_i)& =\underset{u_i}{\min }{V}_{i+1}(x_{i+1})+\frac{1}{2}{u}_i^T{R}_i{u}_i+\frac{1}{2}{x}_i^T{Q}_i{x}_i\\ & =\underset{u_i}{\min }\frac{1}{2}{u}_i^T{R}_i{u}_i+\frac{1}{2}{x}_i^T{Q}_i{x}_i+\frac{1}{2}(A_i{x}_i+B_i{u}_i{)}^T{S}_{i+1}(A_i{x}_i+B_i{u}_i)\\ & =\underset{u_i}{\min }\frac{1}{2}{u}_i^T{R}_i{u}_i+\frac{1}{2}{x}_i^T{Q}_i{x}_i+\frac{1}{2}{x}_i^T{A}^T{S}_{i+1}A{x}_i+\frac{1}{2}{u}_i^T{B}_i^T{S}_{i+1}{B}_i{u}_i+x_i^T{A}_i^T{S}_{i+1}{B}_i{u}_i\\ & =\underset{u_i}{\min }\frac{1}{2}{u}_i^T(R_i+B_i^T{S}_{i+1}{B}_i)u_i+\frac{1}{2}{x}_i^T(Q_i+A_i^T{S}_{i+1}{A}_i)x_i+u_i^T(B_i^T{S}_{i+1}{A}_i)x_i\end{array}$$

接下来求导
$$\begin{array}{rlr}\frac{\partial V}{\partial u}& =(R_i+B_i^T{S}_{i+1}{B}_i)u_i+(B_i^T{S}_{i+1}{A}_i)x_i& =0\end{array}$$

$$u_i=-(R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}(B_i^T{S}_{i+1}{A}_i)x_i$$
$$u_i=-K_i{x}_i,K_i=(R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}(B_i^T{S}_{i+1}{A}_i)$$

Riccati

代入最优控制

$$\begin{array}{rl}{V}_i(x_i)=& \frac{1}{2}{u}_i^{\ast T}(R_i+B_i^T{S}_{i+1}{B}_i)u_i^{\ast }+\frac{1}{2}{x}_i^T(Q_i+A_i^T{S}_{i+1}{A}_i)x_i+u_i^{\ast T}(B_i^T{S}_{i+1}{A}_i)x_i\\ =& \frac{1}{2}{x}_i^T{K}_i^T(R_i+B_i^T{S}_{i+1}{B}_i)K_i{x}_i+\frac{1}{2}{x}_i^T(Q_i+A_i^T{S}_{i+1}{A}_i)x_i-x_i^T{K}_i^T(B_i^T{S}_{i+1}{A}_i)x_i\\ =& \frac{1}{2}{x}_i^T(K_i^T(R_i+B_i^T{S}_{i+1}{B}_i)K_i+Q_i+A_i^T{S}_{i+1}{A}_i-2K_i^T(B_i^T{S}_{i+1}{A}_i))x_i\end{array}$$

完整形式Riccati方程

$$\begin{array}{rl}{S}_i=& Q_i+A_i^T{S}_{i+1}{A}_i-2K^T(B_i^T{S}_{i+1}{A}_i)+K_i^T{R}_i{K}_i+K_i^T(B_i^T{S}_{i+1}{B}_i)K_i\\ =& Q_i+K_i^T{R}_i{K}_i+(A_i-B_i{K}_i{)}^T{S}_{i+1}(A_i-B_i{K}_i)\end{array}$$

标准的 离散时间 Riccati 矩阵方程 (DARE)：

将Ki代入

$$\begin{array}{rl}{S}_i=& Q_i+K_i^T{R}_i{K}_i+(A_i-B_i{K}_i{)}^T{S}_{i+1}(A_i-B_i{K}_i)\\ =& Q_i+((R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}(B_i^T{S}_{i+1}{A}_i){)}^T{R}_i(R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}(B_i^T{S}_{i+1}{A}_i)+(A_i-B_i(R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}(B_i^T{S}_{i+1}{A}_i){)}^T{S}_{i+1}(A_i-B_i(R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}(B_i^T{S}_{i+1}{A}_i))\\ =& Q_i+A_i^T{S}_{i+1}{A}_i-A_i^T{S}_{i+1}{B}_i(R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}{B}_i^T{S}_{i+1}{A}_i\end{array}$$

iLQR

状态转移方程变成非线性，即

$$x_{k+1}=f(x_k,u_k)$$

目标函数为

$$J(x_0,U)=h(x_N)+{\Sigma }_{k=0}^{N-1}l(x_k,u_k)$$

如果我们用iLQR去优化轨迹

假设我们有一个粗节轨迹$(\bar{\mathbf{x}},\bar{\mathbf{u}})$，我们希望计算小的扰动$\delta x,\delta u$来改善轨迹。

扰动为

$$\begin{array}{r}\delta x_k=x_k-{\bar{x}}_k\\ \delta u_k=u_k-{\bar{u}}_k\end{array}$$

我们需要将非线性泰勒展开，得到线性的局部目标函数及状态转移方程。

对状态转移方程进行展开
$$\begin{array}{rl}& x_{k+1}\approx f({\bar{x}}_k,{\bar{u}}_k)+\frac{\partial f}{\partial x}{|}_{\bar{k}}(x_k-{\bar{x}}_k)+\frac{\partial f}{\partial u}{|}_{\bar{k}}(u_k-{\bar{u}}_k)\\ & A_k=\frac{\partial f}{\partial x}{|}_{\bar{k}}\\ & B_k=\frac{\partial f}{\partial u}{|}_{\bar{k}}\\ & c_k=f({\bar{x}}_k,{\bar{u}}_k)-{\bar{x}}_{k+1}\end{array}$$

$$\delta x_{k+1}=A_k\delta x_k+B_k\delta u_k+c_k$$

目标函数的二次化（局部成本）

$$\delta J=\delta h(x_N)+{\Sigma }_{k=0}^{N-1}\delta l(x_k,u_k)$$

运行成本

对l进行泰勒展开（在粗解的point上）

$$l(x_k,u_k)\approx l({\bar{x}}_k,{\bar{u}}_k)+l_x^T\delta x_k+l_u^T\delta u_k+\frac{1}{2}\delta x_k^T{l}_{xx}\delta x_k+\frac{1}{2}\delta u_k^T{l}_{uu}\delta u_k+\delta x_k^T{l}_{xu}\delta u_k,l({\bar{x}}_k,{\bar{u}}_k)为常数$$

终端成本

$$h(x_N)\approx h({\bar{x}}_N)+h_x^T\delta x_N+\frac{1}{2}\delta x_N^T{h}_{xx}\delta x_N,h({\bar{x}}_N)为常数$$

此时可以发现，与LQR相比，iLQR除了二次型外，还有线性项

iLQR 与 LQR 的线性项差异

🔹 LQR (Linear Quadratic Regulator)：纯二次型

核心目标： 将状态驱动到原点 $\mathbf{x}=0$。

A. 零参考和零梯度 (Zero Reference & Zero Gradient)

LQR 的局部目标函数是纯二次型，且假设最优轨迹是 $\mathbf{x}=0,\mathbf{u}=0$。
在原点 $\mathbf{x}=0$ 处，成本函数对于状态 $\mathbf{x}$ 的梯度（一阶导数）总是 零：
$$\frac{\partial l}{\partial \mathbf{x}}{|}_{\mathbf{x}=0,\mathbf{u}=0}=0$$

B. 贝尔曼方程的性质：线性项消失

由于成本函数在原点没有梯度，所以值函数 $V_k$ 在 $\mathbf{x}=0$ 附近也没有梯度。
值函数 $V_k$ 的泰勒展开式为：
$$V_k({\mathbf{x}}_k)\approx V_k(0)+\underset{\text{梯度}}{\underbrace{{\mathbf{v}}_k^T}}{\mathbf{x}}_k+\frac{1}{2}{\mathbf{x}}_k^T{\mathbf{V}}_{\mathbf{xx}}{\mathbf{x}}_k$$
由于 $V_k(0)=0$ 且 ${\mathbf{v}}_k=\frac{\partial V_k}{\partial \mathbf{x}}{|}_{\mathbf{x}=0}=0$，所有线性项都消失了。

LQR 结论： 值函数是纯二次型：
$$V_k({\mathbf{x}}_k)=\frac{1}{2}{\mathbf{x}}_k^T{\mathbf{S}}_k{\mathbf{x}}_k$$

🔹 iLQR (Iterative LQR)：仿射二次型 (Affine Quadratic)

核心目标： 在一条任意名义轨迹 $(\bar{\mathbf{x}},\bar{\mathbf{u}})$ 附近进行局部优化。

A. 轨迹偏离原点，存在非零梯度 (Non-Zero Gradient)

名义轨迹 $(\bar{\mathbf{x}},\bar{\mathbf{u}})$ 通常不经过原点，且在优化过程中通常不是最优的。
我们在名义点 ${\bar{\mathbf{x}}}_k$ 处计算局部成本 $\mathbf{\delta l}$。由于 ${\bar{\mathbf{x}}}_k$ 不是最优轨迹，局部成本函数 $l(\mathbf{x},\mathbf{u})$ 在 ${\bar{\mathbf{x}}}_k$ 处对 $\mathbf{x}$ 和 $\mathbf{u}$ 的导数不为零：
$${\mathbf{l}}_{\mathbf{x}}=\frac{\partial l}{\partial \mathbf{x}}{|}_{\bar{k}}\ne 0$$
$${\mathbf{l}}_{\mathbf{u}}=\frac{\partial l}{\partial \mathbf{u}}{|}_{\bar{k}}\ne 0$$

B. 线性项的引入：驱动优化过程

这些非零的梯度 ${\mathbf{l}}_{\mathbf{x}}$ 和 ${\mathbf{l}}_{\mathbf{u}}$ 在 Q 函数 ${\mathcal{Q}}_k$ 的泰勒展开中引入了线性分量 $\mathbf{q}$ 和 $\mathbf{r}$：
$${\mathcal{Q}}_k\approx \cdots +\underset{\text{非零}}{\underbrace{{\mathbf{q}}^T{\mathbf{\delta x}}_k}}+\underset{\text{非零}}{\underbrace{{\mathbf{r}}^T{\mathbf{\delta u}}_k}}+\frac{1}{2}{\mathbf{\delta x}}_k^T\mathbf{Q}{\mathbf{\delta x}}_k+\dots$$
由于 $\mathbf{q}$ 和 $\mathbf{r}$ 不为零，通过贝尔曼方程逆向递推得到的值函数 $V_k$ 的梯度 ${\mathbf{v}}_k$ 也不为零：
$${\mathbf{v}}_k=\mathbf{q}-\mathbf{M}{\mathbf{R}}^{-1}\mathbf{r}\ne 0$$

iLQR 结论： 值函数是仿射二次型（包含线性项）：
$$V_k({\mathbf{\delta x}}_k)\approx \text{const}+\underset{\text{非零线性项}}{\underbrace{{\mathbf{v}}_k^T{\mathbf{\delta x}}_k}}+\frac{1}{2}{\mathbf{\delta x}}_k^T{\mathbf{V}}_{\mathbf{xx}}{\mathbf{\delta x}}_k$$

🎯 总结：线性项的作用

iLQR 中的非零线性项 ${\mathbf{v}}_k^T{\mathbf{\delta x}}_k$ 意味着最优值函数在当前名义点处有一个非零的斜率。

这个斜率就是优化过程的驱动力：它告诉我们沿着哪个方向（即最优反馈控制 ${\mathbf{\delta u}}_k^{\ast }$）移动 $\mathbf{\delta x}$ 可以获得最大的成本下降，从而不断地将名义轨迹推向局部最优。

iLQR的最优目标函数

终端：
在k = N 时，最优未来成本是终端成本
$$V_N(x_N)=h(x_N)$$
将其泰勒展开，去掉常数项，可以得到

$$V_N(\delta x_N)\approx h_x^T\delta x_N+\frac{1}{2}\delta x_N^T{h}_{xx}\delta x_N$$

一般：
$$V_k(\delta x_k)=C_k+v_k^T\delta x_k+\frac{1}{2}\delta x_k^T{V}_{xx,k}\delta x_k$$

其中，在终端的时候
$$v_N=h_x,V_{xx,N}=h_{xx}$$

递推过程k + 1 ----> k：

$$Q_k(\delta x_k,\delta u_k)=l(x_k,u_k)+V_{k+1}(x_{k+1})$$

我们的目标是将 ${\mathcal{Q}}_k$ 在名义轨迹点 $({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)$ 附近展开，得到一个关于扰动 $({\mathbf{\delta x}}_k,{\mathbf{\delta u}}_k)$ 的二次近似：

$${\mathcal{Q}}_k\approx \text{常数}+{\mathbf{q}}^T{\mathbf{\delta x}}_k+{\mathbf{r}}^T{\mathbf{\delta u}}_k+\frac{1}{2}{\mathbf{\delta x}}_k^T\mathbf{Q}{\mathbf{\delta x}}_k+\frac{1}{2}{\mathbf{\delta u}}_k^T\mathbf{R}{\mathbf{\delta u}}_k+{\mathbf{\delta x}}_k^T\mathbf{M}{\mathbf{\delta u}}_k$$

分别展开 $l$ 和 $V_{k+1}$，然后将它们的系数合成。

2. 已知信息 (输入)

在 $k$ 时刻的逆向递推中，我们已知以下信息：

1. 瞬时成本 $l$ 的导数：
   ${\mathbf{l}}_{\mathbf{x}},{\mathbf{l}}_{\mathbf{u}}$ (梯度)
   ${\mathbf{l}}_{\mathbf{xx}},{\mathbf{l}}_{\mathbf{uu}},{\mathbf{l}}_{\mathbf{xu}}$ (Hessian)

2. 未来成本 $V_{k+1}$ 的近似：
   ${\mathbf{v}}_{k+1}$ ($V_{k+1}$ 对 ${\mathbf{\delta x}}_{k+1}$ 的梯度)
   ${\mathbf{V}}_{\mathbf{xx},k+1}$ ($V_{k+1}$ 对 ${\mathbf{\delta x}}_{k+1}$ 的 Hessian)

3. 线性化动力学：
   ${\mathbf{\delta x}}_{k+1}={\mathbf{A}}_k{\mathbf{\delta x}}_k+{\mathbf{B}}_k{\mathbf{\delta u}}_k+{\mathbf{c}}_k$

瞬时成本 $l$ 的展开 (直接展开)

我们首先对 $l({\mathbf{x}}_k,{\mathbf{u}}_k)$ 在 $({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)$ 附近进行二阶泰勒展开：

$$l({\mathbf{x}}_k,{\mathbf{u}}_k)\approx l({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)+{\mathbf{l}}_{\mathbf{x}}^T{\mathbf{\delta x}}_k+{\mathbf{l}}_{\mathbf{u}}^T{\mathbf{\delta u}}_k+\frac{1}{2}{\mathbf{\delta x}}_k^T{\mathbf{l}}_{\mathbf{xx}}{\mathbf{\delta x}}_k+\frac{1}{2}{\mathbf{\delta u}}_k^T{\mathbf{l}}_{\mathbf{uu}}{\mathbf{\delta u}}_k+{\mathbf{\delta x}}_k^T{\mathbf{l}}_{\mathbf{xu}}{\mathbf{\delta u}}_k$$

未来成本 $V_{k+1}$ 的展开 (链式法则)

我们必须将 $V_{k+1}$（它是 ${\mathbf{\delta x}}_{k+1}$ 的函数）转换为 $({\mathbf{\delta x}}_k,{\mathbf{\delta u}}_k)$ 的函数。

我们从 $V_{k+1}$ 的已知近似开始：
$$V_{k+1}({\mathbf{\delta x}}_{k+1})\approx \text{常数}+{\mathbf{v}}_{k+1}^T{\mathbf{\delta x}}_{k+1}+\frac{1}{2}{\mathbf{\delta x}}_{k+1}^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{\delta x}}_{k+1}$$

现在，我们将线性化动力学 ${\mathbf{\delta x}}_{k+1}={\mathbf{A}}_k{\mathbf{\delta x}}_k+{\mathbf{B}}_k{\mathbf{\delta u}}_k+{\mathbf{c}}_k$ 代入上式。

$V_{k+1}$ 的线性项展开

将 ${\mathbf{\delta x}}_{k+1}$ 代入 ${\mathbf{v}}_{k+1}^T{\mathbf{\delta x}}_{k+1}$：
$${\mathbf{v}}_{k+1}^T{\mathbf{\delta x}}_{k+1}={\mathbf{v}}_{k+1}^T({\mathbf{A}}_k{\mathbf{\delta x}}_k+{\mathbf{B}}_k{\mathbf{\delta u}}_k+{\mathbf{c}}_k)$$
$${\mathbf{v}}_{k+1}^T{\mathbf{\delta x}}_{k+1}=\underset{\text{对 }{\mathbf{\delta x}}_k\text{ 线性}}{\underbrace{({\mathbf{A}}_k^T{\mathbf{v}}_{k+1}{)}^T{\mathbf{\delta x}}_k}}+\underset{\text{对 }{\mathbf{\delta u}}_k\text{ 线性}}{\underbrace{({\mathbf{B}}_k^T{\mathbf{v}}_{k+1}{)}^T{\mathbf{\delta u}}_k}}+\underset{\text{常数}}{\underbrace{{\mathbf{v}}_{k+1}^T{\mathbf{c}}_k}}$$

$V_{k+1}$ 的二次项展开

将 ${\mathbf{\delta x}}_{k+1}$ 代入 $\frac{1}{2}{\mathbf{\delta x}}_{k+1}^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{\delta x}}_{k+1}$：
$$\frac{1}{2}({\mathbf{A}}_k{\mathbf{\delta x}}_k+{\mathbf{B}}_k{\mathbf{\delta u}}_k+{\mathbf{c}}_k{)}^T{\mathbf{V}}_{\mathbf{xx},k+1}({\mathbf{A}}_k{\mathbf{\delta x}}_k+{\mathbf{B}}_k{\mathbf{\delta u}}_k+{\mathbf{c}}_k)$$

展开这个二次型（我们只保留到二阶，忽略 ${\mathbf{c}}_k$ 的二次项，因为它只是常数）：

${\mathbf{\delta x}}_k$ 的二次项: $\frac{1}{2}({\mathbf{A}}_k{\mathbf{\delta x}}_k{)}^T{\mathbf{V}}_{\mathbf{xx},k+1}({\mathbf{A}}_k{\mathbf{\delta x}}_k)=\frac{1}{2}{\mathbf{\delta x}}_k^T({\mathbf{A}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{A}}_k){\mathbf{\delta x}}_k$
${\mathbf{\delta u}}_k$ 的二次项: $\frac{1}{2}({\mathbf{B}}_k{\mathbf{\delta u}}_k{)}^T{\mathbf{V}}_{\mathbf{xx},k+1}({\mathbf{B}}_k{\mathbf{\delta u}}_k)=\frac{1}{2}{\mathbf{\delta u}}_k^T({\mathbf{B}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{B}}_k){\mathbf{\delta u}}_k$
交叉项 (${\mathbf{\delta x}}_k,{\mathbf{\delta u}}_k$): ${\mathbf{\delta x}}_k^T({\mathbf{A}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{B}}_k){\mathbf{\delta u}}_k$
线性项 (来自 ${\mathbf{c}}_k$): ${\mathbf{\delta x}}_k^T({\mathbf{A}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{c}}_k)+{\mathbf{\delta u}}_k^T({\mathbf{B}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{c}}_k)$

(注：iLQR 简化忽略了 $f$ 的二阶导数 ${\mathbf{f}}_{\mathbf{xx}},{\mathbf{f}}_{\mathbf{uu}}$，它们本应出现在 $V_{k+1}$ 的展开中。)

合成 Q 函数系数

现在我们将第 1 部分 ($l$ 的展开) 和第 2 部分 ($V_{k+1}$ 的展开) 的同类项系数相加，得到 ${\mathcal{Q}}_k$ 的最终系数。

梯度 $\mathbf{q}$ (对 ${\mathbf{\delta x}}_k$ 的线性项)

$$\mathbf{q}=\frac{\partial {\mathcal{Q}}_k}{\partial {\mathbf{\delta x}}_k}=\underset{\text{来自 }l}{\underbrace{{\mathbf{l}}_{\mathbf{x}}}}+\underset{\text{来自 }{V}_{k+1}\text{ 线性项}}{\underbrace{{\mathbf{A}}_k^T{\mathbf{v}}_{k+1}}}+\underset{\text{来自 }{V}_{k+1}\text{ 二次项与 }{\mathbf{c}}_k\text{ 交叉}}{\underbrace{{\mathbf{A}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{c}}_k}}$$

梯度 $\mathbf{r}$ (对 ${\mathbf{\delta u}}_k$ 的线性项)

$$\mathbf{r}=\frac{\partial {\mathcal{Q}}_k}{\partial {\mathbf{\delta u}}_k}=\underset{\text{来自 }l}{\underbrace{{\mathbf{l}}_{\mathbf{u}}}}+\underset{\text{来自 }{V}_{k+1}\text{ 线性项}}{\underbrace{{\mathbf{B}}_k^T{\mathbf{v}}_{k+1}}}+\underset{\text{来自 }{V}_{k+1}\text{ 二次项与 }{\mathbf{c}}_k\text{ 交叉}}{\underbrace{{\mathbf{B}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{c}}_k}}$$

Hessian $\mathbf{Q}$ (对 ${\mathbf{\delta x}}_k,{\mathbf{\delta x}}_k$ 的二次项)

$$\mathbf{Q}=\frac{{\partial }^2{\mathcal{Q}}_k}{\partial {\mathbf{\delta x}}_k^2}=\underset{\text{来自 }l}{\underbrace{{\mathbf{l}}_{\mathbf{xx}}}}+\underset{\text{来自 }{V}_{k+1}\text{ 二次项}}{\underbrace{{\mathbf{A}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{A}}_k}}$$

Hessian $\mathbf{R}$ (对 ${\mathbf{\delta u}}_k,{\mathbf{\delta u}}_k$ 的二次项)

$$\mathbf{R}=\frac{{\partial }^2{\mathcal{Q}}_k}{\partial {\mathbf{\delta u}}_k^2}=\underset{\text{来自 }l}{\underbrace{{\mathbf{l}}_{\mathbf{uu}}}}+\underset{\text{来自 }{V}_{k+1}\text{ 二次项}}{\underbrace{{\mathbf{B}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{B}}_k}}$$

Hessian $\mathbf{M}$ (对 ${\mathbf{\delta x}}_k,{\mathbf{\delta u}}_k$ 的交叉项)

$$\mathbf{M}=\frac{{\partial }^2{\mathcal{Q}}_k}{\partial {\mathbf{\delta x}}_k\partial {\mathbf{\delta u}}_k}=\underset{\text{来自 }l}{\underbrace{{\mathbf{l}}_{\mathbf{xu}}}}+\underset{\text{来自 }{V}_{k+1}\text{ 二次项}}{\underbrace{{\mathbf{A}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{B}}_k}}$$