泰勒展开（Taylor Expansion）是微积分中一个重要工具，它将一个在某点可导函数表示为无穷级数，从而用多项式近似函数。它广泛用于数值计算、物理建模、SLAM 优化等领域。

---

一、单变量泰勒展开公式

基本形式

设函数 $f(x)$ 在某点 $a$ 处具有无限阶导数，则它在该点的泰勒展开为：

$$f(x)=f(a)+f^{\prime }(a)(x-a)+\frac{f^{\prime \prime }(a)}{2!}(x-a{)}^2+\cdots +\frac{f^{(n)}(a)}{n!}(x-a{)}^n+R_n(x)$$

其中：

• $f^{(n)}(a)$：第 $n$ 阶导数在 $a$ 点的值。
• $R_n(x)$：余项，用于表示截断误差。

麦克劳林展开（Maclaurin）

是泰勒展开在 $a=0$ 处的特例：

$$f(x)=f(0)+f^{\prime }(0)x+\frac{f^{\prime \prime }(0)}{2!}{x}^2+\cdots$$

---

常见函数的泰勒展开（Maclaurin）

函数	展开式
$e^x$	$1+x+\frac{x^2}{2!}+\frac{x^3}{3!}+\cdots$
$\sin x$	$x-\frac{x^3}{3!}+\frac{x^5}{5!}-\cdots$
$\cos x$	$1-\frac{x^2}{2!}+\frac{x^4}{4!}-\cdots$
$\ln (1+x)$	$x-\frac{x^2}{2}+\frac{x^3}{3}-\cdots (-1<x\le 1)$

---

二、多元泰勒展开公式（Multivariable Taylor Expansion）

设函数 $f(x,y)$ 在点 $(a,b)$ 处具有连续偏导数，我们希望对 $f(x,y)$ 在点 $(a,b)$ 处展开，近似表示在附近的值 $f(a+h,b+k)$。

一阶泰勒展开：

$$f(a+h,b+k)\approx f(a,b)+f_x(a,b)h+f_y(a,b)k$$

是函数在点 $(a,b)$ 处的线性近似（即切平面）。

---

二阶泰勒展开：

$$\begin{array}{rl}f(a+h,b+k)\approx & f(a,b)+f_x(a,b)h+f_y(a,b)k\\ & +\frac{1}{2}\left[f_{xx}(a,b)h^2+2f_{xy}(a,b)hk+f_{yy}(a,b)k^2\right]\end{array}$$

这个式子也可写作矩阵形式（见下文）。

---

向量记号的二阶泰勒展开（推广到 $n$ 维）：

设函数 $f:{\mathbb{R}}^n\to \mathbb{R}$，点 ${\mathbf{x}}_0\in {\mathbb{R}}^n$，展开点为 $\mathbf{x}={\mathbf{x}}_0+\Delta \mathbf{x}$：

$$f({\mathbf{x}}_0+\Delta \mathbf{x})\approx f({\mathbf{x}}_0)+\nabla f({\mathbf{x}}_0{)}^{\top }\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^{\top }{H}_f({\mathbf{x}}_0)\Delta \mathbf{x}$$

其中：

• $\nabla f$：梯度列向量（偏导数）；
• $H_f$：Hessian 矩阵（所有二阶偏导）；
• $\Delta \mathbf{x}$：位移向量。

---

示例 1：展开 $f(x)=\ln (1+x)$ 于 $x=0$

$$\begin{array}{rl}f(x)& =\ln (1+x)\\ f^{\prime }(x)& =\frac{1}{1+x},f^{\prime \prime }(x)=-\frac{1}{(1+x{)}^2},\dots \\ \Rightarrow f(x)& =x-\frac{x^2}{2}+\frac{x^3}{3}-\cdots (-1<x\le 1)\end{array}$$

---

示例 2：多元函数 $f(x,y)=e^{x+y}$ 在 $(0,0)$ 处展开

• $f(x,y)=1+(x+y)+\frac{1}{2!}(x+y{)}^2+\cdots$

也可展开为：

$$f(x,y)=1+x+y+\frac{x^2}{2}+xy+\frac{y^2}{2}+\cdots$$

---

示例 3：在优化中的使用（如 SLAM）

在 GTSAM 或 Ceres 等优化库中，经常用一阶或二阶泰勒展开来逼近非线性残差函数：

设残差函数 $r(\mathbf{x})$，优化变量 $\mathbf{x}$ 附近一阶展开：

$$r(\mathbf{x}+\delta )\approx r(\mathbf{x})+J\delta$$

其中 $J=\frac{\partial r}{\partial \mathbf{x}}$ 是雅可比矩阵，这是高斯-牛顿等算法的核心。

---

三、多元函数的极值判别法（极值充分条件）

判断多元函数 $f(x,y)$ 在某点是否取得极值，步骤如下：

Step 1: 求偏导

$$f_x(x,y)=0,f_y(x,y)=0$$

求解得驻点（候选极值点）。

---

Step 2: 构造海森矩阵（Hessian Matrix）

$$H=\left[\begin{array}{cc}{f}_{xx}& f_{xy}\\ f_{yx}& f_{yy}\end{array}\right]$$

记 $D=\det (H)=f_{xx}{f}_{yy}-(f_{xy}{)}^2$

---

Step 3: 二阶判别条件

• 若 $D>0$ 且 $f_{xx}>0$ → 极小值
• 若 $D>0$ 且 $f_{xx}<0$ → 极大值
• 若 $D<0$ → 鞍点（非极值点）
• 若 $D=0$ → 无法判别（需更高阶展开）

---

示例：

$$f(x,y)=x^2+y^2+xy$$

1. 梯度：

   $$f_x=2x+y,f_y=2y+x\Rightarrow x=y=0\text{为驻点}$$

2. 二阶导数：

   $$f_{xx}=2,f_{yy}=2,f_{xy}=1\Rightarrow D=2\cdot 2-1^2=3>0\Rightarrow f_{xx}=2>0\Rightarrow (0,0)为极小值点$$

---

四、误差传播应用实战

在**误差传播（Error Propagation）**中，泰勒展开是分析非线性函数对输入误差敏感性的核心工具，尤其是一阶展开被广泛用于估计输出协方差。下面详细说明：

---

1、误差传播的基本思想

给定一个输入变量向量 $\mathbf{x}\in {\mathbb{R}}^n$，具有均值 $\bar{\mathbf{x}}$ 和协方差矩阵 ${\Sigma }_x$，其经过非线性变换：

$$\mathbf{y}=f(\mathbf{x})\in {\mathbb{R}}^m$$

我们希望估计输出 $\mathbf{y}$ 的协方差矩阵 ${\Sigma }_y$。

---

2、一阶泰勒展开用于误差传播

在输入均值点 $\bar{\mathbf{x}}$ 附近进行一阶展开：

$$f(\mathbf{x})\approx f(\bar{\mathbf{x}})+J(\mathbf{x}-\bar{\mathbf{x}})$$

其中：

• $J={\frac{\partial f}{\partial \mathbf{x}}|}_{\bar{\mathbf{x}}}\in {\mathbb{R}}^{m\times n}$ 是雅可比矩阵

那么，输出协方差 ${\Sigma }_y$ 近似为：

$${\Sigma }_y\approx J{\Sigma }_x{J}^T$$

这就是经典的线性误差传播公式。

---

3、实际示例（坐标转换）

场景：从极坐标 $(r,\theta )$ 转换到笛卡尔坐标 $(x,y)$

定义转换：

$$f(r,\theta )=\left[\begin{array}{c}x\\ y\end{array}\right]=\left[\begin{array}{c}r\cos \theta \\ r\sin \theta \end{array}\right]$$

假设：

• 输入 $\bar{\mathbf{x}}=\left[\begin{array}{c}r\\ \theta \end{array}\right]$
• 输入协方差为：

$${\Sigma }_x=\left[\begin{array}{cc}{\sigma }_r^2& 0\\ 0& {\sigma }_{\theta }^2\end{array}\right]$$

计算雅可比矩阵：

$$J=\left[\begin{array}{cc}\cos \theta & -r\sin \theta \\ \sin \theta & r\cos \theta \end{array}\right]$$

代入误差传播公式：

$${\Sigma }_y=J{\Sigma }_x{J}^T$$

就得到了 $(x,y)$ 的协方差。

---

4、在 SLAM / GTSAM 中的应用场景

应用场景	描述
IMU 预积分误差传播	用一阶泰勒近似推导预积分中速度/位置的协方差更新
点云 ICP / 位姿图优化中	将相对变换误差从局部坐标系传播到世界系：使用雅可比左乘传播
视觉SLAM中三角化点的不确定性传播	将图像点误差通过三角化过程传播到3D点的协方差
优化中残差的线性化	高斯-牛顿、LM 中都依赖于雅可比传播误差与协方差

---

5、Python 示例代码

以上面的极坐标转直角坐标为例：

import numpy as np

# 均值点
r = 2.0
theta = np.pi / 4

# 输入协方差
sigma_r = 0.05
sigma_theta = 0.02
Sigma_x = np.diag([sigma_r**2, sigma_theta**2])

# 计算雅可比
J = np.array([
    [np.cos(theta), -r * np.sin(theta)],
    [np.sin(theta),  r * np.cos(theta)]
])

# 输出协方差
Sigma_y = J @ Sigma_x @ J.T
print("Output covariance Σ_y:\n", Sigma_y)

---

6、总结

项目	内容
使用条件	非线性函数可微，输入误差较小（局部线性近似有效）
优点	计算快速，适合高频状态估计（如 IMU 融合）
局限	线性化误差大时不准，需二阶或采样方法（如 UKF、蒙特卡洛）
常用工具	雅可比矩阵、协方差传播、残差线性化

---

五、总结表格

内容	表达式	作用
一阶泰勒展开	$f(\mathbf{x})\approx f({\mathbf{x}}_0)+\nabla f^{\top }\Delta \mathbf{x}$	一阶近似
二阶泰勒展开	加上 $\frac{1}{2}\Delta x^{\top }H\Delta x$	曲率控制精度更高
极值判断	利用 Hessian 矩阵的行列式 $D$ 和符号	判定极大、极小或鞍点

---