卡尔曼滤波对非线性公式建模的详细步骤

一、运动公式

1. 空气阻力

空气阻力公式：

F = (1/2) * ρ * v² * Cd * A

其中：

• F：空气阻力，单位是牛顿（N）。
• ρ（读作“rho”）：空气密度，单位是千克每立方米（kg/m³）。在海平面、15°C 的标准大气条件下，空气密度约为 1.225 kg/m³。空气密度会随着海拔升高、温度和湿度变化而降低。
• v：物体相对于空气的合速度，单位是米每秒（m/s）。注意：速度是平方项，因此速度对阻力的影响非常大。
• Cd：阻力系数（Drag Coefficient），是一个无量纲的数，它取决于物体的形状和表面的光滑程度。它通过风洞实验或计算流体动力学（CFD）模拟来确定。
  • 例如：一个光滑的球体 Cd ≈ 0.47，一个现代轿车 Cd ≈ 0.25-0.35，一个垂直于气流的平板 Cd ≈ 1.17。
• A：物体在运动方向上的参考投影面积（或称迎风面积），单位是平方米（m²）。这是物体在垂直于运动方向的平面上的投影面积。

计算阻力分量：

• Fx = -F * (vx / v)
• Fy = -F * (vy / v)
• Fz = -F * (vz / v)
  注意：这里的$v$是合速度，$v=\surd (v{x}^2+v{y}^2+v{z}^2)$

2. 系统状态向量

我们定义系统的状态向量为：

$$\mathbf{x}(t)=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\\ v_x\\ v_y\\ v_z\end{array}\right]=\left[\begin{array}{c}\text{位置 }x\\ \text{位置 }y\\ \text{位置 }z\\ \text{速度 }x\\ \text{速度 }y\\ \text{速度 }z\end{array}\right]$$

3. 连续时间动力学方程（非线性）

（1） 基本形式

$$\dot{\mathbf{x}}=f(\mathbf{x})+\mathbf{w}(t)$$

其中 $\mathbf{w}(t)$ 是过程噪声（连续白噪声）。

（2）具体表达式

$$\begin{array}{rl}{\dot{p}}_x& =v_x\\ {\dot{p}}_y& =v_y\\ {\dot{p}}_z& =v_z\\ {\dot{v}}_x& =-\frac{\alpha }{m}|\mathbf{v}|v_x\\ {\dot{v}}_y& =-\frac{\alpha }{m}|\mathbf{v}|v_y\\ {\dot{v}}_z& =-\frac{\alpha }{m}|\mathbf{v}|v_z-g\end{array}$$

其中：

• $\mathbf{v}=[v_x,v_y,v_z{]}^T$：速度向量
• $|\mathbf{v}|=\sqrt{v_x^2+v_y^2+v_z^2}$：速度大小
• $\alpha$：空气阻力系数（与形状、密度相关），$\alpha =(1/2)\rho C_{d}A$
• $m$：质量
• $g$：重力加速度（通常 $9.81{\text{m/s}}^2$）

（3）向量形式表示

令：

• $\mathbf{p}=[p_x,p_y,p_z{]}^T$
• $\mathbf{v}=[v_x,v_y,v_z{]}^T$

则：

$$\frac{d}{dt}\left[\begin{array}{c}\mathbf{p}\\ \mathbf{v}\end{array}\right]=\left[\begin{array}{c}\mathbf{v}\\ -\frac{\alpha }{m}|\mathbf{v}|\mathbf{v}-\mathbf{g}\end{array}\right]$$

其中 $\mathbf{g}=[0,0,g{]}^T$。

---

4. 离散时间运动学

如果假定$\Delta t$时间内，加速度恒定，则离散运动学模型为

$$\begin{array}{rl}{p}_{k+1}& =p_k+v_{x,k}\Delta t-\frac{F_x}{2m}(\Delta t{)}^2\\ p_{k+1}& =p_k+v_{y,k}\Delta t-\frac{F_y}{2m}(\Delta t{)}^2\\ p_{k+1}& =p_k+v_{z,k}\Delta t-\left(\frac{F_z}{m}+g\right)\frac{(\Delta t{)}^2}{2}\\ v_{x,k+1}& =v_{x,k}-\frac{F_x}{m}\Delta t\\ v_{y,k+1}& =v_{y,k}-\frac{F_y}{m}\Delta t\\ v_{z,k+1}& =v_{z,k}-\left(\frac{F_z}{m}+g\right)\Delta t\end{array}$$

二、卡尔曼滤波

1. 线性卡尔曼滤波

标准的卡尔曼滤波器（Kalman Filter, KF） 是处理线性高斯系统的最优估计算法。它的核心假设是：

• 系统动态模型是线性的：

  $${\mathbf{x}}_k=F_k{\mathbf{x}}_{k-1}+B_k{\mathbf{u}}_k+{\mathbf{w}}_k$$

• 观测模型是线性的：
  $${\mathbf{z}}_k=H_k{\mathbf{x}}_k+{\mathbf{v}}_k$$

其中 ${\mathbf{w}}_k$ 和 ${\mathbf{v}}_k$ 是高斯噪声。

初始化

1. 初始化状态向量 ${\mathbf{x}}_0$ 、状态协方差矩阵 ${\mathbf{P}}_0$、过程噪声协方差 $\mathbf{Q}$、观测噪声协方差 $\mathbf{R}$。

预测步骤

2. 状态预测：使用状态转移矩阵 $\mathbf{A}$ 预测下一时刻的状态。
   $${\hat{\mathbf{x}}}_{k|k-1}=\mathbf{A}{\hat{\mathbf{x}}}_{k-1|k-1}$$
3. 协方差预测：更新状态协方差矩阵。
   $${\mathbf{P}}_{k|k-1}=\mathbf{A}{\mathbf{P}}_{k-1|k-1}{\mathbf{A}}^T+\mathbf{Q}$$

更新步骤

4. 计算卡尔曼增益：
   $${\mathbf{K}}_k={\mathbf{P}}_{k|k-1}{\mathbf{H}}^T(\mathbf{HP}\_k|k-1{\mathbf{H}}^T+\mathbf{R}{)}^{-1}$$
5. 状态更新：利用观测值 (\mathbf{z}_k) 更新状态估计。
   $${\hat{\mathbf{x}}}_{k|k}={\hat{\mathbf{x}}}_{k|k-1}+\mathbf{K}\_k(\mathbf{z}\_k-\mathbf{H}{\hat{\mathbf{x}}}_{k|k-1})$$
6. 协方差更新：更新状态协方差矩阵。
   $${\mathbf{P}}_{k|k}=(I-{\mathbf{K}}_k\mathbf{H}){\mathbf{P}}_{k|k-1}$$

2. 扩展卡尔曼滤波（非线性）

但在许多实际问题中（如机器人定位、飞行器导航），系统的状态转移或观测过程是非线性的。这时，标准 KF 不再适用，于是我们引入 扩展卡尔曼滤波（EKF） —— 它是对非线性系统的局部线性化近似下的卡尔曼滤波。

其核心思想是在每一步使用泰勒展开对非线性函数进行一阶线性近似（即雅可比矩阵），然后套用标准卡尔曼滤波框架。

1: 非线性系统描述

假设系统满足以下非线性动态和观测模型：

$$\begin{array}{rl}\text{状态方程：}& {\mathbf{x}}_k=f({\mathbf{x}}_{k-1},{\mathbf{u}}_k)+{\mathbf{w}}_k\\ \text{观测方程：}& {\mathbf{z}}_k=h({\mathbf{x}}_k)+{\mathbf{v}}_k\end{array}$$

其中：

• ${\mathbf{x}}_k$：状态向量（如位置、速度、姿态）
• ${\mathbf{u}}_k$：控制输入
• ${\mathbf{z}}_k$：观测值（传感器读数）
• $f(\cdot )$：非线性状态转移函数
• $h(\cdot )$：非线性观测函数
• ${\mathbf{w}}_k\sim \mathcal{N}(0,Q_k)$：过程噪声
• ${\mathbf{v}}_k\sim \mathcal{N}(0,R_k)$：观测噪声

注意：这里不再有 $F_k$ 和 $H_k$，而是用函数 $f$ 和 $h$ 表示非线性关系。

2：预测（Prediction）

(1) 状态预测（通过非线性函数传播）

$${\hat{\mathbf{x}}}_{k|k-1}=f({\hat{\mathbf{x}}}_{k-1|k-1},{\mathbf{u}}_k)$$

这是将上一时刻的最优估计 ${\hat{\mathbf{x}}}_{k-1|k-1}$ 输入到非线性函数 $f$ 中，得到先验状态估计。

(2) 协方差预测

协方差描述的是状态的分布，因此我们需要知道误差在每一次预测过程中是怎么传递的来改变状态的分布。在线性系统中，转移矩阵既可以用来预测，也可以描述误差的传递。

在当前状态估计 ${\hat{\mathbf{x}}}_{k|k}$ 附近，对非线性状态转移函数 $f$ 进行一阶泰勒展开，得到 ${\mathbf{x}}_{k+1}$ 关于 ${\mathbf{x}}_k$ 的局部线性近似。：

$${\mathbf{x}}_{k+1}\approx f({\hat{\mathbf{x}}}_{k|k})+{\mathbf{F}}_k({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k})\text{\hspace{1em}(1)}$$

${\mathbf{x}}_k$：真实状态向量（at time $k$）

• 含义：系统在时刻 $k$ 的真实但未知的状态，是一个向量。
• 注意：${\mathbf{x}}_k$ 是真值，在实际中是不可直接观测的，我们只能通过传感器去估计它。

${\mathbf{x}}_{k+1}$：下一时刻的真实状态向量

• 含义：系统在时刻 $k+1$ 的真实状态。
• 它由非线性动态方程决定：
  $${\mathbf{x}}_{k+1}=f({\mathbf{x}}_k)+{\mathbf{w}}_k$$
  其中 ${\mathbf{w}}_k$ 是过程噪声（通常假设为高斯白噪声）。

$f(\cdot )$：非线性状态转移函数

• 含义：描述系统如何从 ${\mathbf{x}}_k$ 演化到 ${\mathbf{x}}_{k+1}$ 的非线性函数。

${\hat{\mathbf{x}}}_{k|k}$：后验状态估计（Posterior Estimate）

• 含义：在使用了时刻 $k$ 及之前的所有观测数据后，对时刻 $k$ 状态的最优估计。
• 作用：它是线性化展开的工作点（operating point）。
• 关键点：
  • 它是已知的（由滤波器计算得出）
  • 我们在这个点附近对 $f$ 进行泰勒展开
  • 所以它是“参考点”或“名义状态”

$f({\hat{\mathbf{x}}}_{k|k})$：在估计点处的函数值

• 含义：把当前的最佳状态估计 ${\hat{\mathbf{x}}}_{k|k}$ 代入非线性函数 $f$，得到的预测的下一时刻状态。
• 物理意义：这是 EKF 中对 ${\mathbf{x}}_{k+1}$ 的预测均值（不考虑误差时的“标称”值）。
• 注意：它不是 ${\mathbf{F}}_k{\hat{\mathbf{x}}}_{k|k}$，除非 $f$ 是线性函数且无常数项。

${\mathbf{F}}_k={\frac{\partial f}{\partial \mathbf{x}}|}_{{\hat{\mathbf{x}}}_{k|k}}$：状态转移雅可比矩阵（Jacobian Matrix）

• 含义：非线性函数 $f$ 在点 ${\hat{\mathbf{x}}}_{k|k}$ 处对状态 ${\mathbf{x}}_k$ 的偏导数矩阵。
• 维度：如果状态是 $n$ 维，则 ${\mathbf{F}}_k$ 是 $n\times n$ 矩阵。
• 作用：它表示状态的微小变化 $\delta {\mathbf{x}}_k$ 会引起 ${\mathbf{x}}_{k+1}$ 多大变化，即局部线性化斜率。
• 数学定义：
  $${\mathbf{F}}_k={\left[\begin{array}{ccc}\frac{\partial f_1}{\partial x_1}& \cdots & \frac{\partial f_1}{\partial x_n}\\ ⋮& \ddots & ⋮\\ \frac{\partial f_n}{\partial x_1}& \cdots & \frac{\partial f_n}{\partial x_n}\end{array}\right]}_{\text{at }{\mathbf{x}}_k={\hat{\mathbf{x}}}_{k|k}}$$

$({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k})$：状态误差（偏差）

• 含义：真实状态 ${\mathbf{x}}_k$ 与当前估计 ${\hat{\mathbf{x}}}_{k|k}$ 之间的偏差。
• 记作：$\delta {\mathbf{x}}_k={\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k}$
• 物理意义：这是状态的“不确定性”部分，通常假设它比较小（EKF 的前提）。

${\mathbf{F}}_k({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k})$：误差的线性传播

• 含义：状态误差 $\delta {\mathbf{x}}_k$ 经过雅可比矩阵 ${\mathbf{F}}_k$ 传播后，对 ${\mathbf{x}}_{k+1}$ 的影响。
• 作用：它表示“如果当前状态有误差，这个误差会如何影响下一时刻的状态”。

整个公式的意义总结：

$${\mathbf{x}}_{k+1}\approx \underset{\text{标称预测}}{\underbrace{f({\hat{\mathbf{x}}}_{k|k})}}+\underset{\text{误差传播}}{\underbrace{{\mathbf{F}}_k({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k})}}$$

这表示：

下一时刻的真实状态 ≈
（从当前最佳估计预测出的状态） +
（当前状态误差经线性化模型传播后的影响）

可见计算 雅可比矩阵 $F_k$，它是 $f$ 对状态 $\mathbf{x}$ 在 ${\hat{\mathbf{x}}}_{k-1|k-1}$ 处的一阶偏导数，描述了误差之间的传递：

$$F_k={\frac{\partial f}{\partial \mathbf{x}}|}_{{\hat{\mathbf{x}}}_{k-1|k-1},{\mathbf{u}}_k}$$

然后协方差预测为：

$$P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_k$$

这类似于线性 KF 中的 $P=FP{F}^T+Q$，但这里的 $F_k$ 是在当前工作点计算的。

补充学习过程中有意思的情况

在线性系统中，如果$f(0)=0$（一般运动学都会满足），则${\mathbf{x}}_k=F_k{\mathbf{x}}_{k-1}$，根据泰勒公式可知，其中$F_k$不仅是转移矩阵，同时也是雅可比矩阵。因此对于非线性系统，我们也可以将非线性系统中的每一项逐步转化为线性项，从而转换成线性系统，并将线性系统写成矩阵形式，也可以得到对应的雅可比矩阵。

---

3：更新（Update / Correction）

(3) 计算卡尔曼增益

先计算观测函数 $h$ 的雅可比矩阵 $H_k$：

$$H_k={\frac{\partial h}{\partial \mathbf{x}}|}_{{\hat{\mathbf{x}}}_{k|k-1}}$$

然后计算创新（残差）协方差：

$$S_k=H_k{P}_{k|k-1}{H}_k^T+R_k$$

最后计算卡尔曼增益：

$$K_k=P_{k|k-1}{H}_k^T{S}_k^{-1}$$

(4) 状态更新

计算新息（测量残差）：

$${\tilde{\mathbf{y}}}_k={\mathbf{z}}_k-h({\hat{\mathbf{x}}}_{k|k-1})$$

然后更新状态估计：

$${\hat{\mathbf{x}}}_{k|k}={\hat{\mathbf{x}}}_{k|k-1}+K_k{\tilde{\mathbf{y}}}_k$$

(5) 协方差更新

$$P_{k|k}=(I-K_k{H}_k)P_{k|k-1}$$

---

三、实例分析

我们在运动公式提到了连续时间运动学和离散时间运动学，各有优劣，其中连续时间运动学是微分方程，难以求解转移矩阵 $A$，需要使用数值解；离散时间运动学这里使用的是加速度恒定模型，位置的二阶导正好是加速度，所以它是二阶精度，但是可以通过泰勒展开求得 $A$ 的解析解。对此这里两种方法都使用来举例说明。对于连续运动学，使用四阶 Runge–Kutta 法来计算数值解，精度达到四阶，且可以避免复杂的数学推导；对于离散运动学，则是直接求得一阶解析解。

1. 转移矩阵与预测

1.1.连续运动学模型：数值解

1.1.1 四阶 Runge–Kutta 法

$$\frac{dy}{dt}=f(t,y),y(t_n)=y_n$$

• $\frac{dy}{dt}$：函数 $y$ 对时间 $t$ 的导数，表示变化率
• $f(t,y)$：给定的函数，描述了系统在任意时刻 $t$ 和状态 $y$ 下的变化率
• $y(t_n)=y_n$：初始条件，表示在时间点 $t_n$ 处的函数值为 $y_n$

目的：

• $y_n$：在时间点 $t_n$ 处的已知函数值（数值解）
• $y_{n+1}$：在下一时间点 $t_{n+1}=t_n+h$ 处的待求函数值
• $h$：时间步长（step size），即 $t_{n+1}-t_n$

经典四阶龙格-库塔法的公式为：

$$y_{n+1}=y_n+\frac{h}{6}(k_1+2k_2+2k_3+k_4)$$

其中：

$$k_1=f(t_n,y_n)$$

• 物理意义：在当前点 $(t_n,y_n)$ 处的斜率

• 这是欧拉法使用的唯一斜率估计
  $$k_2=f\left(t_n+\frac{h}{2},y_n+\frac{h}{2}{k}_1\right)$$

• 物理意义：在区间中点处的斜率估计

• 时间点：$t_n+\frac{h}{2}$（区间中点）

• 状态点：使用 $k_1$ 预测的中点状态 $y_n+\frac{h}{2}{k}_1$
  $$k_3=f\left(t_n+\frac{h}{2},y_n+\frac{h}{2}{k}_2\right)$$

• 物理意义：另一个中点斜率估计，但使用 $k_2$ 来预测中点状态

• 这是对中点斜率的改进估计

  $$k_4=f\left(t_n+h,y_n+h{k}_3\right)$$

• 物理意义：在区间终点处的斜率估计

• 时间点：$t_n+h$（下一时间点）

• 状态点：使用 $k_3$ 预测的终点状态 $y_n+h{k}_3$

1.1.2 数值预测

连续时间下的运动学模型为：

$$f(\mathbf{x})=\frac{d}{dt}\left[\begin{array}{c}\mathbf{p}\\ \mathbf{v}\end{array}\right]=\left[\begin{array}{c}\mathbf{v}\\ -\frac{\alpha }{m}|\mathbf{v}|\mathbf{v}-\mathbf{g}\end{array}\right]$$

如果已知某一时刻 $t_n$ 的状态 ${\mathbf{x}}_{\mathbf{n}}=[x,y,z,v_x,v_y,v_z{]}^T$，则：
使用 RK4 数值积分从时间 $t$ 到 $t+\Delta t$ 进行状态更新：

令 ${\mathbf{x}}_n$ 为当前状态，$\Delta t$ 为时间步长，则：

$$\begin{array}{rl}{\mathbf{k}}_1& =f({\mathbf{x}}_n)\\ {\mathbf{k}}_2& =f\left({\mathbf{x}}_n+\frac{\Delta t}{2}{\mathbf{k}}_1\right)\\ {\mathbf{k}}_3& =f\left({\mathbf{x}}_n+\frac{\Delta t}{2}{\mathbf{k}}_2\right)\\ {\mathbf{k}}_4& =f\left({\mathbf{x}}_n+\Delta t{\mathbf{k}}_3\right)\\ {\mathbf{x}}_{n+1}& ={\mathbf{x}}_n+\frac{\Delta t}{6}({\mathbf{k}}_1+2{\mathbf{k}}_2+2{\mathbf{k}}_3+{\mathbf{k}}_4)\end{array}$$

1.1.3 中心差分法求雅可比矩阵

$${\mathbf{F}}_k=\left[\begin{array}{cccccc}\frac{\partial f_1}{\partial x}& \frac{\partial f_1}{\partial y}& \frac{\partial f_1}{\partial z}& \frac{\partial f_1}{\partial v_x}& \frac{\partial f_1}{\partial v_y}& \frac{\partial f_1}{\partial v_z}\\ \frac{\partial f_2}{\partial x}& \cdots & \cdots & \cdots & \cdots & \frac{\partial f_2}{\partial v_z}\\ \frac{\partial f_3}{\partial x}& \cdots & \cdots & \cdots & \cdots & \frac{\partial f_3}{\partial v_z}\\ \frac{\partial f_4}{\partial x}& \cdots & \cdots & \cdots & \cdots & \frac{\partial f_4}{\partial v_z}\\ \frac{\partial f_5}{\partial x}& \cdots & \cdots & \cdots & \cdots & \frac{\partial f_5}{\partial v_z}\\ \frac{\partial f_6}{\partial x}& \cdots & \cdots & \cdots & \cdots & \frac{\partial f_6}{\partial v_z}\end{array}\right]$$

需要求解雅可比矩阵 ${\mathbf{F}}_k$ ，观察其第一列，是需要扰动x变量，然后求得对应的$f(x)$，即：
令：

• ${\mathbf{x}}^{+}={\mathbf{x}}_k+\epsilon {\mathbf{e}}_1=[x_k+\epsilon ,y_k,z_k,{\dot{x}}_k,{\dot{y}}_k,{\dot{z}}_k{]}^{\top }$
• ${\mathbf{x}}^{-}={\mathbf{x}}_k-\epsilon {\mathbf{e}}_1=[x_k-\epsilon ,y_k,z_k,{\dot{x}}_k,{\dot{y}}_k,{\dot{z}}_k{]}^{\top }$
• ${\mathbf{x}}_{n+1}=rk4\_step({\mathbf{x}}_n,\Delta t)={\mathbf{x}}_n+\frac{\Delta t}{6}({\mathbf{k}}_1+2{\mathbf{k}}_2+2{\mathbf{k}}_3+{\mathbf{k}}_4)$

则：

$${\mathbf{F}}_k[:,0]=\frac{\text{rk4\_step}({\mathbf{x}}^{+},\Delta t)-\text{rk4\_step}({\mathbf{x}}^{-},\Delta t)}{2\times \epsilon }$$

以此类推，即可求得${\mathbf{F}}_k$每一列数据。

1.2 离散运动学模型：解析解

1.2.1 离散公式：预测

我们使用恒定加速度近似（在 $\Delta t$ 内加速度不变），这是最常用的离散化方式：

$$x_{k+1}=f(\mathbf{x})=\left[\begin{array}{c}x+v_x\Delta t+\frac{1}{2}{a}_x(\mathbf{v})(\Delta t{)}^2\\ y+v_y\Delta t+\frac{1}{2}{a}_y(\mathbf{v})(\Delta t{)}^2\\ z+v_z\Delta t+\frac{1}{2}{a}_z(\mathbf{v})(\Delta t{)}^2\\ v_x+a_x(\mathbf{v})\Delta t\\ v_y+a_y(\mathbf{v})\Delta t\\ v_z+a_z(\mathbf{v})\Delta t\end{array}\right]$$

其中加速度为（二次空气阻力 + 重力）：

$$\begin{array}{rl}{a}_x& =-\frac{\alpha }{m}{v}_x|v|\\ a_y& =-\frac{\alpha }{m}{v}_y|v|\\ a_z& =-\frac{\alpha }{m}{v}_z|v|-g\end{array}\text{且}|v|=\sqrt{v_x^2+v_y^2+v_z^2}$$

1.2.2 计算雅可比矩阵${\mathbf{F}}_k=\frac{\partial f}{\partial \mathbf{x}}$

由于 $f$ 只显式依赖于 $\mathbf{x}$ 中的速度分量（位置只出现在前 3 项的 $x,y,z$ 上），雅可比是一个 $6\times 6$ 矩阵：

$${\mathbf{F}}_k=\left[\begin{array}{cccccc}\frac{\partial f_1}{\partial x}& \frac{\partial f_1}{\partial y}& \frac{\partial f_1}{\partial z}& \frac{\partial f_1}{\partial v_x}& \frac{\partial f_1}{\partial v_y}& \frac{\partial f_1}{\partial v_z}\\ \frac{\partial f_2}{\partial x}& \cdots & \cdots & \cdots & \cdots & \frac{\partial f_2}{\partial v_z}\\ \frac{\partial f_3}{\partial x}& \cdots & \cdots & \cdots & \cdots & \frac{\partial f_3}{\partial v_z}\\ \frac{\partial f_4}{\partial x}& \cdots & \cdots & \cdots & \cdots & \frac{\partial f_4}{\partial v_z}\\ \frac{\partial f_5}{\partial x}& \cdots & \cdots & \cdots & \cdots & \frac{\partial f_5}{\partial v_z}\\ \frac{\partial f_6}{\partial x}& \cdots & \cdots & \cdots & \cdots & \frac{\partial f_6}{\partial v_z}\end{array}\right]$$

第 1-3 行：位置更新对状态的偏导

对位置的偏导（$\partial f_i/\partial x_j$）：

• $\frac{\partial f_1}{\partial x}=1$，其余为 0
• 类似地，$\frac{\partial f_2}{\partial y}=1$，$\frac{\partial f_3}{\partial z}=1$

对速度的偏导（$\partial f_i/\partial v_j$）：

以 $f_1=x+v_x\Delta t+\frac{1}{2}{a}_x(\Delta t{)}^2$ 为例：

$$\frac{\partial f_1}{\partial v_x}=\Delta t+\frac{1}{2}(\Delta t{)}^2\frac{\partial a_x}{\partial v_x}$$

需要计算 $\frac{\partial a_x}{\partial v_x}$

---

计算加速度对速度的偏导

令 $|v|=\sqrt{v_x^2+v_y^2+v_z^2}$

对 $a_x=-\frac{\alpha }{m}{v}_x|v|$ 求偏导：

$$\frac{\partial a_x}{\partial v_x}=-\frac{\alpha }{m}\left(|v|+v_x\cdot \frac{v_x}{|v|}\right)=-\frac{\alpha }{m}\left(|v|+\frac{v_x^2}{|v|}\right)$$

$$\frac{\partial a_x}{\partial v_y}=-\frac{\alpha }{m}{v}_x\cdot \frac{v_y}{|v|},\frac{\partial a_x}{\partial v_z}=-\frac{\alpha }{m}{v}_x\cdot \frac{v_z}{|v|}$$

类似可得 $a_y,a_z$ 的偏导。

---

最终雅可比矩阵 ${\mathbf{F}}_k$

令：

• $s=|v|=\sqrt{v_x^2+v_y^2+v_z^2}$
• $\beta =\frac{\alpha }{m}$

定义辅助量：

$$\frac{\partial a_x}{\partial v_x}=-\beta \left(s+\frac{v_x^2}{s}\right),\frac{\partial a_x}{\partial v_y}=-\beta \frac{v_x{v}_y}{s},\frac{\partial a_x}{\partial v_z}=-\beta \frac{v_x{v}_z}{s}$$

其他类似。

---

构造 ${\mathbf{F}}_k$：

$${\mathbf{F}}_k=\left[\begin{array}{cccccc}1& 0& 0& \Delta t+\frac{1}{2}(\Delta t{)}^2\frac{\partial a_x}{\partial v_x}& \frac{1}{2}(\Delta t{)}^2\frac{\partial a_x}{\partial v_y}& \frac{1}{2}(\Delta t{)}^2\frac{\partial a_x}{\partial v_z}\\ 0& 1& 0& \frac{1}{2}(\Delta t{)}^2\frac{\partial a_y}{\partial v_x}& \Delta t+\frac{1}{2}(\Delta t{)}^2\frac{\partial a_y}{\partial v_y}& \frac{1}{2}(\Delta t{)}^2\frac{\partial a_y}{\partial v_z}\\ 0& 0& 1& \frac{1}{2}(\Delta t{)}^2\frac{\partial a_z}{\partial v_x}& \frac{1}{2}(\Delta t{)}^2\frac{\partial a_z}{\partial v_y}& \Delta t+\frac{1}{2}(\Delta t{)}^2\frac{\partial a_z}{\partial v_z}\\ 0& 0& 0& 1+\Delta t\frac{\partial a_x}{\partial v_x}& \Delta t\frac{\partial a_x}{\partial v_y}& \Delta t\frac{\partial a_x}{\partial v_z}\\ 0& 0& 0& \Delta t\frac{\partial a_y}{\partial v_x}& 1+\Delta t\frac{\partial a_y}{\partial v_y}& \Delta t\frac{\partial a_y}{\partial v_z}\\ 0& 0& 0& \Delta t\frac{\partial a_z}{\partial v_x}& \Delta t\frac{\partial a_z}{\partial v_y}& 1+\Delta t\frac{\partial a_z}{\partial v_z}\end{array}\right]$$

这就是“转移矩阵”——它是一个依赖于当前速度的时变雅可比矩阵。

---

2. 观测矩阵 H

由于系统仅能观测到三维位置信息 $(X,Y,Z)$，而状态向量 ${\mathbf{x}}_k$ 包含了位置和速度两个维度的信息，因此需要通过观测矩阵 $\mathbf{H}$ 将高维状态空间映射到低维观测空间。

观测向量定义为：

$${\mathbf{z}}_k=\left[\begin{array}{c}{X}_k\\ Y_k\\ Z_k\end{array}\right]$$

对应的观测矩阵为：

$$\mathbf{H}=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]$$

该矩阵的作用是从 6 维状态向量中提取前 3 维位置信息作为观测值。

3. 过程噪声协方差 Q

$$\mathbf{Q}=\mathrm{diag}(0,0,0,a_1,a_2,a_3)$$

该协方差矩阵体现了物理系统的运动特性：

• 位置维度不引入噪声（假设位置不会发生突变）
• 速度维度存在噪声（反映加速度的不确定性）

物理意义链路：加速度不确定性 → 速度缓慢漂移 → 位置随之变化

参数调节建议：

• 初始值设为较小值（如 0.01）
• 逐步增大直至滤波器稳定收敛

4. 观测噪声协方差 R

静态标定方法

1. 将目标物体置于已知静止位置（如 $[0,0,1]$）
2. 连续采集 $N$ 组观测数据：${\mathbf{z}}_1,{\mathbf{z}}_2,...,{\mathbf{z}}_N$
3. 计算各方向的样本方差：

$${\hat{\sigma }}_x^2=\frac{1}{N-1}\sum _{i=1}^N(z_{i,x}-{\bar{z}}_x{)}^2$$

4. 构建观测噪声协方差矩阵：
   $$\mathbf{R}=\mathrm{diag}({\hat{\sigma }}_x^2,{\hat{\sigma }}_y^2,{\hat{\sigma }}_z^2)$$

动态误差处理

当观测误差具有动态特性时，可根据传感器精度指标确定：

• 若精度为 $\pm 2\text{cm}$，则 ${\sigma }^2=(0.02{)}^2=4\times 10^{-4}$

5. 初始状态量

对于初始状态量 ${\mathbf{x}}_0$ 的设定，通常采用以下两种策略：

策略一（推荐）：基于观测数据计算

• 位置：使用第一次观测数据 ${\mathbf{z}}_1$ 作为初始位置
• 速度：通过前两次观测数据计算得到，${\mathbf{v}}_0={\mathbf{z}}_2-{\mathbf{z}}_1$

这种方法能够提供更接近真实值的初始状态，但需要至少两次观测数据。

策略二：简化设定

• 位置：使用第一次观测数据 ${\mathbf{z}}_1$
• 速度：直接初始化为零向量 $0$

虽然此方法仅需一次观测，但初始误差较大。不过随着卡尔曼滤波的迭代更新，状态估计会逐渐收敛到真实值。

6. 初始协方差矩阵

基于对初始状态不确定性的估计设置：

• 位置不确定性：若标准差为 1 米，则协方差项设为 $1^2=1$
• 速度不确定性：若标准差为 0.5 m/s，则协方差项设为 $0.5^2=0.25$

补充说明：连续时间系统处理

可以使用伴随方程法来计算转移矩阵连续时间的转移矩阵，但是这个计算量非常大，所以一般使用离散时间。

理论基础

状态协方差 $\mathbf{P}(t)$ 的时间演化遵循以下微分方程：

$$\dot{\mathbf{P}}(t)=\mathbf{A}(t)\mathbf{P}(t)+\mathbf{P}(t){\mathbf{A}}^T(t)+{\mathbf{Q}}_c(t)$$

此即伴随方程（或连续 Riccati 方程），其中：

• $\mathbf{A}(t)=\frac{\partial f}{\partial \mathbf{x}}{|}_{\mathbf{x}(t)}$：系统状态转移的连续雅可比矩阵
• ${\mathbf{Q}}_c(t)$：连续过程噪声协方差（单位：每秒）