SPP伪距观测模型的雅可比矩阵H推导

BitTwinkle

18人浏览 · 2026-06-16 09:55:44

BitTwinkle · 2026-06-16 09:55:44 发布

雅可比矩阵 $\mathbf{H} = \frac{\partial \mathbf{H}}{\partial \mathbf{x}}$ 又叫设计矩阵，描述待估参数的系数，一个观测值一行。适用于 GNSS 伪距或载波相位定位，状态向量为接收机位置和钟差。

1. 定义状态向量与观测模型

设接收机状态向量为

$\mathbf{x} = \begin{bmatrix} x_r, \; y_r, \; z_r, \; |\; c\cdot\delta t_r \end{bmatrix}^T$

其中 (x_r, y_r, z_r) 为接收机三维位置， $c\cdot\delta t_r$ 为接收机钟差引起的等效距离误差。

对于第 i 颗卫星（卫星位置已知为 $(x^{s_i}, y^{s_i}, z^{s_i})$ ，伪距观测方程为

$h_i(\mathbf{x}) = \sqrt{(x^{s_i} - x_r)^2 + (y^{s_i} - y_r)^2 + (z^{s_i} - z_r)^2} + c\cdot\delta t_r \underbrace{\,- \, c\cdot\delta t^s \,+\, T \,+\, I}_{spp \,const}$

记几何距离为

$\rho_i = \sqrt{(x^{s_i} - x_r)^2 + (y^{s_i} - y_r)^2 + (z^{s_i} - z_r)^2}$

则

$h_i = \rho_i + c\cdot\delta t_r - c\cdot\delta t^s+ T + I$

2. 对状态向量各分量求偏导

(1) 对接收机位置 x_r 的偏导

$\dfrac{\partial h_i}{\partial x_r} = \dfrac{\partial \rho_i}{\partial x_r} + \dfrac{\partial (c\cdot\delta t_r)}{\partial x_r}\displaystyle$

其中 $\frac{\partial (c\cdot\delta t_r)}{\partial x_r} = 0$ ，且

$\frac{\partial \rho_i}{\partial x_r} = \frac{1}{2} \cdot \frac{2(x^{s_i} - x_r) \cdot (-1)}{\rho_i} = -\frac{x^{s_i} - x_r}{\rho_i}$

因此

$\frac{\partial h_i}{\partial x_r} = -\frac{x_{s_i} - x_r}{r_i}$

(2) 对接收机位置 y_r 的偏导

$\frac{\partial h_i}{\partial y_r} = -\frac{y_{s_i} - y_r}{r_i}$

(3) 对接收机位置 z_r 的偏导

$\frac{\partial h_i}{\partial z_r} = -\frac{z_{s_i} - z_r}{r_i}$

(4) 对钟差项 $c\cdot\delta t_r$ 的偏导

$\frac{\partial h_i}{\partial (c\cdot\delta t_r)} = 0 + 1 = 1$

3. 雅可比矩阵 H 的形式

对于 m 颗可见卫星， $\mathbf{h} = [h_1, h_2, \dots, h_m]^T$ ，则 $\mathbf{H}$ 为 m x 4 矩阵（一个观测值一行），其第 i 行为

$\mathbf{H}_{i,\,:} = \begin{bmatrix} -\dfrac{x^{s_i} - x_r}{\rho_i} & -\dfrac{y^{s_i} - y_r}{\rho_i} & -\dfrac{z^{s_i} - z_r}{\rho_i} & 1 \end{bmatrix}$

定义视线方向的单位向量（接收机看向卫星）：

$\mathbf{e}_i = \begin{bmatrix} \dfrac{x^{s_i} - x_r }{\rho_i} \; \dfrac{y^{s_i} - y_r }{\rho_i} \; \dfrac{z^{s_i} - z_r}{\rho_i} \end{bmatrix}^T$
则偏导数部分为 $-\mathbf{e}_i^T$ ，即

$\mathbf{H}_{i,:} = \begin{bmatrix} -\mathbf{e}_i^T & 1 \end{bmatrix}$

完整矩阵写作：

$\mathbf{H} = \begin{bmatrix} -\frac{x^{s_1} - x_r}{\rho_1} & -\frac{y^{s_1} - y_r}{\rho_1} & -\frac{z^{s_1} - z_r}{\rho_1} & 1 \\ -\frac{x^{s_2} - x_r}{\rho_2} & -\frac{y^{s_2} - y_r}{\rho_2} & -\frac{z^{s_2} - z_r}{\rho_2} & 1 \\ \vdots & \vdots & \vdots & \vdots \\ -\frac{x^{s_m} - x_r}{\rho_m} & -\frac{y^{s_m} - y_r}{\rho_m} & -\frac{z^{s_m} - z_r}{\rho_m} & 1 \end{bmatrix}$

4. 几何意义

\mathbf{H} 的前三列是接收机到卫星方向单位向量的相反数，表示观测值对接收机位置变化的敏感度；最后一列为常数 1，对应接收机钟差的影响。该矩阵用于线性化观测方程，在卡尔曼滤波或最小二乘定位中实现状态更新。

AtomGit开源社区

AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。

更多推荐

山东云弈创峰：重构跨境电商的生成式交互与履约链路

AtomGit开源社区

Gitee 代码扫描推荐：面向国内研发团队的 DevSecOps 质量左移方案评估

值得强调的是，Gitee Scan 并非一个黑盒的单一扫描器——它在方案层支持可复用的规则集组合与质量门禁配置，也就是说，对于同一技术栈的多个仓库，你可以只维护一份扫描方案，然后在不同的扫描任务里直接复用，并通过门禁阈值决定本次提交是否能合入。但从"提高审查覆盖率、补全盲区、统一规范执行尺度"的角度来看，将 AI 队友与 Gitee Scan 的 SAST/SCA 门禁串联起来，确实更接近一个现代