10. 凸优化与时间序列

# 卡尔曼滤波

卡尔曼滤波（Kalman Filtering）由 Rudolf E. Kalman 在 1960 年提出，是一种高效的递归估计器，用于估计带噪声的线性离散时间动态系统的状态。它是状态估计和信号处理中的经典方法。

1. 系统模型与观测模型

   动态系统模型

   线性离散时间动态系统可以表示为：

   $$x(k+1) = A x(k) + B u(k) + w(k)$$

   其中：

   • $x \in \mathbb{R}^n$ 是需要估计的状态向量
   • $u \in \mathbb{R}^m$ 是已知的控制输入
   • $A \in \mathbb{R}^{n \times n}$ 是系统矩阵
   • $B \in \mathbb{R}^{n \times m}$ 是控制矩阵
   • $w(k) \in \mathbb{R}^n$ 是过程噪声（系统噪声）

   观测模型

   我们可以获得测量值 $z \in \mathbb{R}^l$：

   $$z(k) = H x(k) + v(k)$$

   其中：

   • $H \in \mathbb{R}^{l \times n}$ 是观测矩阵
   • $v(k) \in \mathbb{R}^l$ 是测量噪声

   噪声假设

   假设 $w(k)$ 和 $v(k)$ 是多元正态分布的扰动：

   $$w(k) \sim \mathcal{N}(0, Q), \quad v(k) \sim \mathcal{N}(0, R)$$

   其中：

   • $Q \in \mathbb{R}^{n \times n}$ 是过程噪声的协方差矩阵
   • $R \in \mathbb{R}^{l \times l}$ 是测量噪声的协方差矩阵

   此外，假设 $x(0)$、$w$ 和 $v$ 相互独立。

2. 贝叶斯推导

   历史数据

   设 $Z_k = (z(1), \ldots, z(k))$ 表示时刻 $k$ 的历史观测值，$X_k = (x(1), \ldots, x(k))$ 表示时刻 $k$ 的历史状态值。

   目标

   我们的目标是获得给定 $Z_k$ 时 $X_k$ 的后验分布。

   贝叶斯规则

   使用贝叶斯规则和独立性假设，我们有：

   $$\begin{aligned} \text{Pr}(X_k \mid Z_k) &= \frac{\text{Pr}(Z_k \mid X_k) \text{Pr}(X_k)}{\text{Pr}(Z_k)} \\ &= \frac{\text{Pr}(x(0)) \prod_{t=1}^k \text{Pr}(z(t) \mid x(t)) \prod_{t=1}^{k-1} \text{Pr}(x(t+1) \mid x(t))}{\text{Pr}(Z_k)} \\ &\propto \text{Pr}(x(0)) \prod_{t=1}^k \text{Pr}[z(t) - H x(t)] \prod_{t=1}^{k-1} \text{Pr}[x(t+1) - A x(t) - B u(t)] \\ &= \text{Pr}(x(0)) \prod_{t=1}^k \text{Pr}(v(t)) \prod_{t=1}^{k-1} \text{Pr}(w(t)) \end{aligned}$$

3. 最大后验概率（MAP）估计

   MAP 估计问题

   可以将卡尔曼滤波视为最大后验概率（MAP）估计器：

   $$\begin{aligned} \min_{x(t)} \quad & \prod_{t=1}^k e^{-\frac{1}{2}[z(t) - H x(t)]^T R^{-1}[z(t) - H x(t)]} \\ &+ \prod_{t=1}^{k-1} e^{-\frac{1}{2}[x(t+1) - A x(t) - B u(t)]^T Q^{-1}[x(t+1) - A x(t) - B u(t)]} \end{aligned}$$

   最小二乘形式

   对似然函数取自然对数，可以将卡尔曼滤波转化为最小二乘问题：

   $$\begin{aligned} \min_{x(t)} \quad J_k &= \sum_{t=1}^k [z(t) - H x(t)]^T R^{-1}[z(t) - H x(t)] \\ &\quad + \sum_{t=1}^{k-1} [x(t+1) - A x(t) - B u(t)]^T Q^{-1}[x(t+1) - A x(t) - B u(t)] \end{aligned}$$

   这等价于最小化：

   • 观测误差的加权平方和（第一项）
   • 状态转移误差的加权平方和（第二项）

4. 递归形式

   递归结构

   注意到新的目标函数满足：

   $$\begin{aligned} \min_{x(t)} J_k &= J_{k-1} + [z(k) - H x(k)]^T R^{-1}[z(k) - H x(k)] \\ &\quad + [x(k) - A x(k-1) - B u(k-1)]^T Q^{-1}[x(k) - A x(k-1) - B u(k-1)] \end{aligned}$$

   这种递归结构使得我们可以高效地计算状态估计。

   标准递归形式

   可以得到标准的卡尔曼滤波递归形式：

   $$\begin{aligned} K_{t+1} &= (A P_t A^T + Q) H^T [H(A P_t A^T + Q) H^T + R]^{-1} \\ \hat{x}(t+1) &= A \hat{x}(t) + B u(t) + K_{t+1}[z(t+1) - H(A \hat{x}(t) + B u(t))] \\ P_{t+1} &= (I - K_{t+1} H)(A P_t A^T + Q) \end{aligned}$$

   其中：

   • $\hat{x}(t+1)$ 是时刻 $t+1$ 的状态递归估计
   • $K_t$ 是卡尔曼增益矩阵
   • $P_t$ 是估计误差的协方差矩阵

   算法步骤

   1. 预测步骤（Predict）：

      • 预测状态：$\hat{x}(t+1 \mid t) = A \hat{x}(t) + B u(t)$
      • 预测协方差：$P(t+1 \mid t) = A P_t A^T + Q$

   2. 更新步骤（Update）：

      • 计算卡尔曼增益：$K_{t+1} = P(t+1 \mid t) H^T [H P(t+1 \mid t) H^T + R]^{-1}$
      • 更新状态估计：$\hat{x}(t+1) = \hat{x}(t+1 \mid t) + K_{t+1}[z(t+1) - H \hat{x}(t+1 \mid t)]$
      • 更新协方差：$P_{t+1} = (I - K_{t+1} H) P(t+1 \mid t)$

5. 卡尔曼滤波的特点

   优势

   • 递归计算，计算效率高
   • 最优性：在线性高斯假设下，是最优估计器（最小均方误差）
   • 提供不确定性量化（通过协方差矩阵 $P_t$）
   • 可以处理时变系统

   局限性

   • 假设系统是线性的
   • 假设噪声是高斯分布的
   • 对于非线性系统，需要使用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）

6. 应用

   • 导航系统：GPS、惯性导航
   • 目标跟踪：雷达、声纳跟踪
   • 控制系统：状态反馈控制
   • 信号处理：信号去噪、平滑

# L1 滤波与总体方差

$\ell_1$ 滤波和总体方差（Total Variation, TV）滤波是用于一维信号去噪的方法，特别适用于具有分段线性趋势的时间序列。

1. $\ell_1$ 滤波 ($\ell_1$ Filtering)

   问题设定

   $\ell_1$ 滤波旨在抑制一维信号的噪声。它产生分段线性趋势估计结果，因此适用于具有潜在分段线性趋势的时间序列。

   给定标量时间序列 $y(t), t = 1, \ldots, k$，$\ell_1$ 滤波旨在获得估计的标量时间序列 $x(t)$，使得：

   $$\min_{x(t)} \quad \sum_{t=1}^k [y(t) - x(t)]^2 + \lambda \sum_{t=1}^{k-1} |x(t+1) - x(t)|$$

   其中：

   • 第一项是数据拟合项，衡量估计值 $x(t)$ 与观测值 $y(t)$ 的差异
   • 第二项是正则化项，惩罚相邻时刻估计值之间的变化
   • $\lambda > 0$ 是正则化参数，控制平滑性和拟合误差的权衡

   特点

   • 产生分段常数或分段线性的估计
   • 对异常值鲁棒（由于使用 $\ell_1$ 范数）
   • 可以检测趋势变化点（change points）

   求解方法

   这是一个凸优化问题，可以转化为线性规划问题求解。

2. 总体方差（Total Variation, TV）滤波

   问题设定

   总体方差滤波是 $\ell_1$ 滤波的推广，最小化信号的一阶差分（或更高阶差分）的 $\ell_1$ 范数：

   $$\min_{x(t)} \quad \sum_{t=1}^k [y(t) - x(t)]^2 + \lambda \sum_{t=1}^{k-1} |x(t+1) - x(t)|$$

   这与 $\ell_1$ 滤波的形式相同，但通常称为 TV 滤波。

   TV 范数的定义

   对于一维信号 $x = [x_1, \ldots, x_n]^T$，总体方差（TV）定义为：

   $$\text{TV}(x) = \sum_{i=1}^{n-1} |x_{i+1} - x_i|$$

   这是信号一阶差分的 $\ell_1$ 范数。

   高阶 TV

   可以定义高阶总体方差：

   $$\text{TV}_d(x) = \sum_{i=1}^{n-d} |\Delta^d x_i|$$

   其中 $\Delta^d$ 是 $d$ 阶差分算子。

   TV 去噪问题

   总体方差去噪问题：

   $$\min_x \quad \frac{1}{2}\|x - y\|_2^2 + \lambda \text{TV}(x)$$

   其中 $y$ 是观测信号，$x$ 是去噪后的信号。

3. $\ell_1$ 趋势滤波 ($\ell_1$ Trend Filtering)

   问题形式

   $\ell_1$ 趋势滤波是 $\ell_1$ 滤波的另一种表述，旨在估计时间序列的趋势：

   $$\min_{x(t)} \quad \sum_{t=1}^k [y(t) - x(t)]^2 + \lambda \sum_{t=2}^{k-1} |x(t+1) - 2x(t) + x(t-1)|$$

   其中第二项惩罚二阶差分（加速度），产生分段线性趋势。

   特点

   • 估计结果是分段线性的
   • 可以检测趋势的转折点
   • 对异常值鲁棒

4. 求解算法

   直接算法

   对于一维 TV 去噪，存在直接算法（如 Condat 算法），可以在 $O(n)$ 时间内求解。

   转化为线性规划

   TV 滤波问题可以转化为线性规划问题：

   $$\begin{align*} \min_{x, d} \quad & \sum_{t=1}^k [y(t) - x(t)]^2 + \lambda \sum_{t=1}^{k-1} d(t) \\ \text{s.t.} \quad & -d(t) \leq x(t+1) - x(t) \leq d(t), \quad t = 1, \ldots, k-1 \\ & d(t) \geq 0, \quad t = 1, \ldots, k-1 \end{align*}$$

   其中 $d(t)$ 是辅助变量，表示 $|x(t+1) - x(t)|$。

5. 应用

   • 信号去噪：去除时间序列中的噪声
   • 趋势估计：估计时间序列的潜在趋势
   • 变化点检测：检测时间序列中的结构变化
   • 数据平滑：平滑观测数据

# 轨迹规划

轨迹规划（Trajectory Planning）是自动化和机器人领域中的核心问题，旨在为动态系统设计满足约束条件的最优轨迹。凸优化在轨迹规划中发挥着重要作用，特别是通过凸松弛技术处理非凸约束。

# 轨迹规划基础

1. 问题定义

   轨迹规划问题通常可以表述为最优控制问题：

   $$\begin{align*} \min_{u(t), x(t)} \quad & J = \int_{t_0}^{t_f} L(x(t), u(t), t) dt + \Phi(x(t_f)) \\ \text{s.t.} \quad & \dot{x}(t) = f(x(t), u(t), t) \\ & x(t_0) = x_0, \quad x(t_f) = x_f \\ & g(x(t), u(t), t) \leq 0 \\ & h(x(t), u(t), t) = 0 \end{align*}$$

   其中：

   • $x(t) \in \mathbb{R}^n$ 是状态向量
   • $u(t) \in \mathbb{R}^m$ 是控制输入向量
   • $L$ 是运行代价函数
   • $\Phi$ 是终端代价函数
   • $f$ 是系统动力学
   • $g, h$ 是路径约束

2. 凸优化在轨迹规划中的作用

   • 离散化：将连续时间问题离散化为有限维优化问题
   • 凸松弛：将非凸约束松弛为凸约束，得到可求解的凸优化问题
   • 损失凸化（Lossless Convexification）：在某些条件下，非凸约束的凸松弛是精确的（无损失）
   • 实时求解：凸优化问题可以高效求解，满足实时性要求

3. 主要挑战

   • 非凸约束：动力学约束、障碍物避让、输入约束等通常是非凸的
   • 计算复杂度：高维状态空间和长时间范围导致计算困难
   • 实时性要求：需要快速生成可行轨迹

# 一维规划

一维轨迹规划是最简单的情况，通常涉及沿一条路径的速度规划或位置规划。

1. 问题形式

   对于一维轨迹规划，状态通常是位置 $p(t)$ 和速度 $v(t)$：

   $$\begin{align*} \min_{u(t), p(t), v(t)} \quad & \int_{t_0}^{t_f} u(t)^2 dt \\ \text{s.t.} \quad & \dot{p}(t) = v(t) \\ & \dot{v}(t) = u(t) \\ & p(t_0) = p_0, \quad p(t_f) = p_f \\ & v(t_0) = v_0, \quad v(t_f) = v_f \\ & v_{\min} \leq v(t) \leq v_{\max} \\ & |u(t)| \leq u_{\max} \end{align*}$$

   其中 $u(t)$ 是加速度（控制输入）。

2. 离散化

   将时间区间 $[t_0, t_f]$ 离散化为 $N$ 个时间步：

   $$\begin{align*} \min_{u_k, p_k, v_k} \quad & \sum_{k=0}^{N-1} u_k^2 \\ \text{s.t.} \quad & p_{k+1} = p_k + v_k \Delta t \\ & v_{k+1} = v_k + u_k \Delta t \\ & p_0 = p_0, \quad p_N = p_f \\ & v_0 = v_0, \quad v_N = v_f \\ & v_{\min} \leq v_k \leq v_{\max} \\ & |u_k| \leq u_{\max} \end{align*}$$

   这是一个二次规划（QP）问题，可以高效求解。

3. 应用

   • 车辆纵向控制
   • 电梯调度
   • 一维机器人运动规划

# 二维规划

二维轨迹规划涉及在平面内的位置和方向规划，通常需要考虑避障和路径约束。

1. 问题形式

   对于二维轨迹规划，状态包括位置 $(x(t), y(t))$ 和方向 $\theta(t)$：

   $$\begin{align*} \min_{u(t), x(t), y(t), \theta(t)} \quad & \int_{t_0}^{t_f} [u_1(t)^2 + u_2(t)^2] dt \\ \text{s.t.} \quad & \dot{x}(t) = v(t) \cos(\theta(t)) \\ & \dot{y}(t) = v(t) \sin(\theta(t)) \\ & \dot{\theta}(t) = \omega(t) \\ & \dot{v}(t) = u_1(t) \\ & \dot{\omega}(t) = u_2(t) \\ & \text{边界条件} \\ & \text{障碍物约束} \\ & \text{输入约束} \end{align*}$$

   其中 $v(t)$ 是速度，$\omega(t)$ 是角速度，$u_1(t), u_2(t)$ 是控制输入。

2. 障碍物避让

   障碍物避让约束通常是非凸的：

   $$(x(t), y(t)) \notin \mathcal{O}_i, \quad i = 1, \ldots, M$$

   其中 $\mathcal{O}_i$ 是第 $i$ 个障碍物区域。

   凸松弛方法：

   • 将非凸障碍物约束松弛为凸约束
   • 使用分离超平面或半空间约束
   • 迭代细化约束以逼近非凸区域

3. 求解方法

   • 序列二次规划（SQP）：迭代求解二次规划子问题
   • 凸优化：通过凸松弛将问题转化为凸优化问题
   • 采样方法：结合采样和优化

4. 应用

   • 移动机器人路径规划
   • 自动驾驶车辆轨迹规划
   • 无人机路径规划

# 三维规划

三维轨迹规划是最复杂的情况，涉及三维空间中的位置、姿态和动力学约束。PPT 中重点讨论了非凸约束的凸松弛方法。

1. 问题形式

   三维轨迹规划通常涉及位置 $(x, y, z)$ 和姿态（如欧拉角或四元数）：

   $$\begin{align*} \min_{u(t), x(t), \theta(t)} \quad & \int_{t_0}^{t_f} L(x(t), u(t), t) dt \\ \text{s.t.} \quad & \dot{x}(t) = f(x(t), u(t), t) \\ & \text{边界条件} \\ & \text{非凸约束} \end{align*}$$

   其中非凸约束可能包括：

   • 推力方向约束（thrust pointing constraint）
   • 输入幅值约束
   • 障碍物约束

2. 损失凸化（Lossless Convexification）

   基本思想

   损失凸化是一种将非凸最优控制问题转化为凸优化问题的技术，在满足某些条件下，凸松弛是精确的（无损失），即凸松弛问题的最优解也是原非凸问题的最优解。

   非凸输入约束

   考虑非凸输入约束：

   $$u \in \mathcal{U} = \{u \in \mathbb{R}^3 : \|u\|_2 = \sigma, \ \hat{n}_u^T u \geq \sigma \cos(\theta_{\max})\}$$

   其中：

   • $\|u\|_2 = \sigma$ 是输入幅值约束（非凸，因为等式约束）
   • $\hat{n}_u^T u \geq \sigma \cos(\theta_{\max})$ 是推力方向约束
   • $\theta_{\max}$ 是最大角度约束

   凸松弛

   将非凸约束松弛为凸约束：

   $$(u, \sigma) \in \mathcal{V} = \{(u, \sigma_2) \in \mathbb{R}^3 : \hat{n}_u^T u \geq \sigma_2 \cos(\theta_{\max}), \ \|u\|_2 \leq \sigma_2\}$$

   注意：将等式约束 $\|u\|_2 = \sigma$ 松弛为不等式约束 $\|u\|_2 \leq \sigma_2$。

   损失凸化的条件

   对于 $(u, \sigma) \in \partial_{\text{LCvx}} \mathcal{V}$ 的特殊情况，其中：

   $$\partial_{\text{LCvx}} \mathcal{V} \triangleq \{(u, \sigma_2) \in \mathbb{R}^3 : \hat{n}_u^T u \geq \sigma_2 \cos(\theta_{\max}), \ \|u\|_2 = \sigma_2\}$$

   输入 $u$ 是可行的，满足原未松弛的约束。在这种情况下，凸松弛是精确的（无损失）。

   几何解释

   通过在半空间 $\hat{n}_u^T u \geq \sigma_2 \cos(\theta_{\max})$ 中提升 $(u, \sigma)$ 空间，可以将非凸输入集松弛为凸集。如果最优输入 $(u^*, \sigma^*) \in \partial_{\text{LCvx}} \mathcal{V}$，则 $u^*$ 对原非凸约束是可行的。

   缺点

   原不可行的输入对于松弛后的表达式是可行的，因此需要额外的条件来保证损失凸化。

3. 应用

   • 垂直起降火箭（VTVL）：软着陆问题中的推力方向约束
   • 行星着陆：燃料最优的精确着陆轨迹规划
   • 无人机：三维空间中的轨迹规划
   • 航天器：姿态和位置联合规划

4. 求解方法

   • 凸优化：将问题转化为凸优化问题后使用内点法求解
   • 序列凸优化：迭代求解凸优化子问题
   • 混合整数规划：处理离散决策变量

5. 优势与挑战

   优势：

   • 可以高效求解（凸优化）
   • 在某些条件下保证全局最优
   • 可以处理复杂的约束

   挑战：

   • 需要满足损失凸化的条件
   • 对于不满足条件的约束，松弛可能引入保守性
   • 高维问题计算复杂度仍然较高