[学习]RTKLib详解：rtkcmn.c与rtkpos.c

Part A、Rrtkcmn.c

一、总体功能

$rtkcmn.c$ 是RTKLIB的核心公共模块，是GNSS数据处理的核心，涵盖了从原始观测数据到高精度定位解算的全流程，依赖高效的矩阵运算与数值方法（如卡尔曼滤波），实现动态环境下的实时定位，主要提供以下四类功能：

1. 时间系统转换：GPS时间、UTC时间、年月日时分秒等格式的相互转换，如$gpst2utc()$
2. 坐标系统转换：地心地固坐标系（ECEF）与大地坐标系（WGS84）的相互转换，如$ecef2pos()$
3. 矩阵运算：提供基础线性代数运算，支持最小二乘解算等算法需求，如$matmul()$
4. 数据校验：实现GNSS通信协议中使用的CRC24Q、CRC32等校验算法，如$crc24q()$

二、关键API列表

分类	函数原型	功能说明
时间转换	void gpst2utc(const gtime_t t, double *utc)	GPS时间转UTC时间
坐标转换	void ecef2pos(const double *r, double *pos)	ECEF转经纬高（WGS84）
矩阵运算	void matcpy(double *A, const double *B, int n, int m)	矩阵复制
数据校验	uint32_t crc24q(const uint8_t *buff, int len)	计算CRC24Q校验值
几何计算	double geodist(const double *rs, const double *rr)	计算卫星与接收机几何距离

三、核心算法实现

1. CRC校验算法：

   • 使用查表法优化计算速度
   • 多项式定义：

     #define POLYCRC24Q 0x1864CFBu  /* CRC24Q多项式 */
     

   • 实现特点：逐字节异或运算，支持数据流式处理

2. 坐标转换算法：

   • 使用迭代法求解大地纬度
     $${\varphi }_{k+1}=\arctan \left(\frac{z+e^{\prime 2}b{\sin }^3\theta }{(p-e^{2}a{\cos }^3\theta )}\right)$$
     其中$p=\sqrt{x^2+y^2}$，$\theta =\arctan (za/pb)$

3. 矩阵运算优化：

   • 内存连续存储提高缓存命中率
   • 使用memcpy实现矩阵快速复制
   • 矩阵求逆采用LU分解法：
     $$A=PLU\Rightarrow A^{-1}=U^{-1}{L}^{-1}{P}^T$$

四、函数功能与参数说明

1. uniqnav

• 功能：清理导航数据中的重复星历，更新载波波长。
• 输入：nav_t *nav（导航数据结构）。
• 输出：无返回值，修改nav中的星历数据与波长表。

2. lsq

• 功能：加权最小二乘估计，求解参数x及协方差矩阵Q。
• 输入：
  • const double *A：设计矩阵的转置（n×m）。
  • const double *y：观测向量（m×1）。
  • int n, m：参数数量与观测数。
• 输出：
  • double *x：估计参数（n×1）。
  • double *Q：协方差矩阵（n×n）。

3. filter

• 功能：卡尔曼滤波更新步骤，计算增益矩阵K并修正状态与协方差。
• 输入：
  • double *x, *P：当前状态与协方差。
  • const double *H, *v, *R：观测矩阵转置、残差、观测噪声协方差。
  • int n, m：状态数与观测数。
• 输出：
  • double *xp, *Pp：更新后的状态与协方差。

4. matmul

• 功能：矩阵乘法（支持转置），计算C = alpha*A*B + beta*C。
• 输入：
  • const char *tr：转置标志（“N”/“T”）。
  • int n, k, m：矩阵维度。
  • double alpha, beta：标量系数。
• 输出：结果矩阵C（n×k）。

5. satazel

• 功能：计算卫星方位角与仰角。
• 输入：
  • const double *pos：接收机地心坐标（纬度、经度、高度）。
  • const double *e：卫星至接收机单位向量（ECEF）。
• 输出：
  • double *azel：方位角（0_{2π）与仰角（-π/2}π/2）。

6. ionmapf

• 功能：电离层映射函数（投影因子），将垂直延迟转换为斜路径延迟。
• 输入：
  • const double *pos：接收机位置（纬度、经度、高度）。
  • const double *azel：卫星方位角/仰角。
• 输出：返回斜路径与垂直路径延迟比值。

7. geodist

• 功能：计算接收机与卫星之间的几何距离（含地球自转修正）。
• 输入：
  • const double *rs：卫星位置（ECEF）。
  • const double *rr：接收机位置（ECEF）。
• 输出：修正后的几何距离（米）。

8. timeadd

• 功能：对时间结构gtime_t增加指定秒数。
• 输入：
  • gtime_t t：原始时间。
  • double sec：秒数（可为负）。
• 输出：更新后的时间结构。

9. dgetrf_ / dgetri_（LAPACK接口）

• 功能：LU分解与矩阵求逆（底层线性代数运算）。
• 输入：
  • int *n：矩阵阶数。
  • double *A：输入矩阵。
• 输出：分解后的矩阵或逆矩阵。

---

五、工作流程说明

4.1 模块在RTKLib中的位置

在RTKlib内部，该模块调关系如下所示：

4.2 整体作用与工作流程分析

该代码是RTKLIB库的核心模块（rtkcmn.c），实现了GNSS数据处理中的基础功能，包括：

1. 导航数据处理：管理卫星轨道、钟差、电离层模型等数据。
2. 观测数据处理：观测值去重、筛选、时间同步。
3. 矩阵运算：线性代数运算（乘法、求逆、最小二乘解）。
4. 卡尔曼滤波：状态更新与协方差矩阵调整。
5. 时空转换：坐标系转换（ECEF/ENU）、时间系统转换（GPS/UTC）。
6. 卫星几何计算：方位角/仰角、相位缠绕修正、潮汐效应。

工作流程：

• 输入：原始观测数据、导航电文、配置参数。
• 处理：通过矩阵运算和卡尔曼滤波优化定位解，结合星历数据计算卫星位置，修正观测值误差（如电离层延迟）。
• 输出：高精度定位结果（位置、速度、时间）及质量指标（DOP值）。

---

4.3 模块内部函数调用关系

---

4.4 关键流程说明

1. 导航数据处理：

   • uniqnav调用uniqeph（GPS）、uniqgeph（GLONASS）、uniqseph（SBAS）去除重复星历，确保每个卫星仅保留最新轨道数据。
   • 调用satwavelen更新各频点载波波长。

2. 最小二乘解算：

   • lsq通过matmul构建法方程Q=A'A，调用matinv求逆矩阵，最终计算参数解x=Q^-1*A*y。

3. 卡尔曼滤波更新：

   • filter将状态变量划分为有效子集（非零且正定协方差），调用filter_计算卡尔曼增益K，更新状态与协方差。

4. 卫星几何计算：

   • satazel通过xyz2enu将ECEF坐标转换为站心坐标系，计算方位角与仰角。

5. 矩阵运算：

   • 使用LAPACK加速时，matmul、matinv调用dgemm_、dgetrf_等底层函数优化性能；否则使用纯C实现。

---

Part B、rtkpos.c

一、整体作用与工作流程分析

rtkpos.c 是 RTKLIB 库中实现高精度实时动态定位（RTK）的核心模块，主要负责从原始观测数据到最终位置解算的全流程处理，它实现了 RTK 定位的核心算法，涵盖状态估计、误差修正、模糊度解析及解算验证。代码结构清晰，模块化程度高，支持多频多系统观测数据处理，适用于高精度定位场景。其核心功能包括：

• 状态估计与卡尔曼滤波：维护状态向量（位置、速度、加速度、电离层延迟、对流层延迟、相位偏差等）及其协方差矩阵，通过时间更新和观测更新进行动态调整。
• 多系统支持：兼容 GPS、GLONASS、Galileo、BeiDou、QZSS 等多频段卫星系统。
• 模糊度解析：采用 LAMBDA 算法或三频宽窄巷（WLNL/TCAR）方法固定整周模糊度，提升定位精度。
• 误差模型与修正：处理电离层延迟、对流层延迟、卫星钟差、硬件偏差等误差项，并通过状态估计进行实时校正。
• 解算验证：通过残差分析、卡方检验等方法验证解算结果的可靠性。

---

二、工作流程

2.1 流程综述

1. 初始化

   • rtkinit：初始化 RTK 控制结构体 rtk_t，分配状态向量和协方差矩阵内存。
   • rtkopenstat：打开解算状态输出文件（如位置、速度、残差等）。

2. 观测数据预处理

   • selsat：选择基站与流动站共同观测的卫星。
   • intpres：对残差进行时间插值（适用于后处理）。

3. 状态时间更新

   • udpos：更新位置、速度、加速度的状态估计（基于动力学模型）。
   • udion：更新电离层延迟参数。
   • udtrop：更新对流层延迟参数。
   • udrcvbias：更新接收机硬件偏差。
   • udbias：更新相位偏差，检测周跳。

4. 残差计算

   • zdres：计算单差残差（相位和伪距），考虑卫星钟差、几何距离、大气延迟等。
   • ddres：构建双差残差及其雅可比矩阵，用于卡尔曼滤波观测更新。

5. 卡尔曼滤波

   • filter：执行卡尔曼滤波更新（未在代码中展示，但被 relpos 和 resamb_LAMBDA 调用），结合预测状态与观测残差。

6. 模糊度解析

   • resamb_LAMBDA：使用 LAMBDA 算法求解整数模糊度，通过比率测试验证结果。
   • resamb_WLNL/TCAR：三频宽巷-窄巷模糊度解析（需外部扩展支持）。

7. 解算验证与输出

   • valpos：验证解算结果的残差是否符合统计阈值。
   • holdamb：保持已固定的模糊度以供后续历元使用。
   • outsolstat：输出解算状态至文件（位置、速度、残差等）。

8. 主流程

   • rtkpos：根据定位模式（单点、RTK、PPP）调用相应的解算函数，如 relpos（相对定位）或 pppos（PPP 定位）。

---

以下是基于 rtkpos.c 的函数调用关系 ：

---

流程说明

1. 主入口函数 rtkpos

   • 根据定位模式（单点、PPP、相对定位、移动基线）调用不同的核心函数。
   • 单点定位：调用 pntpos。
   • PPP 模式：调用 pppos。
   • 相对定位/移动基线：调用 relpos。

2. 相对定位核心 relpos

   • 状态更新：调用 udstate，进一步调用以下子函数：
     • udpos：位置、速度、加速度的状态更新。
     • udion：电离层参数更新。
     • udtrop：对流层参数更新。
     • udrcvbias：接收机硬件偏差更新。
     • udbias：相位偏差更新（含周跳检测）。
   • 残差计算：
     • zdres：单差残差计算。
     • ddres：双差残差及雅可比矩阵构建，依赖：
       • prectrop：对流层延迟模型。
       • gloicbcorr：GLONASS 通道间偏差校正。
       • test_sys：卫星系统类型判断。
   • 卡尔曼滤波：调用 filter（未在代码中展示，但逻辑上存在）。
   • 模糊度解析：根据配置调用 resamb_LAMBDA（默认）或 resamb_WLNL/TCAR。
     • resamb_LAMBDA：LAMBDA 算法求解整数模糊度，依赖：
       • valpos：验证固定解。
       • holdamb：保持已固定模糊度。
   • 解算验证与输出：
     • valpos：验证残差是否符合统计阈值。
     • outsolstat：输出解算状态至文件。

3. 辅助函数

   • selsat：选择基站与流动站的共视卫星。
   • intpres：后处理中的残差时间插值。
   • statlevel：控制输出级别，决定是否生成状态文件。

---

2.2 相对定位流程

流程说明：

• rtkpos 根据定位模式调用 relpos。
• relpos 调用 udstate 更新状态（位置、电离层、对流层、硬件偏差、相位偏差）。
• zdres 和 ddres 计算单差/双差残差，依赖对流层模型 prectrop 和 GLONASS 通道间偏差校正 gloicbcorr。
• filter 执行卡尔曼滤波更新（未显式在代码中展示）。
• resamb_LAMBDA 使用 LAMBDA 算法求解整数模糊度，通过 valpos 验证后调用 holdamb 保持模糊度。

---

2. 3 模糊度解析流程

流程说明：

• resamb_LAMBDA 调用 ddmat 构建双差变换矩阵，依赖外部库 lambda 进行整数最小二乘搜索。
• 通过比率测试（ratio-test）验证结果，成功后调用 holdamb 保持固定解。

---

2.4 状态更新流程

流程说明：

• udstate 依次调用各子函数更新状态参数（位置、电离层、对流层、硬件偏差、相位偏差）。

---

2.5 注意事项

• filter 的具体实现未在代码中展示，但逻辑上是卡尔曼滤波的核心步骤。
• resamb_LAMBDA 依赖的 lambda 是外部库函数（如 CLIB 的 LAMBDA 算法实现）。
• valpos 在 resamb_LAMBDA 和 relpos 中多次调用，用于验证浮点解和固定解的残差。

三、函数详解

1. rtkinit

• 功能：初始化 RTK 控制结构体。
• 输入参数：
  • rtk: RTK 控制结构体指针。
  • opt: 定位选项（如定位模式、误差模型）。
• 输出参数：
  • rtk->x, P: 状态向量和协方差矩阵初始化。
  • rtk->ssat: 卫星状态初始化。

2. relpos

• 功能：执行相对定位（RTK）流程。
• 输入参数：
  • obs: 观测数据数组。
  • nu, nr: 流动站和基站观测数据的数量。
  • nav: 导航数据（卫星轨道、钟差等）。
• 输出参数：
  • rtk->sol: 解算结果（位置、速度、状态）。
  • rtk->x, P: 更新后的状态向量和协方差。
  • rtk->ssat: 卫星状态（残差、有效标志）。

3. udpos

• 功能：更新位置、速度、加速度的状态估计。
• 输入参数：
  • rtk: RTK 结构体。
  • tt: 时间步长。
• 输出参数：
  • rtk->x, P: 状态向量和协方差更新。

4. udion

• 功能：更新电离层延迟参数。
• 输入参数：
  • rtk: RTK 结构体。
  • tt, bl: 时间步长、基线长度。
  • sat, ns: 公共卫星列表及数量。

5. udtrop

• 功能：更新对流层延迟参数（包括梯度项）。
• 输入参数：
  • rtk: RTK 结构体。
  • tt, bl: 时间步长、基线长度。

6. udbias

• 功能：更新相位偏差，检测周跳（通过 LLI、几何自由相位跳跃、多普勒残差）。
• 输入参数：
  • rtk: RTK 结构体。
  • obs: 观测数据。
  • sat, iu, ir: 卫星列表及索引。
  • ns, nav: 公共卫星数、导航数据。

7. zdres

• 功能：计算单差残差（相位和伪距），考虑大气延迟、天线相位中心修正等。
• 输入参数：
  • base: 接收机标识（1: 基站, 0: 流动站）。
  • obs: 观测数据。
  • rs, dts: 卫星位置和钟差。
  • rr: 接收机位置。
  • opt: 定位选项（如电离层模型）。
• 输出参数：
  • y: 残差向量。
  • e, azel: 卫星视线向量、方位角/仰角。

8. ddres

• 功能：构建双差残差及其雅可比矩阵，用于卡尔曼滤波。
• 输入参数：
  • rtk: RTK 结构体。
  • nav: 导航数据。
  • dt: 时间差。
  • x, P: 当前状态和协方差。
  • sat, y, e, azel: 卫星列表、残差、视线向量。
• 输出参数：
  • v, H, R: 残差向量、雅可比矩阵、观测噪声协方差。

9. resamb_LAMBDA

• 功能：使用 LAMBDA 算法求解整数模糊度。
• 输入参数：
  • rtk: RTK 结构体。
  • bias, xa: 模糊度偏差、固定解状态。
• 输出参数：
  • rtk->xa, Pa: 固定解状态和协方差。
  • rtk->sol.stat: 解算状态（浮点解/固定解）。

10. valpos

• 功能：验证解算结果的残差是否符合统计阈值。
• 输入参数：
  • v, R: 残差向量、观测噪声协方差。
  • vflg: 残差标识（卫星对、频率等）。
  • nv, thres: 残差数量、阈值。
• 输出参数：
  • 返回验证结果（成功/失败）。

---

四、关键算法与数据结构

1. 状态向量组织

   • 通过宏 NP, NI, NT, NL, NB 计算不同参数（位置、电离层、对流层、硬件偏差、相位偏差）在状态向量中的索引。

2. 卡尔曼滤波

   • 预测步骤：由 udstate 调用各 ud* 函数更新状态。
   • 更新步骤：通过 ddres 构建观测矩阵并调用 filter 进行卡尔曼增益计算。

3. 模糊度解析

   • LAMBDA 算法通过整数最小二乘（ILS）搜索最优整数解，并通过比率测试验证可靠性。

4. 多系统支持

   • 通过 test_sys 判断卫星所属系统（GPS/GLO/GAL/BDS/QZS）。

---

研究学习不易，点赞易。
工作生活不易，收藏易，点收藏不迷茫 ：）

---